旋转三维平面与某一坐标平面平行

在上一篇文章（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 pla