From e881f0ed245f6cbb3aba0affd56c5d840c0455fa Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Mon, 14 Dec 2020 10:00:38 +0100 Subject: [PATCH 1/2] [fix] Change CeresCostFunctions namespace to use different symbols than LV's Strangely, when the SLAM is compiled within LV, some of the cost functions used were LV's, and not SLAM's. Since !66, the cost functions are not compatible with LV's, so in these cases, SLAM completely failed. To ensure that the correct residuals implementations are used, the namespace have been changed so that the symbols differ. This also add the missing include into the CMakeList to properly export headers. --- slam_lib/CMakeLists.txt | 1 + slam_lib/include/LidarSlam/CeresCostFunctions.h | 4 ++-- slam_lib/src/KeypointsRegistration.cxx | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/slam_lib/CMakeLists.txt b/slam_lib/CMakeLists.txt index 8e60e5e0..59da8615 100644 --- a/slam_lib/CMakeLists.txt +++ b/slam_lib/CMakeLists.txt @@ -19,6 +19,7 @@ set(LidarSlam_PUBLIC_HEADERS include/LidarSlam/GlobalTrajectoriesRegistration.h include/LidarSlam/KDTreePCLAdaptor.h include/LidarSlam/KeypointsRegistration.h + include/LidarSlam/CeresCostFunctions.h ${SLAM_g2o_headers} ) diff --git a/slam_lib/include/LidarSlam/CeresCostFunctions.h b/slam_lib/include/LidarSlam/CeresCostFunctions.h index c7518f0d..83d6fd4e 100644 --- a/slam_lib/include/LidarSlam/CeresCostFunctions.h +++ b/slam_lib/include/LidarSlam/CeresCostFunctions.h @@ -27,7 +27,7 @@ // EIGEN #include -namespace CostFunctions +namespace CeresCostFunctions { //------------------------------------------------------------------------------ /** @@ -619,6 +619,6 @@ struct EuclideanDistanceAffineIsometryResidual private: const Eigen::Vector3d X, Y; }; -} // end of namepsace CostFunctions +} // end of namespace CeresCostFunctions #endif // CERES_COST_FUNCTIONS_H \ No newline at end of file diff --git a/slam_lib/src/KeypointsRegistration.cxx b/slam_lib/src/KeypointsRegistration.cxx index 2e634dee..ec62b729 100644 --- a/slam_lib/src/KeypointsRegistration.cxx +++ b/slam_lib/src/KeypointsRegistration.cxx @@ -127,8 +127,8 @@ KeypointsRegistration::RegistrationError KeypointsRegistration::EstimateRegistra void KeypointsRegistration::AddIcpResidual(const Eigen::Matrix3d& A, const Eigen::Vector3d& P, const Eigen::Vector3d& X, double time, double weight) { // The Ceres residuals to use depending on undistortion mode - using RigidResidual = CostFunctions::MahalanobisDistanceAffineIsometryResidual; - using UndistortionResidual = CostFunctions::MahalanobisDistanceInterpolatedMotionResidual; + using RigidResidual = CeresCostFunctions::MahalanobisDistanceAffineIsometryResidual; + using UndistortionResidual = CeresCostFunctions::MahalanobisDistanceInterpolatedMotionResidual; // If OPTIMIZED mode, we need to optimize both start and end poses if (this->Undistortion == UndistortionMode::OPTIMIZED) -- GitLab From d75754a7f68c4b9a5f51f26113d0dbae5c739795 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Mon, 14 Dec 2020 10:07:30 +0100 Subject: [PATCH 2/2] [deprec] Remove unused ceres residuals The CeresCostFunctions.h file comes from LidarView code base. Most of the residuals defined in this file are not used in the SLAM, but only in LV. This commit removes them for more clarity and less maintenance. --- .../include/LidarSlam/CeresCostFunctions.h | 414 ------------------ 1 file changed, 414 deletions(-) diff --git a/slam_lib/include/LidarSlam/CeresCostFunctions.h b/slam_lib/include/LidarSlam/CeresCostFunctions.h index 83d6fd4e..958ec584 100644 --- a/slam_lib/include/LidarSlam/CeresCostFunctions.h +++ b/slam_lib/include/LidarSlam/CeresCostFunctions.h @@ -116,166 +116,6 @@ private: const double Weight; }; -//------------------------------------------------------------------------------ -/** - * \class MahalanobisDistanceLinearDistortionResidual - * \brief Cost function to optimize the affine isometry transformation H1 - * (rotation R1 and translation T1) so that the linearly interpolated - * transform - * (R, T) = (R0^(1-t) * R1^t, (1 - t)T0 + tT1) - * applied to X acquired at time t minimizes the mahalanobis distance - * between X and its neighborhood encoded by the mean point C and the - * variance covariance matrix A. - * - * It takes one 6D parameters block : - * - 3 first parameters to encode translation T1 : X, Y, Z - * - 3 last parameters to encode rotation R1 with euler angles : rX, rY, rZ - */ -struct MahalanobisDistanceLinearDistortionResidual -{ - MahalanobisDistanceLinearDistortionResidual(const Eigen::Matrix3d& argA, - const Eigen::Vector3d& argC, - const Eigen::Vector3d& argX, - const Eigen::Isometry3d& argH0, - double argTime, - double argWeight) - : A(argA) - , C(argC) - , X(argX) - , H0(argH0) - , Weight(argWeight) - , Time(argTime) - {} - - template - bool operator()(const T* const w, T* residual) const - { - using Vector3T = Eigen::Matrix; - using Isometry3T = Eigen::Transform; - - // Create H1 transform in static way. - // The idea is that all residual functions will need to - // evaluate those variables so we will only compute them - // once each time the parameters values change - static Isometry3T H1 = Isometry3T::Identity(); - static T lastW[6] = {T(-1.), T(-1.), T(-1.), T(-1.), T(-1.), T(-1.)}; - static LinearTransformInterpolator transformInterpolator; - - // Update H1 if needed - if (!std::equal(w, w + 6, lastW)) - { - H1.linear() << RotationMatrixFromRPY(w[3], w[4], w[5]); - H1.translation() << w[0], w[1], w[2]; - transformInterpolator.SetH1(H1); - std::copy(w, w + 6, lastW); - } - - // Convert internal double matrix H0 to a Jet matrix for auto diff calculous - Isometry3T H0T = this->H0.cast(); - transformInterpolator.SetH0(H0T); - - // Compute the transform to apply to X depending on (R0, T0) and (R1, T1). - // The applied isometry will be the linear interpolation between them : - // (R, T) = (R0^(1-t) * R1^t, (1 - t)T0 + tT1) - const Isometry3T H = transformInterpolator(T(this->Time)); - - // Compute final residual value which is: - // Yt * A * Y with Y = R(theta) * X + T - C - const Vector3T Y = H.linear() * this->X + H.translation() - this->C; - const T squaredResidual = this->Weight * (Y.transpose() * this->A * Y)(0); - - // Since t -> sqrt(t) is not differentiable in 0, we check the value of the - // distance infenitesimale part. If it is not finite, it means that the - // first order derivative has been evaluated in 0 - residual[0] = squaredResidual < 1e-6 ? T(0) : ceres::sqrt(squaredResidual); - - return true; - } - -private: - const Eigen::Matrix3d A; - const Eigen::Vector3d C; - const Eigen::Vector3d X; - const Eigen::Isometry3d H0; - const double Weight; - const double Time; -}; - -//------------------------------------------------------------------------------ -/** - * \class MahalanobisDistanceIsometryAndLinearDistortionResidual - * \brief Cost function to optimize the rotation R1 and translation T1 so that - * the linearly interpolated transform: - * (R, T) = (R1 * R1^t, t * R1 * T1 + T1) - * applies to X acquired at time t minimizes the mahalanobis distance. - * - * It takes one 6D parameters block : - * - 3 first parameters to encode translation T1 : X, Y, Z - * - 3 last parameters to encode rotation R1 with euler angles : rX, rY, rZ - */ -struct MahalanobisDistanceIsometryAndLinearDistortionResidual -{ - MahalanobisDistanceIsometryAndLinearDistortionResidual(const Eigen::Matrix3d& argA, - const Eigen::Vector3d& argC, - const Eigen::Vector3d& argX, - double argTime, - double argWeight) - : A(argA) - , C(argC) - , X(argX) - , Time(argTime) - , Weight(argWeight) - {} - - template - bool operator()(const T* const w, T* residual) const - { - using Vector3T = Eigen::Matrix; - using Isometry3T = Eigen::Transform; - - // Create H1 transform in static way. - // The idea is that all residual functions will need to - // evaluate those variables so we will only compute them - // once each time the parameters values change - static Isometry3T H1 = Isometry3T::Identity(); - static T lastW[6] = {T(-1.), T(-1.), T(-1.), T(-1.), T(-1.), T(-1.)}; - static LinearTransformInterpolator transformInterpolator; - - // Update H1 if needed - if (!std::equal(w, w + 6, lastW)) - { - H1.linear() << RotationMatrixFromRPY(w[3], w[4], w[5]); - H1.translation() << w[0], w[1], w[2]; - transformInterpolator.SetTransforms(H1, H1 * H1); - std::copy(w, w + 6, lastW); - } - - // Compute the transform to apply to X depending on (R0, T0) and (R1, T1). - // The applied isometry will be the linear interpolation between them : - // (R, T) = (R0^(1-t) * R1^t, (1 - t)T0 + tT1) - const Isometry3T H = transformInterpolator(T(this->Time)); - - // Compute final residual value which is: - // Yt * A * Y with Y = R(theta) * X + T - C - const Vector3T Y = H.linear() * this->X + H.translation() - this->C; - const T squaredResidual = this->Weight * (Y.transpose() * this->A * Y)(0); - - // Since t -> sqrt(t) is not differentiable in 0, we check the value of the - // distance infenitesimale part. If it is not finite, it means that the - // first order derivative has been evaluated in 0 - residual[0] = squaredResidual < 1e-6 ? T(0) : ceres::sqrt(squaredResidual); - - return true; - } - -private: - const Eigen::Matrix3d A; - const Eigen::Vector3d C; - const Eigen::Vector3d X; - const double Time; - const double Weight; -}; - //------------------------------------------------------------------------------ /** * \class MahalanobisDistanceInterpolatedMotionResidual @@ -365,260 +205,6 @@ private: const double Time; const double Weight; }; - -//------------------------------------------------------------------------------ -/** - * \class FrobeniusDistanceRotationCalibrationResidual - * \brief Cost function to optimize the calibration rotation between two sensors - * based on their trajectory. To do that, we exploit the "solid-system" - * constraint that links the coordinate reference frame of the two sensors. - * - * Using the geometric constraints that come from the solid-assumption, it is - * possible to estimate the 3-DoF rotation calibration (R in SO(3)) between - * two sensors by using the estimation of their poses over the time. - * - * Let's t0 and t1 be two times. - * Let's R be the orientation of the sensor 1 according to the sensor 2 reference frame. - * Since we have the solid-assumption, R is constant and not time depending. - * - * Let's P0 (resp P1) in SO(3) be the orientation of the sensor 1 at time t0 (resp t1) - * - * Let's Q0 (resp Q1) in SO(3) be the orientation of the sensor 2 at time t0 (resp t1) - * - * From these two temporal points on the poses "trajectory", it is possible to express - * the change of orientation between the sensor 1 at time t0 and sensor 1 at time t1 - * using two differents way - * - * 1- By using the poses of the sensor 1 at time t0 and t1: - * dR0 = P0' * P1 - * - * 2- By using the solid-assumption and firstly express the point in the - * other sensor reference frame using the calibration parameters. Then, using - * method 1 for the second sensor and finally using the calibration again to - * dR1 = R' * Q0' * Q1 * R - * - * And finally, we are looking for R that satisfy: - * dR1 = dR0 - * - * This lead to a non-linear least square problem that can be solved using a - * levenberg-marquardt algorithm. The non-linearity comes from R belonging to - * SO(3) manifold. We estimate R using the Euler-Angle mapping between R^3 and SO(3) : - * R(rx, ry, rz) = Rz(rz) * Ry(ry) * Rx(rx) - * - * It takes one parameters block of 3 parameters to encode rotation R with euler - * angles : rX, rY, rZ. - */ -struct FrobeniusDistanceRotationCalibrationResidual -{ - FrobeniusDistanceRotationCalibrationResidual(const Eigen::Matrix3d& argP0, - const Eigen::Matrix3d& argP1, - const Eigen::Matrix3d& argQ0, - const Eigen::Matrix3d& argQ1) - : P0(argP0) - , P1(argP1) - , Q0(argQ0) - , Q1(argQ1) - {} - - template - bool operator()(const T* const w, T* residual) const - { - using Matrix3T = Eigen::Matrix; - - // Get rotation matrix from euler angles, in a static way. - // The idea is that all residual functions will need to evaluate those - // sin/cos so we only compute them once each time the parameters change. - static Matrix3T R = Matrix3T::Identity(); - static T lastRot[3] = {T(-1.), T(-1.), T(-1.)}; - if (!std::equal(w, w + 3, lastRot)) - { - R = RotationMatrixFromRPY(w[0], w[1], w[2]); - std::copy(w, w + 3, lastRot); - } - - // Compute the residual matrix - const Matrix3T residualMatrix = R.transpose() * Q0.transpose() * Q1 * R - P0.transpose() * P1; - - // Compute final residual value, which is the frobenius norm of the residual matrix - const T squaredResidual = (residualMatrix.transpose() * residualMatrix).trace(); - - // Since t -> sqrt(t) is not differentiable in 0, we check the value of the - // distance infenitesimale part. If it is not finite, it means that the - // first order derivative has been evaluated in 0 - residual[0] = squaredResidual < 1e-6 ? T(0) : ceres::sqrt(squaredResidual); - - return true; - } - -private: - const Eigen::Matrix3d P0, P1, Q0, Q1; -}; - -//------------------------------------------------------------------------------ -/** - * \class FrobeniusDistanceRotationAndTranslationCalibrationResidual - * \brief Cost function to optimize the calibration rotation and translation - * between two sensors based on their trajectory. To do that, we exploit - * the "solid-system" constraint that links the coordinate reference - * frame of the two sensors. - * - * Using the geometric constraints that come from the solid-assumption, it is - * possible to estimate the 6-DoF calibration (R in SO(3), T in R^3) between - * two sensors by using the estimation of their poses over the time. - * - * Let's t0 and t1 be two times. - * Let's R, T be the pose of the sensor 1 according to the sensor 2 reference frame. - * Since we have the solid-assumption, R and T are constant and not time depending. - * - * Let's P0 (resp P1) in SO(3) be the orientation of the sensor 1 at time t0 (resp t1). - * Let's V0 (resp V1) in R^3 be the position of the sensor 1 at time t0 (resp t1). - * - * Let's Q0 (resp Q1) in SO(3) be the orientation of the sensor 2 at time t0 (resp t1). - * Let's U0 (resp U1) in R^3 be the position of the sensor 2 at time t0 (resp t1). - * - * From these two temporal points on the poses "trajectory", it is possible to - * express the change of reference frame between the sensor 1 at time t0 and - * sensor 1 at time t1 using two differents way : - * - * 1- By using the poses of the sensor 1 at time t0 and t1: - * dR0 = P0' * P1 - * dT0 = P0' * (V1 - V0) - * - * 2- By using the solid-assumption and firstly express the point in the other - * sensor reference frame using the calibration parameters. Then, using - * method 1 for the second sensor and finally using the calibration again to: - * dR1 = R' * Q0' * Q1 * R - * dT1 = R' * (Q0' * (Q1 * T + (U1 - U0)) - T) - * - * And finally, we are looking for (R, T) that satisfies: - * dR1 = dR0 - * dT1 = dT0 - * - * This lead to a non-linear least square problem that can be solved using a - * levenberg-marquardt algorithm. The non-linearity comes from R belonging to - * SO(3) manifold. We estimate R using the Euler-Angle mapping between R^3 and SO(3) - * R(rx, ry, rz) = Rz(rz) * Ry(ry) * Rx(rx) - * - * It takes one 6D parameters block : - * - 3 first parameters to encode translation T : X, Y, Z - * - 3 last parameters to encode rotation R with euler angles : rX, rY, rZ - */ -struct FrobeniusDistanceRotationAndTranslationCalibrationResidual -{ - FrobeniusDistanceRotationAndTranslationCalibrationResidual(const Eigen::Matrix3d& argP0, const Eigen::Matrix3d& argP1, - const Eigen::Matrix3d& argQ0, const Eigen::Matrix3d& argQ1, - const Eigen::Vector3d& argV0, const Eigen::Vector3d& argV1, - const Eigen::Vector3d& argU0, const Eigen::Vector3d& argU1) - : P0(argP0), P1(argP1) - , Q0(argQ0), Q1(argQ1) - , V0(argV0), V1(argV1) - , U0(argU0), U1(argU1) - {} - - template - bool operator()(const T* const w, T* residual) const - { - using Matrix3T = Eigen::Matrix; - using Vector3T = Eigen::Matrix; - - // Get translation part - Eigen::Map trans(&w[0]); - - // Get rotation part from euler angles, in a static way. - // The idea is that all residual functions will need to evaluate those - // sin/cos so we only compute them once each time the parameters change. - static Matrix3T rot = Matrix3T::Identity(); - static T lastRot[3] = {T(-1.), T(-1.), T(-1.)}; - if (!std::equal(w + 3, w + 6, lastRot)) - { - rot = RotationMatrixFromRPY(w[3], w[4], w[5]); - std::copy(w + 3, w + 6, lastRot); - } - - // CHECK : do we need to consider both conditions on R and T ? - const Vector3T dT0 = P0.transpose() * (V1 - V0); - const Vector3T dT1 = rot.transpose() * (Q0.transpose() * (Q1 * trans + (U1 - U0)) - trans); - const T squaredResidual = ((dT0 - dT1).transpose() * (dT0 - dT1))(0); - - // Since t -> sqrt(t) is not differentiable in 0, we check the value of the - // distance infenitesimale part. If it is not finite, it means that the - // first order derivative has been evaluated in 0 - residual[0] = squaredResidual < 1e-6 ? T(0) : ceres::sqrt(squaredResidual); - - return true; - } - -private: - const Eigen::Matrix3d P0, P1, Q0, Q1; - const Eigen::Vector3d V0, V1, U0, U1; -}; - -//------------------------------------------------------------------------------ -/** - * \class EuclideanDistanceAffineIsometryResidual - * \brief Cost function to optimize the rotation and translation between two - * sensors based on their trajectory. To do that we minimize the - * euclidean distance between matched points from the trajectory - * according to the rotational and translational parameters. It results - * an affine transform "best" fit the second trajectory upon the second. - * - * Let's t in R be the current time. - * Let's X be the position of the sensor 1 at time t. - * Met's Y be the position of the sensor 2 at time t. - * - * We want to estimate R in SO(3) and T in R^3 such that - * Y = R*X + T - * - * This lead to a non-linear least square problem that can be solved using a - * levenberg-marquardt algorithm. The non-linearity comes from R belonging to - * SO(3) manifold. We estimate R using the Euler-Angle mapping between R^3 and SO(3) - * R(rx, ry, rz) = Rz(rz) * Ry(ry) * Rx(rx) - * - * It takes one 6D parameters block : - * - 3 first parameters to encode translation T : X, Y, Z - * - 3 last parameters to encode rotation R with euler angles : rX, rY, rZ - */ -struct EuclideanDistanceAffineIsometryResidual -{ - EuclideanDistanceAffineIsometryResidual(const Eigen::Vector3d& argX, const Eigen::Vector3d& argY) - : X(argX) - , Y(argY) - {} - - template - bool operator()(const T* const w, T* residual) const - { - using Matrix3T = Eigen::Matrix; - using Vector3T = Eigen::Matrix; - - // Get translation part - Eigen::Map trans(&w[0]); - - // Get rotation part from euler angles, in a static way. - // The idea is that all residual functions will need to evaluate those - // sin/cos so we only compute them once each time the parameters change. - static Matrix3T rot = Matrix3T::Identity(); - static T lastRot[3] = {T(-1.), T(-1.), T(-1.)}; - if (!std::equal(w + 3, w + 6, lastRot)) - { - rot = RotationMatrixFromRPY(w[3], w[4], w[5]); - std::copy(w + 3, w + 6, lastRot); - } - - const Vector3T L = rot * X + trans - Y; - const T squaredResidual = (L.transpose() * L)(0); - - // Since t -> sqrt(t) is not differentiable in 0, we check the value of the - // distance infenitesimale part. If it is not finite, it means that the - // first order derivative has been evaluated in 0 - residual[0] = squaredResidual < 1e-6 ? T(0) : ceres::sqrt(squaredResidual); - - return true; - } - -private: - const Eigen::Vector3d X, Y; -}; } // end of namespace CeresCostFunctions #endif // CERES_COST_FUNCTIONS_H \ No newline at end of file -- GitLab