结合SLAM十四讲的示例程序理解SE3, se(3), so(3),R, t等
博客中程序参考/slambook/project/0.2/src/visual_odometry.cpp
一：理解各种表示下的R,t变量关系
void VisualOdometry::poseEstimationPnP()
{
vector pts_3d;
vector 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_(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"<(0,0), RR.at(0,1), RR.at(0,2),
RR.at(1,0), RR.at(1,1), RR.at(1,2),
RR.at(2,0), RR.at(2,1), RR.at(2,2);
Sophus::SO3 SO3_R(Rvec);//旋转矩阵构造SO3
T_c_r_estimated_ =Sophus::SE3(
SO3_R,
Vector3d( tvec.at(0,0), tvec.at(1,0), tvec.at(2,0)));
cout<<"李代数so3：\n"< 看一下输出结果：（颜色相同的代表相等的量）
旋转向量：rvec
[-0.0042691901518842;
0.008202525696573983;
-0.01332348400744696]
[-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(0,0), RR.at(0,1), RR.at(0,2),
RR.at(1,0), RR.at(1,1), RR.at(1,2),
RR.at(2,0), RR.at(2,1), RR.at(2,2);
Sophus::SO3 SO3_R(Rvec);
cout<<"so3：\n"<(0,0), rvec.at(1,0),rvec.at(2,0);
Sophus::SO3 SO3_Q(Rvecc(0,0),Rvecc(1,0),Rvecc(2,0));
cout<<"so3*：\n"< 看一下两个的输出的区别：
so3：
-0.00426919
0.00820253
-0.0133235
so3*：
-0.00432375
0.00817395
-0.0133409