lio-sam算法整理＜2＞

msg/cloud_info.msg

• cloud_info.msg
  

可以看到，这里面定夜猫子点云,imu的初始化,以及ringindex的相关msg.

在对应的CMake里面也加入

这个相当于是自定义的了，因此也需要有 msg_generation相关.在package.xml也要有对应的

src/imageProjection.cpp

这个代码开始定义了自定义的 结构体

• VelodynePointXYZIRT
  因为正常的pointcloud2里面没有这个类型的，因此这里进行了自定义. （后面会尝试自行定义在此基础上加入RGB信息的类型）

• OusterPointXYZIRT

这个主要是用于室外的时候，里面有noise

后面 using PointXYZIRT = VelodynePointXYZIRT;

接下来就是 ImageProjection这个类
这个类大致做的事情就是将bag文件中的topic发布的点云信息拿到，并做处理，整理成lio-sam可以使用的格式，并且发布出去供后面的代码进行使用.

上面定义的 VelodynePointXYZIRT 其实就是为了bag文件里的topic信息的.

这个 laserCloudIn就是为了拿到bag文件中的点云信息的,
从 kitti2bag.py 中可以看到，bag文件 里至少有以下信息

接下来看看 laserCloudIn是如何拿到bag中的topic 中的点云信息的.

先看

这里定义的接收器传入的callback函数是 cloudHandler

   void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
      {
          if (!cachePointCloud(laserCloudMsg))
              return;

          if (!deskewInfo())
              return;

          projectPointCloud();

          cloudExtraction();

          publishClouds();

          resetParameters();
      }

这里个人理解，callback函数在处理的时候，就会把msg给到 laserCloudMsg.
所以laserCloudMsg在进入下面的四个函数之前已经拿到了msg的点云信息.

接下来看一下 cachePointCloud这个函数， 这个函数的主要功能是

• 把接收到的msg 点云 laserCloudMsg放到队列 cloudQueue中去
• 做一些check,比如
  • is_dense
  • ring index是否缺失
  • check time这个field是否有
• convert cloud
  代码如下

 // convert cloud
          currentCloudMsg = std::move(cloudQueue.front());
          cloudQueue.pop_front();
          if (sensor == SensorType::VELODYNE)
          {
              pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
          }
          else if (sensor == SensorType::OUSTER)
          {
              // Convert to Velodyne format
              pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
              laserCloudIn->points.resize(tmpOusterCloudIn->size());
              laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
              for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
              {
                  auto &src = tmpOusterCloudIn->points[i];
                  auto &dst = laserCloudIn->points[i];
                  dst.x = src.x;
                  dst.y = src.y;
                  dst.z = src.z;
                  dst.intensity = src.intensity;
                  dst.ring = src.ring;
                  dst.time = src.t * 1e-9f;
              }
          }
          else
          {
              ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
              ros::shutdown();
          }

我理解这一小部分的功能就是体现了 cache，这里每次从队列的前头拿出要处理的. 并根据sensor的类型做相应的判断.

也就是说无论每次接收到的是什么msg, 经过这个函数处理了之后,拿到的都是队列最前头的数据. 这也是建立队列的道理所在.

此外还有一个函数

       bool deskewInfo()
      {
          std::lock_guard lock1(imuLock);
          std::lock_guard lock2(odoLock);

          // make sure IMU data available for the scan
          if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
          {
              ROS_DEBUG("Waiting for IMU data ...");
              return false;
          }

          imuDeskewInfo();

          odomDeskewInfo();

          return true;
      }

根据代码可以看出来，这个是为了确保imu和lidar数据的对应,主要是根据时间戳来进行的对应. 回过头来再来看细节.先继续往下看.

projectPointCloud函数

这个函数的主要功能有两个

• 得到range,把 rangeMat填完
• 点的预处理,并放入fullCloud

range就不多说了，其实range根据定义就是norm,即到原点的距离,如果传入两个点，就是两个点之间的距离.

  float range = pointDistance(thisPoint);
 float pointDistance(PointType p)
  {
      return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
  }

