Direct LiDAR Odometry

DLO-SLAM

这篇SLAM论文《Direct LiDAR Odometry: Fast Localization with Dense Point Clouds》作为NASA喷气推进实验室CoSTAR团队研究和开发的新作，收到了学术界广泛的关注。其主要用作DARPA地下挑战的里程计，提出了一种能够实现高速高精度处理高速实时处理密集点云的激光里程计(LO)的思路，下面是他们的Github开源代码。

点评

• 代码实现中一些小的 trick还是不错的。
• 本文采用稠密点云进行匹配，说是：精度比提取特征(loam系列)的高。
• 由于计算量大的问题，本文首先不进行点云去畸变降低耗时，同时 动态确定 阈值筛选关键帧
• 自适应阈值筛选关键帧，窄道时阈值会缩小。
• 采用 激光与激光匹配和激光与submap匹配，其中submap采用关键帧空间构建子图。
• 子图由 邻近关键帧+关键帧凸包+关键帧凹包构成，基于帧标记确定是否需要重新生成。
• 本文采用 NanoGIcp 轻量级 激光点匹配。
• 子图协方差、法向量由链接关键帧近似，代码中采用scan2scan直接赋值，不需重新计算

论文

点云预处理，比较简单，应用了两个滤波器，

• 一是滤除了自身周围1立方米的点
• 二是通过单位为0.25m的体素格降采样，不需要像loam那样提取特征点。

GICP的位姿计算

• 分两部：scan-to-scan + point-to-map

scan-to-scan

•                                                            X                                  ^                                          k                               L                                      =                                                                a                                     r                                     g                                     min                                     ⁡                                                           X                                     k                                     L                                                             ε                            (                                       X                               k                               L                                                 P                               k                               s                                      ,                                       P                               k                               t                                      )                                  \hat X^L_k = \underset{X^L_k}{arg \min} \varepsilon (X^L_kP_k^s,P_k^t)                     X^kL​=XkL​argmin​ε(XkL​Pks​,Pkt​)- 其中：转换关系                                                                             X                                        ^                                                  k                                     L                                                      \hat X^L_k                           X^kL​，源点                                                               P                                     k                                     s                                                      P^s_k                           Pks​，目标                                                                P                                     k                                     t                                                      P_k^t                           Pkt​，其中                                                               P                                     k                                     t                                              =                                               P                                                   k                                        −                                        1                                                  s                                                      P_k^t= P_{k-1}^s                           Pkt​=Pk−1s​- 其中误差项：                                                  ε                                  (                                               X                                     k                                     L                                                           P                                     k                                     s                                              ,                                               P                                     k                                     t                                              )                                  =                                               ∑                                     i                                     N                                                           d                                     i                                     T                                              (                                               C                                                   k                                        ,                                        i                                                  t                                              +                                               X                                     k                                     L                                                           C                                                   k                                        ,                                        i                                                  s                                                                         X                                        k                                        L                                                  T                                                           )                                                   −                                        1                                                                        d                                     i                                                      \varepsilon (X^L_kP_k^s,P_k^t) = \sum_i^N d_i^T(C^t_{k,i}+X^L_k C^s_{k,i} {X^L_k}^T)^{-1}d_i                           ε(XkL​Pks​,Pkt​)=∑iN​diT​(Ck,it​+XkL​Ck,is​XkL​T)−1di​
  

• 因此 X ^ k L = a r g min ⁡ X k L ∑ i N d i T ( C k , i t + X k L C k , i s X k L T ) − 1 d i \hat X^L_k = \underset{X^L_k}{arg \min} \sum_i^N d_i^T(C^t_{k,i}+X^L_k C^s_{k,i} {X^L_k}^T)^{-1}d_i X^kL​=XkL​argmin​∑iN​diT​(Ck,it​+XkL​Ck,is​XkL​T)−1di​
• N个相关点形成的误差函数， d i = p i t − X k L p i s , p i s ∈ P k s , p i t ∈ P k t , ∀ i d_i = p_i^t-X_k^L p_i^s,\ \ \ p_i^s \in P_k^s,p_i^t \in P_k^t,\forall i di​=pit​−XkL​pis​, pis​∈Pks​,pit​∈Pkt​,∀i
• 先验： IMU ？ X ~ k L = X ~ k B \tilde X_k^L = \tilde X_k^B XkL​=XkB​ : X ~ k L = I \tilde X_k^L = \mathbf I X~kL​=I

scan-to-map

•                                                            X                                  ^                                          k                               W                                      =                                                                a                                     r                                     g                                     min                                     ⁡                                                           X                                     k                                     W                                                             ε                            (                                       X                               k                               W                                                 P                               k                               s                                      ,                                       S                               k                                      )                                  \hat X^W_k = \underset{X^W_k}{arg \min} \varepsilon (X^W_kP_k^s,S_k)                     X^kW​=XkW​argmin​ε(XkW​Pks​,Sk​)-                                                                              X                                        ^                                                  k                                     W                                              =                                                                            a                                           r                                           g                                           min                                           ⁡                                                                     X                                           k                                           L                                                                                      ∑                                     j                                     M                                                           d                                     i                                     T                                              (                                               C                                                   k                                        ,                                        i                                                  S                                              +                                               X                                     k                                     W                                                           C                                                   k                                        ,                                        i                                                  s                                                                         X                                        k                                        W                                                  T                                                           )                                                   −                                        1                                                                        d                                     i                                                      \hat X^W_k = \underset{X^L_k}{arg \min} \sum_j^M d_i^T(C^S_{k,i}+X^W_k C^s_{k,i} {X^W_k}^T)^{-1}d_i                           X^kW​=XkL​argmin​∑jM​diT​(Ck,iS​+XkW​Ck,is​XkW​T)−1di​- 初始位姿使用scan-to-scan：                                                                             X                                        ~                                                  k                                     W                                              =                                                             X                                        ^                                                                k                                        −                                        1                                                  W                                                                         X                                        ^                                                  k                                     L                                                      \tilde X^W_k = \hat X^W_{k-1} \hat X^L_k                           X~kW​=X^k−1W​X^kL​
  

优化先验

•                                                            w                                  ^                                          k                                      =                                       w                               k                                      +                                       b                               k                               w                                      +                                       n                               k                               w                                            \hat {w}_k = w_k +b^w_k +n^w_k                     w^k​=wk​+bkw​+nkw​
  

