外参数获取推导（求坐标系转换矩阵方法）

最新推荐文章于 2024-08-09 02:59:20 发布

火星机器人life

最新推荐文章于 2024-08-09 02:59:20 发布

阅读量3.5k

点赞数

分类专栏： SLAM 文章标签：坐标系变换外参数矩阵

本文链接：https://blog.csdn.net/ktigerhero3/article/details/88292499

版权

SLAM 专栏收录该内容

46 篇文章 22 订阅

订阅专栏

问题：假如已知两个坐标系如图（粗的代表摄像头坐标系，细的代表世界坐标系），如图红色为x轴，绿色为y轴，蓝色为z。
如何得到外参数（坐标系变换矩阵：包括旋转和平移）？
在这里插入图片描述
外参数矩阵(extrinsic) world->camera变换T12表示为[R12|t12]

1. 观察变换推导法

步骤：（1）求旋转矩阵R12
（2）求平移t12
1）求旋转矩阵R12：很显然，世界坐标系轴绕z轴逆时针旋转90度就能对应上上图的角度。
2）求平移t12
求由摄像头坐标系到世界坐标系的平移的关键是知道两个坐标系原点的坐标。
ｔ＝摄像头原点在世界坐标系上的坐标－世界坐标系原点的坐标
上图中摄像头原点在世界坐标系上的坐标为（0,0,２）
世界坐标系原点的坐标为（0,0,0）
t12=(0,0,２)-(0,0,0)
因此world->camera变换T12表示为[R12|t12]

因此可以得到坐标点之间的关系：
设同一个点P在世界坐标系下表示为P0(x0,y0,z0),在摄像头坐标系下表示为P1(x1,y1,z1)。
任意一点满足方程
$P_{0}=R_{12}*P_{1}+t_{12}$
或者表示为
$P_{1}=R_{12}^{-1}*(P_{0}-t_{12})$
如下面程序段所示：

  Eigen::Vector3d PInWorld={1,2,3};
  cout<<"PinWorld"<<endl<<PInWorld<<endl;
  Eigen::Vector3d PInCam=R12.inverse()*(PInWorld-t12);

2线性变换求旋转矩阵法

设同一个点P在世界坐标系下表示为P0(x0,y0,z0),在摄像头坐标系下表示为P1(x1,y1,z1)。
要分解了来求
步骤：（１）先求旋转矩阵Ｒ
　　　（２）再求平移ｔ

2．１先求旋转矩阵R12

不考虑平移，我们可以发现一下有以下规律
世界->摄像头
x0=-y1,y0=x1,z1=z0

P在在世界坐标系的值为x0的坐标对应摄像头坐标系下值为-y1
P在在世界坐标系的值为y0的坐标对应摄像头坐标系下值为x1
P在在世界坐标系的值为z0的坐标对应摄像头坐标系下值为z1

！不考虑平移！
$P_{0}=R_{12}*P_{1}$
用矩阵形式表示为
$\begin{bmatrix} x_{0}\\ y_{0}\\ z_{0} \end{bmatrix}=\begin{bmatrix} 0 & -1 & 0\\ 1& 0 & 0\\ 0 & 1 & 0 \end{bmatrix}\cdot \begin{bmatrix} x_{1}\\ y_{1}\\ z_{1} \end{bmatrix}$
可以得到旋转矩阵R12为
$R_{12}=\begin{bmatrix} 0 &-1 &0\\ 1& 0 & 0\\ 0 & 1 & 0 \end{bmatrix}$

2．２再求平移t12

求由摄像头坐标系到世界坐标系的平移的关键是知道两个坐标系原点的坐标。
ｔ＝摄像头原点在世界坐标系上的坐标－世界坐标系原点的坐标
上图中摄像头原点在世界坐标系上的坐标为（0,0,２）
世界坐标系原点的坐标为（0,0,0）
t12=(0,0,２)-(0,0,0)

3.实验程序

对应以上推导的程序如下

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include<time.h>
#include <unistd.h>
#include<iostream>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
#include <tf/transform_broadcaster.h>
using namespace std;
using namespace Eigen;

int main( int argc, char** argv )
{

  ros::init(argc, argv, "tfshow");
  cout<<"start program"<<endl;
  //    //1.p1 world position
  double p1yaw=0;
  double p1x=0;
  double p1y=0;
  Eigen::AngleAxisd rotzp1(p1yaw*M_PI/180, Eigen::Vector3d::UnitZ());
  Eigen::Vector3d  t1= Eigen::Vector3d(p1x,p1y, 0);
  Eigen::Quaterniond q1=Eigen::Quaterniond(rotzp1);
  //cout<<"q1.vec()"<<q1.vec()<<endl;
  cout<<"p1 eular angle"<<(180/M_PI)*q1.matrix().eulerAngles(0,1,2)<<endl;

  //2. t12 value
  double t12yaw=90;
  double t12x=0;
  double t12y=0;
  double t12z=2;

  Eigen::AngleAxisd rott12(t12yaw*M_PI/180, Eigen::Vector3d::UnitZ());
  Eigen::Vector3d  t12= Eigen::Vector3d(t12x,t12y, t12z);
  Eigen::Quaterniond q12=Eigen::Quaterniond(rott12);
  cout<<"t12 rotation eular angle "<<(180/M_PI)*q12.matrix().eulerAngles(0,1,2)<<endl;
   Eigen::Matrix3d R12=rott12.matrix();
   cout<<"R12="<<R12<<endl;


  //    3.calculate p2 world positon
  Eigen::Quaterniond q2=q1*q12;
  // cout<<" q2 vector " <<q2.vec()<<endl;
  Eigen::Vector3d t2=t1+q1.toRotationMatrix()*t12;
  cout<<"t2="<<t2<<endl;
  cout<<"t2 eular angle "<<(180/M_PI)*q2.matrix().eulerAngles(0,1,2)<<endl;


  tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(t12(0),t12(1), t12(2)));

  tf::Quaternion q(q12.x(),q12.y(),q12.z(),q12.w());
  transform.setRotation(q);

  //P1 in world
  Eigen::Vector3d PInWorld={1,2,3};
  cout<<"PinWorld"<<endl<<PInWorld<<endl;
  Eigen::Vector3d PInCam=R12.inverse()*(PInWorld-t12);
  cout<<"PInCam"<<endl<<PInCam<<endl;
  int k=0;
  while(k<100)
  {
      br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera"));
      k++;
      sleep(1);
  }
  return 0;
}