From 83454bd5e6083ae9a0064e1dc9af61a8eced1180 Mon Sep 17 00:00:00 2001
From: koide3 <31344317+koide3@users.noreply.github.com>
Date: Mon, 13 Nov 2023 16:13:52 +0900
Subject: [PATCH] se3 expmap (#139)

Co-authored-by: k.koide <k.koide@aist.go.jp>
---
 .../gicp/impl/lsq_registration_impl.hpp       |  8 ++---
 include/fast_gicp/so3/so3.hpp                 | 29 ++++++++++++++++++-
 2 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/include/fast_gicp/gicp/impl/lsq_registration_impl.hpp b/include/fast_gicp/gicp/impl/lsq_registration_impl.hpp
index 8894bbf..a9fb8ba 100644
--- a/include/fast_gicp/gicp/impl/lsq_registration_impl.hpp
+++ b/include/fast_gicp/gicp/impl/lsq_registration_impl.hpp
@@ -111,9 +111,7 @@ bool LsqRegistration<PointTarget, PointSource>::step_gn(Eigen::Isometry3d& x0, E
   Eigen::LDLT<Eigen::Matrix<double, 6, 6>> solver(H);
   Eigen::Matrix<double, 6, 1> d = solver.solve(-b);
 
-  delta.setIdentity();
-  delta.linear() = so3_exp(d.head<3>()).toRotationMatrix();
-  delta.translation() = d.tail<3>();
+  delta = se3_exp(d);
 
   x0 = delta * x0;
   final_hessian_ = H;
@@ -136,9 +134,7 @@ bool LsqRegistration<PointTarget, PointSource>::step_lm(Eigen::Isometry3d& x0, E
     Eigen::LDLT<Eigen::Matrix<double, 6, 6>> solver(H + lm_lambda_ * Eigen::Matrix<double, 6, 6>::Identity());
     Eigen::Matrix<double, 6, 1> d = solver.solve(-b);
 
-    delta.setIdentity();
-    delta.linear() = so3_exp(d.head<3>()).toRotationMatrix();
-    delta.translation() = d.tail<3>();
+    delta = se3_exp(d);
 
     Eigen::Isometry3d xi = delta * x0;
     double yi = compute_error(xi);
diff --git a/include/fast_gicp/so3/so3.hpp b/include/fast_gicp/so3/so3.hpp
index 5d298aa..66521ee 100644
--- a/include/fast_gicp/so3/so3.hpp
+++ b/include/fast_gicp/so3/so3.hpp
@@ -61,7 +61,7 @@ inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
   double theta;
   double imag_factor;
   double real_factor;
-  if(theta_sq < 1e-10) {
+  if (theta_sq < 1e-10) {
     theta = 0;
     double theta_quad = theta_sq * theta_sq;
     imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;
@@ -76,6 +76,33 @@ inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
   return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());
 }
 
+// Rotation-first
+inline Eigen::Isometry3d se3_exp(const Eigen::Matrix<double, 6, 1>& a) {
+  using std::cos;
+  using std::sin;
+  const Eigen::Vector3d omega = a.head<3>();
+
+  double theta = std::sqrt(omega.dot(omega));
+  const Eigen::Quaterniond so3 = so3_exp(omega);
+  const Eigen::Matrix3d Omega = skewd(omega);
+  const Eigen::Matrix3d Omega_sq = Omega * Omega;
+  Eigen::Matrix3d V;
+
+  if (theta < 1e-10) {
+    V = so3.matrix();
+    /// Note: That is an accurate expansion!
+  } else {
+    const double theta_sq = theta * theta;
+    V = (Eigen::Matrix3d::Identity() + (1.0 - cos(theta)) / (theta_sq)*Omega + (theta - sin(theta)) / (theta_sq * theta) * Omega_sq);
+  }
+
+  Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();
+  se3.linear() = so3.toRotationMatrix();
+  se3.translation() = V * a.tail<3>();
+
+  return se3;
+}
+
 }  // namespace fast_gicp
 
 #endif
\ No newline at end of file