mirror of https://github.com/koide3/small_gicp.git
99 lines
3.4 KiB
C++
99 lines
3.4 KiB
C++
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
|
// SPDX-License-Identifier: MIT
|
|
#pragma once
|
|
|
|
#include <Eigen/Core>
|
|
#include <Eigen/Geometry>
|
|
|
|
namespace small_gicp {
|
|
|
|
/// @brief Create skew symmetric matrix
|
|
/// @param x Rotation vector
|
|
/// @return Skew symmetric matrix
|
|
inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) {
|
|
Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
|
|
skew(0, 1) = -x[2];
|
|
skew(0, 2) = x[1];
|
|
skew(1, 0) = x[2];
|
|
skew(1, 2) = -x[0];
|
|
skew(2, 0) = -x[1];
|
|
skew(2, 1) = x[0];
|
|
|
|
return skew;
|
|
}
|
|
|
|
/*
|
|
* SO3 expmap code taken from Sophus
|
|
* https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585
|
|
*
|
|
* Copyright 2011-2017 Hauke Strasdat
|
|
* 2012-2017 Steven Lovegrove
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to
|
|
* deal in the Software without restriction, including without limitation the
|
|
* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
|
|
* sell copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
|
* IN THE SOFTWARE.
|
|
*/
|
|
|
|
/// @brief SO3 expmap.
|
|
/// @param omega [rx, ry, rz]
|
|
/// @return Quaternion
|
|
inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
|
|
double theta_sq = omega.dot(omega);
|
|
|
|
double imag_factor;
|
|
double real_factor;
|
|
if (theta_sq < 1e-10) {
|
|
double theta_quad = theta_sq * theta_sq;
|
|
imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;
|
|
real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad;
|
|
} else {
|
|
double theta = std::sqrt(theta_sq);
|
|
double half_theta = 0.5 * theta;
|
|
imag_factor = std::sin(half_theta) / theta;
|
|
real_factor = std::cos(half_theta);
|
|
}
|
|
|
|
return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());
|
|
}
|
|
|
|
// Rotation-first
|
|
/// @brief SE3 expmap (Rotation-first).
|
|
/// @param a Twist vector [rx, ry, rz, tx, ty, tz]
|
|
/// @return SE3 matrix
|
|
inline Eigen::Isometry3d se3_exp(const Eigen::Matrix<double, 6, 1>& a) {
|
|
const Eigen::Vector3d omega = a.head<3>();
|
|
|
|
const double theta_sq = omega.dot(omega);
|
|
const double theta = std::sqrt(theta_sq);
|
|
|
|
Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();
|
|
se3.linear() = so3_exp(omega).toRotationMatrix();
|
|
|
|
if (theta < 1e-10) {
|
|
se3.translation() = se3.linear() * a.tail<3>();
|
|
/// Note: That is an accurate expansion!
|
|
} else {
|
|
const Eigen::Matrix3d Omega = skew(omega);
|
|
const Eigen::Matrix3d V = (Eigen::Matrix3d::Identity() + (1.0 - std::cos(theta)) / theta_sq * Omega + (theta - std::sin(theta)) / (theta_sq * theta) * Omega * Omega);
|
|
se3.translation() = V * a.tail<3>();
|
|
}
|
|
|
|
return se3;
|
|
}
|
|
|
|
} // namespace small_gicp
|