非线性最小二乘法

最近和学长论文里面的LM算法较上劲了，明明只是构建了简单的雅可比矩阵用CV::solve求解的，通篇鼓吹自己的LM算法，我奇怪了，怎么还有两步LM优化，还说是以上一步中的结果作为下一步优化的约束，明明不存在约束的关系啊，只是把它分成了两步来做。
我实在是难以复现他的过程，我准备自己写一个LM算法。论文里面这一部分是用来优化transformCur这个矩阵数组的，
我们先来把它这个数学模型复现一下，
float x0 = pointSel.x; float x1 = tripod1.x; float x2 = tripod2.x;
float y0 = pointSel.y; float y1 = tripod1.y; float y2 = tripod2.y;
float z0 = pointSel.z; float z1 = tripod1.z; float z2 = tripod2.z;
代码中，x0 是将cornerpointssharp经过transformCur这个矩阵变换，你就是逆时针的yaw,roll,pitch的变化，然后转存成序列点，这个坐标系应该是在雷达坐标系下面完成的，然后将这个功能是在TransformToStart()这个功能下面实现的。论文中又企图去构建一个函数约束关系，即就是x0,x1,x2之间的函数关系，作者假设，x0与x2共线，计算x1 到直线的距离。以这个距离作为优化的约束来求取transformCur里面的几个参数。x1是最近的点，x2是次近点。我现在有点不清楚的是，这三个点之间中究竟哪两个点是同一次扫描，哪一个点是下一次的扫描。我去翻翻原文章。
float m11 = ((x0 - x1)(y1 - y2) - (x1 - x2)(y0 - y1));
float m22 = ((x0 - x1)(z1 - z2) - (x1 - x2)(z0 - z1));
float m33 = ((y0 - y1)(z1 - z2) - (y1 - y2)(z0 - z1));
float a012 = sqrt(m11 * m11 + m22 * m22 + m33 * m33);
float l12 = sqrt((x1 - x2)(x1 - x2) + (y1 - y2)(y1 - y2) + (z1 - z2)*(z1 - z2));
float ld2 = a012 / l12;
上述公式其实就是计算了 x0到 由 x1与x2确定的直线之间的距离。x0所在的边界是咋Pk+1 次扫描中，所以要转化对应到Pk次扫描，然后求解其与由X1与X2确定的直线在Pk 次扫描的距离。虽然论文这一块儿解释的不是很清楚，并且关于点到直线的距离公式推导存在错误。但是先按照代码里面的点的搜索办法来吧。

写到这里罗嗦一两句， pointscornersharp 与 LaserCloudCornerLast 是什么关系，经过我仔细的分析代码，cornerPointsSharp = laserCloudCornerLast; 这一段代码，首先是作为初始化用的，然后，cornerPointsSharp经过功能transformToStart，点存储在pointSquel 里面，然后经过优化计算，新的transformCur人那后 transformToEnd这个功能，及选新的laserCloudCornerLast 并发布; 目前这个数学模型已经很清楚了。我看看能否套用光束平差法中的数学模型，想要把光束平差法的数学模型套用在LEGO——LOAM 中的数学问题上面，就先了解一下两者的数学模型有多么接近。

光束法平差模型：
在解析摄影测量中，将外方位元素（局外点）和模型点坐标（局内点）的计算放在一个整体内进行，此时称其为光束法。光束法平差是以共线方程式作为数学模型，像点的像平面坐标观测值是未知数的非线性函数，经过线性化后按照最小二乘法原理进行计算。该计算也是在提供一个近似解的基础上，逐次迭代来达到趋近于最佳值的。
共线方程式的表达：
设S为摄影中心，在世界坐标系下的坐标为这里写图片描述;M为空间一点，在世界坐标系下的坐标为（X,Y,Z），m是M在影像上的构象，其像平面和像空间辅助坐标分别为（x，y，-f），这里写图片描述，此时可知S、m、M三点共线。可得
但是要把这个模型应用到LEGO_LOAM中的transformCur中的话，u 参数可以是pointSel中的点，其中包括transformCur,但是该点到直线的距离可以改为计算该点的到垂足的距离，垂足刚好在临近点与次临近地那确定的直线上面。

