VINS-Mono阅读笔记一

VINS-Mono阅读笔记一
标签： vslam IMU

1. 概要

---

本阅读笔记将围绕vins-mono的ros功能包展开介绍，第二部分介绍camera_model的相机球体模型，抽出其中的鱼眼相机模型进行详细分析；eature_model模块介绍前端的特征点检测算法及光流跟踪模块；vis_estimator介绍算法的后端主体部分，涉及相机、IMU外参数标定，状态初始化，数据对齐，非线性优化等内容；pose_graph介绍图优化相关的内容。

2. camera_model

---

vins-mono支持的相机格式为等距投影模型的鱼眼相机模型（与opencv的鱼眼模型一致），针孔相机模型及全景相机模型，下面将比较vins鱼眼相机模型中的代码与opencv中的鱼眼相机模型代码，并作详细分析。　 图2.1显示了等距模型的鱼眼相机成像模型，$P_c$为相机坐标系下的点，成像平面A垂直于光轴Z且与球体相切。绿点$P_{undistorted}$为$P_c$归一化后的3D点，前两维坐标即为畸变矫正归一化后的像素坐标点。黄点为入射光线与单位球体的交点，其与球体上顶点构成的弧线向成像平面等距投影后的蓝点$P_{distorted}$即为畸变图像归一化后的像素坐标点。下面代码为vins中的等距鱼眼模型代码：

    template<typename T>
　　// 畸变系数拟合
    T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)
    {
        // k1 = 1
        return theta +
               k2 * theta * theta * theta +
               k3 * theta * theta * theta * theta * theta +
               k4 * theta * theta * theta * theta * theta * theta * theta +
               k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;
    }
    
    // 将2D畸变原图坐标转为投影平面的3D坐标（矫正过程）
    void EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
    {
        // Lift points to normalised plane
        Eigen::Vector2d p_u;
        p_u << m_inv_K11 * p(0) + m_inv_K13,
               m_inv_K22 * p(1) + m_inv_K23;
    
        // Obtain a projective ray
        double theta, phi;
        backprojectSymmetric(p_u, theta, phi);
    
        P(0) = sin(theta) * cos(phi);
        P(1) = sin(theta) * sin(phi);
        P(2) = cos(theta);
    }
    
    // 将射线上的3D点投影到成像平面（投影过程）
    void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const
    {
        double theta = acos(P(2) / P.norm());
        double phi = atan2(P(1), P(0));
    
        Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));
    
        // Apply generalised projection matrix
        p << mParameters.mu() * p_u(0) + mParameters.u0(),
             mParameters.mv() * p_u(1) + mParameters.v0();
    }

下图为OPENCV对鱼眼相机模型的描述，采用同vins相同的等距投影模型，其中$x^{‘}$与$y^{‘}$为上述代码中的p_u向量，即：归一化的畸变平面像素坐标$P_{distorted}$。其计算公式利用图2.1中黄色、绿色三角形相似计算而得，同vins中EquidistantCamera::spaceToPlane函数利用$\varphi$计算一致。
　　

3. feature_model

---

feature_model功能包入口函数为feature_tracker_node.cpp中的main函数，其代码如下，主函数在img_callback回调中将检测到的特征角点发布出去。
　　该功能包订阅了IMAGE_TOPIC（/cam0/image_raw），并创建了三个发布的话题：
　　pub_img–发布feature topic
　　pub_match–发布feature_img topic
　　pub_restart–发布restart topic

3.1 代码流程图

　　　　　　　　　　　 图3.1 feature_model节点代码流程图

3.2 各关键代码段分析

3.2.1 img_callback()

下面就img_callback回调函数中的关键代码详细分析：

   //若当前帧时间与前一帧时间差<1s或时间错位则重新开始第一帧
    if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
    {
        ROS_WARN("image discontinue! reset the feature tracker!");
        first_image_flag = true; 
        last_image_time = 0;
        pub_count = 1;
        std_msgs::Bool restart_flag;
        restart_flag.data = true;
        pub_restart.publish(restart_flag);
        return;
    }

