使用ICP得到的变换矩阵更新相机位置

大多数关于ICP的介绍都是介绍了ICP算法如何配准两个点云，但是没有介绍如何更新相机位置，本文将对此进行介绍。

已知：

点云X0和X1，点云X0对应的相机外参矩阵M0。

待求：

点云M1对应的相机外参矩阵M1。

过程：

将点云X0作为目标点云，使用PCL的ICP算法计算出：点云X1配准到点云X0的变换矩阵M。
根据下图的推导过程，计算出M1=M0*M.inverse()。
注：世界坐标系下坐标=相机姿态矩阵*相机坐标系下坐标，这种情况才适用下图中的推导。



部分代码如下：

// ICP 参数设置
pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icp;
icp.setInputSource(X1);
icp.setInputTarget(X0);
icp.setMaxCorrespondenceDistance(500);
icp.setTransformationEpsilon(1e-10);
icp.setEuclideanFitnessEpsilon(0.0001);
icp.setMaximumIterations(600);

// ICP 迭代
pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>(DEPTH_IMAGE_WIDTH, DEPTH_IMAGE_HEIGHT));
icp.align(*out);

// 取出变换矩阵M
Eigen::Matrix4f transformation = icp.getFinalTransformation();
Eigen::Matrix4d trans_d;
trans_d(0, 0) = transformation(0, 0); trans_d(0, 1) = transformation(0, 1); trans_d(0, 2) = transformation(0, 2); trans_d(0, 3) = transformation(0, 3);
trans_d(1, 0) = transformation(1, 0); trans_d(1, 1) = transformation(1, 1); trans_d(1, 2) = transformation(1, 2); trans_d(1, 3) = transformation(1, 3);
trans_d(2, 0) = transformation(2, 0); trans_d(2, 1) = transformation(2, 1); trans_d(2, 2) = transformation(2, 2); trans_d(2, 3) = transformation(2, 3);
trans_d(3, 0) = transformation(3, 0); trans_d(3, 1) = transformation(3, 1); trans_d(3, 2) = transformation(3, 2); trans_d(3, 3) = transformation(3, 3);

// 更新矩阵M1
m_mLastCameraTransRotate = m_mCurCameraTransRotate;
m_mCurCameraTransRotate = m_mCurCameraTransRotate * trans_d.inverse();