// Copyright 2020 The MediaPipe Authors.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mediapipe/modules/face_geometry/libs/procrustes_solver.h"
#include <cmath>
#include <memory>
#include "Eigen/Dense"
#include "absl/memory/memory.h"
#include "mediapipe/framework/port/ret_check.h"
#include "mediapipe/framework/port/status.h"
#include "mediapipe/framework/port/status_macros.h"
#include "mediapipe/framework/port/statusor.h"
namespace mediapipe {
namespace face_geometry {
namespace {
class FloatPrecisionProcrustesSolver : public ProcrustesSolver {
public:
FloatPrecisionProcrustesSolver() = default;
absl::Status SolveWeightedOrthogonalProblem(
const Eigen::Matrix3Xf& source_points, //
const Eigen::Matrix3Xf& target_points, //
const Eigen::VectorXf& point_weights,
Eigen::Matrix4f& transform_mat) const override {
// Validate inputs.
MP_RETURN_IF_ERROR(ValidateInputPoints(source_points, target_points))
<< "Failed to validate weighted orthogonal problem input points!";
MP_RETURN_IF_ERROR(
ValidatePointWeights(source_points.cols(), point_weights))
<< "Failed to validate weighted orthogonal problem point weights!";
// Extract square root from the point weights.
Eigen::VectorXf sqrt_weights = ExtractSquareRoot(point_weights);
// Try to solve the WEOP problem.
MP_RETURN_IF_ERROR(InternalSolveWeightedOrthogonalProblem(
source_points, target_points, sqrt_weights, transform_mat))
<< "Failed to solve the WEOP problem!";
return absl::OkStatus();
}
private:
static constexpr float kAbsoluteErrorEps = 1e-9f;
static absl::Status ValidateInputPoints(
const Eigen::Matrix3Xf& source_points,
const Eigen::Matrix3Xf& target_points) {
RET_CHECK_GT(source_points.cols(), 0)
<< "The number of source points must be positive!";
RET_CHECK_EQ(source_points.cols(), target_points.cols())
<< "The number of source and target points must be equal!";
return absl::OkStatus();
}
static absl::Status ValidatePointWeights(
int num_points, const Eigen::VectorXf& point_weights) {
RET_CHECK_GT(point_weights.size(), 0)
<< "The number of point weights must be positive!";
RET_CHECK_EQ(point_weights.size(), num_points)
<< "The number of points and point weights must be equal!";
float total_weight = 0.f;
for (int i = 0; i < num_points; ++i) {
RET_CHECK_GE(point_weights(i), 0.f)
<< "Each point weight must be non-negative!";
total_weight += point_weights(i);
}
RET_CHECK_GT(total_weight, kAbsoluteErrorEps)
<< "The total point weight is too small!";
return absl::OkStatus();
}
static Eigen::VectorXf ExtractSquareRoot(
const Eigen::VectorXf& point_weights) {
Eigen::VectorXf sqrt_weights(point_weights);
for (int i = 0; i < sqrt_weights.size(); ++i) {
sqrt_weights(i) = std::sqrt(sqrt_weights(i));
}
return sqrt_weights;
}
// Combines a 3x3 rotation-and-scale matrix and a 3x1 translation vector into
// a single 4x4 transformation matrix.
static Eigen::Matrix4f CombineTransformMatrix(const Eigen::Matrix3f& r_and_s,
const Eigen::Vector3f& t) {
Eigen::Matrix4f result = Eigen::Matrix4f::Identity();
result.leftCols(3).topRows(3) = r_and_s;
result.col(3).topRows(3) = t;
return result;
}
// The weighted problem is thoroughly addressed in Section 2.4 of:
// D. Akca, Generalized Procrustes analysis and its applications
// in photogrammetry, 2003, https://doi.org/10.3929/ethz-a-004656648
//
// Notable differences in the code presented here are:
//
// * In the paper, the weights matrix W_p is Cholesky-decomposed as Q^T Q.
// Our W_p is diagonal (equal to diag(sqrt_weights^2)),
// so we can just set Q = diag(sqrt_weights) instead.
//
// * In the paper, the problem is presented as
// (for W_k = I and W_p = tranposed(Q) Q):
// || Q (c A T + j tranposed(t) - B) || -> min.
//
// We reformulate it as an equivalent minimization of the transpose's
// norm:
// || (c tranposed(T) tranposed(A) - tranposed(B)) tranposed(Q) || -> min,
// where tranposed(A) and tranposed(B) are the source and the target point
// clouds, respectively, c tranposed(T) is the rotation+scaling R sought
// for, and Q is diag(sqrt_weights).
//
// Most of the derivations are therefore transposed.
//
// Note: the output `transform_mat` argument is used instead of `StatusOr<>`
// return type in order to avoid Eigen memory alignment issues. Details:
// https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
static absl::Status InternalSolveWeightedOrthogonalProblem(
const Eigen::Matrix3Xf& sources, const Eigen::Matrix3Xf& targets,
const Eigen::VectorXf& sqrt_weights, Eigen::Matrix4f& transform_mat) {
// tranposed(A_w).
Eigen::Matrix3Xf weighted_sources =
sources.array().rowwise() * sqrt_weights.array().transpose();
// tranposed(B_w).
Eigen::Matrix3Xf weighted_targets =
targets.array().rowwise() * sqrt_weights.array().transpose();
// w = tranposed(j_w) j_w.
float total_weight = sqrt_weights.cwiseProduct(sqrt_weights).sum();
// Let C = (j_w tranposed(j_w)) / (tranposed(j_w) j_w).
// Note that C = tranposed(C), hence (I - C) = tranposed(I - C).
//
// tranposed(A_w) C = tranposed(A_w) j_w tranposed(j_w) / w =
// (tranposed(A_w) j_w) tranposed(j_w) / w = c_w tranposed(j_w),
//
// where c_w = tranposed(A_w) j_w / w is a k x 1 vector calculated here:
Eigen::Matrix3Xf twice_weighted_sources =
weighted_sources.array().rowwise() * sqrt_weights.array().transpose();
Eigen::Vector3f source_center_of_mass =
twice_weighted_sources.rowwise().sum() / total_weight;
// tranposed((I - C) A_w) = tranposed(A_w) (I - C) =
// tranposed(A_w) - tranposed(A_w) C = tranposed(A_w) - c_w tranposed(j_w).
Eigen::Matrix3Xf centered_weighted_sources =
weighted_sources - source_center_of_mass * sqrt_weights.transpose();
Eigen::Matrix3f rotation;
MP_RETURN_IF_ERROR(ComputeOptimalRotation(
weighted_targets * centered_weighted_sources.transpose(), rotation))
<< "Failed to compute the optimal rotation!";
MP_ASSIGN_OR_RETURN(
float scale,
ComputeOptimalScale(centered_weighted_sources, weighted_sources,
weighted_targets, rotation),
_ << "Failed to compute the optimal scale!");
// R = c tranposed(T).
Eigen::Matrix3f rotation_and_scale = scale * rotation;
// Compute optimal translation for the weighted problem.
// tranposed(B_w - c A_w T) = tranposed(B_w) - R tranposed(A_w) in (54).
const auto pointwise_diffs =
weighted_targets - rotation_and_scale * weighted_sources;
// Multiplication by j_w is a respectively weighted column sum.
// (54) from the paper.
const auto weighted_pointwise_diffs =
pointwise_diffs.array().rowwise() * sqrt_weights.array().transpose();
Eigen::Vector3f translation =
weighted_pointwise_diffs.rowwise().sum() / total_weight;
transform_mat = CombineTransformMatrix(rotation_and_scale, translation);
return absl::OkStatus();
}
// `design_matrix` is a transposed LHS of (51) in the paper.
//
// Note: the output `rotation` argument is used instead of `StatusOr<>`
// return type in order to avoid Eigen memory alignment issues. Details:
// https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
static absl::Status ComputeOptimalRotation(
const Eigen::Matrix3f& design_matrix, Eigen::Matrix3f& rotation) {
RET_CHECK_GT(design_matrix.norm(), kAbsoluteErrorEps)
<< "Design matrix norm is too small!";
Eigen::JacobiSVD<Eigen::Matrix3f> svd(
design_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f postrotation = svd.matrixU();
Eigen::Matrix3f prerotation = svd.matrixV().transpose();
// Disallow reflection by ensuring that det(`rotation`) = +1 (and not -1),
// see "4.6 Constrained orthogonal Procrustes problems"
// in the Gower & Dijksterhuis's book "Procrustes Analysis".
// We flip the sign of the least singular value along with a column in W.
//
// Note that now the sum of singular values doesn't work for scale
// estimation due to this sign flip.
if (postrotation.determinant() * prerotation.determinant() <
static_cast<float>(0)) {
postrotation.col(2) *= static_cast<float>(-1);
}
// Transposed (52) from the paper.
rotation = postrotation * prerotation;
return absl::OkStatus();
}
static absl::StatusOr<float> ComputeOptimalScale(
const Eigen::Matrix3Xf& centered_weighted_sources,
const Eigen::Matrix3Xf& weighted_sources,
const Eigen::Matrix3Xf& weighted_targets,
const Eigen::Matrix3f& rotation) {
// tranposed(T) tranposed(A_w) (I - C).
const auto rotated_centered_weighted_sources =
rotation * centered_weighted_sources;
// Use the identity trace(A B) = sum(A * B^T)
// to avoid building large intermediate matrices (* is Hadamard product).
// (53) from the paper.
float numerator =
rotated_centered_weighted_sources.cwiseProduct(weighted_targets).sum();
float denominator =
centered_weighted_sources.cwiseProduct(weighted_sources).sum();
RET_CHECK_GT(denominator, kAbsoluteErrorEps)
<< "Scale expression denominator is too small!";
RET_CHECK_GT(numerator / denominator, kAbsoluteErrorEps)
<< "Scale is too small!";
return numerator / denominator;
}
};
} // namespace
std::unique_ptr<ProcrustesSolver> CreateFloatPrecisionProcrustesSolver() {
return absl::make_unique<FloatPrecisionProcrustesSolver>();
}
} // namespace face_geometry
} // namespace mediapipe
Codebase Browser
chromium