视觉slam十四讲课后习题ch3-7

题目回顾:

设有小萝卜一号和小萝卜二号位于世界坐标系中，小萝卜一号的位姿为：q1=[0.35,0.2,0.3,0.1],
t2=[0.3,0.1,0.1]^T (q的第一项为实部。请你把q归一化后在进行计算)。这里的q和t的表达的是Tcw，也就是世界到相机的变换关系。小萝卜二号的位姿为q2=[-0.5,0.4,-0.1,0.2],t=[-0.1,0.5,0.3]^T.现在，小萝卜一号看到某个点在自身的坐标系下，坐标为p=[0.5,0,0.2]^T ,求该向量在小萝卜二号坐标系下的坐标，请编程实现此事。
解:
pw:某个点在世界坐标系下的坐标
T_1w :表示世界坐标系到小萝卜一号坐标系的变换关系
T_2w:表示世界坐标系到小萝卜二号坐标系的变换关系
P2 :表示该点在小萝卜二号坐标系下的坐标（即为所求）
单位四元数到旋转矩阵R的变化关系可参考书上55页。之后变换矩阵T=[R t]
[0 1]
由变换关系可列出下面的式子:
p = T_1w * Pw 可解出来pw
p2=T_2W*pW 带入上式解出来的Pw即可求出来p2

具体代码实现如下:

 #include<iostream>
 #include<Eigen/Core>

 //包含几何模块
 #include<Eigen/Geometry>
 using namespace std;

 int main(int argc,char **argv)
 {
     /*变量定义*/
     Eigen::Quaterniond Q1(0.2,0.3,0.1,0.35); //四元数的表示(w ,x,y,z)
     Eigen::Quaterniond Q2(0.4,-0.1,0.2,-0.5);
     Eigen::Vector3d t1(0.3,0.1,0.1);
     Eigen::Vector3d t2(-0.1,0.5,0.3);
     Eigen::Vector3d p(0.5,,0.2);   //在一号小萝卜下的坐标
     Eigen::Vector3d pw ;            //世界坐标
     Eigen::Vector3d p2;             //求在二号小萝卜的坐标 p2

    /*欧氏变换矩阵使用Eigen::Isometry */
     Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
     Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();

     /*归一化*/
     Q1.normalize();
     Q2.normalize();

     /*输出归一化参数*/
 //    cout<<"Q1 is "<<Q1.x()<<endl<<Q1.y()<< endl <<Q1.z()<< endl<<Q1.w()<<endl;
 //    cout<<"Q2 is "<<Q2.x()<<endl<<Q2.y()<< endl <<Q2.z()<< endl<<Q2.w()<<endl;

     cout<<"after normalize； "<< endl << Q2.coeffs()<<endl;

     /*设置变换矩阵的参数*/
     T_1w.rotate(Q1);
     T_1w.pretranslate(t1);
     T_2w.rotate(Q2);
     T_2w.pretranslate(t2);

     /* p = T1w * pw  求解pw*/
     pw = T_1w.inverse() * p;

     /* p2 = T_2w * pw  求解p2*/
     p2 = T_2w * pw;

     /*输出在小萝卜二号下的该点坐标*/
     cout<<"该点在小萝卜二号下的坐标为: "<<p2.transpose()<<endl;

     return ;
 }

欢迎大家关注我的微信公众号「佛系师兄」，里面有关于 Ceres 以及 OpenCV 库的一些文章。

比如

「反复研究好几遍，我才发现关于 CMake 变量还可以这样理解！」

更多好的文章会优先在里面不定期分享！打开微信客户端，扫描下方二维码即可关注！