多种形式ICP问题的ceres实例应用

ICP方法主要解决空间点云3D-3D的运动估计问题，
已知：$t_1$和$t_2$时刻感知到的两个点云信息（基于传感器坐标系），和$t_1$时刻的传感器位于全局坐标系下的坐标$p_1$。
求解：传感器$t_2$时刻相对$t_1$时刻的位姿变化。
通常，在点云配准过程中，将$t_2$时刻的感知点云称为source，将$t_1$时刻的感知点云称为target。
解决ICP问题的基本步骤如下：

1. 确定$t_1$、$t_2$时刻之间的传感器位姿变化初值$R_0$、$t_0$，实车案例中取imu与轮速计融合计算的dr值。
2. 将$t_2$时刻的点云数据，即source点云，利用位姿变化初值坐标转换至$t_1$时刻target点云的坐标系下。
3. 将target点云构建为KDtree，坐标转换后的source点云在KDtree${}_{target}$中找到最近点，完成数据关联。
4. 构建代价函数，进行R、t优化。代价函数有多种形式，点对点的欧氏距离，点对线的垂直距离以及矢量的欧氏距离和夹角，最后一种形式在利用库位检测信息时将每个库位简化为一个矢量点进行优化。
5. 迭代优化。将第4步优化后的R、t作为第1步的位姿变换值，重新第2、3、4步，直到前后两次迭代的R、t变化值满足要求。

利用KDtree解决数据关联问题，在另一个博客中已经介绍
KDtree相关库nanoflann的应用

以下代码为利用ceres解决优化问题的示例：

