DLO-SLAM

這篇SLAM論文《Direct LiDAR Odometry: Fast Localization with Dense Point Clouds》作為NASA噴氣推進(jìn)實(shí)驗(yàn)室CoSTAR團(tuán)隊(duì)研究和開(kāi)發(fā)的新作，收到了學(xué)術(shù)界廣泛的關(guān)注。其主要用作DARPA地下挑戰(zhàn)的里程計(jì)，提出了一種能夠?qū)崿F(xiàn)高速高精度處理高速實(shí)時(shí)處理密集點(diǎn)云的激光里程計(jì)(LO)的思路，下面是他們的Github開(kāi)源代碼。

點(diǎn)評(píng)

• 代碼有些小trick還是很不錯(cuò)的。
• 本文采用稠密點(diǎn)云進(jìn)行匹配，說(shuō)是：精度比提取特征(loam系列)的高。
• 由于計(jì)算量大的問(wèn)題，本文首先不進(jìn)行點(diǎn)云去畸變降低耗時(shí)，同時(shí) 動(dòng)態(tài)確定 閾值篩選關(guān)鍵幀
• 自適應(yīng)閾值篩選關(guān)鍵幀，窄道時(shí)閾值會(huì)縮小。
• 采用 激光與激光匹配和激光與submap匹配，其中submap采用關(guān)鍵幀空間構(gòu)建子圖。
• 子圖由 鄰近關(guān)鍵幀+關(guān)鍵幀凸包+關(guān)鍵幀凹包構(gòu)成，基于幀標(biāo)記確定是否需要重新生成。
• 本文采用 NanoGIcp 輕量級(jí) 激光點(diǎn)匹配。
• 子圖協(xié)方差、法向量由鏈接關(guān)鍵幀近似，代碼中采用scan2scan直接賦值，不需重新計(jì)算

代碼解析

OdomNode

代碼結(jié)構(gòu)

主函數(shù) main

描述：

• 主函數(shù)基本啥也沒(méi)干，就定義了OdomNode，然后等待ros結(jié)束，捕捉了程序終止的指令
• OdomNode 主要是兩個(gè)回調(diào)，激光和imu回調(diào)，下面有其詳細(xì)的描述

void controlC(int sig) {dlo::OdomNode::abort();}int main(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();return 0;}

核心為 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);//發(fā)布
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);

激光回調(diào)函數(shù) icpCB

主要功能：

• imu -> s2s -> s2m 的一個(gè)match 得到姿態(tài)

步驟：

• 兩個(gè)檢測(cè)，不滿(mǎn)足直接返回
  • 若 點(diǎn)云中 點(diǎn)個(gè)數(shù)過(guò)少
  • DLO 程序未初始化，則初始化（IMU 校準(zhǔn)，重力對(duì)齊）
• 數(shù)據(jù)預(yù)處理，點(diǎn)云+關(guān)鍵幀閾值
  • 點(diǎn)云預(yù)處理 （去 Nan點(diǎn) + 區(qū)域?yàn)V波+ 體素濾波）
  • 計(jì)算關(guān)鍵幀指標(biāo)，獨(dú)立線(xiàn)程計(jì)算
    • 該幀點(diǎn)云長(zhǎng)度中位數(shù)進(jìn)行低通濾波，然后 基于長(zhǎng)度設(shè)定關(guān)鍵幀選擇閾值
  • 基于指標(biāo)確定關(guān)鍵幀閾值
  • 若目標(biāo)信息為空時(shí)，進(jìn)行初始化，然后 return
• 當(dāng)前幀設(shè)為源數(shù)據(jù)，得到 當(dāng)前幀的位姿 getNextPose
  • IMU + S2S + S2M 三者共同作用得到的
• 更新關(guān)鍵幀 ，得到關(guān)鍵幀周?chē)鷶?shù)量 + 最近關(guān)鍵幀距離及id
• 啟動(dòng)獨(dú)立線(xiàn)程，發(fā)送數(shù)據(jù)到ros
• 啟動(dòng)獨(dú)立線(xiàn)程，調(diào)試語(yǔ)句并發(fā)布自定義 DLO 消息

