Skip to main content

坐标系转换

激光点云坐标转到世界坐标系​

首先明确，订阅激光topic得到的坐标是lidar坐标系下的；

车辆定位 localization 数据格式是geometry_msgs/TransformStamped，他是有[x, y, z, qx, qy, qz, qw]的，当只取坐标(x, y, z)时，这个坐标是在世界坐标系下的，当取pose[x, y, z, qx, qy, qz, qw]时，这其实应该叫变换，即 TF 了，这个 TF 就是车辆坐标系到世界坐标系的变换（localization 在Apollo里面时融合了 GPS和IMU 数据，再转化到世界坐标系下的，所以这个TF是已知的）。

激光点云坐标转到世界坐标系，首先要转到车辆坐标系，再转到世界坐标系，所以有两个变换，lidar坐标系->车辆坐标系，记为 T_vl，车辆坐标系->世界坐标系，记为 T_wv。

激光点云在世界坐标系下记为 P_w，在lidar坐标系下记为 P_l

那么计算公式为P_w = T_wv * T_vl * P_l

其中 P_l 和 T_wv 是已知的，能分别从激光和 localization 的topic获得，T_vl 是求出来的，可以参考之前做激光和imu标定的lidar_align。

// 已知条件：
// inss为订阅的localization数据
// lidar2inss_vector为求出的lidar到车辆的变换[x, y, z, qx, qy, qz, qw]
// 注意Eigen::Quaterniond是qw在第一个

pcl::PointXYZI Fusion::get_global_pos(pcl::PointXYZI& pos) {
  pcl::PointXYZI res;
  Eigen::Quaterniond q_w2i(
      inss.transform.rotation.w, inss.transform.rotation.x,
      inss.transform.rotation.y, inss.transform.rotation.z),
      q_i2l(lidar2inss_vector[6], lidar2inss_vector[3], lidar2inss_vector[4],
            lidar2inss_vector[5]);
   q_w2i.normalize();        // 归一化
   q_i2l.normalize();
  Eigen::Vector3d t_w2i(inss.transform.translation.x,
                        inss.transform.translation.y,
                        inss.transform.translation.z),
      t_i2l(lidar2inss_vector[0], lidar2inss_vector[1], lidar2inss_vector[2]);
  Eigen::Vector3d p_l(pos.x, pos.y, pos.z);

  Eigen::Isometry3d T_w2i(q_w2i), T_i2l(q_i2l);
  T_w2i.pretranslate(t_w2i);
  T_i2l.pretranslate(t_i2l);

  Eigen::Vector3d p_l_w = T_w2i * T_i2l * p_l;
  // std::cout << p_l_w.transpose() << std::endl;
  res.x = p_l_w[0];
  res.y = p_l_w[1];
  res.z = p_l_w[2];
  res.intensity = pos.intensity;

  return res;
}

坐标系之间简单变换​

如图，假设在lidar坐标系有一个点，坐标值为 P1(2,-1,0)，怎么知道该点 P1 在另一个imu坐标系中的坐标值 P2 呢？

1.png

目前条件是，已知lidar坐标系下点 P为(2,-1,0)，求点 P 在imu坐标系下的坐标

首先，将lidar坐标系旋转到imu坐标系，顺时针旋转90°，

绕Z轴顺时针旋转的旋转矩阵是T=[cosA,-sinA,0; sinA,cosA, 0; 0,0,1]

所以旋转矩阵 T 为[0,-1,0; 1,0,0; 0,0,1]

所以T*P1+T偏移=P2

1.png

参考：