[代碼閱讀]hdl_graph_slam中的地面約束與g2o中的平面誤差模型推導

參考資料：

1.Code:hdl_graph_slam

2.Paper:Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019

1. hdl_slam中的地面約束

在hdl_slam中的每一幀關鍵幀都會對應提取一個地面參數；

建圖開始的時候將一個Floor Plane Node設置爲true，並固定，參數設置爲決定的垂直於地面的法向量[0,0,1,0]

後面的每一幀keyframe的地面參數coeffs都要和Floor Plane Node構建平面誤差，如下所示：

void computeError() override {
    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]); //T:[R,t]
    const g2o::VertexPlane* v2 = static_cast<const g2o::VertexPlane*>(_vertices[1]); //[0,0,1,0]

    Eigen::Isometry3d w2n = v1->estimate().inverse();// T.inverse()= T_iw;i是當前幀，w是世界座標系
    Plane3D local_plane = w2n * v2->estimate();// T_iw*[0 0 1 0]= 將一個絕對的垂直於Z的法向量，投影到當前幀；
    _error = local_plane.ominus(_measurement);//將[0 0 1 0]變換到當前幀之後，和當前幀的地面的法向量 做ominus 作爲誤差
}

思路是，根據當前第i幀keyframe的位姿$T_{wi}$，將初始的嚴格垂直於地面的法向量[0,0,1,0]變換到第i幀的座標系下；

並用這個參數來初始化Plane3D local_plane;其底層對*操作符進行了重載；具體如下：

inline Plane3D operator*(const Isometry3& t, const Plane3D& plane){
    Vector4 v=plane._coeffs;
    Vector4 v2;
    Matrix3 R=t.rotation();
    v2.head<3>() = R*v.head<3>();
    v2(3)=v(3) - t.translation().dot(v2.head<3>());
    return Plane3D(v2);
};

可以看到，傳入參數t就是$T_{iw}$，plane就是[0,0,1,0]，經過一個變換，變成到當前幀雷達座標系下的一個Vector4d值。

再使用這個值和當前幀點雲擬合出來的地面點，進行一個$\ominus$操作，獲得誤差，誤差的具體定義如下：

inline Vector3 ominus(const Plane3D& plane){
    //construct the rotation that would bring the plane normal in (1 0 0)
    Matrix3 R=rotation(normal()).transpose();
    Vector3 n=R*plane.normal();
    number_t d=distance()-plane.distance();
    return Vector3(azimuth(n), elevation(n), d);
}

那麼平面誤差是如何定義的呢？

下面則是重點推導部分。

2. g2o中的平面誤差推導

首先，在三維平面下，平面方程可以使用4個參數來表示：
$Ax+By+Cz+D=0$
所以，如果想要達到hdl_slam中對地面的約束，就需要藉助平面方程，對地面點雲進行約束；

1.當前幀點雲的地面方程：

那麼，當前幀雷達點雲的地面方程擬合可以借鑑這篇文章[透徹理解]由最小二乘到SVD分解，那麼可以獲得當前幀雷達點雲(第i幀)的地面方程爲$plane_i$：
$A_ix+B_iy+C_iz+D_i=0$
該參數記做$P_i\in \mathbb{R}^4$

2.全局座標系下的點雲地面方程：

hdl_slam對此約束的處理思路是，最開始初始化一個全局地面方程參數爲[0,0,1,0]，記爲：
$A_wx+B_wy+C_wz+D_i=0$
也即，$A_w=B_w=D_w=0,C_w=1$，該參數記做$P_w \in \mathbb{R}^4$

3. 平面方程的座標系統一

上面構建的兩個平面方程，其座標系並不統一，因此需要統一座標系；按照hdl_slam的理解，我們將世界座標系下的平面方程$P_w$投影到當前第i幀雷達座標系下$T_wi$，(該位姿視作已知)：
$P_w' = T_{iw}P_w=T_{wi}^{-1}P_w = [A_w' \ \ B_w' \ \ C_w' \ \ D_w']$
然後在第$i$幀雷達座標系下進行平面方程的誤差構建，即如下代碼部分：

Eigen::Isometry3d w2n = v1->estimate().inverse();
Plane3D local_plane = w2n * v2->estimate();
_error = local_plane.ominus(_measurement);

4. g2o中的平面誤差

對$P_w'，P_i$兩個平面參數進行$\ominus$操作，分爲以下幾步：

• 1.Matrix3 R=rotation(normal()).transpose();

  • 以$P_w'$的向量(前三維，也即法向量)爲基底，將$P_w'$轉換爲旋轉矩陣。

• Vector3 n=R*plane.normal();

  • 將$P_i$轉換到以$P_w'$爲基底的旋轉空間中去，得到向量n;
    

• 對n計算仰角azimuth和方位角elevation

• 計算$d=D_w'-D_i$

• 誤差組合爲Vector3(azimuth(n), elevation(n), d)