一、对极几何

对极几何（Epipolar geometry）又叫对极约束，是根据图像二维平面信息来估计单目相机帧间运动或双目相机相对位姿关系的一种算法。直观来讲，当相机在两个不同视角对同一物体进行拍摄时，物体在两幅图像中的成像肯定会有不同，那么，根据这两幅不同的图像，我们如何判断出相机的位姿发生了怎样的变化，这正是对极几何要解决的问题。
需要明确的是，在对极几何中，我们的已知条件仅仅是每幅图像中特征点的像素坐标，当然，计算对极约束的前提是我们必须知道两幅图像中特征点之间准确的匹配关系。
  

三、PnP问题

PnP（Perspective-n-Point）是根据图像中特征点的二维像素坐标及其对应的三维空间坐标，来估计相机在参考坐标系中位姿的一类算法。直观来讲，当相机观察到空间中的某一物体时，我们已经知道了该物体在某一参考坐标系下的位置和姿态，那么如何通过图片中物体的成像判断出相机此时在参考坐标系下的位姿？这正是PnP要解决的问题，即利用已知三维结构与图像的对应关系求解相机与参考坐标系的相对关系（相机的外参）。
PnP的过程可描述如下，一堆世界坐标系下的三维地图点P(X,Y,Z)，

<1> 首先经过我们要优化的位姿转换至当前帧的坐标系下P’(X’,Y’,Z’)，

<2> 然后透过相机内参投影至当前帧图像坐标系下p’(x’,y’)，

<3> 最后缩小地图点在当前图像帧上投影点p’与匹配点pmatch(xmatch,ymatch)距离误差，来优化位姿。

上述的过程就是PnP投影过程，实际的优化有若干次这样的迭代。

如果把我现在的问题简化为PnP问题并且加BA中的LM算法优化，这个是最能和源代码衔接上的，做法就是先确定两个点之间的垂直平面并且从pointSel因出来的垂线的垂足刚好在这个边界上面。只是，需要修改一下，PnP问题中的矩阵，把所有的点先投影到这个平面中，刚好接近像PnP问题中的像平面，然后，构造罚函数，雅可比矩阵，就可以了。我决定把公式推到一下，然后吧代码写出来。

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include <iostream>
#include "ceres/ceres.h"


#include "common/tools/rotation.h"
#include "common/projection.h"

//定义误差类，这个类的作用就是在类中重载()运算符，写出误差的计算方式
class SnavelyReprojectionError
{
public:
    //构造函数，主要将观测坐标读进来
    SnavelyReprojectionError(double observation_x, double observation_y):observed_x(observation_x),observed_y(observation_y){}

    //这里定义个模板()函数，定义误差计算方式，这里也很简单，重投影误差，预测值减去观测值就得误差
template<typename T>
    bool operator()(const T* const camera,
                const T* const point,
                T* residuals)const{                  
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    /**
     * 这里用Create()函数生成ceresBundle.cpp中problem->AddResidualBlock()需要的第一个CostFunction*类型的参数.
     * 仔细观察return那一句发现：
     * SnavelyReprojectionError(observed_x,observed_y)就是调用了上方的重载()函数，结果就是个误差
     * ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>()是个模板函数，传入一堆的模板参数：误差类型、输出维度、camera输入维度、point输入维度
     * 那问题来了，整个这一句抽象为框架的话这样的：AutoDiffCostFunction(an_error)，
     * 看起来就是传入一个误差，然后用其构造AutoDiffCostFunction对象，而这个就是problem->AddResidualBlock()需要的CostFunction
     * @return 返回一个CostFunction*，一直没想出CostFunction的很贴切的名字，代价函数太直白不够形象，这里其实有点导函数的意思，但名字上又完全不相关
     */
    static ceres::CostFunction* Create(const double observed_x, const double observed_y)
    {
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>(
                new SnavelyReprojectionError(observed_x,observed_y)));
    }


private:
    //私有成员就是观测的xy坐标了
    double observed_x;
    double observed_y;
};

#endif // SnavelyReprojection.h

然后ceresBundle.cpp

#include <iostream>
#include <fstream>
#include "ceres/ceres.h"

