diff --git a/slam_lib/CMakeLists.txt b/slam_lib/CMakeLists.txt index 8e60e5e04c65726773e98e4f28465337546fe7b7..59da8615b6d1d412fcf54cd2e3e2959885cc4523 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 c7518f0db6bc54e3eab869b2c8c38022d2e6ea0b..958ec584b449b11023bc50b4fe053082f37b5101 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 { //------------------------------------------------------------------------------ /** @@ -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 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 2e634deed63a9feedb9d7c669eeffbbee0898b54..ec62b7298f09526b361f0cc2b96a6d49bf10d5a9 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)