•                                                q                                           k                                  +                                  1                                                 =                                       q                               k                                      +                            (                                       1                               2                                                 q                               k                                      ⊗                                       w                               k                                      )                            Δ                            t                                  q_{k+1}=q_k+(\frac 1 2 q_k \otimes w_k)\Delta t                     qk+1​=qk​+(21​qk​⊗wk​)Δt
  

• zero translation

基于关键帧的快速子图优化

• 并不是直接将点云存储到kdtree中，而是创建一个关键帧的副本点云进行搜索，其中每个关键帧都与关键帧字典中的相应点云关联。因此，用于帧图匹配的局部子图，可以通过字典中的点云串联生成，而不用通过地理位置的搜索。
• 这样做的好处有两方面： 一是关键帧空间比点云空间更加节约计算资源；二是基于关键帧的子图构建的子图比基于距离的子图构建的子图更加宽松，这个意味着基于距离的子图构建重复位置比较多，而基于关键帧的子图具有较少的重复。
• 子图的拼接： C K S = ∑ n N C k , n S C^S_K = \sum ^N_n C^S_{k,n} CKS​=∑nN​Ck,nS​

自适应关键帧

Nano-GICP

• FastGICP + NanoFLANN 两个包进行合并
• 使用NanoGICP的NanoFLANN来高效构建KD-tree, 然后通过FastGICP进行高效匹配.

OdomNode

代码结构

主函数 main

描述：

• 主函数基本啥也没干，就定义了OdomNode，然后等待ros结束，捕捉了程序终止的指令
• OdomNode 主要是两个回调，激光和imu回调，下面有其详细的描述

voidcontrolC(int sig){

  dlo::OdomNode::abort();}intmain(int argc,char** argv){

  ros::init(argc, argv,"dlo_odom_node");
  ros::NodeHandle nh("~");signal(SIGTERM, controlC);sleep(0.5);

  dlo::OdomNode node(nh);
  ros::AsyncSpinner spinner(0);
  spinner.start();
  node.start();
  ros::waitForShutdown();return0;}

核心为 dlo::OdomNode

核心成员：

// 订阅
ros::Subscriber icp_sub;
ros::Subscriber imu_sub;
this->icp_sub = this->nh.subscribe("pointcloud",1,&dlo::OdomNode::icpCB, this);
this->imu_sub = this->nh.subscribe("imu",1,&dlo::OdomNode::imuCB, this);//发布
ros::Publisher odom_pub;
ros::Publisher pose_pub;
ros::Publisher keyframe_pub;
ros::Publisher kf_pub;
this->odom_pub = this->nh.advertise<nav_msgs::Odometry>("odom",1);
this->pose_pub = this->nh.advertise<geometry_msgs::PoseStamped>("pose",1);
this->kf_pub = this->nh.advertise<nav_msgs::Odometry>("kfs",1, true);
this->keyframe_pub = this->nh.advertise<sensor_msgs::PointCloud2>("keyframe",1, true);

回调

激光回调函数 icpCB

主要功能：

• imu -> s2s -> s2m 的一个match 得到姿态
• 说明： - 红色：代表调用的函数，有些短的步骤已经在下面解释了- 绿色：代表 一些文章独特名词- 蓝色：代表一些理解

步骤：

• 两个检测，不满足直接返回- 若 点云中 点个数过少- DLO 程序未初始化，则初始化（IMU 校准，重力对齐） initializeDLO- 若 使用imu 但未标定时，直接 返回- 若 使用imu+已经标定+需重力对齐+未初始化 - 进行重力对齐 gravityAlign- 若提供初始位姿，则初始位姿进行赋值 ·initial_position_·- 该步骤可以使用初始位姿，代表可以连续建图
• 数据预处理，点云+关键帧阈值- 点云预处理 （去 Nan点 + 区域滤波+ 体素滤波） preprocessPoints- 去除Nan：pcl::removeNaNFromPointCloud- Crop box滤波：crop.filter- 体素滤波：vf_scan.filter- 计算关键帧指标，现启独立线程计算dlo::OdomNode::computeMetrics- 该帧点云长度中位数进行低通滤波，然后 基于长度设定关键帧选择阈值 - computeSpaciousness 平均长度- 计算当前激光水平长度 x 2 + y 2 \sqrt{x^2+y^2} x2+y2​的中位数，并用低通滤波器均衡化 0.95 L m i + 0.5 L c u r r 0.95L_{mi}+0.5L_{curr} 0.95Lmi​+0.5Lcurr​- 变量 ：spaciousness- 基于指标确定关键帧阈值 setAdaptiveParams- 若目标信息为空时，进行初始化 initializeInputTarget，然后 return
• 当前帧设为源数据 setInputSources，得到 当前帧的位姿getNextPose- 设置 S2S gicp 的输入源 gicp_s2s.setInputSource \ \ gicp.registerInputSource- 当前帧的位姿： IMU + S2S + S2M 三者共同作用得到的
• 更新关键帧 updateKeyframes ，得到关键帧周围数量 + 最近关键帧距离及id
• 启动独立线程，发送数据到ros publishToROS
• 启动独立线程，调试语句并发布自定义 DLO 消息