#include "SnavelyReprojectionError.h"
#include "common/BALProblem.h"
#include "common/BundleParams.h"


using namespace ceres;

void SetLinearSolver(ceres::Solver::Options* options, const BundleParams& params)
{
    //linear solver选取
    CHECK(ceres::StringToLinearSolverType(params.linear_solver, &options->linear_solver_type));
    CHECK(ceres::StringToSparseLinearAlgebraLibraryType(params.sparse_linear_algebra_library, &options->sparse_linear_algebra_library_type));
    CHECK(ceres::StringToDenseLinearAlgebraLibraryType(params.dense_linear_algebra_library, &options->dense_linear_algebra_library_type));
    options->num_linear_solver_threads = params.num_threads;

}

//消元顺序设置
void SetOrdering(BALProblem* bal_problem, ceres::Solver::Options* options, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int point_block_size = bal_problem->point_block_size();
    double* points = bal_problem->mutable_points();

    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    double* cameras = bal_problem->mutable_cameras();


    //这里如果设置为自动话直接return
    if (params.ordering == "automatic")
        return;

    //创建个ParameterBlockOrdering类型的对象，在下面按顺序把参数码进去，达到排序的目的，进行按序消元
    ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering;

    // The points come before the cameras
    for(int i = 0; i < num_points; ++i)
       ordering->AddElementToGroup(points + point_block_size * i, 0);

    for(int i = 0; i < num_cameras; ++i)
        ordering->AddElementToGroup(cameras + camera_block_size * i, 1);

    //最后就是用这句设置消元顺序
    options->linear_solver_ordering.reset(ordering);

}

void SetMinimizerOptions(Solver::Options* options, const BundleParams& params)
{
    //最大迭代次数：
    options->max_num_iterations = params.num_iterations;
    //标准输出端输出优化log日志
    options->minimizer_progress_to_stdout = true;
    //设置线程，加速雅克比矩阵计算
    options->num_threads = params.num_threads;
    // options->eta = params.eta;
    // options->max_solver_time_in_seconds = params.max_solver_time;

    //下降策略选取
    CHECK(StringToTrustRegionStrategyType(params.trust_region_strategy,
                                        &options->trust_region_strategy_type));

}

void SetSolverOptionsFromFlags(BALProblem* bal_problem,
                               const BundleParams& params,
                               Solver::Options* options)
{
    //这里其实可以堆一堆的优化选项设置，也就是列一堆options->，只不过根据设置的功能，又单分成了三个函数
    //书上P267的求解设置就是一堆options->
    //这里也可以发现，ceres的设置是比较简单的，定义个option对象，直接一通options->就好了。
    SetMinimizerOptions(options,params);
    SetLinearSolver(options,params);
    SetOrdering(bal_problem, options,params);
}

/**
 * 构建问题，主要是将优化数据和传入problem
 * @param bal_problem 数据
 * @param problem 要构建的优化问题，感觉ceres中的problem就类似于g2o中的optimizer，就是构建出优化问题。
 * @param params 优化所需参数
 */
void BuildProblem(BALProblem* bal_problem, Problem* problem, const BundleParams& params)
{
    //读出路标和相机的维度，
    const int point_block_size = bal_problem->point_block_size();
    const int camera_block_size = bal_problem->camera_block_size();
    //还有路标和相机的数据首位置
    double* points = bal_problem->mutable_points();
    double* cameras = bal_problem->mutable_cameras();

    // Observations is 2 * num_observations long array observations
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x 
    // and y position of the observation. 
    const double* observations = bal_problem->observations();

    for(int i = 0; i < bal_problem->num_observations(); ++i)
    {

        // Each Residual block takes a point and a camera as input 
        // and outputs a 2 dimensional Residual
        //定义problem->AddResidualBlock()函数中需要的cost_function
        CostFunction* cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]);

        // If enabled use Huber's loss function.
        //定义problem->AddResidualBlock()函数中需要的Huber核函数
        LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL;

        // Each observatoin corresponds to a pair of a camera and a point 
        // which are identified by camera_index()[i] and point_index()[i]
        // respectively.
        //定义problem->AddResidualBlock()函数中需要的待估计参数
        double* camera = cameras + camera_block_size * bal_problem->camera_index()[i];
        double* point = points + point_block_size * bal_problem->point_index()[i];

        //往问题中添加误差项：
        /**
         * 看一下函数定义：
         * cost_function： 中文怎么称呼，代价函数?
         * 多说几句这个cost_function，既不是误差，因为他是误差构造出来的。用法上瞅着有点像导数的意味，类似于g2o的雅克比矩阵？
         * loss_function： 损失函数，就是核函数
         * 紧接着的数组就是待优化参数了，由于各参数维度不同，所以类型为double*,有几个就写几个，这里两个，camera和point
         * ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1);
         */
        problem->AddResidualBlock(cost_function, loss_function, camera, point);
    }

}

