Eigen库要点「建议收藏」

Isometry3d Tcw(rotation)；//rotation可以是旋转矩阵，可以是四元数，可以是旋转向量
Tcw.pretranslate(t);//添加平移向量
//或者：
Isometry3d Tcw=Isometry3d::Identity()//如果没有直接初始化，先设为单位阵
Tcw.prerotate(rotation1);//然后添加旋转矩阵，或者向量，或者四元数
Tcw1.pretranslate(t1);//添加平移向量

Tcw.rotation()；//返回旋转矩阵
Tcw.translation()；//返回旋转矩阵

设有小萝卜一号和小萝卜二号位于世界坐标系中，小萝卜一号的位姿为：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 ,求该向量在小萝卜二号坐标系下的坐标，请编程实现此事。

    Quaterniond q1={0.35,0.2,0.3,0.1},q2={-0.5,0.4,-0.1,0.2};
    Vector3d t1={0.3,0.1,0.1},t2={-0.1,0.5,0.3},p={0.5,0,0.2};
    Vector3d world,cam2;
    q1=q1.normalized();
    q2=q2.normalized();
    world=q1.inverse()*(p-t1);
    cam2=q2*world+t2;
    cout<<cam2<<endl;

    Matrix3d rotation1=q1.toRotationMatrix();
    Matrix3d rotation2=q2.toRotationMatrix();
    Matrix3d R1,R1_inv,R2;
    world=rotation1.inverse()*(p-t1);
    cam2=rotation2*world+t2;
    cout<<cam2<<endl;

  Isometry3d Tcw1(rotation1),Twc1,Tcw2(rotation2),Twc2;//直接初始化，不需要设定单位阵
    Tcw1.pretranslate(t1);
    Tcw2.pretranslate(t2);
    //cout<<Tcw1.matrix()<<endl;
    Twc1=Tcw1.inverse();
    //cout<<Twc1.matrix()<<endl;
    world=Twc1*p;    cout<<rotation1<<endl;
    cam2=Tcw2*world;
    cout<<cam2<<endl;
    return 0;

    Isometry3d Tcw1=Isometry3d::Identity(),Twc1,Tcw2=Isometry3d::Identity();//如果没有直接初始化，先设为单位阵
    Tcw1.prerotate(rotation1);
    Tcw2.prerotate(rotation2);
    Tcw1.pretranslate(t1);
    Tcw2.pretranslate(t2);
    //cout<<Tcw1.matrix()<<endl;
    Twc1=Tcw1.inverse();
    //cout<<Twc1.matrix()<<endl;
    world=Twc1*p;
    cam2=Tcw2*world;
    cout<<cam2<<endl;
    return 0;

    Isometry3d Tcw1=Isometry3d::Identity(),Twc1,Tcw2=Isometry3d::Identity();//如果没有直接初始化，先设为单位阵
    Tcw1.prerotate(rotation1);
    Tcw2.prerotate(rotation2);
    Tcw1.pretranslate(t1);
    Tcw2.pretranslate(t2);
    world=Tcw1.rotation().inverse()*(p-Tcw1.translation());
    cam2=Tcw2.rotation()*world+Tcw2.translation();
    cout<<cam2<<endl;
    return 0;

int i;
double *aMat = new double[20];
for(i =0;i<20;i++)
{
  aMat[i] = rand()%11;
}
Eigen:Map<Matrix<double,4,5> > staMat(aMat);

virtual void oplusImpl(const double* update) override
{
     Eigen::VectorXd::ConstMapType v(update,VectexCameraBAL::Dimension);
     _estimate+=v;
}

int i;
double *aMat = new double[20];
for(i =0;i<20;i++)
{
  aMat[i] = rand()%11;
}
Eigen::Map<MatrixXd> dymMat(aMat,4,5);

virtual void oplusImpl(const double* update) override
{
     Eigen::Vector3d::ConstMapType v(update);
     _estimate+=v;
}