vins并非将每一帧检测到的特征点都发送，而是控制以不大于10Hz的频率

    // 节点发布频率控制 < 10Hz
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
        PUB_THIS_FRAME = true;
        // reset the frequency control, FREQ = 10
        // 控制特征点的发布频率<10Hz,否则发布flag置零        
        if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
        {
            first_image_time = img_msg->header.stamp.toSec();
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false;

消息节点发布的内容

        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_point;   // 特征点ID
        sensor_msgs::ChannelFloat32 u_of_point;    // 特征点坐标u
        sensor_msgs::ChannelFloat32 v_of_point;     // 特征点坐标v
        sensor_msgs::ChannelFloat32 velocity_x_of_point;  // 光流速度x
        sensor_msgs::ChannelFloat32 velocity_y_of_point;  // 光流速度y

        feature_points->header = img_msg->header;  // 图像头部，含时间戳
        feature_points->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            auto &un_pts = trackerData[i].cur_un_pts;// 畸变矫正归一化2D点
            auto &cur_pts = trackerData[i].cur_pts; // 原图畸变2D点
            auto &ids = trackerData[i].ids;         // 特征点ID号标识
            auto &pts_velocity = trackerData[i].pts_velocity; // 前后帧特征点光流速度
            for (unsigned int j = 0; j < ids.size(); j++)
            {
                if (trackerData[i].track_cnt[j] > 1)
                {
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);
                    geometry_msgs::Point32 p;
                    p.x = un_pts[j].x;
                    p.y = un_pts[j].y;
                    p.z = 1;

                    feature_points->points.push_back(p);
                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                    u_of_point.values.push_back(cur_pts[j].x);
                    v_of_point.values.push_back(cur_pts[j].y);
                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
                }
            }
        }
        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);

3.2.2 readImage()

readImage函数代码分析如下，

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;
    // 亮度均衡开启开关
    if (EQUALIZE)
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    // 第一帧时为空，否则forw_img为当前帧图像
    if (forw_img.empty())
    {
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        forw_img = img;
    }

    forw_pts.clear();

    // 如果已经存在前帧光流特征
    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        //光流跟踪
        // cur_img/前一帧图像  forw_img/当前帧图像 status/ 1:跟踪成功 0:跟踪失败
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        // 遍历角点，过滤在图像外的点
        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;

        // 根据status状态更新所有点及ID         
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    // 将所有跟踪的角点跟踪次数累加
    for (auto &n : track_cnt)
        n++;

    // 满足发布频率条件下
    if (PUB_THIS_FRAME)
    {
        // ransac 过滤外点
        rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        // 为跟踪到的特征点添加模板，防止二次检测
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;
        // 若跟踪角点< 150则重新检测其他区域的角点
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }

    // 将图片数据、角点存入前一帧
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;
    // 畸变矫正，计算光流速度
    undistortedPoints();
    prev_time = cur_time;
}

3.2.3 rejectWithF()

rejectWithF()函数利用F矩阵和ransac过滤外点,ransac距离阈值为1像素;此外，所有的特征点畸变矫正到统一固定的图像尺寸，防止因为尺寸差异导致ransac距离阈值无统一的尺度。

void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            // 将畸变原点投影到投影平面（矫正），归一化后转化为 FOCAL_LENGTH =460 ，偏移量为固定值的图像上
            Eigen::Vector3d tmp_p;
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        // F_THRESHOLD = 1 为ransac阈值
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        // 删除外点，更新角点信息        
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

3.2.4 undistortedPoints()

将原图上的畸变点矫正为投影平面上的归一化点，同时计算前后帧间已跟踪的特征点之间的光流速度

void FeatureTracker::undistortedPoints()
{
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
        // 将原图畸变点投影到3D投影平面，畸变矫正归一化平面
        m_camera->liftProjective(a, b);
        cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }
    // caculate points velocity
    if (!prev_un_pts_map.empty())
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)
            {
                std::map<int, cv::Point2f>::iterator it;
                // 寻找id号为ids[i]的特征点，计算前后两帧的光流速度
                it = prev_un_pts_map.find(ids[i]);
                if (it != prev_un_pts_map.end())
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;
                    double v_y = (cur_un_pts[i].y - it->second.y) / dt;
                    pts_velocity.push_back(cv::Point2f(v_x, v_y));
                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));
            }
            else
            {
                pts_velocity.push_back(cv::Point2f(0, 0));
            }
        }
    }
    else
    {
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    prev_un_pts_map = cur_un_pts_map;
}