/**
 * ICP Point Cloud Callback
 **/void dlo::OdomNode::icpCB(const sensor_msgs::PointCloud2ConstPtr& pc){double then = ros::Time::now().toSec();/// 当前时刻更新
  this->scan_stamp = pc->header.stamp;
  this->curr_frame_stamp = pc->header.stamp.toSec();// If there are too few points in the pointcloud, try again/// 如果点云中的点太少，该帧直接丢弃
  this->current_scan = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);
  pcl::fromROSMsg(*pc,*this->current_scan);if(this->current_scan->points.size()< this->gicp_min_num_points_){ROS_WARN("Low number of points!");return;}// DLO Initialization procedures (IMU calib, gravity align)/// DLO 初始化程序（IMU 校准，重力对齐）if(!this->dlo_initialized){
    this->initializeDLO();return;}// Preprocess points 预处理点云
  this->preprocessPoints();// Compute Metrics  计算关键帧指标，独立线程计算
  this->metrics_thread = std::thread(&dlo::OdomNode::computeMetrics, this );
  this->metrics_thread.detach();// detach()的作用是将子线程和主线程的关联分离// Set Adaptive Parameters  根据空间度量设置关键帧阈值if(this->adaptive_params_use_){
    this->setAdaptiveParams();}// Set initial frame as target  初始化目标信息if(this->target_cloud == nullptr){
    this->initializeInputTarget();return;}// Set source frame 设置源数据
  this->source_cloud = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);
  this->source_cloud = this->current_scan;// Set new frame as input source for both gicp objects
  this->setInputSources();// Get the next pose via IMU + S2S + S2M
  this->getNextPose();// 更新当前关键帧，得到关键帧周围数量 + 最近关键帧距离及id// Update current keyframe poses and map
  this->updateKeyframes();// Update trajectory  更新轨迹
  this->trajectory.push_back( std::make_pair(this->pose, this->rotq));// Update next time stamp
  this->prev_frame_stamp = this->curr_frame_stamp;// Update some statistics 计算本次耗时
  this->comp_times.push_back(ros::Time::now().toSec()- then);// Publish stuff to ROS  启动独立线程，发送数据到ros
  this->publish_thread = std::thread(&dlo::OdomNode::publishToROS, this );
  this->publish_thread.detach();// Debug statements and publish custom DLO message// 启动独立线程，调试语句并发布自定义 DLO 消息
  this->debug_thread = std::thread(&dlo::OdomNode::debug, this );
  this->debug_thread.detach();}

imu回调 imuCB

功能：

• 该回调中主要做的活：先静止几秒算bias，然后数据去偏差放入队列中

步骤：

• 得到imu的测量数据，角速度+线速度
• 若未标定过，则进行求bias偏差 - 先取静止三秒的 陀螺仪和加速度计数据- 然后求均值作为偏差
• 将测量数据减去bias得到实际测量值
• 将实际测量值放入队列中

void dlo::OdomNode::imuCB(const sensor_msgs::Imu::ConstPtr& imu){// 不使用imu时，直接returnif(!this->imu_use_){return;}double ang_vel[3], lin_accel[3];// Get IMU samples 得到imu的测量数据，角速度+线速度
  ang_vel[0]= imu->angular_velocity.x;
  ang_vel[1]= imu->angular_velocity.y;
  ang_vel[2]= imu->angular_velocity.z;

  lin_accel[0]= imu->linear_acceleration.x;
  lin_accel[1]= imu->linear_acceleration.y;
  lin_accel[2]= imu->linear_acceleration.z;if(this->first_imu_time ==0.){
    this->first_imu_time = imu->header.stamp.toSec();}// IMU calibration procedure - do for three seconds IMU 校准程序 - 做三秒钟 if(!this->imu_calibrated){//imu未标定时staticint num_samples =0;static bool print = true;if((imu->header.stamp.toSec()- this->first_imu_time)< this->imu_calib_time_){

      num_samples++;

      this->imu_bias.gyro.x += ang_vel[0];
      this->imu_bias.gyro.y += ang_vel[1];
      this->imu_bias.gyro.z += ang_vel[2];

      this->imu_bias.accel.x += lin_accel[0];
      this->imu_bias.accel.y += lin_accel[1];
      this->imu_bias.accel.z += lin_accel[2];// 一次打印，imu开始标定if(print){
        std::cout <<"Calibrating IMU for "<< this->imu_calib_time_ <<" seconds... "; std::cout.flush();
        print = false;}}else{// 计算 陀螺仪b和加速度g的平均偏差，标定成功
      this->imu_bias.gyro.x /= num_samples;
      this->imu_bias.gyro.y /= num_samples;
      this->imu_bias.gyro.z /= num_samples;

      this->imu_bias.accel.x /= num_samples;
      this->imu_bias.accel.y /= num_samples;
      this->imu_bias.accel.z /= num_samples;

      this->imu_calibrated = true;

      std::cout <<"done"<< std::endl;
      std::cout <<"  Gyro biases [xyz]: "<< this->imu_bias.gyro.x <<", "<< this->imu_bias.gyro.y <<", "<< this->imu_bias.gyro.z << std::endl << std::endl;}}else{// Apply the calibrated bias to the new IMU measurements
    this->imu_meas.stamp = imu->header.stamp.toSec();// 去过畸变的 bias，将其放入队列中
    this->imu_meas.ang_vel.x = ang_vel[0]- this->imu_bias.gyro.x;
    this->imu_meas.ang_vel.y = ang_vel[1]- this->imu_bias.gyro.y;
    this->imu_meas.ang_vel.z = ang_vel[2]- this->imu_bias.gyro.z;

    this->imu_meas.lin_accel.x = lin_accel[0];
    this->imu_meas.lin_accel.y = lin_accel[1];
    this->imu_meas.lin_accel.z = lin_accel[2];// Store into circular buffer
    this->mtx_imu.lock();
    this->imu_buffer.push_front(this->imu_meas);
    this->mtx_imu.unlock();}}

其他函数

重力对齐 gravityAlign

步骤：

• 原理：缓存1s以内加速度的值，将初始方向设为：重力方向与加速度方向的夹角
• 取一秒之内的 imu 加速度，并计算其 均值加速度 ，并归一化
• 定义重力矢量（假设点向下） g r a v = { 0 , 0 , 1 } \mathbf{grav}= { 0,0,1} grav={0,0,1}
• 计算两个向量之间的角度，作为重力对齐方向，并进行归一化 Eigen::Quaternionf::FromTwoVectors
• 设置重力对齐方向，并打印 grav_q.toRotationMatrix().eulerAngles(2, 1, 0)

