Table of contents

1. Topic

A known

(1) The transformation relationship from the world coordinate system to the camera 1 coordinate system is $T_{(R_{1},W)}$ , that is, the pose of camera 1 q1=[0.35,0.2,0.3,0.1]^T, t1=[0.3,0.1,0.1]^T.

(2) The transformation relationship from the world coordinate system to the camera 2 coordinate system is $T_{(R_{2},W)}$ , that is, the pose of camera 2 q1=[-0.5,0.4,-0.1,0.2]^T, t2=[-0.1,0.5,0.3]^T.

(3) The observed coordinates of a point under camera 1 are $P_{R_{1}}$

Find the coordinates of the point in the camera 2 coordinate system. Assuming that the world coordinates of the point are $P_W$ , then there is

2. Use Euclidean transformation

Coordinate transformation from the coordinates in the world coordinate system to the camera 1 coordinate system ( using the Euclidean transformation formula )

$P_{R_{1}} = T_{(R_{1},W)} P_{W}$

Coordinate transformation from world coordinate system to camera 2 coordinate system

$P_{R_{2}} = T_{(R_{2},W)}P_{W}$

Multiply both sides of the first formula by the inverse of T to obtain P_W and then bring it into the second formula to obtain the coordinates of the point in the camera 2 coordinate system

$P_{R_{2}} = T_{(R_{2},W)}T_{(R_{1},W)}^{-1}P_{R_{1}} = T_{(R_{2},W)}T_{(W,R_{1})}P_{R_{1}}$

The formula in the book is as follows.

Note : In SLAM, the pose is the transformation from the world coordinate system to the camera coordinate system, including rotation and translation.

3. Use four elements

Again, assuming the point's world coordinates are $P_W$

Coordinate transformation from coordinates in the world coordinate system to camera 1 coordinate system ( four-element transformation formula )

$P_{R_{1}} = q_{1}P_{W}q_{1}^{-1} + t_{1}$ ---------------Formula 1

Coordinate transformation from world coordinate system to camera 2 coordinate system

$P_{R_{2}} = q_{2}P_{W}q_{2}^{-1} + t_{2}$ --------------- Formula 2

Transform the first formula by multiplying both sides to the left by the inverse of q_1, as follows.

$q_1^{-1}P_{R_1}=P_Wq_1^{-1}+q_1^{-1}t_1$

Multiply both sides by q_1 to the right, as follows.

$q_1^{-1}P_{R_1}q_1=P_W+q_1^{-1}t_1q_1$

P_W is brought into Equation 2, and the mathematical formula is as follows .

$P_{R_2} = q_2(q_1^{-1}P_{R_1}q_1 - q_1^{-1}t_1q_1)q_2^{-1} + t_2 = q_2q_1^{-1}(P_{R_1}-t_1) q_1q_2^{-1}+t_2 = q_2q_1^{-1}(P_{R_1}-t_1)(q_2q_1^{-1})^{-1}+t_2$

The characteristic of eigen is $qpq^{-1}$ that it is written as qp, which saves the subsequent inversion operation, so the above formula eigen has the following form.

$P_{R_2} = q_2q_1^{-1}(P_{R_1}-t_1)+t_2$

#include <iostream>
#include <vector>
#include <algorithm>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
 
using namespace std;
using namespace Eigen;
 
int main(int argc, char** argv) {
  // 已知两个相机的位姿
  // 即p1的位姿是q1 = [0.35,0.2,0.3,0.1]^T, t1 = [0.3,0.1,0.1]^T
  // 即p2的位姿是q2 = [-0.5,0.4,-0.1,0.2]^T, t2 = [-0.1,0.5,0.3]^T    
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);  // 旋转。coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  q1.normalize();  // 注意，四元素使用之前需要归一化。
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);  // 平移

  // 已知p1在相机1坐标系下的坐标
  Vector3d p1(0.5, 0, 0.2);
 
  // 1, 利用四元素变换
  Vector3d p2;
  p2 = q2 * q1.inverse() * (p1 - t1) + t2;
  cout << endl << p2.transpose() << endl;
  
  // 2, 利用欧式变换矩阵
  Isometry3d T1w(q1), T2w(q2);  // 用四元素，初始化欧式变换矩阵
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);
  // 求p1在相机2坐标系下的坐标
  p2 = T2w * T1w.inverse() * p1;  // 公式
  cout << endl << p2.transpose() << endl;

  return 0;
}

-0.0309731 0.73499 0.296108

slam fourteenth lecture 03 Coordinate transformation in different coordinate systems practiced by Eigen

1. Topic

2. Use Euclidean transformation

3. Use four elements

Guess you like