4. vins_estimator

---

4.1 姿态解算方法回顾

4.1.1 2D-2D姿态解算-对极几何约束

　　　　　　　　　　 　图4.1 对极约束说明
　　如上图，点p1和点p2是世界坐标系下的同一点P向两幅不同成像平面投影而成，O1与O2分别为两相机的中心。对极约束的物理含义表述为线段$O_1{p}_1$、$O_2{p}_2$与线段$O_1{O}_2$共面。其数学含义可用下式表示$$(O_1{p}_1\times O_1{O}_2)O_2{p}_2=0$$ 由针孔相机模型获得的对极几何约束为：$$x_2^T{t}^{\wedge }R{x}_1=0$$

对极几何约束推导如下：
令$P_1$ 为相机$O_1$坐标系下的点，则由针孔相机模型可得：$$s_1{p}_1=K{P}_1{s}_2{p}_2=K(R_{21}{P}_1+t_{21})$$ 左侧归一化带入右侧归一化坐标，取齐次坐标前两维：$$x_1=K^{-1}{p}_1=P_1{x}_2=K^{-1}{p}_2$$ $$x_2=R_{21}{x}_1+t_{21}$$ 两侧左乘矩阵$t_{21}^{\wedge }$得（共线向量叉乘等于0向量）：$$t_{21}^{\wedge }{x}_2=t_{21}^{\wedge }{R}_{21}{x}_1$$ 两侧对$x_2$内积（相互垂直向量点乘为0），即得对极几何约束：$$x_2^T{t}^{\wedge }R{x}_1=0$$
向量外积：
两向量外积$a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b=a^{\wedge }b$，其中，$a^{\wedge }$记作向量$a$的反对称矩阵

其中$x_2$与$x_1$分别为图4.1中p1与p2图像点畸变矫正后的归一化坐标点，将归一化公式带入可得等价表达式：$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$$ 令$E=t^{\wedge }R$为本质矩阵，$F=K^{-T}{t}^{\wedge }R{K}^{-1}$为基础矩阵，则对极几何约束可表示为：$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$由两帧图像之间的多对匹配特征点可求解出$E$、$F$矩阵，最终可将其分解，得到两帧图像之间的姿态$R$、$T$，具体求解思路可参考高翔《视觉SLAM十四讲》。

4.1.2 三角化深度求解

三角化（三角定位）是利用两相机的相对姿态与对应的像素坐标求解其齐次坐标的深度$s$,从而求解出2D点对应的3D坐标。由针孔相机模型得：$$s_2{x}_2=s_1{R}_{21}{x}_1+t_{21}$$ 两侧同乘$x_2^{\wedge }$可得：$$s_2{x}_2^{\wedge }{x}_2=s_1{R}_{21}{x}_1+t_{21}=0$$ 求解出$s_1$的零解，带入原式求解出$s_2$零解；由此可知同一3D点在各相机坐标系下的坐标。

