结合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的前三维“平移向量”
   左下角:0^T
   右下角:0

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的方法参考我的另一个博客 点击打开链接
评论 11
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值