SLAM十四讲第三章课后习题

第5题：参考这篇博客，Eigen块操作讲的很细！

Matrix4f m;
m<< 1,2,3,4,
    5,6,7,8,
    9,10,11,12,
    13,14,15,16;
cout<<m.block<2,2>(0,0)<<endl;
//或者
for(int i=0;i<4;i++)
{
    cout<<m.block(0,0,i,i)<<endl;
}

第7题：

设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为，,这里和表达的是，也就是世界坐标系到相机坐标系的变换关系。小萝卜二号位姿为，。现在小萝卜一号看到一个点在自身坐标系下坐标为，求该点在小萝卜二号坐标系下的坐标。请编程实现。

遇到的问题（未解决）

1、归一化 第一次忘了归一化，发现是否归一化会对结果会有影响，不知道为什么 ？

2、pretranslate()这个函数，加与不加pre，二者有什么不同？

#include<iostream>
#include<Eigen/Dense>
#include<Eigen/Geometry>
using namespace Eigen;
int main()
{
//小萝卜一号位姿Tcw
Quaternion<float> q1TW(0.35,0.2,0.3,0.1);
Vector3f t1(0.3,0.1,0.1);
//小萝卜二号位姿Tcw
Quaternionf q2TW(-0.5,0.4,-0.1,0.2);
Vector3f t2(-0.1,0.5,0.3);

//小萝卜一号看到的自身坐标系下的坐标点p
Vector3f p1_camera(0.5,0,0.2);

//归一化 第一次忘了归一化，发现是否归一化会对结果会有影响，不知道为什么
q1TW.normalize();
q2TW.normalize();


//Tcw1
Isometry3f T1_World2Camera=Isometry3f::Identity();
T1_World2Camera.rotate(q1TW);
T1_World2Camera.pretranslate(t1);

Isometry3f T2_World2Camera=Isometry3f::Identity();
T2_World2Camera.rotate(q2TW);
T2_World2Camera.pretranslate(t2);

//p点在全局的坐标
Vector3f p2_world=T1_World2Camera.inverse()*p1_camera;
//p点在二号小萝卜二号坐标系下的坐标
Vector3f p2_camera=T2_World2Camera*p2_world;
std::cout<<p2_camera<<std::endl;

}