但通常由于误差导致上式无零解存在，常见做法是构建超定方程组，求解其最小二乘解，具体证明如下：
令世界坐标系下的点$Ｐ=[X,Y,Z,1]$，两帧间的姿态矩阵$T_{cw}=\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]=[R|T]$，$x=[u,v,1{]}^T$为归一化后的平面坐标，$\lambda$为其深度，则有：
　　　　　　　　　　　　　　　　　　　　　$\lambda x=TP$
　　　　　　　　　　　　　　　　　　$x^{\wedge }\times \lambda x=x^{\wedge }\times TP=0$
展开上式，得：
　　　　　　　　　　$x^{\wedge }\times TP=\left[\begin{array}{ccc}0& -1& v\\ 1& 0& -u\\ -v& u& 0\end{array}\right]\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]P$
　　　　　　　　　　　　　　　$=\left[\begin{array}{c}-r_2+v{r}_3\\ r_1-u{r}_3\\ -v{r}_1+u{r}_2\end{array}\right]P=0$ 上式中第三行为可通过消元化简，最终一个点可列两个方程组：
　　　　　　　　　　　　　　　　　$\left[\begin{array}{c}-r_2+v{r}_3\\ r_1-u{r}_3\end{array}\right]P=0$ 则两对以上的匹配点便可构建超静定方程组　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　$\left[\begin{array}{c}-r_2^{(1)}+v^{(1)}{r}_3^{(1)}\\ r_1^{(1)}-u^{(1)}{r}_3^{(1)}\\ -r_2^{(2)}+v^{(2)}{r}_3^{(2)}\\ r_1^{(2)}-u^{(2)}{r}_3^{(2)}\\ ⋮\end{array}\right]P=0$
上述方程通过SVD可求得最小二乘解，将其归一为齐次坐标即可得世界坐标系下的3D点P
　　　　　　　　　　　　　　　　　$P=\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{c}{X}_0/X_3\\ X_1/X_3\\ X_2/X_3\\ X_3/X_3\end{array}\right]$ 上述算法在vins-mono中的实现代码如下：
`

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						         Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	Matrix4d design_matrix = Matrix4d::Zero();
	design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
	design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
	design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
	design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
	Vector4d triangulated_point;
	triangulated_point =
		      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
	point_3d(0) = triangulated_point(0) / triangulated_point(3);
	point_3d(1) = triangulated_point(1) / triangulated_point(3);
	point_3d(2) = triangulated_point(2) / triangulated_point(3);
}

4.1.3 3D-2D姿态解算-PNP

4.1.2介绍了由对极几何求解2D-2D的相邻帧间姿态解算问题。该方法通常是单目slam初始化的必经步骤，若初始化尺度不正确将导致后面较大的误差累计。当初始化成功后，便可将第一帧的相机坐标系设为世界坐标系，在已知世界坐标系的3D点下，可通过PNP（Perspective-N-Point）求解后续的相机姿态，该方法不需要较多的特征点，slam中常用的姿态解算方法。　　　　　　　　　　　　　　　　　　　　　　　　　　　图4.2 P3P求解说明示意图

PNP算法求解相机姿态的步骤如下：

• 根据相似定理，已知匹配点3D世界坐标及当前帧的图像坐标情况下，求解匹配点的相机坐标系3D坐标（具体细节参考《视觉slam十四讲》）
• 利用ICP，已知世界坐标系的3D点和对应相机坐标系的3D点求解相机坐标系与世界坐标系间的姿态

4.1.4 3D-3D姿态解算-ICP

3D-3D的姿态估计是利用匹配点对间的姿态转换约束构建方程组，通过最小化误差求解出两者间的最优姿态矩阵。
　　假设有一组匹配3D点对$P=\left\{\begin{array}{c}{p}_1,\cdots ,p_n\end{array}\right\}$，$P^{\ast }=\left\{\begin{array}{c}{p}_1^{\ast },\cdots ,p_n^{\ast }\end{array}\right\}$，则两者将的坐标转换可通过下式实现：
　　　　　　　　　　　　　　　　　　　　$$\forall i,p_i=R{p}_i^{\ast }+t$$
求解ICP常用方法有两种（具体求解推导参考《视觉SLAM十四讲》）：

• SVD法
  首先构建如下表示的最小二乘问题，求解使误差平方和最小的$R,T$，
  $$mi{n}_{(R,t)}J=\frac{1}{2}\sum _{i=1}^n||p_i-(R{p}_i^{\ast }+t)|{|}_2^2$$
• 非线性优化法
  利用李代数表达目标函数为：$$mi{n}_{\xi }J=\frac{1}{2}\sum _{i=1}^n||p_i-\exp ({\xi }^{\wedge })p_i^{\ast }|{|}_2^2$$利用李代数扰动模型计算单误差对$\delta \xi$的偏导数：$$\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cc}I& -(R{p}^{\ast }+t{)}^{\wedge }\\ 0^T& 0^{\ast }\end{array}\right]$$利用LM算法不断迭代$\xi$便可求解出最优$\xi$。
  　　需要注意的是，vslam中的点对是已经匹配好的，这个最小二乘问题实际具有解析解，即：ICP迭代一次后的结果和SVD的代数解十分接近。利用ICP的好处是在匹配点误差较大或误匹配时，能获得比SVD更加准确的结果。

未完…