void dlo::OdomNode::gravityAlign(){// get average acceleration vector for 1 second and normalize
  Eigen::Vector3f lin_accel = Eigen::Vector3f::Zero();///< 取一秒之内的 imu 加速度，并计算其 均值加速度  constdouble then = ros::Time::now().toSec();int n=0;while((ros::Time::now().toSec()- then)<1.){
    lin_accel[0]+= this->imu_meas.lin_accel.x;
    lin_accel[1]+= this->imu_meas.lin_accel.y;
    lin_accel[2]+= this->imu_meas.lin_accel.z;++n;}
  lin_accel[0]/= n; lin_accel[1]/= n; lin_accel[2]/= n;// normalize  归一化（单位1）double lin_norm =sqrt(pow(lin_accel[0],2)+pow(lin_accel[1],2)+pow(lin_accel[2],2));
  lin_accel[0]/= lin_norm; lin_accel[1]/= lin_norm; lin_accel[2]/= lin_norm;// define gravity vector (assume point downwards) 定义重力矢量（假设点向下）
  Eigen::Vector3f grav;
  grav <<0,0,1;// calculate angle between the two vectors 计算两个向量之间的角度
  Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(lin_accel, grav);// normalize  角度进行归一化double grav_norm =sqrt(grav_q.w()*grav_q.w()+ grav_q.x()*grav_q.x()+ grav_q.y()*grav_q.y()+ grav_q.z()*grav_q.z());
  grav_q.w()/= grav_norm; grav_q.x()/= grav_norm; grav_q.y()/= grav_norm; grav_q.z()/= grav_norm;// set gravity aligned orientation  设置重力对齐方向
  this->rotq = grav_q;
  this->T.block(0,0,3,3)= this->rotq.toRotationMatrix();
  this->T_s2s.block(0,0,3,3)= this->rotq.toRotationMatrix();
  this->T_s2s_prev.block(0,0,3,3)= this->rotq.toRotationMatrix();// rpy 并进行打印auto euler = grav_q.toRotationMatrix().eulerAngles(2,1,0);double yaw = euler[0]*(180.0/M_PI);double pitch = euler[1]*(180.0/M_PI);double roll = euler[2]*(180.0/M_PI);

  std::cout <<"done"<< std::endl;
  std::cout <<"  Roll [deg]: "<< roll << std::endl;
  std::cout <<"  Pitch [deg]: "<< pitch << std::endl << std::endl;}

设定关键帧阈值setAdaptiveParams

步骤：

• 根据空间度量spaciousness设置关键帧阈值
• 关键帧步长： - 若激光均值长度 [ 20 , + ∞ ] [20,+\infty] [20,+∞]：10.0 m- 若激光均值长度 [ 10 , 20 ] [10,20] [10,20]：5.0 m- 若激光均值长度 [ 5 , 10 ] [5,10] [5,10]：1.0 m- 若激光均值长度 [ 0.0 , 5.0 ] [0.0,5.0] [0.0,5.0]：0.5 m

void dlo::OdomNode::setAdaptiveParams(){// Set Keyframe Thresh from Spaciousness Metricif(this->metrics.spaciousness.back()>20.0){
    this->keyframe_thresh_dist_ =10.0;}elseif(this->metrics.spaciousness.back()>10.0&& this->metrics.spaciousness.back()<=20.0){
    this->keyframe_thresh_dist_ =5.0;}elseif(this->metrics.spaciousness.back()>5.0&& this->metrics.spaciousness.back()<=10.0){
    this->keyframe_thresh_dist_ =1.0;}elseif(this->metrics.spaciousness.back()<=5.0){
    this->keyframe_thresh_dist_ =0.5;}// set concave hull alpha
  this->concave_hull.setAlpha(this->keyframe_thresh_dist_);}

updateKeyframes

步骤：

• 将当前点云基于全局位姿转换到 map坐标系 transformCurrentScan- > this->current_scan_t = pcl::PointCloud::Ptr (new pcl::PointCloud);> pcl::transformPointCloud (*this->current_scan, *this->current_scan_t, this->T);
• 遍历关键帧，计算当前姿势附近的数量 + 最近距离及id- 计算当前姿势和关键帧姿势之间的距离 Δ d = ∣ ∣ p s t a t e − p i ∣ ∣ \Delta d = ||p_{state}-p_{i}|| Δd=∣∣pstate​−pi​∣∣- 统计小于1.5倍阈值的个数 num_nearby： - Δ d < 1.5 ∗ k e y f r a m e _ t h r e s h _ d i s t _ \Delta d < 1.5 * \mathbf{keyframe_thresh_dist_} Δd<1.5∗keyframe_thresh_dist_? ++num_nearby;- 存储最近的下标closest_idx以及最近的距离closest_d
• 得到最近的 位置+旋转，进而得到与当前state的 差异(位置+角度)- 位置差异： Δ d = ∣ ∣ p s t a t e − p i ∣ ∣ \Delta d = ||p_{state}-p_{i}|| Δd=∣∣pstate​−pi​∣∣- 角度差异： - δ q = ∣ q s t a t e ∗ q c l o s e − 1 ∣ \delta q = |q_{state}q_{close}^{-1}| δq=∣qstate​∗qclose−1​∣- 得到旋转角度： θ = 2 ∗ a t a n 2 { δ q v e c . n o r m ( ) , δ q w } \theta = 2\mathbf{atan2}{\delta q_{vec.norm()},\delta q_w} θ=2∗atan2{δqvec.norm()​,δqw​}- 旋转角度差异： Θ = 180. ∗ θ / π \Theta = 180. * \theta / \pi Θ=180.∗θ/π
• 新关键帧评判标准，感觉1，2 可以合起来 确定是否为关键帧- 1、若 位置差异 大于 位置阈值 或者 角度差异大于 角度阈值时，newKeyframe = true;- 2、若 位置差异 小于 位置阈值 时，newKeyframe = false;- 3、若 位置差异 小于 位置阈值 或者 角度差异大于 角度阈值 且满足 位置小于1.5倍的个数<=1 时，newKeyframe = true;
• 如果 当前帧添加新关键帧，放入关键帧队列和normal队列- keyframes .push- keyframe_normals 来自 gicp_s2s.calculateSourceCovariances()

