关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)

首先接触的是loam那一套,要排列成一个图像一样的阵列。

对于蝴蝶式的扫描的,例如livox mid40的如何处理?
https://www.livoxtech.com/mid-40-and-mid-100

1、velodyne16的扫描方式的处理

2、livox mid40扫描方式的处理

livox mid-40 mid-100的坐标系定义:

https://www.livoxtech.com/3296f540ecf5458a8829e01cf429798e/downloads/20190712/Livox%20Mid%20Series%20User%20Manual%20v1.0%20CHS.pdf

关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第1张图片
关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第2张图片

根robotsense的不一样:
关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第3张图片
https://zh.wikipedia.org/wiki/%E7%90%83%E5%BA%A7%E6%A8%99%E7%B3%BB
关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第4张图片

livox mid用的是“物理学中通常使用的球坐标”

关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第5张图片

3、livox horizon 扫描方式的处理

关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第6张图片

横向扫,6条线。

这种每个点带有line号,也确实是一条水平的线。
所以,可以根据line号码,按照点的先后顺序,排在一起就好了。
然后,将intensity,timeoffset等组合在pointxyzi的对应字段中。

看livox_mapping中,对于horizon的处理:
先是经过了livox_repub的处理,将原始消息进行转换:

void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
  livox_data.push_back(livox_msg_in);
  if (livox_data.size() < TO_MERGE_CNT) return;

  pcl::PointCloud<PointType> pcl_in;

  for (size_t j = 0; j < livox_data.size(); j++) {
    auto& livox_msg = livox_data[j];
    auto time_end = livox_msg->points.back().offset_time;
    for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
      PointType pt;
      pt.x = livox_msg->points[i].x;
      pt.y = livox_msg->points[i].y;
      pt.z = livox_msg->points[i].z;
      float s = livox_msg->points[i].offset_time / (float)time_end;

      pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp
      pt.curvature = s*0.1;
      pcl_in.push_back(pt);
    }
  }

  unsigned long timebase_ns = livox_data[0]->timebase;
  ros::Time timestamp;
  timestamp.fromNSec(timebase_ns);

  sensor_msgs::PointCloud2 pcl_ros_msg;
  pcl::toROSMsg(pcl_in, pcl_ros_msg);
  pcl_ros_msg.header.stamp.fromNSec(timebase_ns);
  pcl_ros_msg.header.frame_id = "/livox";
  pub_pcl_out1.publish(pcl_ros_msg);
  livox_data.clear();
}

关键就在上面把line拿下放到intensity字段,给下游用了:

PointType point;
  std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].x;
    point.y = laserCloudIn.points[i].y;
    point.z = laserCloudIn.points[i].z;
    point.intensity = laserCloudIn.points[i].intensity;
    point.curvature = laserCloudIn.points[i].curvature;
    int scanID = 0;
    if (N_SCANS == 6) {
      scanID = (int)point.intensity;
    }
    laserCloudScans[scanID].push_back(point);
  }

  pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());

  for (int i = 0; i < N_SCANS; i++) {
    *laserCloud += laserCloudScans[i];
  }

下游直接使用了这个line作为scanID。因为确实是拍好的点。

然后注意:
对于这些scanID,结果全部被串在了laserCloud这个大的队列了。
也没有去计算各个点的horizon角度。

拿到这个串起来的laserCloud数据后,直接对里面的点进行处理:
计算特征,没用用到类似loam那里的处理方式。

for (int i = 5; i < cloudSize - 5; i += count_num ) {

}

4、livox avia的处理

avia有两种扫描方式,一种是非重复性的,就是那种蝴蝶样子的,一种是重复的:
关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第7张图片

我把fastlio的horizon(其实是avia的lidar,看launch的命名也可知道)回调中收到的每一条line的数据打出来看,
不同颜色代表的是不同的line,可以看到完全不是想象中按照什么水平角排的阿。
其实想想也是正常的,为什么要按照水平角排呢?空间并没有这个格子阿!
在空间任意连续投影都可以阿。
关于激光雷达不同扫描方式得到的数据的整理总结(未完待续)_第8张图片
这个fastlio就是按照这个方式,就可以进行特征点识别了。

for(uint i=1; i<plsize; i++)
  {
    if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)
        && (!IS_VALID(msg->points[i].x)) && (!IS_VALID(msg->points[i].y)) && (!IS_VALID(msg->points[i].z)))
    {
      pl_full[i].x = msg->points[i].x;
      pl_full[i].y = msg->points[i].y;
      pl_full[i].z = msg->points[i].z;
      pl_full[i].intensity = msg->points[i].reflectivity;
      //curvature在imu 的矫正模块UndistortPcl,会用来进行排序
      pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
      //结合laserMapping中的代码来看,offset_time的单位应该是纳秒,除于1000000后此时curvature记录的是毫秒为单位的
      //1s(秒)=10^3ms(毫秒)=10^6μs(微秒)=10^9ns(纳秒)=10^12ps(皮秒)=10^15fs(飞秒)=10^18as(阿秒)=10^21zm(仄秒)=10^24ym(幺秒)
      if((std::abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
          || (std::abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
          || (std::abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
      {
        pl_buff[msg->points[i].line].push_back(pl_full[i]);
      }
    }
  }
  if(pl_buff[0].size() <= 7) {return;}

  for(int j=0; j<N_SCANS; j++)
  {
    pcl::PointCloud<PointType> &pl = pl_buff[j];
    vector<orgtype> &types = typess[j];
    plsize = pl.size();
    types.resize(plsize);
    plsize--;
    for(uint i=0; i<plsize; i++)
    {

      types[i].range = sqrt(pl[i].x*pl[i].x + pl[i].y*pl[i].y);
      vx = pl[i].x - pl[i+1].x;
      vy = pl[i].y - pl[i+1].y;
      vz = pl[i].z - pl[i+1].z;
      types[i].dista = vx*vx + vy*vy + vz*vz;
      // std::cout<
    }
    // plsize++;
    types[plsize].range = sqrt(pl[plsize].x*pl[plsize].x + pl[plsize].y*pl[plsize].y);

    give_feature(pl, types, pl_corn, pl_surf);
  }

  ros::Time ct;
  ct.fromNSec(msg->timebase);
  pub_func(pl_full, pub_full, msg->header.stamp);
  pub_func(pl_surf, pub_surf, msg->header.stamp);
  pub_func(pl_corn, pub_corn, msg->header.stamp);
  std::cout<<"[~~~~~~~~~~~~ Feature Extract ]: time: "<< omp_get_wtime() - t1<<" "<<msg->header.stamp.toSec()<<std::endl;
}

再仔细思考一下,line在这里的作用是什么?
是不是还是保证了某种程度上的空间点的连续性?
如果没有这个line信息,传递过来的点,就要按照某种方式,让它们处于某种有序状态。

那又有个疑问了,如果是这样的话,loam中为何不直接按照vlp16的取点方式,将点竖着排列来使用呢?难道就是因为它判断、提取edge点的方式是横向提取的?

那么对于任何一款激光雷达来说,只要带着有ring、line、等信息,是否都可以直接使用这个fastlio的方法来取得特征点,而不需要用loam的那种方式?

你可能感兴趣的:(lidar,lidar)