Mark一下～激光雷达点云投影到图像的方法（基于autoware的lidar_camera_calibration,外参不匹配的一些坑

这篇文章对自己很有用，为防止将来万一找不到，特地转载。原文章链接请见：
https://blog.csdn.net/mxdsdo09/article/details/88582588

按上篇博客的思路，先使用autoware完成了对lidar和cam的外参标定工作，得到的外参包括33R（旋转矩阵）和31T（平移向量），统一在4*4的外参矩阵里。

之后使用autoware标定自带的投影在rviz中显示点云投影在图像上，效果很好，然而想自己投影，使用OpenCVC的函数

cv::projectPoints(pts_3d, r_vec, t_vec, camera_matrix, distortion_coeff, pts_2d);
其中r_vec是旋转向量，从旋转矩阵到旋转向量可以用转化

cv::Rodrigues(rvec_tmp,r_vec);
内参和外参采用的就是autoware标定的结果，结果点云投影到图像上是斜的，感觉就是外参不匹配的缘故，困扰了好几天

后来自己写了个基于针孔模型的投影方法，代码如下：

    double camera_rvec[3][3] = {2.6960817933551429e-02, 1.7189582417358329e-01,
                      9.8474612968421371e-01,
                      6.3004339837293838e-03, -9.8511387765312808e-01,
                      1.7178752162725369e-01,
                      9.9961663592998007e-01, 1.5727958869600073e-03,
                      -2.7642494191505218e-02};

    double camera_m[3][3] = {3.6769854086804582e+02, 0., 2.8971501865187105e+02, 0.,
                     3.6692726915965966e+02, 2.4315986177519471e+02, 0., 0., 1.};

    double camera_tvec[3] = {-1.9169004581234389e-01, 7.3983893466082556e-02, -6.4970195440243628e-02};
    double rvec_pts3d_[3];
    double project_p[3];
    for (size_t i = 0; i < pts_3d.size(); ++i)
    {

        rvec_pts3d_[2] = camera_rvec[2][0]*pts_3d[i].x + camera_rvec[2][1]*pts_3d[i].y + camera_rvec[2][2]*pts_3d[i].z + camera_tvec[2];
        rvec_pts3d_[0] = (camera_rvec[0][0]*pts_3d[i].x + camera_rvec[0][1]*pts_3d[i].y + camera_rvec[0][2]*pts_3d[i].z + camera_tvec[0])/rvec_pts3d_[2];
        rvec_pts3d_[1] = (camera_rvec[1][0]*pts_3d[i].x + camera_rvec[1][1]*pts_3d[i].y + camera_rvec[1][2]*pts_3d[i].z + camera_tvec[1])/rvec_pts3d_[2];
        rvec_pts3d_[2] = 1;
        project_p[0] = camera_m[0][0]*rvec_pts3d_[0] + camera_m[0][1]*rvec_pts3d_[1] + camera_m[0][2]*rvec_pts3d_[2];
        project_p[1] = camera_m[1][0]*rvec_pts3d_[0] + camera_m[1][1]*rvec_pts3d_[1] + camera_m[1][2]*rvec_pts3d_[2];
        project_p[2] = camera_m[2][0]*rvec_pts3d_[0] + camera_m[2][1]*rvec_pts3d_[1] + camera_m[2][2]*rvec_pts3d_[2];

        std::cout << project_p[0] << std::endl;
        std::cout << project_p[1] << std::endl;
        std::cout << project_p[2] << std::endl;
        pts_2d.push_back(cv::Point2d(project_p[0], project_p[1]));

    }

投影结果和OpenCV的一样，于是开始怀疑是外参标定的问题了，但是autoware自身的投影又是很正确的，就搞到很奇怪。

先分析了一天的坐标系，假设相机镜头轴线和雷达x轴轴线对齐，这时雷达的坐标系是由右（x轴）、前（y轴）、上（z轴）构成的右手坐标系，而相机的坐标系是前（z轴）、右（x轴）、下（y轴）构成的。

右手坐标系

更正：此处velodyne激光雷达的坐标系有误，不是前左上而是右前上！

我的相机是侧着安装在雷达底下的，所以有一个90度旋转角，并且雷达x轴线并不是正对前方也有一个小的安装角，后来在处理旋转矩阵的的时候就弄得很晕…

初步想自己测量安装角和杆臂，然后算出旋转矩阵和平移向量，看看标定得到的外参是不是有问题：

这部分就涉及到坐标系转换和欧拉角与旋转矩阵（方向余弦矩阵）的转换。关于欧拉角，目前不同的地方可能定义不一样，具体可以参考这个博客：

https://blog.csdn.net/linuxheik/article/details/78842428

