在上一篇文章（https://blog.csdn.net/weixin_38636815/article/details/109495227）中我寫了如何使用ceres，根據一系列的點來擬合一個平面，很難保證ORB-SLAM輸出的軌跡嚴格與某一個坐標平面平行，所以這篇文章我我將說一下，如何將不與任何一個

坐標平面平行的三維平面繞著一個軸，旋轉一個角度，使得其與某一個坐標平面平行。

一、原理分析

實現步驟：

1. 獲得擬合出的平面的法向量

2. 找到參考向量，如要與XOY平面平行，參考向量為(0,0,1)的轉置，與XOZ平面平行，參考向量為(0,1,0)的轉置，與YOZ平面平行，參考向量為(1, 0, 0)的轉置。

因為SLAM中的相機坐標系為（水平朝前放置相機時）Z軸朝前，X軸朝右， Y軸朝下，也就是說Z軸就是相機朝著的方向，我們在實際安裝時也多數水平朝前安裝

有時會有一些俯仰角，所以一般情況下我會把XOZ平面作為參考面，那么參考向量就是(0,1,0)向量的轉置。

3. 根據“向量的點積”來求解兩個向量之間的夾角。

?

4. 根據“向量的叉乘”求解垂直于兩個向量構成的平面的法向量。因為兩個向量的叉乘得到的新向量

就是一個同時垂直于兩個向量的向量。

上面第4步求解出的向量就是我們的旋轉軸，第三步求解出來的角度就是需要旋轉的角度，我們將擬合出來的

平面繞著這個軸，旋轉一個角度，就可以將這個平面旋轉到與參考平面平行的地方。

注意：(1)上面求解出的角度是以弧度為單位，(2)上面求解出的旋轉軸向量要進行歸一化處理，否則后續求解的結果會出錯。

二、代碼講解

下面有兩種方法可以進行求解，一種是使用opencv的方法，一種是使用Eigen的方法。

1. 首先求解出旋轉軸和旋轉向量

//a,b,c為求解出的擬合平面的法向量，是進行歸一化處理之后的向量。
cv::Point3d plane_norm(a, b, c);
//xz_norm是參考向量，也就是XOZ坐標平面的法向量
cv::Point3d xz_norm(0.0, 1.0, 0.0);
std::cout<<"plane_norm: "<<plane_norm<<std::endl;
std::cout<<"xz_norm: "<<xz_norm<<std::endl;
//求解兩個向量的點乘
double v1v2 = plane_norm.dot(xz_norm);
//計算平面法向量和參考向量的模長，因為兩個向量都是歸一化之后的，所以這里的結果都是1.
double v1_norm = plane_norm.x*plane_norm.x + plane_norm.y*plane_norm.y + plane_norm.z*plane_norm.z;
double v2_norm = xz_norm.x*xz_norm.x + xz_norm.y*xz_norm.y + xz_norm.z*xz_norm.z;
//計算兩個向量的夾角
double theta  = std::acos(v1v2/(std::sqrt(v1_norm)*std::sqrt(v2_norm)));//根據向量的叉乘求解同時垂直于兩個向量的法向量。
cv::Point3d axis_v1v2 = xz_norm.cross(plane_norm);//對旋轉向量進行歸一化處理
double v1v2_2 = axis_v1v2.x*axis_v1v2.x + axis_v1v2.y*axis_v1v2.y + axis_v1v2.z*axis_v1v2.z;
double v1v2_n = std::sqrt(v1v2_2);
axis_v1v2 = axis_v1v2/v1v2_n;
std::cout<<"axis_v1v2: "<<axis_v1v2<<std::endl;
std::cout<<"theta: "<<theta<<std::endl;

2. 根據旋轉軸和旋轉向量求解旋轉矩陣

（opencv法）

//-theta表示改變旋轉的方向
cv::Point3d axis_v1v2_cv = -theta* axis_v1v2;//軸角，角度乘以方向
std::cout<<"axis_v1v2_cv: "<<axis_v1v2_cv<<std::endl;cv::Mat R_vec=(cv::Mat_<double>(3,1)<<axis_v1v2_cv.x,axis_v1v2_cv.y,axis_v1v2_cv.z);cv::Mat rotation;
cv::Rodrigues(R_vec,rotation);
std::cout<<" rotation  CV::"<<rotation<<std::endl;for (size_t i = 0; i < locations.size(); i++)
{cv::Mat Pt=(cv::Mat_<double>(3,1)<<locations[i][0],locations[i][1],locations[i][2]);cv::Mat afterPt=rotation*Pt;std::cout<<"orignal     "<<locations[i].transpose()<<"      cv  Translate   "<<afterPt.t()<<std::endl;}

