按上篇博客的思路，先使用autoware完成了對lidar和cam的外參標定工作，得到的外參包括3*3R（旋轉矩陣）和3*1T（平移向量），統一在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軸）構成的。
右手座標系
我的相機是側着安裝在雷達底下的，所以有一個90度旋轉角，並且雷達x軸線並不是正對前方也有一個小的安裝角，後來在處理旋轉矩陣的的時候就弄得很暈....
初步想自己測量安裝角和杆臂，然後算出旋轉矩陣和平移向量，看看標定得到的外參是不是有問題：
這部分就涉及到座標系轉換和歐拉角與旋轉矩陣（方向餘弦矩陣）的轉換。關於歐拉角，目前不同的地方可能定義不一樣，具體可以參考這個博客：
https://blog.csdn.net/linuxheik/article/details/78842428
分類
歐拉角按旋轉的座標系分爲內旋（intrinsic rotation）和外旋（extrinsic rotation）。
按旋轉軸分爲經典歐拉角（Proper Euler Angle）和泰特布萊恩角（Tait–Bryan angles）。
按(z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)軸序列旋轉，即第一個旋轉軸和最後一個旋轉軸相同
按(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)軸序列旋轉，即三個不同的軸
繞物體自身的座標系object-space 旋轉，舉個例子，一個(φ, θ, ψ) （xyz，instrinsic）的歐拉角，指繞物體的x軸轉φ後，再繞物體的y’軸（這裏用y’表示這個新的y軸已經和一開始世界座標系下的那個物體的y軸不一樣了）旋轉θ，最後繞z’軸旋轉φ，每一次旋轉都會改變下一次旋轉的軸。這種情況下旋轉的軸是動態（moving axis）的。
下圖引自wiki，一個zxz的內旋
繞慣性系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_<float>(0,0,3,3)));
std::cout << extrinsics << std::endl;
extrinsics.at<double>(0,3) = camera_velodyne_translation.x;
extrinsics.at<double>(1,3) = camera_velodyne_translation.y;
extrinsics.at<double>(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;
總結：