void ScanAlign::Align(LocalizedRangeScan* pSourceScan, LocalizedRangeScan* pTargetScan,Pose2 &rBestpose, Eigen::Vector3d& rDelta)
{SetSource(pSourceScan);SetTarget(pTargetScan);// construct posetransform from  src to current.Eigen::Vector3d ypr_w_src(pSourceScan->GetPredictPose().GetYaw_Rad(),0,0);Eigen::Vector3d t_w_src(pSourceScan->GetPredictPose().GetX(),pSourceScan->GetPredictPose().GetY(),0);common::PoseTransformation pose_w_src(ypr_w_src, t_w_src);// construct posetransform from  target to current.Eigen::Vector3d ypr_w_target(pTargetScan->GetCorrectedPose().GetYaw_Rad(),0,0);Eigen::Vector3d t_w_target(pTargetScan->GetCorrectedPose().GetX(),pTargetScan->GetCorrectedPose().GetY(),0);common::PoseTransformation pose_w_target(ypr_w_target, t_w_target);// src frame relatived to target frame. raw statecurrent_transform_ = pose_w_src.GetTransform(pose_w_target);last_transform_ = current_transform_;for (size_t opti_counter = 0; opti_counter < 10; ++opti_counter){// 数据关联：统一在target坐标系下，确定source中每个点在target中的最近点Association();// Build The Problemceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;options.minimizer_progress_to_stdout = false;//options.max_solver_time_in_seconds = SOLVER_TIME * 3;options.max_num_iterations = 100;ceres::Solver::Summary summary;ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);// 二维问题解决过参数化问题时，由于只使用 yaw角，故使用AngleLocalParameterization；// 三维问题使用四元数时，则使用 QuaternionParameterizationceres::LocalParameterization* angle_local_parameterization =  ceres::slam2d::AngleLocalParameterization::Create();//Add parameter blocksproblem.AddParameterBlock(current_transform_.ypr_.data(), 1, angle_local_parameterization);problem.AddParameterBlock(current_transform_.t_.data(), 2);double *t_array     = current_transform_.t_.data();double *euler_array = current_transform_.ypr_.data();for(size_t src_index=0; src_index<m_vAssociation.size(); src_index++){int target_index = m_vAssociation.at(src_index).first;if(target_index == -1)  //no match is found for this source pointcontinue;CloudPoint src_pt    = m_stCloudPoints_src.at(src_index);CloudPoint target_pt = m_stCloudPoints_target.at(target_index);if(src_pt.type == 1)  // parkingsite{//point to point constraintEigen::DiagonalMatrix<double, 2> cov(0.1, 0.1);Eigen::Matrix2d  sqrt_information = cov;sqrt_information = sqrt_information.inverse().llt().matrixL();double weight = 1.0;ceres::CostFunction* cost_function = ceres::slam2d::PP2dErrorTerm::Create(src_pt.x, src_pt.y, target_pt.x, target_pt.y,sqrt_information, weight);problem.AddResidualBlock(cost_function, loss_function, euler_array, t_array);}else if(src_pt.type == 2)  // laneline{//point to line constraintEigen::Matrix<double, 1, 1>  sqrt_information;sqrt_information(0, 0) = std::sqrt(1/0.1);double weight = 1.0;ceres::CostFunction* cost_function = ceres::slam2d::PL2dErrorTerm::Create(src_pt.x, src_pt.y,target_pt.x, target_pt.y,target_pt.norm_x, target_pt.norm_y,sqrt_information, weight);problem.AddResidualBlock(cost_function, loss_function, euler_array, t_array);}} // end add ResidualBlock//Solve the problemceres::Solve(options, &problem, &summary);//update current transformation, ypr and t are updated by optimizercurrent_transform_.UpdateFromEulerAngle_Translation();//check if we have convergedcommon::PoseTransformation estimation_change = current_transform_.GetTransform(last_transform_);last_transform_ = current_transform_;if(estimation_change.t_.norm() < 1e-5 && estimation_change.ypr_.norm()< 1e-5){break;}} //end iteration (association and optimization cycles)//we need to update the poseinfo object as wellpose_w_src = pose_w_target*current_transform_;rBestpose.SetX(pose_w_src.t_[0]);rBestpose.SetY(pose_w_src.t_[1]);rBestpose.SetYaw_Rad(pose_w_src.ypr_[0]);rDelta[0] = current_transform_.t_[0];   // delta xrDelta[1] = current_transform_.t_[1];   // delta yrDelta[2] = current_transform_.ypr_[0]; // delta yaw// cout icp result
//    std::cout<<"icp_target_src :  x = "<<current_transform_.t_[0]
//            <<" y = "<<current_transform_.t_[1]
//           <<" z = "<<current_transform_.t_[2]
//          <<" yaw = "<<current_transform_.ypr_[0]
//         <<" pitch = "<<current_transform_.ypr_[1]
//        <<" roll = "<<current_transform_.ypr_[2]<<std::endl<<std::endl;}

针对不同的代价计算方式，我写了三种costfunction的仿函数。
第一种是点到点的欧式距离，也是最基本的icp形式

// 点到点
struct PP2dErrorTerm
{
public:PP2dErrorTerm(double obs_src_x, double obs_src_y, double obs_target_x, double obs_target_y,Eigen::Matrix2d sqrt_information, double weight=1.0):obs_src_x_(obs_src_x), obs_src_y_(obs_src_y), obs_target_x_(obs_target_x),obs_target_y_(obs_target_y),sqrt_information_(sqrt_information),weight_(weight){}template <typename T>bool operator()(const T* yaw, const T* t_param, T* residuals) const{const Eigen::Matrix<T, 2, 1> t(t_param[0], t_param[1]);const Eigen::Matrix<T, 2, 2> R = ceres::slam2d::RotationMatrix2D(*yaw);const Eigen::Vector2d obs_src_t(obs_src_x_, obs_src_y_);const Eigen::Vector2d obs_target_t(obs_target_x_, obs_target_y_);Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals_map(residuals);residuals_map.template head<2>() = (R * obs_src_t + t - obs_target_t.cast<T>());//weight these constraintsresiduals_map = residuals_map * weight_;// Scale the residuals by the square root information matrix to account for// the measurement uncertainty.residuals_map = sqrt_information_.template cast<T>() * residuals_map;return true;}static ceres::CostFunction* Create(double obs_src_x, double obs_src_y, double obs_target_x, double obs_target_y,Eigen::Matrix2d sqrt_information, double weight=1.0){return (new ceres::AutoDiffCostFunction<PP2dErrorTerm, 2, 1, 2>(new PP2dErrorTerm(obs_src_x, obs_src_y, obs_target_x, obs_target_y, sqrt_information, weight)));}double obs_src_x_, obs_src_y_;double obs_target_x_, obs_target_y_;Eigen::Matrix2d  sqrt_information_;double weight_;
};

第二种是点到线的垂直距离，我的计算方式是先求出 target点云中每个点的法向量，然后数据关联后，利用法向量快速求解点到线的距离。其中利用了PCA主成分分析方法计算法向量，在后续博客会更新

// 点到线
struct PL2dErrorTerm
{
public:PL2dErrorTerm(double obs_src_x, double obs_src_y, double obs_target_x, double obs_target_y,double obs_target_norm_x, double obs_target_norm_y,Eigen::Matrix<double, 1, 1> sqrt_information, double weight=1.0):obs_src_x_(obs_src_x), obs_src_y_(obs_src_y), obs_target_x_(obs_target_x),obs_target_y_(obs_target_y),obs_target_norm_x_(obs_target_norm_x),obs_target_norm_y_(obs_target_norm_y),sqrt_information_(sqrt_information),weight_(weight){}template <typename T>bool operator()(const T* const yaw, const T* t_param, T* residuals) const{const Eigen::Matrix<T, 2, 1> t(t_param[0], t_param[1]);const Eigen::Matrix<T, 2, 2> R = ceres::slam2d::RotationMatrix2D(*yaw);const Eigen::Vector2d obs_src_t(obs_src_x_, obs_src_y_);const Eigen::Vector2d obs_target_t(obs_target_x_, obs_target_y_);const Eigen::Vector2d obs_target_norm(obs_target_norm_x_, obs_target_norm_y_);residuals[0] = (R * obs_src_t + t - obs_target_t ).dot(obs_target_norm);//weight these constraintsresiduals[0] = residuals[0] * weight_;// Scale the residuals by the square root information matrix to account for// the measurement uncertainty.residuals[0] = sqrt_information_(0, 0) * residuals[0];return true;}static ceres::CostFunction* Create(double obs_src_x, double obs_src_y, double obs_target_x, double obs_target_y,double obs_target_norm_x, double obs_target_norm_y,Eigen::Matrix<double, 1, 1> sqrt_information, double weight=1.0){return (new ceres::AutoDiffCostFunction<PL2dErrorTerm, 1, 1, 2>(new PL2dErrorTerm(obs_src_x, obs_src_y, obs_target_x, obs_target_y,obs_target_norm_x, obs_target_norm_y,sqrt_information, weight)));}double obs_src_x_, obs_src_y_;double obs_target_x_, obs_target_y_;double obs_target_norm_x_, obs_target_norm_y_;Eigen::Matrix<double, 1, 1>  sqrt_information_;double weight_;
};

第三种是矢量点的欧式距离与夹角，这是在实际应用中，将库位信息简化为一个带方向的二维点，以此进行更准确快速的库位关联

// 矢量形式的点到点
struct Vector2dErrorTerm
{
public:Vector2dErrorTerm(double obs_src_x, double obs_src_y, double obs_src_yaw,double obs_target_x, double obs_target_y,double obs_target_yaw,Eigen::Matrix3d sqrt_information, double weight=1.0):obs_src_x_(obs_src_x), obs_src_y_(obs_src_y), obs_src_yaw_(obs_src_yaw),obs_target_x_(obs_target_x),obs_target_y_(obs_target_y),obs_target_yaw_(obs_target_yaw),sqrt_information_(sqrt_information),weight_(weight){}template <typename T>bool operator()(const T* const yaw, const T* t_param, T* residuals) const{const Eigen::Matrix<T, 2, 1> t(t_param[0], t_param[1]);const Eigen::Matrix<T, 2, 2> R = RotationMatrix2D(*yaw);const Eigen::Vector2d obs_src_t(obs_src_x_, obs_src_y_);const Eigen::Vector2d obs_target_t(obs_target_x_, obs_target_y_);const Eigen::Matrix<T, 2, 2> obs_src_R = RotationMatrix2D(static_cast<T>(obs_src_yaw_));Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals);residuals_map.template head<2>() = (R * obs_src_t + t - obs_target_t.cast<T>());const Eigen::Matrix<T, 2, 2> expected_target_R = R * obs_src_R;residuals_map(2) = ceres::slam2d::NormalizeAngle(RotationMatrixToYaw(expected_target_R) - static_cast<T>(obs_target_yaw_));//weight these constraintsresiduals_map = residuals_map * weight_;// Scale the residuals by the square root information matrix to account for// the measurement uncertainty.residuals_map = sqrt_information_.template cast<T>() * residuals_map;return true;}static ceres::CostFunction* Create(double obs_src_x, double obs_src_y, double obs_src_yaw,double obs_target_x, double obs_target_y, double obs_target_yaw,Eigen::Matrix3d sqrt_information, double weight=1.0){return (new ceres::AutoDiffCostFunction<Vector2dErrorTerm, 3, 1, 2>(new Vector2dErrorTerm(obs_src_x, obs_src_y,  obs_src_yaw,obs_target_x, obs_target_y,obs_target_yaw,sqrt_information, weight)));}double obs_src_x_, obs_src_y_, obs_src_yaw_;double obs_target_x_, obs_target_y_, obs_target_yaw_;Eigen::Matrix3d  sqrt_information_;double weight_;
};