🎨 integrate Eigen::umeyama

doggydoggy0101 · Jan 26, 2025 · 075abf0 · 075abf0
1 parent d6e66ca
commit 075abf0
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 81 deletions.
diff --git a/registration/include/registration/utils.h b/registration/include/registration/utils.h
@@ -72,16 +72,6 @@ std::pair<PointCloud, Eigen::Vector3d> get_zero_mean_point_cloud(const PointClou
  */
 Eigen::Matrix3d project(const Eigen::Matrix3d mat);
 
-/**
- * @brief Horn's closed-form solution for point cloud registration.
- *
- * @param pcd1 The source point cloud.
- * @param pcd2 The target point cloud.
- *
- * @return The estimated registration.
- */
-Eigen::Matrix4d compute_initial_guess(PointCloud pcd1, PointCloud pcd2);
-
 /**
  * Compute the quadratic matrix of the square of residuals.
  *

diff --git a/registration/src/solver.cpp b/registration/src/solver.cpp
@@ -7,59 +7,6 @@
 
 namespace registration {
 
-QGM::QGM(const size_t& max_iteration, const double& tolerance, const double& threshold_c) {
-  this->max_iter = max_iteration;
-  this->tol = tolerance;
-  this->c = threshold_c;
-}
-
-Eigen::MatrixXd QGM::updateWeight(std::vector<Eigen::MatrixXd>& terms, Eigen::VectorXd& x) {
-  mat_w = Eigen::MatrixXd::Zero(13, 13);
-  for (auto& mat_i : terms) {
-    mat_w +=
-        mat_i / ((x.transpose() * mat_i * x + this->c * this->c) * (x.transpose() * mat_i * x + this->c * this->c));
-  }
-  return mat_w;
-}
-
-Eigen::VectorXd QGM::updateVariable(Eigen::MatrixXd& mat_w) {
-  temp = mat_w.ldlt().solve(e);
-  return (1 / (e.transpose() * temp)) * temp;
-}
-
-Eigen::Matrix4d QGM::solve(const PointCloud& pcd1, const PointCloud& pcd2, const double& noise_bound) {
-  std::vector<Eigen::MatrixXd> terms = compute_residual_terms(pcd1, pcd2, noise_bound * noise_bound);
-
-  Eigen::Matrix4d init_mat = compute_initial_guess(pcd1, pcd2);
-  x = se3_mat_to_vec(init_mat);
-
-  mat_w = Eigen::MatrixXd::Zero(13, 13);
-  for (auto& mat_i : terms) {
-    mat_w += mat_i;
-  }
-
-  double prev_cost = x.transpose() * mat_w * x;
-  double cost = 0.0;
-
-  for (int i = 0; i < this->max_iter; i++) {
-    // weight update
-    mat_w = updateWeight(terms, x);
-    // variable update
-    x = updateVariable(mat_w);
-
-    // stopping criteria
-    cost = x.transpose() * mat_w * x;
-    if (std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) < this->tol) {
-      break;
-    }
-    prev_cost = cost;
-  }
-
-  se3 = se3_vec_to_mat(x);
-  se3.block<3, 3>(0, 0) = project(se3.block<3, 3>(0, 0));
-  return se3;
-}
-
 FracGM::FracGM(const size_t& max_iteration, const double& tolerance, const double& threshold_c) {
   this->max_iter = max_iteration;
   this->tol = tolerance;
@@ -117,7 +64,7 @@ void FracGM::update_terms_cache(std::vector<Fractional>* terms, Eigen::VectorXd*
 Eigen::Matrix4d FracGM::solve(const PointCloud& pcd1, const PointCloud& pcd2, const double& noise_bound) {
   std::vector<Fractional> terms = compute_fractional_terms(pcd1, pcd2, noise_bound * noise_bound, this->c * this->c);
 
-  Eigen::Matrix4d init_mat = compute_initial_guess(pcd1, pcd2);
+  Eigen::Matrix4d init_mat = Eigen::umeyama(pcd1.transpose(), pcd2.transpose(), false);
   x = se3_mat_to_vec(init_mat);
 
   for (int i = 0; i < this->max_iter; i++) {
@@ -141,4 +88,58 @@ Eigen::Matrix4d FracGM::solve(const PointCloud& pcd1, const PointCloud& pcd2, co
   return se3;
 }
 
+
+QGM::QGM(const size_t& max_iteration, const double& tolerance, const double& threshold_c) {
+  this->max_iter = max_iteration;
+  this->tol = tolerance;
+  this->c = threshold_c;
+}
+
+Eigen::MatrixXd QGM::updateWeight(std::vector<Eigen::MatrixXd>& terms, Eigen::VectorXd& x) {
+  mat_w = Eigen::MatrixXd::Zero(13, 13);
+  for (auto& mat_i : terms) {
+    mat_w +=
+        mat_i / ((x.transpose() * mat_i * x + this->c * this->c) * (x.transpose() * mat_i * x + this->c * this->c));
+  }
+  return mat_w;
+}
+
+Eigen::VectorXd QGM::updateVariable(Eigen::MatrixXd& mat_w) {
+  temp = mat_w.ldlt().solve(e);
+  return (1 / (e.transpose() * temp)) * temp;
+}
+
+Eigen::Matrix4d QGM::solve(const PointCloud& pcd1, const PointCloud& pcd2, const double& noise_bound) {
+  std::vector<Eigen::MatrixXd> terms = compute_residual_terms(pcd1, pcd2, noise_bound * noise_bound);
+
+  Eigen::Matrix4d init_mat = Eigen::umeyama(pcd1.transpose(), pcd2.transpose(), false);
+  x = se3_mat_to_vec(init_mat);
+
+  mat_w = Eigen::MatrixXd::Zero(13, 13);
+  for (auto& mat_i : terms) {
+    mat_w += mat_i;
+  }
+
+  double prev_cost = x.transpose() * mat_w * x;
+  double cost = 0.0;
+
+  for (int i = 0; i < this->max_iter; i++) {
+    // weight update
+    mat_w = updateWeight(terms, x);
+    // variable update
+    x = updateVariable(mat_w);
+
+    // stopping criteria
+    cost = x.transpose() * mat_w * x;
+    if (std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) < this->tol) {
+      break;
+    }
+    prev_cost = cost;
+  }
+
+  se3 = se3_vec_to_mat(x);
+  se3.block<3, 3>(0, 0) = project(se3.block<3, 3>(0, 0));
+  return se3;
+}
+
 }  // namespace registration
diff --git a/registration/src/utils.cpp b/registration/src/utils.cpp
@@ -66,23 +66,6 @@ Eigen::Matrix3d project(const Eigen::Matrix3d mat) {
   return rot;
 }
 
-Eigen::Matrix4d compute_initial_guess(PointCloud pcd1, PointCloud pcd2) {
-  std::pair<Eigen::Matrix<double, Eigen::Dynamic, 3>, Eigen::Vector3d> pcd1_mean_pair = get_zero_mean_point_cloud(pcd1);
-  pcd1 << std::get<0>(pcd1_mean_pair);
-  Eigen::Vector3d mean1 = std::get<1>(pcd1_mean_pair);
-
-  std::pair<Eigen::Matrix<double, Eigen::Dynamic, 3>, Eigen::Vector3d> pcd2_mean_pair = get_zero_mean_point_cloud(pcd2);
-  pcd2 << std::get<0>(pcd2_mean_pair);
-  Eigen::Vector3d mean2 = std::get<1>(pcd2_mean_pair);
-
-  Eigen::Matrix4d mat = Eigen::Matrix4d::Identity(4, 4);
-  Eigen::Matrix3d rot = project(pcd2.transpose() * pcd1);
-  mat.block<3, 3>(0, 0) = rot;
-  mat.block<3, 1>(0, 3) = mean2 - rot * mean1;
-
-  return mat;
-}
-
 std::vector<Eigen::MatrixXd> compute_residual_terms(PointCloud pcd1, PointCloud pcd2, double noise_bound_2) {
   std::vector<Eigen::MatrixXd> terms;
   terms.reserve(pcd1.rows());