void dlo::OdomNode::updateKeyframes(){// transform point cloud
  this->transformCurrentScan();// calculate difference in pose and rotation to all poses in trajectoryfloat closest_d = std::numeric_limits<float>::infinity();int closest_idx =0;int keyframes_idx =0;int num_nearby =0;for(constauto& k : this->keyframes){// calculate distance between current pose and pose in keyframesfloat delta_d =sqrt(pow(this->pose[0]- k.first.first[0],2)+pow(this->pose[1]- k.first.first[1],2)+pow(this->pose[2]- k.first.first[2],2));// count the number nearby current poseif(delta_d <= this->keyframe_thresh_dist_ *1.5){++num_nearby;}// store into variableif(delta_d < closest_d){
      closest_d = delta_d;
      closest_idx = keyframes_idx;}

    keyframes_idx++;}// get closest pose and corresponding rotation
  Eigen::Vector3f closest_pose = this->keyframes[closest_idx].first.first;
  Eigen::Quaternionf closest_pose_r = this->keyframes[closest_idx].first.second;// calculate distance between current pose and closest pose from abovefloat dd =sqrt(pow(this->pose[0]- closest_pose[0],2)+pow(this->pose[1]- closest_pose[1],2)+pow(this->pose[2]- closest_pose[2],2));// calculate difference in orientation
  Eigen::Quaternionf dq = this->rotq *(closest_pose_r.inverse());float theta_rad =2.*atan2(sqrt(pow(dq.x(),2)+pow(dq.y(),2)+pow(dq.z(),2)), dq.w());float theta_deg = theta_rad *(180.0/M_PI);// update keyframe
  bool newKeyframe = false;if(abs(dd)> this->keyframe_thresh_dist_ ||abs(theta_deg)> this->keyframe_thresh_rot_){
    newKeyframe = true;}if(abs(dd)<= this->keyframe_thresh_dist_){
    newKeyframe = false;}if(abs(dd)<= this->keyframe_thresh_dist_ &&abs(theta_deg)> this->keyframe_thresh_rot_ && num_nearby <=1){
    newKeyframe = true;}if(newKeyframe){++this->num_keyframes;// voxelization for submapif(this->vf_submap_use_){
      this->vf_submap.setInputCloud(this->current_scan_t);
      this->vf_submap.filter(*this->current_scan_t);}// update keyframe vector
    this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), this->current_scan_t));// compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources())*this->keyframes_cloud +=*this->current_scan_t;*this->keyframe_cloud =*this->current_scan_t;

    this->gicp_s2s.setInputSource(this->keyframe_cloud);
    this->gicp_s2s.calculateSourceCovariances();
    this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances());

    this->publish_keyframe_thread = std::thread(&dlo::OdomNode::publishKeyframe, this );
    this->publish_keyframe_thread.detach();}}

初始化目标数据 initializeInputTarget

步骤：

• 先将 当前点云数据转为 NanoGICP 格式的数据- gicp_s2s 设置 Target，gicp_s2s.setInputTarget- 并求协方差 gicp_s2s.calculateTargetCovariances
• 初始化关键帧 first_keyframe
• 若使用 submap时，初始化 submap，并进行体素滤波 vf_submap
• 保留历史关键帧树- 关键帧 位姿+ 是否第一次 keyframes.push_back- 关键帧 点云 keyframes_cloud- 当前关键帧 点云 keyframe_cloud
• 计算 kdtree 和关键帧法线- gicp_s2s 设置： gicp_s2s.setInputSource- gicp_s2s 计算协方差：gicp_s2s.calculateSourceCovariances();- 关键帧法线：keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()
• 发布 关键帧点云，独立线程 publish_keyframe_thread = std::thread
• 关键帧个数+1

相关信息

• NanoGICP gicp_s2s,gicp- 定制的迭代最接近点解算器，用于轻量级点云扫描与交叉对象数据共享匹配

void dlo::OdomNode::initializeInputTarget(){

  this->prev_frame_stamp = this->curr_frame_stamp;// Convert ros message    点云 pcl-> NanoGICP 的转换，并计算其协方差
  this->target_cloud = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);
  this->target_cloud = this->current_scan;
  this->gicp_s2s.setInputTarget(this->target_cloud);
  this->gicp_s2s.calculateTargetCovariances();// initialize keyframes 初始化关键帧
  pcl::PointCloud<PointType>::Ptr first_keyframe(new pcl::PointCloud<PointType>);
  pcl::transformPointCloud(*this->target_cloud,*first_keyframe, this->T);// voxelization for submap 若使用submap时，设定submap 并进行体素滤波if(this->vf_submap_use_){
    this->vf_submap.setInputCloud(first_keyframe);
    this->vf_submap.filter(*first_keyframe);}// keep history of keyframes 保留历史关键帧
  this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), first_keyframe));*this->keyframes_cloud +=*first_keyframe;*this->keyframe_cloud =*first_keyframe;// compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources())// 计算 kdtree 和关键帧法线（使用 gicp_s2s 输入源作为临时存储，因为它将被 setInputSources() 覆盖
  this->gicp_s2s.setInputSource(this->keyframe_cloud);
  this->gicp_s2s.calculateSourceCovariances();
  this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances());// 定义发布关键帧线程
  this->publish_keyframe_thread = std::thread(&dlo::OdomNode::publishKeyframe, this );
  this->publish_keyframe_thread.detach();// 关键帧数量+1++this->num_keyframes;}

得到下一个位姿 getNextPose

步骤：

• 基于s2s得到全局姿态，先得到相对关系 c u r p r e T ^{pre}{cur}T curpre​T，在得全局位姿 n o w G T = p r e G T ∗ c u r p r e T ^G{now}T=^G_{pre}T*^{pre}_{cur}T nowG​T=preG​T∗curpre​T- 若使用imu时，则imu预积分得帧间位姿 integrateIMU： imu_SE3- 角度：取[pre_frame,curr_frame_stamp) IMU预积分- 位置： [ 0 ， 0 ， 0 ] [0，0，0] [0，0，0]- 否则：imu_SE3 作为初值，得到 s2s的关系 gicp_s2s.align(*aligned- 基于上次全局姿态 进而得到当前位姿位姿propagateS2S
• 得到 submap地图帧- 先基于当前位姿得到submap地图帧索引 getSubmapKeyframes- 欧式距离 dist + 凸包距离dist + 凹包距离dist- 若索引有变化，则重新生成地图+法向量 gicp.setInputTarget +gicp.setTargetCovariances
• 以 s2s 为初值，进行 s2m匹配，得到最终的全局位姿 gicp.align(*aligned, this->T_s2s);
• 数据进行更新，s2s值更新，last值更新等 propagateS2M()

void dlo::OdomNode::getNextPose(){//// FRAME-TO-FRAME PROCEDURE//// Align using IMU prior if available
  pcl::PointCloud<PointType>::Ptr aligned(new pcl::PointCloud<PointType>);// 若使用imu,则 imu进行积分，并进行对齐if(this->imu_use_){
    this->integrateIMU();
    this->gicp_s2s.align(*aligned, this->imu_SE3);}else{
    this->gicp_s2s.align(*aligned);}// Get the local S2S transform     通过 点到点匹配器 得到最后一帧的姿态
  Eigen::Matrix4f T_S2S = this->gicp_s2s.getFinalTransformation();// Get the global S2S transform //计算得到最后一帧与前一帧的相对关系
  this->propagateS2S(T_S2S);// reuse covariances from s2s for s2m// 为 s2m 重用 s2s 的协方差
  this->gicp.source_covs_ = this->gicp_s2s.source_covs_;// Swap source and target (which also swaps KdTrees internally) for next S2S// 为下一个 S2S 交换源和目标（也在内部交换 KdTrees）
  this->gicp_s2s.swapSourceAndTarget();//// FRAME-TO-SUBMAP  //// Get current global submap人 得到该帧submap
  this->getSubmapKeyframes();// 如果子图已经改变，则s2m匹配的target就改变了if(this->submap_hasChanged){// Set the current global submap as the target cloud
    this->gicp.setInputTarget(this->submap_cloud);// Set target cloud's normals as submap normals
    this->gicp.setTargetCovariances( this->submap_normals );}// Align with current submap with global S2S transformation as initial guess// 与当前子图对齐，将全局 S2S 转换作为初始猜测
  this->gicp.align(*aligned, this->T_s2s);// Get final transformation in global frame 在全局坐标系下得到最终的转换
  this->T = this->gicp.getFinalTransformation();// Update the S2S transform for next propagation 更新s2s的转换为下次迭代做准备
  this->T_s2s_prev = this->T;// Update next global pose// Both source and target clouds are in the global frame now, so tranformation is global//更新下一个全局姿势// 源云和目标云现在都在全局框架中，所以转换是全局的，不需要啥操作
  this->propagateS2M();// Set next target cloud as current source cloud  将下一个目标云设置为当前源云*this->target_cloud =*this->source_cloud;}