/*** ICP Point Cloud Callback**/
void dlo::OdomNode::icpCB(const sensor_msgs::PointCloud2ConstPtr& pc) {double then = ros::Time::now().toSec();/// 當(dāng)前時(shí)刻更新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/// 如果點(diǎn)云中的點(diǎn)太少，該幀直接丟棄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 校準(zhǔn)，重力對(duì)齊）if (!this->dlo_initialized) {this->initializeDLO();return;}// Preprocess points 預(yù)處理點(diǎn)云this->preprocessPoints();// Compute Metrics  計(jì)算關(guān)鍵幀指標(biāo)，獨(dú)立線(xiàn)程計(jì)算this->metrics_thread = std::thread( &dlo::OdomNode::computeMetrics, this );this->metrics_thread.detach(); // detach()的作用是將子線(xiàn)程和主線(xiàn)程的關(guān)聯(lián)分離// Set Adaptive Parameters  根據(jù)空間度量設(shè)置關(guān)鍵幀閾值if (this->adaptive_params_use_){this->setAdaptiveParams();}// Set initial frame as target  初始化目標(biāo)信息if(this->target_cloud == nullptr) {this->initializeInputTarget();return;}// Set source frame 設(shè)置源數(shù)據(jù)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 objectsthis->setInputSources();// Get the next pose via IMU + S2S + S2Mthis->getNextPose();// 更新當(dāng)前關(guān)鍵幀，得到關(guān)鍵幀周?chē)鷶?shù)量 + 最近關(guān)鍵幀距離及id// Update current keyframe poses and mapthis->updateKeyframes();// Update trajectory  更新軌跡this->trajectory.push_back( std::make_pair(this->pose, this->rotq) );// Update next time stampthis->prev_frame_stamp = this->curr_frame_stamp;// Update some statistics 計(jì)算本次耗時(shí)this->comp_times.push_back(ros::Time::now().toSec() - then);// Publish stuff to ROS  啟動(dòng)獨(dú)立線(xiàn)程，發(fā)送數(shù)據(jù)到rosthis->publish_thread = std::thread( &dlo::OdomNode::publishToROS, this );this->publish_thread.detach();// Debug statements and publish custom DLO message// 啟動(dòng)獨(dú)立線(xiàn)程，調(diào)試語(yǔ)句并發(fā)布自定義 DLO 消息this->debug_thread = std::thread( &dlo::OdomNode::debug, this );this->debug_thread.detach();}

初始化 initializeDLO

步驟：

• 若 使用imu 但未標(biāo)定時(shí)，直接 返回
• 若 使用imu+已經(jīng)標(biāo)定+需重力對(duì)齊+未初始化
  • 進(jìn)行重力對(duì)齊
• 若提供初始位姿，則初始位姿進(jìn)行賦值

void dlo::OdomNode::initializeDLO() {// Calibrate IMU 使用imu但未標(biāo)定時(shí)，returnif (!this->imu_calibrated && this->imu_use_) {return;}// Gravity Align 重力對(duì)齊if (this->gravity_align_ && this->imu_use_ && this->imu_calibrated && !this->initial_pose_use_) {std::cout << "Aligning to gravity... "; std::cout.flush();this->gravityAlign();}// Use initial known pose  若提供初始位姿時(shí)，進(jìn)行賦值if (this->initial_pose_use_) {std::cout << "Setting known initial pose... "; std::cout.flush();// set known position 賦值變量有：pose T,t_s2s,T_s2s_prev,originthis->pose = this->initial_position_;this->T.block(0,3,3,1) = this->pose;this->T_s2s.block(0,3,3,1) = this->pose;this->T_s2s_prev.block(0,3,3,1) = this->pose;this->origin = this->initial_position_;// set known orientation 賦值變量有：rotq,T,T_s2s,T_s2s_prevthis->rotq = this->initial_orientation_;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();std::cout << "done" << std::endl << std::endl;}this->dlo_initialized = true;std::cout << "DLO initialized! Starting localization..." << std::endl;}

重力對(duì)齊 gravityAlign

步驟：

• 原理：緩存1s以?xún)?nèi)加速度的值，將初始方向設(shè)為：重力方向與加速度方向的夾角
• 取一秒之內(nèi)的 imu 加速度，并計(jì)算其 均值加速度
• 歸一化均值加速度
• 定義重力矢量（假設(shè)點(diǎn)向下）
• 計(jì)算兩個(gè)向量之間的角度，并進(jìn)行歸一化
• 設(shè)置重力對(duì)齊方向，并打印

void dlo::OdomNode::gravityAlign() {// get average acceleration vector for 1 second and normalizeEigen::Vector3f lin_accel = Eigen::Vector3f::Zero();///< 取一秒之內(nèi)的 imu 加速度，并計(jì)算其 均值加速度  const double 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) 定義重力矢量（假設(shè)點(diǎn)向下）Eigen::Vector3f grav;grav << 0, 0, 1;// calculate angle between the two vectors 計(jì)算兩個(gè)向量之間的角度Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(lin_accel, grav);// normalize  角度進(jìn)行歸一化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  設(shè)置重力對(duì)齊方向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 并進(jìn)行打印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;
}

點(diǎn)云預(yù)處理 preprocessPoints

步驟：

• 保留原始點(diǎn)云 original_scan
• 當(dāng)前點(diǎn)云 移除 Nans 的點(diǎn)
• 當(dāng)前點(diǎn)云 過(guò)濾指定立方體的點(diǎn)云：1m 以?xún)?nèi)的
• 當(dāng)前點(diǎn)云 體素濾波：scan:0.05/ submap:0.1

