生成相机-激光雷达标定的模拟数据

代码来源：https://github.com/MegviiRobot/CamLaserCalibraTool

void GenerateSimData( std::vector<Oberserve>& obs)
{

  // we set laser frame as the world frame
  // generate Tlc: Transformation matrix from camera frame to lasr frame;
  Eigen::Matrix3d Rlc;//激光雷达到相机的旋转
  Rlc << 0 , 0 , 1, 
        -1 , 0 , 0,
        0 , -1 , 0;
  Eigen::Quaterniond qlc(Rlc); //四元数            
  Eigen::Vector3d tlc(0.1, 0.2, 0.3);

  std::cout << "============= Ground Truth of Rotation and translation =============== " << std::endl;
  std::cout << Rlc << std::endl;
  std::cout << tlc << std::endl;
  std::cout << "============= End =============== " << std::endl;

  std::random_device rd;
  std::default_random_engine generator(rd());
  // random CamPose distribution
  std::uniform_real_distribution<double> rpy_rand(-M_PI/6. , M_PI/6.);
  std::uniform_real_distribution<double> xy_rand(-3, 3.0);
  std::uniform_real_distribution<double> z_rand(1., 5.);

  for (size_t i = 0; i < 50; i++)
  {
    // generate apriltage pose in camera frame, then transfrom it to world frame
    Eigen::Matrix3d Rca;
    
// Try it!!! You will find the treasure /    
    // You can try rotating the calibration plate around the camera's different axes to see the observability of the system.
    // rotate all axis
    Rca = Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitZ())
        *Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitY())
        *Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitX());

    // Try me!!! remove yaw, you will find that the system observability is not affected by Z-axis
    // Rca = Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitY())
    //     *Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitX());
      
    // Try me!!! ONLY pitch, Wow!!!! you will find we can not estimate the tlc.z() 
    // Rca = Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitY());

    // Try me!!! ONLY roll
    // Rca = Eigen::AngleAxisd(rpy_rand(generator),Eigen::Vector3d::UnitX());

///

    Eigen::Vector3d tca(xy_rand(generator),xy_rand(generator), z_rand(generator));
    // Eigen::Vector3d tca(0,0, z_rand(generator));
    Eigen::Quaterniond qca(Rca);

    Eigen::Quaterniond qla(Rlc * Rca);
    Eigen::Vector3d tla = Rlc * tca + tlc;

    // get apriltag plane equation , then transform to laser frame.
    Eigen::Vector4d plane_in_aprilframe(0,0,1,0); 

    Eigen::Matrix4d Tla = Eigen::Matrix4d::Identity();
    Tla.block(0, 0, 3, 3) = qla.toRotationMatrix();
    Tla.block(0, 3, 3, 1) = tla;
    Eigen::Vector4d plane_in_laserframe = (Tla.inverse()).transpose() * plane_in_aprilframe;
    Eigen::Vector3d n = plane_in_laserframe.head(3);
    double d = plane_in_laserframe[3];


    // Obtaining laser observations through the intersection of rays and planes
    std::vector< Eigen::Vector3d > points;

    for (size_t j = 0; j < 180; j++)
    {
      double theta = -M_PI_2 + j * M_PI/180;
      Eigen::Vector3d ray(cos(theta), sin(theta), 0);
      double depth = -d/(ray.dot(n));
        
      if( std::isnan(depth) || depth < 0)
        continue;
      
      Eigen::Vector3d p = depth * ray;
 
      // valid laser points
      if( std::fabs(p.x()) < 5 && std::fabs(p.y()) < 5)
      {
          points.push_back(p);
      }
      // std::cout << p.transpose() << std::endl;
    }

    Oberserve ob;
    ob.tagPose_Qca = qca;
    ob.tagPose_tca = tca;
    ob.points = points;
    obs.push_back(ob);    

  }

• 标定板平面在激光雷达坐标系下的表示：
  这里令标定板平面参数为$[0,0,1,0]$，即$Z=0$
  而对于标定板坐标系下的坐标点$[X_a,Y_a,Z_a]$和其在激光坐标系下的对应点$[X_l,Y_l,Z_l]$之间存在对应关系
  $$[X_a,Y_a,Z_a,1]=T_{al}[X_l,Y_l,Z_l,1]$$
  而且，$dot([X_a,Y_a,Z_a,1],[0,0,1,0])=0$，
  所以标定板平面在激光坐标系下的表示为$T_{la}^{-T}[0,0,1,0]$

• 对于激光雷达与标定板平面相交的点，应满足$(depth\ast ray)\ast n+d=0$