//这个函数就是整个优化问题被单拉了出来。参数也很原始：待优化数据和优化参数
void SolveProblem(const char* filename, const BundleParams& params)
{
    //同样一开始，用BALProblem类对数据进行处理
    BALProblem bal_problem(filename);

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;

    // store the initial 3D cloud points and camera pose..
    if(!params.initial_ply.empty())
    {
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;

    // add some noise for the intial value
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma, params.translation_sigma,
                        params.point_sigma);

    std::cout << "Normalization complete..." << std::endl;

    //构建最小二乘问题，problem其实就是目标函数
    Problem problem;
    BuildProblem(&bal_problem, &problem, params);

    std::cout << "the problem is successfully build.." << std::endl;

    //优化选项设置
    Solver::Options options;
    //？？
    SetSolverOptionsFromFlags(&bal_problem, params, &options);
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;

    //summary输出优化简报
    Solver::Summary summary;
    //真正的优化就这一句：Solve()函数，传入选项设置、目标函数、简报输出变量。
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";

    // write the result into a .ply file.   
    if(!params.final_ply.empty())
    {
        bal_problem.WriteToPLYFile(params.final_ply);  // pay attention to this: ceres doesn't copy the value into optimizer, but implement on raw data! 
    }
}

int main(int argc, char** argv)
{    
    BundleParams params(argc,argv);  // set the parameters here.

    google::InitGoogleLogging(argv[0]);
    std::cout << params.input << std::endl;
    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input <path for dataset>";
        return 1;
    }

    SolveProblem(params.input.c_str(), params);

    return 0;
}

四、ICP问题

ICP（Iterative Closest Point）是根据前后两帧图像中匹配好的特征点在相机坐标系下的三维坐标，求解相机帧间运动的一种算法。直观来讲，当相机在某处观察某一物体时，我们知道了相机此时与物体之间的相对位姿关系；当相机运动到另一处，我们亦知道此时相机与物体的相对位姿关系，那么，如何通过这两次相机与物体的相对位姿关系来确定相机发生了怎样的运动？这正是ICP要解决的问题。

在ICP问题中，图像信息仅仅用来做特征点的匹配，而并不参与视图几何的运算。也就是说，ICP问题的求解用不到相机的内参与特征点的像素坐标。

#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool next_iteration = false;

void
print4x4Matrix (const Eigen::Matrix4d & matrix)
{
  printf ("Rotation matrix :\n");
  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
  printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
  printf ("Translation vector :\n");
  printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}

void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
                       void* nothing)
{
  if (event.getKeySym () == "space" && event.keyDown ())
    next_iteration = true;
}