得到 getSubmapKeyframes

步骤：

1. 先清空用于生成子图的关键帧索引容器submap_kf_idx_curr.clear()
2. 计算每个帧到当前帧的欧氏距离（平移量） Δ d = ∣ ∣ p s t a t e − p i ∣ ∣ \Delta d = ||p_{state}-p_{i}|| Δd=∣∣pstate​−pi​∣∣
3. 将每种 符合前K个最近邻帧放入 submap队列1. 查找来自 距离 关键帧的前k个最邻近帧2. 查找来自 凸包 关键帧的前k个最邻近帧 computeConvexHull3. 查找来自 凹包 关键帧的前k个最邻近帧 computeConcaveHull
4. 将submap关键帧index 进行排序，然后删除重复的id1. sort 排序+unique删除重复
5. 当前submap_index 与旧submap_index 是否一致1. 一致，则直接结束2. 否则，重新生成 submap点云地图+法向量，并更新 旧submap_index

void dlo::OdomNode::getSubmapKeyframes(){// clear vector of keyframe indices to use for submap// 清空用于子图的关键帧索引容器
  this->submap_kf_idx_curr.clear();//// TOP K NEAREST NEIGHBORS FROM ALL KEYFRAMES// 来自所有关键帧的 前K个最近邻帧// calculate distance between current pose and poses in keyframe set
  std::vector<float> ds;
  std::vector<int> keyframe_nn;int i=0;
  Eigen::Vector3f curr_pose = this->T_s2s.block(0,3,3,1);for(constauto& k : this->keyframes){// 计算位姿（平移量）的欧氏距离float d =sqrt(pow(curr_pose[0]- k.first.first[0],2)+pow(curr_pose[1]- k.first.first[1],2)+pow(curr_pose[2]- k.first.first[2],2));
    ds.push_back(d);
    keyframe_nn.push_back(i); i++;}// get indices for top K nearest neighbor keyframe poses// 得到 k个最近 关键帧位姿的下标
  this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn);//// TOP K NEAREST NEIGHBORS FROM CONVEX HULL//// get convex hull indices  得到关键帧位姿凸包的index
  this->computeConvexHull();// get distances for each keyframe on convex hull// 得到凸包中每个关键帧到当前帧位姿的距离
  std::vector<float> convex_ds;for(constauto& c : this->keyframe_convex){
    convex_ds.push_back(ds[c]);}// get indicies for top kNN for convex hull // 筛选出凸包 k个邻近关键帧的下标
  this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex);//// TOP K NEAREST NEIGHBORS FROM CONCAVE HULL//// get concave hull indices 获取凹包的indexs
  this->computeConcaveHull();// get distances for each keyframe on concave hull
  std::vector<float> concave_ds;for(constauto& c : this->keyframe_concave){
    concave_ds.push_back(ds[c]);}// get indicies for top kNN for convex hull
  this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave);//// BUILD SUBMAP//// concatenate all submap clouds and normals// 将submap关键帧 进行排序，然后删除重复的id
  std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());
  this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end());// sort current and previous submap kf list of indices
  std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());
  std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end());// check if submap has changed from previous iteration 检测submap是否改变if(this->submap_kf_idx_curr == this->submap_kf_idx_prev){
    this->submap_hasChanged = false;}else{
    this->submap_hasChanged = true;// reinitialize submap cloud, normals// 重新初始化 子图 点云+法线，并赋值上一次点云
    pcl::PointCloud<PointType>::Ptr submap_cloud_(boost::make_shared<pcl::PointCloud<PointType>>());
    this->submap_normals.clear();for(auto k : this->submap_kf_idx_curr){// create current submap cloud 生成submap*submap_cloud_ +=*this->keyframes[k].second;// grab corresponding submap cloud's normals
      this->submap_normals.insert( std::end(this->submap_normals), std::begin(this->keyframe_normals[k]), std::end(this->keyframe_normals[k]));}
    
    this->submap_cloud = submap_cloud_;
    this->submap_kf_idx_prev = this->submap_kf_idx_curr;}}

取k个最近帧下标 pushSubmapIndices

思路：

• 先按照优先队列的思路，找到第k小的dist元素
• 然后遍历 frame_dist，将小于等于该值的 frame放入 submap_kf_idx_curr