（Eigen法）

Eigen::AngleAxisd ro_vector(-theta, Eigen::Vector3d(axis_v1v2.x, axis_v1v2.y, axis_v1v2.z));
Eigen::Matrix3d ro_matrix = ro_vector.toRotationMatrix();
std::cout<<"rotation eigen "<<ro_matrix<<std::endl;std::vector<Eigen::Vector3d> new_points;
double sum  = 0;
for(int i = 0; i<locations.size(); i++)
{Eigen::Vector3d newP;//計算點到平面上的距離，只處理那些距離平面距離很小的點。double x = locations[i].x();double y = locations[i].y();double z = locations[i].z();//計算每一個真實點到擬合出來的平面的距離，求出總距離，求解平均距離double dist2 = (a*x + b*y + c*z+d)*(a*x + b*y + c*z+d);sum =+ std::sqrt(dist2); newP.x()=x;newP.y()=y;newP.z()=z;Eigen::Vector3d new_point = ro_matrix*newP;std::cout<<"newP: "<<new_point.transpose()<<"    orignal"<<newP.transpose()<<std::endl;      }

根據上面的步驟，我們就可以將擬合出來的平面旋轉到參考平面。

如何驗證我們的旋轉對不對呢，你可以檢驗經過旋轉之后的3D點是不是某一個維度上的值全部穩定在一個值附近。下圖中y軸的值穩定在比較小的值，這樣

我們取出x和z軸上的兩維坐標去畫圖，得到的軌跡失真性更小。

?

三、將旋轉后的3D平面畫在圖像上

將軌跡點畫到圖像上，其中一個重要的思想就是點坐標的轉換。

在下面段代碼中，將原始軌跡點按照求解出來的旋轉變換，進行旋轉，旋轉到平行于指定的坐標平面。并且求出實際點的最大值和最小值（作用在下文中有體現）

double min_x = 10e5;
double max_x = 10e-5;
double min_y = 10e5;
double max_y = 10e-5;
for (size_t i = 0; i < locations.size(); i++){cv::Mat Pt=(cv::Mat_<double>(3,1)<<locations[i][0],locations[i][1],locations[i][2]);cv::Mat afterPt=rotation*Pt;double x = afterPt.at<double>(1,0);double y = afterPt.at<double>(2,0);plane_points.push_back(cv::Point2d(x, y));if(min_x > x) min_x = x;if(max_x < x) max_x = x;if(min_y > y) min_y = y;if(max_y < y) max_y = y;//求出這些點中的水平方向的最大值最小值和豎直方向的最大值最小值std::cout<<"orignal     "<<locations[i].transpose()<<"      after rotated   "<<afterPt.t()<<std::endl;}

?step1: 根據實際點的大小計算生成圖像的長寬

step2: 將實際點中為負的點，通過減去最小值，轉換到正點上

step3: 然后就是把轉換后的實際點按照原始比例畫在圖像上

    //計算實際點水平方向與豎直方向的長寬比值double wh = (max_x-min_x)/( max_y-min_y);//只設置高度，然后根據實際比利計算長度，這樣可以確保畫出來的軌跡圖不失真。int img_height = 720;int img_width = img_height * wh;cv::Mat img(img_height, img_width, CV_8UC3, cv::Scalar(255, 255, 255));std::cout<<"img.size(): "<<img.size()<<std::endl;double scale_ = max(max_x-min_x, max_y-min_y);std::cout<<"scale_: "<<scale_<<std::endl;cv::Point2i center_p;std::vector<cv::Point2i> pixels;for(int i = 0; i<plane_points.size(); i++){//首先將負數部分都轉換到正數部分double x = plane_points[i].x - min_x;double y = plane_points[i].y - min_y;//std::cout<<"x: "<<x<<"   y:"<<y<<std::endl;// int u = img_width*x/(scale_);// int v = img_height*y/(scale_);int u = img_width*x/(max_x-min_x);int v = img_height*y/(max_y-min_y);std::cout<<"u: "<<u<<"  v:"<<v<<std::endl;cv::Point2i center_p(u,v);pixels.push_back(center_p);cv::circle(img, center_p, 2, cv::Scalar(0, 0, 255), -1, 8);}for(int i = 1; i<pixels.size(); i++){cv::Point2i start_point(pixels[i-1].x, pixels[i-1].y);cv::Point2i end_point(pixels[i].x, pixels[i].y);cv::line(img, start_point, end_point, cv::Scalar(0, 0, 255), 2);}

?