float pointDistance(PointType p1, PointType p2)
  {
      return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
  }

接下来主要看一下fullCloud

    pcl::PointCloud::Ptr   fullCloud;

这里的 PointType 在utility.h 里面定义着

typedef pcl::PointXYZI PointType;

[其实后面要想给点云加上颜色，也是在这里改就行了,或者自定义点云的类型，也是在这里改即可.]

再回过头来看 projectPointCloud，其实就是把 laserCloudIn中的点的信息把xyzi拿出来放到了 fullCloud里面去.

这里面有个函数 deskewPoint 后面再作详细解析.

cloudExtraction函数

这个函数比较短

  void cloudExtraction()
          {
              int count = 0;
              // extract segmented cloud for lidar odometry
              for (int i = 0; i < N_SCAN; ++i)
              {
              cloudInfo.startRingIndex[i] = count - 1 + 5;

              for (int j = 0; j < Horizon_SCAN; ++j)
              {
                  if (rangeMat.at(i,j) != FLT_MAX)
                  {
                      // mark the points' column index for marking occlusion later
                      cloudInfo.pointColInd[count] = j;
                      // save range info
                      cloudInfo.pointRange[count] = rangeMat.at(i,j);
                      // save extracted cloud
                      extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                      // size of extracted cloud
                      ++count;
                  }
              }
              cloudInfo.endRingIndex[i] = count -1 - 5;
          }
}

这里的cloudInfo就是开头定义的msg了

     lio_sam::cloud_info cloudInfo;

不太清楚为何,startingIndex为何要这样设置.
总的来看这个函数就是把cloudInfo中的信息给补上.比如 pointColInd, pointRange, 补完这些信息之后，将其放入 extractedCloud中去

 pcl::PointCloud::Ptr   extractedCloud;

本来 fullCloud中的点好像是无序的，而这样操作了之后，extractedCloud中的点好像是按照一个大的矩阵 (N_SCAN, Horizon_SCAN) 这样一排一排的放进去的.

publishClouds函数

两个发布者的定义如下

  pubExtractedCloud = nh.advertise ("lio_sam/deskew/cloud_deskewed", 1);
  pubLaserCloudInfo = nh.advertise ("lio_sam/deskew/cloud_info", 1);

 void publishClouds()
      {
          cloudInfo.header = cloudHeader;
          cloudInfo.cloud_deskewed  = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
          pubLaserCloudInfo.publish(cloudInfo);
      }

这个很好理解，就是把之前extract到的给重新发布出去，供后面的算法使用.

这里 publishCloud 的定义在utility.h 中

sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
  {
      sensor_msgs::PointCloud2 tempCloud;
      pcl::toROSMsg(*thisCloud, tempCloud);
      tempCloud.header.stamp = thisStamp;
      tempCloud.header.frame_id = thisFrame;
      if (thisPub->getNumSubscribers() != 0)
          thisPub->publish(tempCloud);
      return tempCloud;
  }

这个可以认为是一个发布模板了. 这个在函数里面判断接收者的数量，如果不为0就发布，发布的是标准的点云msg. 同时有返回值.返回值给了 cloudInfo.cloud_deskewed 然后liosam发布出去

resetParamerers函数

上面已经梳理了 这份代码的主要流程，先是接收bag中的topic点云信息，然后处理成lio-sam能够使用的方式,再进行发布出去，每发布一次这里的参数就会重置一次.

  void resetParameters()
      {
          laserCloudIn->clear();
          extractedCloud->clear();
          // reset range matrix for range image projection
          rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));

          imuPointerCur = 0;
          firstPointFlag = true;
          odomDeskewFlag = false;

          for (int i = 0; i < queueLength; ++i)
          {
              imuTime[i] = 0;
              imuRotX[i] = 0;
              imuRotY[i] = 0;
              imuRotZ[i] = 0;
          }
      }