SLAM从入门到放弃：SLAM十四讲第三章习题

以下均为简单笔记，如有错误，请多多指教。

#include <Eigen/Core>
#include <Eigen/Dense>

int main()
{
  Eigen::Matrix<double,10,10> originMat = Eigen::Matrix<double,10,10>::Random(10,10);
  Eigen::Matrix<double,3,3> eyeMat = Eigen::Matrix<double,3,3>::Identity(3,3);
  originMat.block<3,3>(0,0) = eyeMat;
  
  return 0;
}

6. 一般线性方程$Ax=b$有哪几种做法？你能在Eigen中实现吗？
   解：
   参考：

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

int main()
{
  
   MatrixXf A = MatrixXf::Random(3, 2);
   cout << "Here is the matrix A:\n" << A << endl;
   VectorXf b = VectorXf::Random(3);
   cout << "Here is the right hand side b:\n" << b << endl;
   
   // SVD
   cout << "The least-squares solution is:\n"
        << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
        
  // QR
  cout << "The solution using the QR decomposition is:\n"
     << A.colPivHouseholderQr().solve(b) << endl;
     
  // Normal Equations
  cout << "The solution using normal equations is:\n"
     << (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl;
     
  return 0;
}

7.设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为$q_1=[0.35,0.2,0.3,0.1],t_1=[0.3,0.1,0.1{]}^T$（$q$的第一项为实部，请把$q$归一化后再进行计算）。这里的$q$和$t$表达的是$T_{cw}$，也就是世界坐标系到相机坐标系的变换关系。小萝卜二号的位姿为$q_2=[-0.5,0.4,-0.1,0.2],t_2=[-0.1,0.5,0.3{]}^T$。现在，小萝卜一号看到某个点在自身坐标系下坐标为$p_1=[0.5,0,0.2{]}^T$，求该点在小萝卜二号坐标系下的坐标。请编程实现。
解：
思路分析：记该点为$P_w$，则$p_1=T_1{P}_w$，$p_2=T_2{P}_w$，故$p_2=T_2(T_1{)}^{-1}{p}_1$；又知$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right],T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$。代码如下：

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

#include <iostream>

Eigen::Matrix4d GetT(const Eigen::Quaterniond &q, const Eigen::Vector3d &t)
{
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
  Eigen::Matrix3d R = q.toRotationMatrix ();
  T.block<3,3>(0,0) = R;
  T.block<3,1>(0,3) = t;
  
  return T;
}

Eigen::Matrix4d GetTInv(const Eigen::Quaterniond &q, const Eigen::Vector3d &t)
{
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
  Eigen::Matrix3d R = q.toRotationMatrix ();
  T.block<3,3>(0,0) = R.transpose();
  T.block<3,1>(0,3) = -R.transpose()*t;
  
  return T;
}

int main()
{
    // Point in robot 1
    Eigen::Vector4d p1(0.5,0,0.2,1);

    // Robot 1
    Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);
    q1.normalize();
    Eigen::Vector3d t1(0.3,0.1,0.1);

    // Robot 2
    Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
    q2.normalize();
    Eigen::Vector3d t2(-0.1,0.5,0.3);

    // Get T1 T2
    Eigen::Matrix4d T1_inv = GetTInv(q1,t1);
    Eigen::Matrix4d T2 = GetT(q2,t2);  

    std::cout<<T2*T1_inv*p1<<std::endl;

    
    return 0;
}

答案：$[-0.0309731,0.73499,0.296108]$