视觉SLAM习题ch3

3.5 假设有一个大的Eigen矩阵，想把他的左上角3*3的块取出来，然后赋值为I_3*3,请编程实现。

思路如下
尝试产生一个随机方阵，对其使用两个循环实现目的。
编写程序如下

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

using namespace std;

int main()
{ 
  cout << "Ran_max:" << RAND_MAX << endl;
   srand((unsigned)time(NULL));
   const int n =  (1 + rand() % 10);
   cout << n << endl;
   Eigen::Matrix<double, n, n>  matrix_nn;
   matrix_nn = Eigen::Matrix::Random(n,n);
   if (n < 3)
     cout << "Wrong size" << endl;
   else
     for (int i = 0; i <= 2; i++)
     {
       for (int j = 0; j <= 2; j++)
	 matrix_nn(i,j) = 1;
     }
     cout << "new matrix_nn = " << matrix_nn << endl;
   return 0;
}

程序未能成功，报错信息提示Eigen::Matrix::Random不能接受变量参数，意味着对于Eigen矩阵的定义一定要明确。问题核心即是两个for循环，因此不再修改本程序。

3.7 设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为q
[0.35,0.2,0.3,0.1]，t2=[0.3,0.1,0.1](q的第一项为实部。请把q归一化后再
进行计算)。这里的q和t表达的是T_cw，也就是世界坐标系到相机坐标系的变
换关系。小萝卜二号的位姿为q2=[-0.5,0.4，-0.1,0.2]，t=[-0.1,0.5,0.3]现在，小萝卜一号看到某个点在自身的坐标系下坐标为p=[0.5,0,0.2]，求该向量在小萝卜二号坐标系下的坐标。请编程实现。

思路如下
先找到p点在世界坐标系下的坐标，再将其转换至二号机坐标系下即可。难点在于对四元数、角轴、旋转矩阵和旋转向量之间的转换。可参考文章Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换.

编写程序如下

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

using namespace std;

int main()
{
  //四元数转旋转矩阵
  Eigen::Quaterniond q_1(0.35, 0.2, 0.3, 0.1);
  Eigen::Matrix3d r_m1;
  q_1.normalize();
  r_m1 = q_1.matrix();
  cout << "r_m1 =\n" << r_m1 << endl;
  
  Eigen::Quaterniond q_2(-0.5, 0.4, -0.1, 0.2);
  Eigen::Matrix3d r_m2;
  q_2.normalize();
  r_m2 = q_2.matrix();
  cout << "r_m2 =\n" << endl;
  
  //旋转矩阵转变换矩阵  
  Eigen::Isometry3d T_1 = Eigen::Isometry3d::Identity();
  T_1.rotate(r_m1);
  Eigen::Vector3d t1(0.3, 0.1, 0.1);
  T_1.pretranslate(t1);
  cout << "T_1 =\n" << T_1.matrix() << endl;
  
  Eigen::Isometry3d T_2 = Eigen::Isometry3d::Identity();
  T_2.rotate(r_m2);
  Eigen::Vector3d t2(-0.1, 0.5, 0.3);
  T_2.pretranslate(t2);
  cout << "T_2 =\n" << T_2.matrix() << endl;
  
  //对T_2求逆，实际可以直接使用T.inverse()函数
  Eigen::Matrix3d r_m2T = r_m2.transpose();
  Eigen::Vector3d product;
  product = r_m2T * t2;
  
  for (int i = 0; i <= 2; i++)
  {
    for (int j = 0; j <= 2; j++)
    {
      T_2(i,j) = r_m2T(i,j);
    }
  }
  
  for (int i = 0; i <=2; i++)
    T_2(i,3) = -1 * product(i);
  
  cout << "after inverse T_2 =\n" << T_2.matrix() << "\n";
  
  //求p_wc1在世界坐标下的坐标，再转换至二号机坐标
  Eigen::Vector4d p_c1(0.5, 0, 0.2, 1);
  Eigen::Vector4d p_w;
  Eigen::Vector4d p_c2;
  p_w = T_1 * p_c1;
  p_c2 = T_2 * p_w;
  cout << "the location of p in robot 2 =\n" << p_c2 << "\n";
  return 0;
}

首先程序有错误在于，求错了转换矩阵，即T_1、T_2实际应为T__c1w和T__c2w，但是对题目理解有误，求成了T__wc1和T__wc2。
其次，对Eigen库不熟悉，很多计算如求T逆，四元数直接转T矩阵等，都可以使用成员函数完成。
参考文章《视觉slam十四讲》第3讲课后习题.

四元数直接求T

Eigen::Isometry3d T = Eigen::Isometry::Idnetity();
T.rotate(q);
T.pretranslate(t);