大多数关于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();