void dlo::OdomNode::preprocessPoints() {// Original Scan 保留原始點(diǎn)云，用于關(guān)鍵幀存儲(chǔ)*this->original_scan = *this->current_scan;// Remove NaNs 移除Nans點(diǎn)std::vector<int> idx;this->current_scan->is_dense = false;pcl::removeNaNFromPointCloud(*this->current_scan, *this->current_scan, idx);// Crop Box Filter   過(guò)濾指定立方體內(nèi)的點(diǎn)if (this->crop_use_) {this->crop.setInputCloud(this->current_scan);this->crop.filter(*this->current_scan);}// Voxel Grid Filter 體素濾波if (this->vf_scan_use_) {this->vf_scan.setInputCloud(this->current_scan);this->vf_scan.filter(*this->current_scan);}}

關(guān)鍵幀指標(biāo) computeMetrics

步驟：

• 主要調(diào)用了computeSpaciousness 庫(kù)
• 首先計(jì)算了各個(gè)激光點(diǎn)的長(zhǎng)度
• 取長(zhǎng)度的中值，并進(jìn)行 低通濾波
• nth_element是一種部分排序算法，它重新排列 [first, last) 中的元素
• 得到 激光長(zhǎng)度的中位數(shù)
• 低通濾波，0.95，0.05 比例
• 將該值保存，為關(guān)鍵幀閾值做準(zhǔn)備

void dlo::OdomNode::computeMetrics() {this->computeSpaciousness();
}/**計(jì)算當(dāng)前掃描的空間**/
void dlo::OdomNode::computeSpaciousness() {// compute range of points 計(jì)算激光點(diǎn)到原點(diǎn)的長(zhǎng)度 std::vector<float> ds;for (int i = 0; i <= this->current_scan->points.size(); i++) {float d = std::sqrt(pow(this->current_scan->points[i].x, 2) + pow(this->current_scan->points[i].y, 2) + pow(this->current_scan->points[i].z, 2));ds.push_back(d);}// median  取長(zhǎng)度的中值，并進(jìn)行 低通濾波// nth_element 是一種部分排序算法，它重新排列 [first, last) 中的元素std::nth_element(ds.begin(), ds.begin() + ds.size()/2, ds.end());float median_curr = ds[ds.size()/2];static float median_prev = median_curr;float median_lpf = 0.95*median_prev + 0.05*median_curr;median_prev = median_lpf;// pushthis->metrics.spaciousness.push_back( median_lpf );}

設(shè)定關(guān)鍵幀閾值setAdaptiveParams

步驟：

• 根據(jù)空間度量設(shè)置關(guān)鍵幀閾值
• 關(guān)鍵幀步長(zhǎng)：
  • 若激光均值長(zhǎng)度 $[20,+\infty ]$：10.0 m
  • 若激光均值長(zhǎng)度 $[10,20]$：5.0 m
  • 若激光均值長(zhǎng)度 $[5,10]$：1.0 m
  • 若激光均值長(zhǎng)度 $[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;} else if (this->metrics.spaciousness.back() > 10.0 && this->metrics.spaciousness.back() <= 20.0) {this->keyframe_thresh_dist_ = 5.0;} else if (this->metrics.spaciousness.back() > 5.0 && this->metrics.spaciousness.back() <= 10.0) {this->keyframe_thresh_dist_ = 1.0;} else if (this->metrics.spaciousness.back() <= 5.0) {this->keyframe_thresh_dist_ = 0.5;}// set concave hull alphathis->concave_hull.setAlpha(this->keyframe_thresh_dist_);
}

初始化目標(biāo)數(shù)據(jù) initializeInputTarget

步驟：

• 先將點(diǎn)云數(shù)據(jù)轉(zhuǎn)為 NanoGICP 格式的數(shù)據(jù)
• 初始化關(guān)鍵幀
• 若使用 submap時(shí)，初始化 submap，并進(jìn)行體素濾波
• 保留歷史關(guān)鍵幀樹(shù)
  • 關(guān)鍵幀 位姿+ 是否第一次
  • 關(guān)鍵幀 點(diǎn)云
  • 當(dāng)前關(guān)鍵幀 點(diǎn)云
• 計(jì)算 kdtree 和關(guān)鍵幀法線(xiàn)
• 發(fā)布 關(guān)鍵幀點(diǎn)云，獨(dú)立線(xiàn)程
• 關(guān)鍵幀個(gè)數(shù)+1

相關(guān)信息

• NanoGICP gicp_s2s,gicp
  • 定制的迭代最接近點(diǎn)解算器，用于輕量級(jí)點(diǎn)云掃描與交叉對(duì)象數(shù)據(jù)共享匹配

void dlo::OdomNode::initializeInputTarget() {this->prev_frame_stamp = this->curr_frame_stamp;// Convert ros message    點(diǎn)云 pcl-> NanoGICP 的轉(zhuǎn)換，并計(jì)算其協(xié)方差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 初始化關(guān)鍵幀pcl::PointCloud<PointType>::Ptr first_keyframe (new pcl::PointCloud<PointType>);pcl::transformPointCloud (*this->target_cloud, *first_keyframe, this->T);// voxelization for submap 若使用submap時(shí)，設(shè)定submap 并進(jìn)行體素濾波if (this->vf_submap_use_) {this->vf_submap.setInputCloud(first_keyframe);this->vf_submap.filter(*first_keyframe);}// keep history of keyframes 保留歷史關(guān)鍵幀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())// 計(jì)算 kdtree 和關(guān)鍵幀法線(xiàn)（使用 gicp_s2s 輸入源作為臨時(shí)存儲(chǔ)，因?yàn)樗鼘⒈?setInputSources() 覆蓋this->gicp_s2s.setInputSource(this->keyframe_cloud);this->gicp_s2s.calculateSourceCovariances();this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances());// 定義發(fā)布關(guān)鍵幀線(xiàn)程this->publish_keyframe_thread = std::thread( &dlo::OdomNode::publishKeyframe, this );this->publish_keyframe_thread.detach();// 關(guān)鍵幀數(shù)量+1++this->num_keyframes;}

設(shè)置源數(shù)據(jù) setInputSources

• 設(shè)置 S2S gicp 的輸入源
  • 這將構(gòu)建源云的 KdTree
  • 這不會(huì)為 s2m 構(gòu)建 KdTree，因?yàn)?force_no_update 為真
• 使用自定義 NanoGICP 函數(shù)為 S2M gicp 設(shè)置 pcl::Registration 輸入源
• 現(xiàn)在使用之前構(gòu)建的 KdTree 設(shè)置 S2M gicp 的 KdTree

void dlo::OdomNode::setInputSources(){// set the input source for the S2S gicp// this builds the KdTree of the source cloud// this does not build the KdTree for s2m because force_no_update is truethis->gicp_s2s.setInputSource(this->current_scan);// set pcl::Registration input source for S2M gicp using custom NanoGICP functionthis->gicp.registerInputSource(this->current_scan);// now set the KdTree of S2M gicp using previously built KdTreethis->gicp.source_kdtree_ = this->gicp_s2s.source_kdtree_;this->gicp.source_covs_.clear();}

得到下一個(gè)位姿 getNextPose

步驟：

• 基于s2s得到全局姿態(tài)
  • 若使用imu時(shí)，則imu預(yù)積分得幀間位姿： imu_SE3
  • imu_SE3 作為初值，得到 s2s的關(guān)系
  • 基于上次全局姿態(tài) 進(jìn)而得到當(dāng)前位姿位姿
    • T_s2s, pose_s2s, rotSO3_s2s, rotq_s2s
    • 當(dāng)前位姿，當(dāng)前姿態(tài)平移，當(dāng)前姿態(tài)旋轉(zhuǎn)矩陣，歸一化四元數(shù)旋轉(zhuǎn)
    • 為 s2m 賦值 s2s的協(xié)方差
• 得到 submap地圖幀
  • 先基于當(dāng)前位姿得到submap地圖幀索引
    • 歐式距離 dist + 凸包距離dist + 凹包距離dist
  • 若索引有變化，則重新生成地圖+法向量
• 以 s2s 為初值，進(jìn)行 s2m匹配，得到最終的全局位姿
• 數(shù)據(jù)進(jìn)行更新，s2s值更新，last值更新等

void dlo::OdomNode::getNextPose() {//// FRAME-TO-FRAME PROCEDURE//// Align using IMU prior if availablepcl::PointCloud<PointType>::Ptr aligned (new pcl::PointCloud<PointType>);// 若使用imu,則 imu進(jìn)行積分，并進(jìn)行對(duì)齊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     通過(guò) 點(diǎn)到點(diǎn)匹配器 得到最后一幀的姿態(tài)Eigen::Matrix4f T_S2S = this->gicp_s2s.getFinalTransformation();// Get the global S2S transform //計(jì)算得到最后一幀與前一幀的相對(duì)關(guān)系this->propagateS2S(T_S2S);// reuse covariances from s2s for s2m// 為 s2m 重用 s2s 的協(xié)方差this->gicp.source_covs_ = this->gicp_s2s.source_covs_;// Swap source and target (which also swaps KdTrees internally) for next S2S// 為下一個(gè) S2S 交換源和目標(biāo)（也在內(nèi)部交換 KdTrees）this->gicp_s2s.swapSourceAndTarget();//// FRAME-TO-SUBMAP  //// Get current global submap人 得到該幀submapthis->getSubmapKeyframes();// 如果子圖已經(jīng)改變，則s2m匹配的target就改變了if (this->submap_hasChanged) {// Set the current global submap as the target cloudthis->gicp.setInputTarget(this->submap_cloud);// Set target cloud's normals as submap normalsthis->gicp.setTargetCovariances( this->submap_normals );}// Align with current submap with global S2S transformation as initial guess// 與當(dāng)前子圖對(duì)齊，將全局 S2S 轉(zhuǎn)換作為初始猜測(cè)this->gicp.align(*aligned, this->T_s2s);// Get final transformation in global frame 在全局坐標(biāo)系下得到最終的轉(zhuǎn)換this->T = this->gicp.getFinalTransformation();// Update the S2S transform for next propagation 更新s2s的轉(zhuǎn)換為下次迭代做準(zhǔn)備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//更新下一個(gè)全局姿勢(shì)// 源云和目標(biāo)云現(xiàn)在都在全局框架中，所以轉(zhuǎn)換是全局的，不需要啥操作this->propagateS2M();// Set next target cloud as current source cloud  將下一個(gè)目標(biāo)云設(shè)置為當(dāng)前源云*this->target_cloud = *this->source_cloud;}

Imu得幀間 integrateIMU

步驟：

• 從imuBuf中取 兩幀之間的 imu觀(guān)測(cè)數(shù)據(jù) imu_frame
• 觀(guān)測(cè)數(shù)據(jù) imu_frame按時(shí)間進(jìn)行排序
• 進(jìn)行 計(jì)算兩幀之間的角度變化
• 角度歸一化，位置給0，進(jìn)行賦值

void dlo::OdomNode::integrateIMU() {// Extract IMU data between the two framesstd::vector<ImuMeas> imu_frame;for (const auto& i : this->imu_buffer) {// IMU data between two frames is when://   current frame's timestamp minus imu timestamp is positive//   previous frame's timestamp minus imu timestamp is negativedouble curr_frame_imu_dt = this->curr_frame_stamp - i.stamp;double prev_frame_imu_dt = this->prev_frame_stamp - i.stamp;if (curr_frame_imu_dt >= 0. && prev_frame_imu_dt <= 0.) {imu_frame.push_back(i);}}// Sort measurements by timestd::sort(imu_frame.begin(), imu_frame.end(), this->comparatorImu);// Relative IMU integration of gyro and accelerometerdouble curr_imu_stamp = 0.;double prev_imu_stamp = 0.;double dt;Eigen::Quaternionf q = Eigen::Quaternionf::Identity();for (uint32_t i = 0; i < imu_frame.size(); ++i) {if (prev_imu_stamp == 0.) {prev_imu_stamp = imu_frame[i].stamp;continue;}// Calculate difference in imu measurement times IN SECONDScurr_imu_stamp = imu_frame[i].stamp;dt = curr_imu_stamp - prev_imu_stamp;prev_imu_stamp = curr_imu_stamp;// Relative gyro propagation quaternion dynamicsEigen::Quaternionf qq = q;q.w() -= 0.5*( qq.x()*imu_frame[i].ang_vel.x + qq.y()*imu_frame[i].ang_vel.y + qq.z()*imu_frame[i].ang_vel.z ) * dt;q.x() += 0.5*( qq.w()*imu_frame[i].ang_vel.x - qq.z()*imu_frame[i].ang_vel.y + qq.y()*imu_frame[i].ang_vel.z ) * dt;q.y() += 0.5*( qq.z()*imu_frame[i].ang_vel.x + qq.w()*imu_frame[i].ang_vel.y - qq.x()*imu_frame[i].ang_vel.z ) * dt;q.z() += 0.5*( qq.x()*imu_frame[i].ang_vel.y - qq.y()*imu_frame[i].ang_vel.x + qq.w()*imu_frame[i].ang_vel.z ) * dt;}// Normalize quaterniondouble norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z());q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm;// Store IMU guessthis->imu_SE3 = Eigen::Matrix4f::Identity();this->imu_SE3.block(0, 0, 3, 3) = q.toRotationMatrix();}

得到 getSubmapKeyframes

步驟：

1. 先清空用于生成子圖的關(guān)鍵幀索引容器
2. 計(jì)算每個(gè)幀到當(dāng)前幀的歐氏距離（平移量）
3. 將每種 符合前K個(gè)最近鄰幀放入 submap隊(duì)列
   1. 查找來(lái)自 距離 關(guān)鍵幀的前k個(gè)最鄰近幀
   2. 查找來(lái)自 凸包 關(guān)鍵幀的前k個(gè)最鄰近幀
   3. 查找來(lái)自 凹包 關(guān)鍵幀的前k個(gè)最鄰近幀
4. 將submap關(guān)鍵幀index 進(jìn)行排序，然后刪除重復(fù)的id
   1. sort 排序+unique刪除重復(fù)
5. 當(dāng)前submap_index 與舊submap_index 是否一致
   1. 一致，則直接結(jié)束
   2. 否則，重新生成 submap點(diǎn)云地圖+法向量，并更新 舊submap_index

void dlo::OdomNode::getSubmapKeyframes() {// clear vector of keyframe indices to use for submap// 清空用于子圖的關(guān)鍵幀索引容器this->submap_kf_idx_curr.clear();//// TOP K NEAREST NEIGHBORS FROM ALL KEYFRAMES// 來(lái)自所有關(guān)鍵幀的 前K個(gè)最近鄰幀// calculate distance between current pose and poses in keyframe setstd::vector<float> ds;std::vector<int> keyframe_nn; int i=0;Eigen::Vector3f curr_pose = this->T_s2s.block(0,3,3,1);for (const auto& k : this->keyframes) {// 計(jì)算位姿（平移量）的歐氏距離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個(gè)最近 關(guān)鍵幀位姿的下標(biāo)this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn);//// TOP K NEAREST NEIGHBORS FROM CONVEX HULL//// get convex hull indices  得到關(guān)鍵幀位姿凸包的indexthis->computeConvexHull();// get distances for each keyframe on convex hull// 得到凸包中每個(gè)關(guān)鍵幀到當(dāng)前幀位姿的距離std::vector<float> convex_ds;for (const auto& c : this->keyframe_convex) {convex_ds.push_back(ds[c]);}// get indicies for top kNN for convex hull // 篩選出凸包 k個(gè)鄰近關(guān)鍵幀的下標(biāo)this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex);//// TOP K NEAREST NEIGHBORS FROM CONCAVE HULL//// get concave hull indices 獲取凹包的indexsthis->computeConcaveHull();// get distances for each keyframe on concave hullstd::vector<float> concave_ds;for (const auto& c : this->keyframe_concave) {concave_ds.push_back(ds[c]);}// get indicies for top kNN for convex hullthis->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave);//// BUILD SUBMAP//// concatenate all submap clouds and normals// 將submap關(guān)鍵幀 進(jìn)行排序，然后刪除重復(fù)的idstd::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 indicesstd::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 檢測(cè)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// 重新初始化 子圖 點(diǎn)云+法線(xiàn)，并賦值上一次點(diǎn)云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 normalsthis->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個(gè)最近幀下標(biāo) pushSubmapIndices

思路：

• 先按照優(yōu)先隊(duì)列的思路，找到第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// 定義一個(gè) 優(yōu)先隊(duì)列std::priority_queue<float> pq;/// 優(yōu)先隊(duì)列，默認(rèn)大頂堆，top最大，這樣只保留了 較小的 K個(gè)數(shù)for (auto d : dists) {if (pq.size() >= k && pq.top() > d) {pq.push(d);pq.pop();} else if (pq.size() < k) {pq.push(d);}}// get the kth smallest element, which should be at the top of the heap// / 獲取第k小的元素，它應(yīng)該在堆頂float kth_element = pq.top();// get all elements smaller or equal to the kth smallest element// 獲取小于或等于第 k 個(gè)最小元素的所有元素for (int i = 0; i < dists.size(); ++i) {if (dists[i] <= kth_element)this->submap_kf_idx_curr.push_back(frames[i]);}}

關(guān)鍵幀凸包 computeConvexHull

思路：

• 將位姿作為點(diǎn)云，基于凸包進(jìn)行索引，凹包類(lèi)似

步驟：

• 將關(guān)鍵幀位姿 push 作為點(diǎn)云
• 計(jì)算點(diǎn)云的凸包，
• 獲取凸包上關(guān)鍵幀的索引
• 賦值關(guān)鍵幀凸包索引

void dlo::OdomNode::computeConvexHull() {// at least 4 keyframes for convex hullif (this->num_keyframes < 4) {return;}// create a pointcloud with points at keyframespcl::PointCloud<PointType>::Ptr cloud = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);for (const auto& 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 計(jì)算點(diǎn)云的凸包this->convex_hull.setInputCloud(cloud);// get the indices of the keyframes on the convex hull  獲取凸包上關(guān)鍵幀的索引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]);}}

更新關(guān)鍵幀updateKeyframes

步驟：

• 將當(dāng)前點(diǎn)云基于全局位姿轉(zhuǎn)換到 map坐標(biāo)系
• 遍歷關(guān)鍵幀，計(jì)算當(dāng)前姿勢(shì)附近的數(shù)量 + 最近距離及id
  • 計(jì)算當(dāng)前姿勢(shì)和關(guān)鍵幀姿勢(shì)之間的距離
  • 基于距離+1.5閾值 得到 當(dāng)前姿態(tài)附近的數(shù)量

void dlo::OdomNode::updateKeyframes() {// transform point cloudthis->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 (const auto& 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++;}

imu回調(diào) imuCB

功能：

• 該回調(diào)中主要做的活：先靜止幾秒算bias，然后數(shù)據(jù)去偏差放入隊(duì)列中

步驟：

• 得到imu的測(cè)量數(shù)據(jù)，角速度+線(xiàn)速度
• 若未標(biāo)定過(guò)，則進(jìn)行求bias偏差
  • 先取靜止三秒的 陀螺儀和加速度計(jì)數(shù)據(jù)
  • 然后求均值作為偏差
• 將測(cè)量數(shù)據(jù)減去bias得到實(shí)際測(cè)量值
• 將實(shí)際測(cè)量值放入隊(duì)列中

void dlo::OdomNode::imuCB(const sensor_msgs::Imu::ConstPtr& imu) {// 不使用imu時(shí)，直接returnif (!this->imu_use_) {return;}double ang_vel[3], lin_accel[3];// Get IMU samples 得到imu的測(cè)量數(shù)據(jù)，角速度+線(xiàn)速度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 校準(zhǔn)程序 - 做三秒鐘 if (!this->imu_calibrated) { //imu未標(biāo)定時(shí)static int 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開(kāi)始標(biāo)定if(print) {std::cout << "Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; std::cout.flush();print = false;}} else {// 計(jì)算 陀螺儀b和加速度g的平均偏差，標(biāo)定成功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 measurementsthis->imu_meas.stamp = imu->header.stamp.toSec();// 去過(guò)畸變的 bias，將其放入隊(duì)列中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 bufferthis->mtx_imu.lock();this->imu_buffer.push_front(this->imu_meas);this->mtx_imu.unlock();}}

MapNode

主函數(shù)

該主函數(shù)主要構(gòu)建了 MapNode，然后等待直到 ros關(guān)閉為止

#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();return 0;}

MapNode 核心成員

class dlo::MapNode {public:MapNode(ros::NodeHandle node_handle);~MapNode();static void abort() {abort_ = true;}void start();void stop();private:void abortTimerCB(const ros::TimerEvent& e);void publishTimerCB(const ros::TimerEvent& e);void keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe);bool savePcd(direct_lidar_odometry::save_pcd::Request& req,direct_lidar_odometry::save_pcd::Response& res);void getParams();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_;};

構(gòu)造函數(shù) MapNode

步驟：

• 獲取參數(shù)
  • string odom_frame; bool publish_full_map_ ; double publish_freq_; double leaf_size_;
  • 獲取節(jié)點(diǎn) NS 并刪除前導(dǎo)字符 ns
  • 連接幀名稱(chēng)字符串 odom_frame = ns + "/" + this->odom_frame;
• 創(chuàng)建定時(shí)器abort_timer，實(shí)時(shí)檢測(cè) 程序是否停止，停止則使ros停止
• 如果 發(fā)布整個(gè)地圖時(shí)，則 創(chuàng)建發(fā)布地圖定時(shí)器，按一定周期發(fā)布點(diǎn)云地圖PointCloud2

dlo::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) {// 得到參數(shù)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);}// 訂閱關(guān)鍵幀，發(fā)布點(diǎn)云地圖this->keyframe_sub = this->nh.subscribe("keyframes", 1, &dlo::MapNode::keyframeCB, this);this->map_pub = this->nh.advertise<sensor_msgs::PointCloud2>("map", 1);// 保存地圖到 pcd 服務(wù)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");
}

關(guān)鍵幀回調(diào) keyframeCB

步驟：

• 將關(guān)鍵幀數(shù)據(jù)格式由 ros->pcl
• 關(guān)鍵幀點(diǎn)云數(shù)據(jù)進(jìn)行體素濾波降采樣
• 關(guān)鍵幀加載到全局地圖中
• 發(fā)布地圖

void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe) {// convert scan to pcl format    : ros type to pclpcl::PointCloud<PointType>::Ptr keyframe_pcl = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);pcl::fromROSMsg(*keyframe, *keyframe_pcl);// voxel filter :  該關(guān)鍵幀進(jìn)行降采樣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  地圖中保存該關(guān)鍵幀this->map_stamp = keyframe->header.stamp;*this->dlo_map += *keyframe_pcl;if (!this->publish_full_map_) {// 發(fā)布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

步驟：

• 地圖進(jìn)行提速濾波后保存

bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req,direct_lidar_odometry::save_pcd::Response& res) {// 地圖格式轉(zhuǎn)換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  地圖進(jìn)行體素濾波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;}

相關(guān)

點(diǎn)云曲面重建

• 知乎 here

• PCL庫(kù)種surface模塊是用來(lái)對(duì)三維掃描獲取的原始點(diǎn)云進(jìn)行曲面重建的，該模塊包含實(shí)現(xiàn)點(diǎn)云重建的基礎(chǔ)算法與數(shù)據(jù)結(jié)構(gòu)。

• ConvexHull凸包、ConcaveHull凹包

ConvexHull凸包

• Class pcl::ConcaveHull< PointInT >

• 類(lèi)ConvexHull實(shí)現(xiàn)了創(chuàng)建凸多邊形的算法，該類(lèi)的實(shí)現(xiàn)其實(shí)是Hull庫(kù)實(shí)現(xiàn)的接口封裝，ConcvexHull支持二維和三維點(diǎn)集。

• #include <pcl/surface/convex_hull.h>
  ConvexHull () 
  //  空構(gòu)造函數(shù)
  virtual  ~ConvexHull () 
  //  空析構(gòu)函數(shù)
  void  reconstruct (PointCloud &points, std::vector< pcl::Vertices > &polygons) 
  //  計(jì)算所有輸入點(diǎn)的凸多邊形,參數(shù) points存儲(chǔ)最終產(chǎn)生的凹多邊形上的頂點(diǎn)，參數(shù) polygons存儲(chǔ)一系列頂點(diǎn)集,每個(gè)頂點(diǎn)集構(gòu)成的一個(gè)多邊形,Vertices數(shù)據(jù)結(jié)構(gòu)包含一組點(diǎn)的索引,索引是point中的點(diǎn)對(duì)應(yīng)的索引。
  void  reconstruct (PointCloud &points) 
  //  計(jì)算所有輸入點(diǎn)的凸多邊形﹐輸出的維數(shù)取決于輸入的維數(shù)是二維或三維,輸出結(jié)果為多邊形頂點(diǎn)并存儲(chǔ)在參數(shù)output中。
  void  setComputeAreaVolume (bool value) 
  //  是否計(jì)算凸多邊形的面積和體積,如果參數(shù)value為真,調(diào)用qhull庫(kù)計(jì)算凸多邊形的總面積和總體積。為節(jié)省計(jì)算資源,參數(shù)value缺省值為false。
  double  getTotalArea () const 
  //  獲取凸包的總面積,如果 setComputeAreaVolume設(shè)置為真時(shí),才會(huì)有有效輸出。
  double  getTotalVolume () const 
  //  獲取凸包的總體積,如果 setComputeAreaVolume設(shè)置為真時(shí),才會(huì)有有效輸出。
  void  setDimension (int dimension) 
  //  設(shè)置輸入數(shù)據(jù)的維數(shù)(二維或三維),參數(shù)dimension為設(shè)置輸入數(shù)據(jù)的維度﹐如果沒(méi)有設(shè)置,輸入數(shù)據(jù)的維度將自動(dòng)根據(jù)輸入點(diǎn)云設(shè)置。
  int  getDimension () const 
  //  返回計(jì)算得到多邊形的維數(shù)(二維或三維)。
  void  getHullPointIndices (pcl::PointIndices &hull_point_indices) const 
  //  檢索凸包的輸入點(diǎn)云的索引。
  

ConcaveHull凹包

• 類(lèi)ConcaveHull實(shí)現(xiàn)了創(chuàng)建凹多邊形的算法，該類(lèi)的實(shí)現(xiàn)其實(shí)是Hull庫(kù)實(shí)現(xiàn)的接口封裝，ConcaveHull支持二維和三維點(diǎn)集。

• #include <pcl/surface/concave_hull.h>
  ConcaveHull () 
  //  空構(gòu)造函數(shù)
  virtual  ~ConcaveHull () 
  //  空析構(gòu)函數(shù)
  void  setSearchMethod (const KdTreePtr &tree) 
  //  設(shè)置搜索時(shí)所用的搜索機(jī)制，參數(shù)tree指向搜索時(shí)所用的搜索對(duì)象，例如kd-tree、octree等對(duì)象。 
  virtual void  setInputCloud (const PointCloudConstPtr &cloud) 
  //  設(shè)置輸入點(diǎn)云,參數(shù)cloud 為輸入點(diǎn)云的共享指針引用。
  virtual void  setIndices (const IndicesPtr &indices) 
  //  為輸入點(diǎn)云提供點(diǎn)云索引向量的指針。
  virtual void  setIndices (const IndicesConstPtr &indices) 
  //  為輸入點(diǎn)云提供點(diǎn)云索引向量的指針,該索引為常數(shù),不能更改。
  virtual void  setIndices (const PointIndicesConstPtr &indices) 
  //  為輸入點(diǎn)云提供點(diǎn)云不變索引向量指針的指針。
  virtual void  setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols) 
  //  為輸入點(diǎn)云中要用到的部分提供索引,該方法只能用于有序點(diǎn)云。參數(shù)row_start為行偏移、col_start為列偏移、nb_rows表示 row_start進(jìn)行的行偏移數(shù),nb_cols表示進(jìn)行的列偏移數(shù)。
  void  reconstruct (PointCloud &points, std::vector< pcl::Vertices > &polygons) 
  //  計(jì)算所有輸入點(diǎn)的凹多邊形,參數(shù) points存儲(chǔ)最終產(chǎn)生的凹多邊形上的頂點(diǎn)，參數(shù) polygons存儲(chǔ)一系列頂點(diǎn)集,每個(gè)頂點(diǎn)集構(gòu)成的一個(gè)多邊形,Vertices數(shù)據(jù)結(jié)構(gòu)包含一組點(diǎn)的索引,索引是point中的點(diǎn)對(duì)應(yīng)的索引。
  void  reconstruct (PointCloud &output) 
  //  計(jì)算所有輸入點(diǎn)的凹多邊形﹐輸出的維數(shù)取決于輸入的維數(shù)是二維或三維,輸出結(jié)果為多邊形頂點(diǎn)并存儲(chǔ)在參數(shù)output中。
  void  setAlpha (double alpha) 
  //  設(shè)置alpha值,參數(shù)alpha限制voronoi圖中多邊形邊長(zhǎng)的長(zhǎng)短(邊長(zhǎng)越短多邊形越細(xì)分),alpha值是一個(gè)非零的正值,定義了voronoi圖中多邊形頂點(diǎn)到中心點(diǎn)的最大距離。
  double  getAlpha () 
  //  獲取alpha值
  void  setVoronoiCenters (PointCloudPtr voronoi_centers) 
  //  設(shè)置參數(shù)voronoi_centers,如果設(shè)置,最終產(chǎn)生的凹多邊形對(duì)應(yīng)的voronoi圖中的多邊形的中心將被存儲(chǔ)在參數(shù)voronoi_centers中。
  void  setKeepInformation (bool value) 
  //  設(shè)置參數(shù)keep_information,如果keep_information為真,凹多邊形中的頂點(diǎn)就保留其他信息,如 RGB值、法線(xiàn)等。
  int  getDim () const 
  //  返回計(jì)算得到多邊形的維數(shù)(二維或三維)。
  int  getDimension () const 
  //  返回計(jì)算得到多邊形的維數(shù)(二維或三維)。
  void  setDimension (int dimension) 
  //  設(shè)置輸入數(shù)據(jù)的維數(shù)(二維或三維),參數(shù)dimension為設(shè)置輸入數(shù)據(jù)的維度﹐如果沒(méi)有設(shè)置,輸入數(shù)據(jù)的維度將自動(dòng)根據(jù)輸入點(diǎn)云設(shè)置。
  void  getHullPointIndices (pcl::PointIndices &hull_point_indices) const 
  //  檢索凸包的輸入點(diǎn)云的索引。
  

End