结合SLAM十四讲的示例程序理解SE3, se(3), so(3),R, t等

博客中程序参考/slambook/project/0.2/src/visual_odometry.cpp

一：理解各种表示下的R,t变量关系

    void VisualOdometry::poseEstimationPnP()
    {
        vector<Point3f> pts_3d;
        vector<Point2f> pts_2d;
        for (DMatch m:feature_matches_)
        {
            pts_3d.push_back(pts_3d_ref_[m.queryIdx]);
            pts_2d.push_back(keypoints_curr_[m.trainIdx].pt);
        }
        Mat K = ( cv::Mat_<double>(3,3)<<
                ref_->Camera_->fx_, 0, ref_->Camera_->cx_,
                0, ref_->Camera_->fy_, ref_->Camera_->cy_,
                0,0,1
        );
        Mat rvec,tvec,RR;
        solvePnPRansac(pts_3d,pts_2d,K,Mat(),rvec,tvec, false,100,4.0,0.99,inliers);
        cv::Rodrigues(rvec,RR);
        cout<<"旋转向量:\n"<<rvec<<endl;
        cout<<"平移向量:\n"<<tvec<<endl;
        cout<<"旋转向量转旋转矩阵:\n"<<RR<<endl;
        Eigen::Matrix3d Rvec;
        Rvec<<//cv::Mat转Eigen::Matrix,因为sophus库只接受Eigen类的参数
                RR.at<double>(0,0), RR.at<double>(0,1), RR.at<double>(0,2),
                RR.at<double>(1,0), RR.at<double>(1,1), RR.at<double>(1,2),
                RR.at<double>(2,0), RR.at<double>(2,1), RR.at<double>(2,2);
        Sophus::SO3 SO3_R(Rvec);//旋转矩阵构造SO3
        T_c_r_estimated_ =Sophus::SE3(
                SO3_R,
                Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0)));
        cout<<"李代数so3：\n"<<SO3_R.log()<<endl;
        cout<<"so3到反对称矩阵:\n"<<Sophus::SO3::hat(SO3_R.log())<<endl;

        cout<<"李代数se3：\n"<<T_c_r_estimated_.log()<<endl;
        cout<<"se3到4*4变换矩阵:\n"<<Sophus::SE3::hat(T_c_r_estimated_.log())<<endl;
        cout<<"SE3旋转矩阵：\n"<<T_c_r_estimated_.rotationMatrix()<<endl;
        cout<<"SE3平移向量：\n"<<T_c_r_estimated_.translation()<<endl;
    }

看一下输出结果：（颜色相同的代表相等的量）

旋转向量：rvec
[-0.0042691901518842;
 0.008202525696573983;
 -0.01332348400744696]

平移向量：tvec
[-0.005543193547913661;
 -0.007847099671932811;

 -0.02400530011149837]

旋转向量转旋转矩阵：RR
[0.9998776043558208,       0.01330539126494978,      0.008230605746304158;
 -0.01334040863933389,    0.9999021315398384,        0.004214361093120362;

 -0.008173726506317822,  -0.004323644917684513,   0.9999572472309132]

李代数so3：SO3_R.log()
-0.00426919
0.00820253

-0.0133235

so3到反对称矩阵:hat(SO3_R.log())
          0   　　　0.0133235 　　 0.00820253
 -0.0133235      　     0  　　　　0.00426919

-0.00820253　 -0.00426919         　　0

李代数se3：T_c_r_estimated_.log()
-0.00539244
-0.00783242
-0.0240446
-0.00426919
0.00820253

-0.0133235

se3到4*4变换矩阵:hat(T_c_r_estimated_.log())
          0   　　 0.0133235 　　 0.00820253   -0.00539244
 -0.0133235           0  　　　　0.00426919   -0.00783242
-0.00820253   -0.00426919           　0  　　  -0.0240446

          0                  0                          0                  0

SE3旋转矩阵：T_c_r_estimated_.rotationMatrix()
   0.999878      0.0133054     0.00823061
 -0.0133404     0.999902       0.00421436

-0.00817373   -0.00432364   0.999957

SE3平移向量：T_c_r_estimated_.translation()
-0.00554319
-0.0078471
-0.0240053

可以看出：

一：pnp估计相机位姿初始值
　1：se3的六维向量前三维表示的“平移”向量并不是真正意义上的平移向量，后三维是旋转向量
　2：so3的三维向量就是旋转向量
　3：se3转换的4*4矩阵分为四块
　　 左上角：so3的反对称矩阵
　　 右上角：se3的前三维“平移向量”
　　 左下角：０^T
　　 右下角：０

4：se3通过函数：rotationMatrix()，translation()转换出的就是旋转矩阵，平移向量

二：理解构造SO3

//通过旋转矩阵构造SO3
Eigen::Matrix3d Rvec;
Rvec<<
   　　 RR.at<double>(0,0), RR.at<double>(0,1), RR.at<double>(0,2),
       RR.at<double>(1,0), RR.at<double>(1,1), RR.at<double>(1,2),
       RR.at<double>(2,0), RR.at<double>(2,1), RR.at<double>(2,2);
Sophus::SO3 SO3_R(Rvec);
cout<<"so3：\n"<<SO3_R.log()<<endl;
//通过旋转向量构造SO3
Eigen::Vector3d Rvecc;
Rvecc<<
        rvec.at<double>(0,0), rvec.at<double>(1,0),rvec.at<double>(2,0);
Sophus::SO3 SO3_Q(Rvecc(0,0),Rvecc(1,0),Rvecc(2,0));
cout<<"so3*：\n"<<SO3_Q.log()<<endl;

看一下两个的输出的区别：

so3：
-0.00426919
0.00820253
-0.0133235
so3*：
-0.00432375
0.00817395
-0.0133409

可以看出两个输出并不一样，第一个so3和旋转向量一样，而第二种so3*是通过旋转向量构造的，输出结果却和旋转向量不相同，所以Sophus::SO3 SO3_Q(向量)这种方法的输入参数并不是旋转向量，也就是根本就没有通过旋转向量构造SO3的方法，具体构造SO3的方法参考我的另一个博客 点击打开链接。