int
main ()
{
  // The point clouds we will be using
  PointCloudT::Ptr cloud_in (new PointCloudT);  // Original point cloud
  PointCloudT::Ptr cloud_tr (new PointCloudT);  // Transformed point cloud
  PointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP output point cloud


  int iterations = 1;  // Default number of ICP iterations


  pcl::console::TicToc time;
  time.tic ();
  if (pcl::io::loadPLYFile ("fish-2.ply", *cloud_in) < 0)
  {
    PCL_ERROR ("Error loading cloud %s.\n");
    return (-1);
  }
  std::cout << "\nLoaded file " << "fish-2.ply" << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;

  // Defining a rotation matrix and translation vector
  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();

  // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
  double theta = M_PI / 8;  // The angle of rotation in radians
  transformation_matrix (0, 0) = cos (theta);
  transformation_matrix (0, 1) = -sin (theta);
  transformation_matrix (1, 0) = sin (theta);
  transformation_matrix (1, 1) = cos (theta);

  // A translation on Z axis (0.4 meters)
  transformation_matrix (2, 3) = 0.4;

  // Display in terminal the transformation matrix
  std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
  print4x4Matrix (transformation_matrix);

  // Executing the transformation
  pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);
  *cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use

  // The Iterative Closest Point algorithm
  time.tic ();
  pcl::IterativeClosestPoint<PointT, PointT> icp;
  icp.setMaximumIterations (iterations);
  icp.setInputSource (cloud_icp);
  icp.setInputTarget (cloud_in);
  icp.align (*cloud_icp);
  icp.setMaximumIterations (1);  // We set this variable to 1 for the next time we will call .align () function
  std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;

  if (icp.hasConverged ())
  {
    std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
    std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
    transformation_matrix = icp.getFinalTransformation ().cast<double>();
    print4x4Matrix (transformation_matrix);
  }
  else
  {
    PCL_ERROR ("\nICP has not converged.\n");
    return (-1);
  }

  // Visualization
  pcl::visualization::PCLVisualizer viewer ("ICP demo");
  // Create two vertically separated viewports
  int v1 (0);
  int v2 (1);
  viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);

  // The color we will be using
  float bckgr_gray_level = 0.0;  // Black
  float txt_gray_lvl = 1.0 - bckgr_gray_level;

  // Original point cloud is white
  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,
                                                                             (int) 255 * txt_gray_lvl);
  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

  // Transformed point cloud is green
  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
  viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

  // ICP aligned point cloud is red
  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 180, 20, 20);
  viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

  // Adding text descriptions in each viewport
  viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
  viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

  std::stringstream ss;
  ss << iterations;
  std::string iterations_cnt = "ICP iterations = " + ss.str ();
  viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

  // Set background color
  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

  // Set camera position and orientation
  viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
  viewer.setSize (1280, 1024);  // Visualiser window size

  // Register keyboard callback :
  viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);

  // Display the visualiser
  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();

    // The user pressed "space" :
    if (next_iteration)
    {
      // The Iterative Closest Point algorithm
      time.tic ();
      icp.align (*cloud_icp);
      std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;

      if (icp.hasConverged ())
      {
        printf ("\033[11A");  // Go up 11 lines in terminal output.
        printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
        std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
        transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
        print4x4Matrix (transformation_matrix);  // Print the transformation between original pose and current pose

        ss.str ("");
        ss << iterations;
        std::string iterations_cnt = "ICP iterations = " + ss.str ();
        viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
        viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
      }
      else
      {
        PCL_ERROR ("\nICP has not converged.\n");
        return (-1);
      }
    }
    next_iteration = false;
  }
  return (0);
}

如果享用ICP这个模型，可以考虑看看用ICP+GPU，只是就是计算PointSharpCorner以及 LaserCornerLast ,初始的Guess就是TransformCur[]，我得先把代码看懂才能，套用他那么模型。

五、总结

1、对极几何（2D-2D）利用两帧图像中n对特征点的二维像素坐标，估计相机的相对运动R、t，它一般只在单目SLAM初始化的时候用到。
2、三角测量利用两帧图像中匹配特征点的像素坐标以及两个相机之间的相对位姿，估计特征点的三维空间坐标，这在单目以及双目（多目）的SLAM中都非常重要。
3、PnP（2D-3D）利用图像中对特征点的二维像素坐标和与之对应的三维空间坐标，估计相机在空间的位置和姿态，是最重要的一种位姿估计方法。
4、ICP（3D-3D）利用n对特征点在不同相机坐标系下的三维坐标，估计相机之间的相对位姿，适用于RGB-D SLAM和激光SLAM（从原理上来说）。
5、以上种种，要想得到正确的结果，前提是准确的特征匹配以及理想的相机内参（包括针孔模型参数和畸变参数）。然而事实上，我们得到的往往是有误差或者有噪声的结果，所以通常称前面的工作为前端，因为后面还有很多事情要做，那就是后端的优化——从混乱中找寻真理。