void dlo::OdomNode::pushSubmapIndices(std::vector<float> dists,int k, std::vector<int> frames){// maintain max heap of at most k elements// 定义一个 优先队列
  std::priority_queue<float> pq;/// 优先队列，默认大顶堆，top最大，这样只保留了 较小的 K个数for(auto d : dists){if(pq.size()>= k && pq.top()> d){
      pq.push(d);
      pq.pop();}elseif(pq.size()< k){
      pq.push(d);}}// get the kth smallest element, which should be at the top of the heap// / 获取第k小的元素，它应该在堆顶float kth_element = pq.top();// get all elements smaller or equal to the kth smallest element// 获取小于或等于第 k 个最小元素的所有元素for(int i =0; i < dists.size();++i){if(dists[i]<= kth_element)
      this->submap_kf_idx_curr.push_back(frames[i]);}}

关键帧凸包 computeConvexHull

思路：

• 将位姿作为点云，基于凸包进行索引，凹包类似

步骤：

• 将关键帧位姿 push 作为点云
• 计算点云的凸包，
• 获取凸包上关键帧的索引
• 赋值关键帧凸包索引

void dlo::OdomNode::computeConvexHull(){// at least 4 keyframes for convex hullif(this->num_keyframes <4){return;}// create a pointcloud with points at keyframes
  pcl::PointCloud<PointType>::Ptr cloud = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);for(constauto& k : this->keyframes){
    PointType pt;
    pt.x = k.first.first[0];
    pt.y = k.first.first[1];
    pt.z = k.first.first[2];
    cloud->push_back(pt);}// calculate the convex hull of the point cloud 计算点云的凸包
  this->convex_hull.setInputCloud(cloud);// get the indices of the keyframes on the convex hull  获取凸包上关键帧的索引
  pcl::PointCloud<PointType>::Ptr convex_points = pcl::PointCloud<PointType>::Ptr(new        pcl::PointCloud<PointType>);
  this->convex_hull.reconstruct(*convex_points);

  pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr(new pcl::PointIndices);
  this->convex_hull.getHullPointIndices(*convex_hull_point_idx);

  this->keyframe_convex.clear();for(int i=0; i<convex_hull_point_idx->indices.size();++i){
    this->keyframe_convex.push_back(convex_hull_point_idx->indices[i]);}}

MapNode

主函数

该主函数主要构建了

MapNode

，然后等待直到 ros关闭为止

#include "dlo/map.h"

void controlC(int sig){

  dlo::MapNode::abort();}

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

  ros::init(argc, argv, "dlo_map_node");
  ros::NodeHandle nh("~");

  signal(SIGTERM, controlC);
  sleep(0.5);

  dlo::MapNode node(nh);
  ros::AsyncSpinner spinner(1);
  spinner.start();
  node.start();
  ros::waitForShutdown();return0;}

MapNode 核心成员

• stop 外界信号触发

class dlo::MapNode {

public:MapNode(ros::NodeHandle node_handle);~MapNode();staticvoidabort(){
    abort_ = true;}voidstart();voidstop();    

private:voidabortTimerCB(const ros::TimerEvent& e);voidpublishTimerCB(const ros::TimerEvent& e);voidkeyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe);

  bool savePcd(direct_lidar_odometry::save_pcd::Request& req,
               direct_lidar_odometry::save_pcd::Response& res);voidgetParams();

  ros::NodeHandle nh;
  ros::Timer abort_timer;
  ros::Timer publish_timer;

  ros::Subscriber keyframe_sub;
  ros::Publisher map_pub;

  ros::ServiceServer save_pcd_srv;

  pcl::PointCloud<PointType>::Ptr dlo_map;
  pcl::VoxelGrid<PointType> voxelgrid;

  ros::Time map_stamp;
  std::string odom_frame;

  bool publish_full_map_;double publish_freq_;double leaf_size_;static std::atomic<bool> abort_;};

构造函数 MapNode

步骤：

• 获取参数 - odom_frame="odom"; publish_full_map_=true; publish_freq_=1.0; leaf_size_=0.5;- 获取节点 NS 并删除前导字符 ns- 连接帧名称字符串 odom_frame = ns + "/" + this->odom_frame;
• 创建定时器abort_timer，100hz 检测 程序是否停止，停止则使ros停止
• 如果 发布整个地图时，则 创建发布地图定时器，按一定周期发布点云地图PointCloud2
• 订阅 topic 关键帧：“keyframes”, 1, &dlo::MapNode::keyframeCB,

dlo::MapNode::MapNode(ros::NodeHandle node_handle):nh(node_handle){// 得到参数
  this->getParams();

  this->abort_timer = this->nh.createTimer(ros::Duration(0.01),&dlo::MapNode::abortTimerCB, this);if(this->publish_full_map_){
    this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_),&dlo::MapNode::publishTimerCB, this);}// 订阅关键帧，发布点云地图
  this->keyframe_sub = this->nh.subscribe("keyframes",1,&dlo::MapNode::keyframeCB, this);
  this->map_pub = this->nh.advertise<sensor_msgs::PointCloud2>("map",1);// 保存地图到 pcd 服务
  this->save_pcd_srv = this->nh.advertiseService("save_pcd",&dlo::MapNode::savePcd, this);// initialize map 初始化地图
  this->dlo_map = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);ROS_INFO("DLO Map Node Initialized");}

关键帧回调 keyframeCB

步骤：

• 将关键帧数据格式由 ros->pcl pcl::fromROSMsg
• 关键帧点云数据进行体素滤波降采样 voxelgrid.filter
• 关键帧加载到全局地图中 pcl格式，直接+号
• 发布地图

void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe){// convert scan to pcl format    : ros type to pcl
  pcl::PointCloud<PointType>::Ptr keyframe_pcl = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>);
  pcl::fromROSMsg(*keyframe,*keyframe_pcl);// voxel filter :  该关键帧进行降采样
  this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_);
  this->voxelgrid.setInputCloud(keyframe_pcl);
  this->voxelgrid.filter(*keyframe_pcl);// save keyframe to map  地图中保存该关键帧
  this->map_stamp = keyframe->header.stamp;*this->dlo_map +=*keyframe_pcl;if(!this->publish_full_map_){// 发布if(keyframe_pcl->points.size()== keyframe_pcl->width * keyframe_pcl->height){
      sensor_msgs::PointCloud2 map_ros;
      pcl::toROSMsg(*keyframe_pcl, map_ros);
      map_ros.header.stamp = ros::Time::now();
      map_ros.header.frame_id = this->odom_frame;
      this->map_pub.publish(map_ros);}}}

保存地图 savePcd

• 地图进行提速滤波后保存

步骤：