分类
欧拉角按旋转的坐标系分为内旋（intrinsic rotation）和外旋（extrinsic rotation）。
按旋转轴分为经典欧拉角（Proper Euler Angle）和泰特布莱恩角（Tait–Bryan angles）。

经典欧拉角（Proper Euler Angle）

按(z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)轴序列旋转，即第一个旋转轴和最后一个旋转轴相同

泰特布莱恩角（Tait–Bryan angles）

按(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)轴序列旋转，即三个不同的轴

内旋（intrinsic rotation）

绕物体自身的坐标系object-space 旋转，举个例子，一个(φ, θ, ψ) （xyz，instrinsic）的欧拉角，指绕物体的x轴转φ后，再绕物体的y’轴（这里用y’表示这个新的y轴已经和一开始世界坐标系下的那个物体的y轴不一样了）旋转θ，最后绕z’轴旋转φ，每一次旋转都会改变下一次旋转的轴。这种情况下旋转的轴是动态（moving axis）的。

下图引自wiki，一个zxz的内旋

外旋（extrinsic rotation）

绕惯性系upright-space 旋转（upright space指基向量平行于world-space或parent-space，原点与object-space的原点重合的空间）。
也就说，无论是三步旋转中的哪一步，轴都是固定的，是不会动的。
unity中的rotation就是一种外旋。

一个rotation为（0,0,30）的飞机

旋转到（0,90,0）,可以看到飞机并没有沿着旋转后的y轴（灰色箭头）旋转，仍然依照世界坐标系下的y轴（红色箭头）旋转

这就很头痛了，要使用别人给你的欧拉角，就必须知道他对于欧拉角的定义，是经典的还是泰特布莱恩的，是内旋还是外旋（定轴还是非定轴），然后才能用求出对应的旋转矩阵…

所以我们通过autoware得到的旋转矩阵，可能和投影使用的旋转矩阵的定义并不是一致的。

之后查看autoware标定的源码，发现其中有诈…

void CalibrateSensors()
{
std::cout << "Number of points: " << clicked_velodyne_points_.size() << “:” << clicked_image_points_.size() << std::endl;

	if (clicked_velodyne_points_.size() == clicked_image_points_.size()
	    && clicked_image_points_.size() > 8)
	{
		cv::Mat rotation_vector, translation_vector;

		cv::solvePnP(clicked_velodyne_points_,
		             clicked_image_points_,
		             camera_instrinsics_,
		             distortion_coefficients_,
		             rotation_vector,
		             translation_vector,
		             true,
		             CV_EPNP
		);

		cv::Mat rotation_matrix;
		cv::Rodrigues(rotation_vector, rotation_matrix);

		cv::Mat camera_velodyne_rotation = rotation_matrix.t();
		cv::Point3f camera_velodyne_point(translation_vector);
		cv::Point3f camera_velodyne_translation;

		camera_velodyne_translation.x = -camera_velodyne_point.z;
		camera_velodyne_translation.y = camera_velodyne_point.x;
		camera_velodyne_translation.z = camera_velodyne_point.y;

		std::cout << "Rotation:" << camera_velodyne_rotation << std::endl << std::endl;
		std::cout << "Translation:" << camera_velodyne_translation << std::endl;
		std::cout << "RPY: " << get_rpy_from_matrix(camera_velodyne_rotation) << std::endl << std::endl;

		cv::Mat extrinsics = cv::Mat::eye(4,4, CV_64F);
		camera_velodyne_rotation.copyTo(extrinsics(cv::Rect_(0,0,3,3)));

		std::cout << extrinsics << std::endl;

		extrinsics.at(0,3) = camera_velodyne_translation.x;
		extrinsics.at(1,3) = camera_velodyne_translation.y;
		extrinsics.at(2,3) = camera_velodyne_translation.z;

		std::cout << extrinsics << std::endl;

		SaveCalibrationFile(extrinsics ,camera_instrinsics_, distortion_coefficients_, image_size_);
	}
}

在获得标定外参后，有一行代码看的不是很懂为什么要这样做

cv::Mat camera_velodyne_rotation = rotation_matrix.t();
意思就相当于把得到的旋转矩阵转置了一下，然后当做外参输出。

于是就把旋转矩阵转置回原本的样子，在自己写的投影代码里，点云就能和图像匹配上了。。。。

平移向量这也有坑，也要注意一下。

	camera_velodyne_translation.x = -camera_velodyne_point.z;
		camera_velodyne_translation.y = camera_velodyne_point.x;
		camera_velodyne_translation.z = camera_velodyne_point.y;

总结：

————————————————
版权声明：本文为CSDN博主「文锦渡」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/mxdsdo09/article/details/88582588