• 取地图体素大小 + 保存路径
• 将 全局地图 进行体素降采样
• 保存

bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req,
                           direct_lidar_odometry::save_pcd::Response& res){// 地图格式转换
  pcl::PointCloud<PointType>::Ptr m =
    pcl::PointCloud<PointType>::Ptr(boost::make_shared<pcl::PointCloud<PointType>>(*this->dlo_map));float leaf_size = req.leaf_size;
  std::string p = req.save_path;

  std::cout << std::setprecision(2)<<"Saving map to "<< p +"/dlo_map.pcd"<<"... "; std::cout.flush();// voxelize map  地图进行体素滤波
  pcl::VoxelGrid<PointType> vg;
  vg.setLeafSize(leaf_size, leaf_size, leaf_size);
  vg.setInputCloud(m);
  vg.filter(*m);// save map 保存地图int ret = pcl::io::savePCDFileBinary(p +"/dlo_map.pcd",*m);
  res.success = ret ==0;if(res.success){
    std::cout <<"done"<< std::endl;}else{
    std::cout <<"failed"<< std::endl;}return res.success;}

相关

点云曲面重建

• 知乎 here
• PCL库种surface模块是用来对三维扫描获取的原始点云进行曲面重建的，该模块包含实现点云重建的基础算法与数据结构。
• ConvexHull凸包、ConcaveHull凹包

ConvexHull

凸包

• Class pcl::ConcaveHull< PointInT >
• 类ConvexHull实现了创建凸多边形的算法，该类的实现其实是Hull库实现的接口封装，ConcvexHull支持二维和三维点集。
• #include<pcl/surface/convex_hull.h>ConvexHull()// 空构造函数virtual ~ConvexHull()// 空析构函数voidreconstruct(PointCloud &points, std::vector< pcl::Vertices >&polygons)// 计算所有输入点的凸多边形,参数 points存储最终产生的凸多边形上的顶点，参数 polygons存储一系列顶点集,每个顶点集构成的一个多边形,Vertices数据结构包含一组点的索引,索引是point中的点对应的索引。// chull.reconstruct(model, polygons); // 计算凸包，将结果存储在model中，同时保存顶点信息在polygons中voidreconstruct(PointCloud &points)// 计算所有输入点的凸多边形﹐输出的维数取决于输入的维数是二维或三维,输出结果为多边形顶点并存储在参数output中。voidsetComputeAreaVolume(bool value)// 是否计算凸多边形的面积和体积,如果参数value为真,调用qhull库计算凸多边形的总面积和总体积。为节省计算资源,参数value缺省值为false。doublegetTotalArea()const// 获取凸包的总面积,如果 setComputeAreaVolume设置为真时,才会有有效输出。doublegetTotalVolume()const// 获取凸包的总体积,如果 setComputeAreaVolume设置为真时,才会有有效输出。voidsetDimension(int dimension)// 设置输入数据的维数(二维或三维),参数dimension为设置输入数据的维度﹐如果没有设置,输入数据的维度将自动根据输入点云设置。intgetDimension()const// 返回计算得到多边形的维数(二维或三维)。getHullPointIndices函数用于获取凸包的输入点云的索引。该函数通过检索凸包中的点并返回它们的索引，使得这些点可以从原始点云中提取出来。voidgetHullPointIndices(pcl::PointIndices &hull_point_indices)const// 检索凸包的输入点云的索引。

ConcaveHull

凹包

• 类ConcaveHull实现了创建凹多边形的算法，该类的实现其实是Hull库实现的接口封装，ConcaveHull支持二维和三维点集。
• #include<pcl/surface/concave_hull.h>ConcaveHull()// 空构造函数virtual ~ConcaveHull()// 空析构函数voidsetSearchMethod(const KdTreePtr &tree)// 设置搜索时所用的搜索机制，参数tree指向搜索时所用的搜索对象，例如kd-tree、octree等对象。 virtual voidsetInputCloud(const PointCloudConstPtr &cloud)// 设置输入点云,参数cloud 为输入点云的共享指针引用。virtual voidsetIndices(const IndicesPtr &indices)// 为输入点云提供点云索引向量的指针。virtual voidsetIndices(const IndicesConstPtr &indices)// 为输入点云提供点云索引向量的指针,该索引为常数,不能更改。virtual voidsetIndices(const PointIndicesConstPtr &indices)// 为输入点云提供点云不变索引向量指针的指针。virtual voidsetIndices(size_t row_start,size_t col_start,size_t nb_rows,size_t nb_cols)// 为输入点云中要用到的部分提供索引,该方法只能用于有序点云。参数row_start为行偏移、col_start为列偏移、nb_rows表示 row_start进行的行偏移数,nb_cols表示进行的列偏移数。voidreconstruct(PointCloud &points, std::vector< pcl::Vertices >&polygons)// 计算所有输入点的凹多边形,参数 points存储最终产生的凹多边形上的顶点，参数 polygons存储一系列顶点集,每个顶点集构成的一个多边形,Vertices数据结构包含一组点的索引,索引是point中的点对应的索引。voidreconstruct(PointCloud &output)// 计算所有输入点的凹多边形﹐输出的维数取决于输入的维数是二维或三维,输出结果为多边形顶点并存储在参数output中。voidsetAlpha(double alpha)// 设置alpha值,参数alpha限制voronoi图中多边形边长的长短(边长越短多边形越细分),alpha值是一个非零的正值,定义了voronoi图中多边形顶点到中心点的最大距离。doublegetAlpha()// 获取alpha值voidsetVoronoiCenters(PointCloudPtr voronoi_centers)// 设置参数voronoi_centers,如果设置,最终产生的凹多边形对应的voronoi图中的多边形的中心将被存储在参数voronoi_centers中。voidsetKeepInformation(bool value)// 设置参数keep_information,如果keep_information为真,凹多边形中的顶点就保留其他信息,如 RGB值、法线等。intgetDim()const// 返回计算得到多边形的维数(二维或三维)。intgetDimension()const// 返回计算得到多边形的维数(二维或三维)。voidsetDimension(int dimension)// 设置输入数据的维数(二维或三维),参数dimension为设置输入数据的维度﹐如果没有设置,输入数据的维度将自动根据输入点云设置。voidgetHullPointIndices(pcl::PointIndices &hull_point_indices)const// 检索凸包的输入点云的索引。

End