merge files from pcl-1 online code

libs/lidar/pcl_point_type.h

@@ -3,26 +3,27 @@
 #ifndef PCL_POINT_TYPE_H
 #define PCL_POINT_TYPE_H
 
-#include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 namespace pcl {
 struct PointInternal {
-    PCL_ADD_POINT4D
-    PCL_ADD_RGB
-    float intensity;
-    float ring;
-    float label = 0;
-    float distance;
-    double timestamp;
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+  PCL_ADD_POINT4D
+  PCL_ADD_RGB
+  float intensity;
+  float ring;
+  float label = 0;
+  float distance;
+  double timestamp;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointInternal, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
-        (float, intensity, intensity)(float, ring, ring)(float, label, label)
-        (float, distance, distance)(double, timestamp, timestamp))
+    pcl::PointInternal,
+    (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
+        float, intensity, intensity)(float, ring, ring)(float, label, label)(
+        float, distance, distance)(double, timestamp, timestamp))
 
 typedef pcl::PointInternal Point;
 typedef pcl::PointCloud<Point> PointCloud;
@@ -32,19 +33,19 @@ typedef pcl::PointCloud<Point> PointCloud;
 ///
 namespace pcl {
 struct PointRos {
-    PCL_ADD_POINT4D
-    float intensity;
-    float ring;
-    float label = 0;
-    float timestamp;
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+  PCL_ADD_POINT4D
+  float intensity;
+  float ring;
+  float label = 0;
+  float timestamp;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN32;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointRos, (float, x, x)(float, y, y)(float, z, z)
-        (float, intensity, intensity)(float, ring, ring)(float, label, label)
-        (float, timestamp, timestamp))
+    pcl::PointRos,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
+        float, ring, ring)(float, label, label)(float, timestamp, timestamp))
 
 typedef pcl::PointRos PointRos;
 typedef pcl::PointCloud<PointRos> PointCloudRos;
@@ -54,18 +55,19 @@ typedef pcl::PointCloud<PointRos> PointCloudRos;
 ///
 namespace pcl {
 struct PointExport {
-    PCL_ADD_POINT4D
-    PCL_ADD_RGB
-    float intensity = 0;
-    float label = 0;
-    float info = 0;
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+  PCL_ADD_POINT4D
+  PCL_ADD_RGB
+  float intensity = 0;
+  float label = 0;
+  float info = 0;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointExport, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
-        (float, intensity, intensity)(float, label, label)(float, info, info))
+    pcl::PointExport,
+    (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
+        float, intensity, intensity)(float, label, label)(float, info, info))
 
 typedef pcl::PointExport PointExport;
 typedef pcl::PointCloud<PointExport> PointCloudExport;
@@ -73,21 +75,39 @@ typedef pcl::PointCloud<PointExport> PointCloudExport;
 /////////////////////////////////////////////////////////////
 namespace pcl {
 struct PointTemp1 {
-    PCL_ADD_POINT4D
-    float intensity;
-    float ring;
-    float label = 0;
-    float time;
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+  PCL_ADD_POINT4D
+  float intensity;
+  float ring;
+  float label = 0;
+  float time;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointTemp1, (float, x, x)(float, y, y)(float, z, z)
-        (float, intensity, intensity)(float, ring, ring)(float, label, label)
-        (float, time, time))
+    pcl::PointTemp1,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
+        float, ring, ring)(float, label, label)(float, time, time))
 
 typedef pcl::PointTemp1 PointTemp1;
 
+/////////////////////////////////////////////////////////////
+namespace pcl {
+struct PointI {
+  PCL_ADD_POINT4D
+  float intensity;
+  float stamp_offset;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+}  // end namespace pcl
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl::PointI,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                            intensity)(float, stamp_offset,
+                                                       stamp_offset))
+
+typedef pcl::PointI PointI;
+typedef pcl::PointCloud<PointI> PointCloudI;
 
-#endif // PCL_POINT_TYPE_H
+#endif  // PCL_POINT_TYPE_H

libs/reader/pcd_reader.cpp

@@ -49,7 +49,8 @@ bool PcdReader::loadFile() {
 bool PcdReader::getNextFrame(PointCloud*& frame) {
   if (pcdIndex_ >= pcdPathVec_.size()) return false;
   bool suceess = false;
-  PointCloud tempFrame;
+  PointCloudI tempFrame;
+  double time;
   while (!suceess) {
     string pcdPath = pcdPathVec_.at(pcdIndex_);
     pcdIndex_++;
@@ -60,17 +61,25 @@ bool PcdReader::getNextFrame(PointCloud*& frame) {
     if (iter == frameTimeMap_.end()) {
       continue;
     }
-    double time = iter->second;
+    time = iter->second;
     int8_t ret = checkTime(time, durations_);
     if (ret != 1) {
       continue;
     }
-    for (auto& point : tempFrame) {
-      point.timestamp = time;
-    }
+    //    LOG(INFO) << setprecision(15) << "frameId: " << frameId
+    //              << " time: " << time;
     suceess = true;
   }
   frame = new PointCloud;
-  *frame = tempFrame;
+  for (auto& pointI : tempFrame) {
+    Point point;
+    point.x = pointI.x;
+    point.y = pointI.y;
+    point.z = pointI.z;
+    point.timestamp = time + pointI.stamp_offset * 0.001;
+    point.intensity = pointI.intensity;
+    point.distance = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2));
+    frame->push_back(point);
+  }
   return true;
 }

libs/task/maptask.cpp

@@ -5,778 +5,767 @@
 MapTask::MapTask(const vector<vector<Time_Duration>> &timeDurations,
                  boost::shared_ptr<StrategyControl> strategyControl,
                  boost::shared_ptr<ParamsServer> paramsServer)
-    :timeDurations_(timeDurations),
+    : timeDurations_(timeDurations),
       strategyControl_(strategyControl),
-      paramsServer_(paramsServer)
-{
-    voxeled_pointcloud_.reset(new PointCloud);
-    export_cloud_.reset(new PointCloudExport);
-    filtered_cloud_.reset(new PointCloudExport);
-    uint16_t cal_miss_cnt = strategyControl_->getStageInfos().size() - 1;
-    dynamicRemover_.reset(new DynamicRemover(
-                              0.06, MISS_PER_HIT_PER_STAGE * cal_miss_cnt, voxeled_pointcloud_));
-    for(const auto &durationvec : timeDurations_){
-        for(const auto &duration : durationvec){
-            LOG(INFO) <<setprecision(15) << "MapTask "
-                     << " duration.start_sec: " << duration.start_sec
-                     <<" duration.end_sec: " << duration.end_sec;
-        }
-    }
-    configTrajectory();
+      paramsServer_(paramsServer) {
+  voxeled_pointcloud_.reset(new PointCloud);
+  export_cloud_.reset(new PointCloudExport);
+  filtered_cloud_.reset(new PointCloudExport);
+  uint16_t cal_miss_cnt = strategyControl_->getStageInfos().size() - 1;
+  dynamicRemover_.reset(new DynamicRemover(
+      0.06, MISS_PER_HIT_PER_STAGE * cal_miss_cnt, voxeled_pointcloud_));
+  for (const auto &durationvec : timeDurations_) {
+    for (const auto &duration : durationvec) {
+      LOG(INFO) << setprecision(15) << "MapTask "
+                << " duration.start_sec: " << duration.start_sec
+                << " duration.end_sec: " << duration.end_sec;
+    }
+  }
+  configTrajectory();
 }
 
-void MapTask::configTrajectory()
-{
-    auto traj_mode = strategyControl_->getTrajMode();
-    LOG(INFO) << "traj_mode: " << traj_mode;
-    if(traj_mode == PPK_UNDISORT_NODE_MAP_MODE){
-        undistortion_traj_ = paramsServer_->getPPKTraj();
-        mapping_traj_ = paramsServer_->getNodeTraj();
-    }else if(traj_mode == NODE_MODE){
-        undistortion_traj_ = paramsServer_->getNodeTraj();
-        mapping_traj_ = paramsServer_->getNodeTraj();
-    }else if(traj_mode == PPK_MODE){
-        undistortion_traj_ = paramsServer_->getPPKTraj();
-        mapping_traj_ = paramsServer_->getPPKTraj();
-    }
+void MapTask::configTrajectory() {
+  auto traj_mode = strategyControl_->getTrajMode();
+  LOG(INFO) << "traj_mode: " << traj_mode;
+  if (traj_mode == PPK_UNDISORT_NODE_MAP_MODE) {
+    undistortion_traj_ = paramsServer_->getPPKTraj();
+    mapping_traj_ = paramsServer_->getNodeTraj();
+  } else if (traj_mode == NODE_MODE) {
+    undistortion_traj_ = paramsServer_->getNodeTraj();
+    mapping_traj_ = paramsServer_->getNodeTraj();
+  } else if (traj_mode == PPK_MODE) {
+    undistortion_traj_ = paramsServer_->getPPKTraj();
+    mapping_traj_ = paramsServer_->getPPKTraj();
+  }
 }
 
-bool MapTask::addTrajectory(const boost::shared_ptr<Trajectory> &trajectory)
-{
-    TrajType trajType = trajectory->getTrajType();
-    auto traj_mode = strategyControl_->getTrajMode();
-    if(traj_mode == PPK_UNDISORT_NODE_MAP_MODE){
-        if(trajType == PPK){
-            if(!undistortion_traj_){
-                undistortion_traj_ = trajectory;
-            }else{
-                LOG(INFO) << "undistortion_traj_: " << undistortion_traj_->getTrajPath()
-                          << " already set!";
-                return false;
-            }
-        }else if(trajType == NODE){
-            if(!mapping_traj_){
-                mapping_traj_ = trajectory;
-            }else{
-                LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
-                          << " already set!";
-                return false;
-            }
-        }
-    }else if(traj_mode == NODE_MODE){
-        if(trajType == NODE){
-            if(!mapping_traj_){
-                mapping_traj_ = trajectory;
-                undistortion_traj_ = trajectory;
-            }else{
-                LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
-                          << " already set!";
-                return false;
-            }
-        }
-    }else if(traj_mode == PPK_MODE){
-        if(trajType == PPK){
-            if(!mapping_traj_){
-                mapping_traj_ = trajectory;
-                undistortion_traj_ = trajectory;
-            }else{
-                LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
-                          << " already set!";
-                return false;
-            }
-        }
+bool MapTask::addTrajectory(const boost::shared_ptr<Trajectory> &trajectory) {
+  TrajType trajType = trajectory->getTrajType();
+  auto traj_mode = strategyControl_->getTrajMode();
+  if (traj_mode == PPK_UNDISORT_NODE_MAP_MODE) {
+    if (trajType == PPK) {
+      if (!undistortion_traj_) {
+        undistortion_traj_ = trajectory;
+      } else {
+        LOG(INFO) << "undistortion_traj_: " << undistortion_traj_->getTrajPath()
+                  << " already set!";
+        return false;
+      }
+    } else if (trajType == NODE) {
+      if (!mapping_traj_) {
+        mapping_traj_ = trajectory;
+      } else {
+        LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
+                  << " already set!";
+        return false;
+      }
+    }
+  } else if (traj_mode == NODE_MODE) {
+    if (trajType == NODE) {
+      if (!mapping_traj_) {
+        mapping_traj_ = trajectory;
+        undistortion_traj_ = trajectory;
+      } else {
+        LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
+                  << " already set!";
+        return false;
+      }
+    }
+  } else if (traj_mode == PPK_MODE) {
+    if (trajType == PPK) {
+      if (!mapping_traj_) {
+        mapping_traj_ = trajectory;
+        undistortion_traj_ = trajectory;
+      } else {
+        LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
+                  << " already set!";
+        return false;
+      }
     }
-    return true;
+  }
+  return true;
 }
 
-void MapTask::build()
-{
-    configTraj();
-    if(MERGE == strategyControl_->getTaskMode() && !isMerged){
-        return;
-    }
-    if(taskTrajectory_.size() == 0 && strategyControl_->getOption() != "rawframe"){
-        return;
-    }
-    vector<StageInfo> stageInfos = strategyControl_->getStageInfos();
-    request_to_end_ = false;
-    for(stage_ = 0; stage_ < stageInfos.size(); stage_++){
-        stageInfo_ = strategyControl_->getStageInfos()[stage_];
-        LOG(INFO) << "stage_ " << stage_;
-        if(0 == stage_){
-            establishProcessTrds(trdVec_);
-            LOG(INFO) << "establishProcessTrds Done";
-        }
-
-        getRawPointCloud(stageInfos[stage_]);
-        LOG(INFO) << "getRawPointCloud Done";
-
-        waitUntilFramesEmpty();
-        LOG(INFO) << "waitUntilFramesEmpty Done";
-
-        if(stageInfos.size() - 1 == stage_){
-            joinProcessTrds(trdVec_);
-            LOG(INFO) << "joinProcessTrds Done";
-        }
-
-        if(strategyControl_->getFilterMode() == DIRECT_OUTPUT_MODE){
-            auto voxeledPointcloud = dynamicRemover_->getVoxeledPointcloud();
-            for(size_t i = 0; i < voxeledPointcloud->size(); i++){
-                auto p = voxeledPointcloud->at(i);
-                PointExport point;
-                point.x = p.x;point.y = p.y;point.z = p.z;
-                point.intensity = p.intensity;
-                export_cloud_->push_back(point);
-            }
-            return;
-        }
-//        dynamicRemover_->grid_coord_->printGrid();
-    }
-    dynamicRemover_->multiThreadFilter(export_cloud_, filtered_cloud_);
-    LOG(INFO) << "multiThreadFilter";
+void MapTask::build() {
+  configTraj();
+  if (MERGE == strategyControl_->getTaskMode() && !isMerged) {
+    return;
+  }
+  if (taskTrajectory_.size() == 0 &&
+      strategyControl_->getOption() != "rawframe") {
+    return;
+  }
+  vector<StageInfo> stageInfos = strategyControl_->getStageInfos();
+  request_to_end_ = false;
+  for (stage_ = 0; stage_ < stageInfos.size(); stage_++) {
+    stageInfo_ = strategyControl_->getStageInfos()[stage_];
+    LOG(INFO) << "stage_ " << stage_;
+    if (0 == stage_) {
+      establishProcessTrds(trdVec_);
+      LOG(INFO) << "establishProcessTrds Done";
+    }
+
+    getRawPointCloud(stageInfos[stage_]);
+    LOG(INFO) << "getRawPointCloud Done";
+
+    waitUntilFramesEmpty();
+    LOG(INFO) << "waitUntilFramesEmpty Done";
+
+    if (stageInfos.size() - 1 == stage_) {
+      joinProcessTrds(trdVec_);
+      LOG(INFO) << "joinProcessTrds Done";
+    }
+
+    if (strategyControl_->getFilterMode() == DIRECT_OUTPUT_MODE) {
+      auto voxeledPointcloud = dynamicRemover_->getVoxeledPointcloud();
+      for (size_t i = 0; i < voxeledPointcloud->size(); i++) {
+        auto p = voxeledPointcloud->at(i);
+        PointExport point;
+        point.x = p.x;
+        point.y = p.y;
+        point.z = p.z;
+        point.intensity = p.intensity;
+        export_cloud_->push_back(point);
+      }
+      return;
+    }
+    //        dynamicRemover_->grid_coord_->printGrid();
+  }
+  dynamicRemover_->multiThreadFilter(export_cloud_, filtered_cloud_);
+  LOG(INFO) << "multiThreadFilter";
 }
 
-void MapTask::buildMappedFrame(
-        PointCloud*& frame, const StageInfo &stageInfo,
-        size_t &guessUndisortIndex, size_t &guessMapIndex)
-{
-    if(strategyControl_->getOption() == "rawframe"){
-        boost::unique_lock<boost::mutex> lock(Mtx_);
-        if(frame->size()){
-            tempFrames_.push_back(*frame);
-        }
-        return;
-    }
-    auto frameSize = frame->size();
-
-    Matrix4d calib_1;
-    calib_1 = configCalib(stageInfo, 1);
-    if(stageInfo.pointcloudSource == BAG &&
-            strategyControl_->getBagMode() == Disorted){
-        calib_1.block(0, 0, 3, 3) = Matrix3d::Identity();
-//        LOG(INFO) << "calib linear = identity";
-    }
-    Isometry3d T1;
-    T1.translation() = calib_1.block(0, 3, 3, 1);
-    T1.linear() = calib_1.block(0, 0, 3, 3);
-    Isometry3d T = T1;
-    Matrix4d calib_2 = configCalib(stageInfo, 2);
-    Isometry3d T2;
-    T2.translation() = calib_2.block(0, 3, 3, 1);
-    T2.linear() = calib_2.block(0, 0, 3, 3);
-    T = T2 * T1;
-
-    Matrix4d calib;    
-    calib.block(0, 3, 3, 1) = T.translation();
-    calib.block(0, 0, 3, 3) = T.linear();
-    conductCalib(frame, calib);
-    if((stageInfo.pointcloudSource == PCAP)
-            || strategyControl_->getBagMode() == Undisorted){
-
-        conductMappingByPoint(frame, guessUndisortIndex);
-
-        if(0 == frame->size()){
-            return;
-        }
-
-        if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()){
-            conductInverse(frame);
-        }
-    }
-
-    if(strategyControl_->getOption() == "undisortedframe"){
-        if(PPK_UNDISORT_NODE_MAP_MODE != strategyControl_->getTrajMode()){
-            conductInverse(frame);
-        }
-        boost::unique_lock<boost::mutex> lock(Mtx_);
-        if(frame->size()){
-            tempFrames_.push_back(*frame);
-        }
-        return;
-    }
-
-    if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode() &&
-            strategyControl_->getMappingLidar() == Fill &&
-            stageInfo.lidarPosition == TopLidar &&
-            stageInfo.hirMissmode == HIT){
-        PointCloud topGroundCloud;
-        for(const auto &p : *frame){
-            if(p.z < 0 && p.z > -3 &&
-                    fabs(p.x) < 10 && fabs(p.y) < 15){
-                topGroundCloud.push_back(p);
-            }
-        }
-        topGroundCloud.sensor_orientation_ = frame->sensor_orientation_;
-        topGroundCloud.sensor_origin_ = frame->sensor_origin_;
-
-        frame->swap(topGroundCloud);
-//        pcl::io::savePCDFileBinary("/tmp/temp.pcd", *frame);
-//        exit(0);
-    }
-
-    if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()){        
-
-        for(auto &p : *frame){
-            if(p.z < -1.4){
-                p.data[3] = 1.0f;
-            }else{
-                p.data[3] = 0.0f;
-            }
-        }
-        conductMapping(frame, guessMapIndex);
-    }else{
-        for(auto &p : *frame){
-            p.x -= local_traj_center_.x();
-            p.y -= local_traj_center_.y();
-            p.z -= local_traj_center_.z();
-        }
-    }
-
-    if(strategyControl_->getOption() == "mappedframe"){
-        boost::unique_lock<boost::mutex> lock(Mtx_);
-        auto cloud = *frame;
-        Vector3d local_center_from_center = local_traj_center_ +
-                paramsServer_->getCenterToSlamStartOffset();
-        for(auto &p : cloud){
-            p.x += local_center_from_center.x();
-            p.y += local_center_from_center.y();
-            p.z += local_center_from_center.z();
-        }
-        if(cloud.size()){
-            tempFrames_.push_back(cloud);
-        }
-        return;
-    }
+void MapTask::buildMappedFrame(PointCloud *&frame, const StageInfo &stageInfo,
+                               size_t &guessUndisortIndex,
+                               size_t &guessMapIndex) {
+  if (strategyControl_->getOption() == "rawframe") {
+    boost::unique_lock<boost::mutex> lock(Mtx_);
+    if (frame->size()) {
+      tempFrames_.push_back(*frame);
+    }
+    return;
+  }
+
+  Matrix4d calib_1;
+  calib_1 = configCalib(stageInfo, 1);
+  if (stageInfo.pointcloudSource == BAG &&
+      strategyControl_->getBagMode() == Disorted) {
+    calib_1.block(0, 0, 3, 3) = Matrix3d::Identity();
+    //        LOG(INFO) << "calib linear = identity";
+  }
+  Isometry3d T1;
+  T1.translation() = calib_1.block(0, 3, 3, 1);
+  T1.linear() = calib_1.block(0, 0, 3, 3);
+  Isometry3d T = T1;
+  Matrix4d calib_2 = configCalib(stageInfo, 2);
+  Isometry3d T2;
+  T2.translation() = calib_2.block(0, 3, 3, 1);
+  T2.linear() = calib_2.block(0, 0, 3, 3);
+  T = T2 * T1;
+
+  Matrix4d calib;
+  calib.block(0, 3, 3, 1) = T.translation();
+  calib.block(0, 0, 3, 3) = T.linear();
+  conductCalib(frame, calib);
+
+  if ((stageInfo.pointcloudSource != BAG) ||
+      strategyControl_->getBagMode() == Undisorted) {
+    conductMappingByPoint(frame, guessUndisortIndex);
+
+    if (0 == frame->size()) {
+      return;
+    }
+
+    if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()) {
+      conductInverse(frame);
+    }
+  }
+
+  if (strategyControl_->getOption() == "undisortedframe") {
+    if (PPK_UNDISORT_NODE_MAP_MODE != strategyControl_->getTrajMode()) {
+      conductInverse(frame);
+    }
+    boost::unique_lock<boost::mutex> lock(Mtx_);
+    if (frame->size()) {
+      tempFrames_.push_back(*frame);
+    }
+    return;
+  }
+
+  if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode() &&
+      strategyControl_->getMappingLidar() == Fill &&
+      stageInfo.lidarPosition == TopLidar && stageInfo.hirMissmode == HIT) {
+    PointCloud topGroundCloud;
+    for (const auto &p : *frame) {
+      if (p.z < 0 && p.z > -3 && fabs(p.x) < 10 && fabs(p.y) < 15) {
+        topGroundCloud.push_back(p);
+      }
+    }
+    topGroundCloud.sensor_orientation_ = frame->sensor_orientation_;
+    topGroundCloud.sensor_origin_ = frame->sensor_origin_;
+
+    frame->swap(topGroundCloud);
+    //        pcl::io::savePCDFileBinary("/tmp/temp.pcd", *frame);
+    //        exit(0);
+  }
+
+  if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()) {
+    for (auto &p : *frame) {
+      if (p.z < -1.4) {
+        p.data[3] = 1.0f;
+      } else {
+        p.data[3] = 0.0f;
+      }
+    }
+    conductMapping(frame, guessMapIndex);
+  } else {
+    for (auto &p : *frame) {
+      p.x -= local_traj_center_.x();
+      p.y -= local_traj_center_.y();
+      p.z -= local_traj_center_.z();
+    }
+  }
+
+  if (strategyControl_->getOption() == "mappedframe") {
+    boost::unique_lock<boost::mutex> lock(Mtx_);
+    auto cloud = *frame;
+    Vector3d local_center_from_center =
+        local_traj_center_ + paramsServer_->getCenterToSlamStartOffset();
+    for (auto &p : cloud) {
+      p.x += local_center_from_center.x();
+      p.y += local_center_from_center.y();
+      p.z += local_center_from_center.z();
+    }
+    if (cloud.size()) {
+      tempFrames_.push_back(cloud);
+    }
+    return;
+  }
 }
 
-void MapTask::directOutput(vector<PointCloud::Ptr> &Frames)
-{
-    if(Frames.size() == 0){
-        LOG(WARNING) << "Frames.size() == 0";
-        return;
-    }
-    sort(Frames.begin(), Frames.end(),
-         [](const PointCloud::Ptr& query, const PointCloud::Ptr& target) {
-             return query->front().timestamp < target->front().timestamp;
-         });
-    for(const auto &frame : Frames){
-        for(const auto &point : *frame){
-            PointExport p;
-            p.x = point.x;p.y = point.y;p.z = point.z;
-            p.intensity = point.intensity;p.label = point.label;
-            p.rgb = point.rgb;p.info = point.ring;
-            export_cloud_->push_back(p);
-        }
-    }
-    export_cloud_->sensor_origin_ = Frames.front()->sensor_origin_;
-    export_cloud_->sensor_orientation_ = Frames.front()->sensor_orientation_;
+void MapTask::directOutput(vector<PointCloud::Ptr> &Frames) {
+  if (Frames.size() == 0) {
+    LOG(WARNING) << "Frames.size() == 0";
+    return;
+  }
+  sort(Frames.begin(), Frames.end(),
+       [](const PointCloud::Ptr &query, const PointCloud::Ptr &target) {
+         return query->front().timestamp < target->front().timestamp;
+       });
+  for (const auto &frame : Frames) {
+    for (const auto &point : *frame) {
+      PointExport p;
+      p.x = point.x;
+      p.y = point.y;
+      p.z = point.z;
+      p.intensity = point.intensity;
+      p.label = point.label;
+      p.rgb = point.rgb;
+      p.info = point.ring;
+      export_cloud_->push_back(p);
+    }
+  }
+  export_cloud_->sensor_origin_ = Frames.front()->sensor_origin_;
+  export_cloud_->sensor_orientation_ = Frames.front()->sensor_orientation_;
 }
 
-vector<vector<TrajPoint>> MapTask::getTaskTrajectory()
-{
-    return taskTrajectory_;
+vector<vector<TrajPoint>> MapTask::getTaskTrajectory() {
+  return taskTrajectory_;
 }
 
-PointCloudExport::Ptr MapTask::getExportCloud()
-{
-    return export_cloud_;
-}
+PointCloudExport::Ptr MapTask::getExportCloud() { return export_cloud_; }
 
-PointCloudExport::Ptr MapTask::getFilteredCloud()
-{
-    return filtered_cloud_;
-}
+PointCloudExport::Ptr MapTask::getFilteredCloud() { return filtered_cloud_; }
 
-PointCloud::Ptr MapTask::getVoxeledCloud()
-{
-    return voxeled_pointcloud_;
-}
+PointCloud::Ptr MapTask::getVoxeledCloud() { return voxeled_pointcloud_; }
 
-Vector3d MapTask::getLocalOffset()
-{
-    return local_traj_center_;
-}
+Vector3d MapTask::getLocalOffset() { return local_traj_center_; }
 
-void MapTask::waitUntilFramesEmpty()
-{
-    while(!frames_.empty()){        
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
-    }
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+void MapTask::waitUntilFramesEmpty() {
+  while (!frames_.empty()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
 }
 
-void MapTask::getRawPointCloud(const StageInfo &stageInfo)
-{
-    for(const auto &meshDuration : timeDurations_){
-        if(!reader_){
-            DevicePosition lidarPos = stageInfo.lidarPosition;
-            PointCloudSource pcSource = stageInfo.pointcloudSource;
-            reader_.reset(new TaskReader(meshDuration, lidarPos, pcSource, paramsServer_));
-        }
-        PointCloud* frame;
-        uint32_t boostSize = 0;
-        bool dataRemain = true;
-        while(dataRemain){
-            auto start   = system_clock::now();
-            dataRemain = reader_->getNextFrame(frame);
-            auto end   = system_clock::now();
-            auto duration = duration_cast<milliseconds>(end - start);
-            double elapse = double(duration.count()) / 1000;
-//            if(elapse > 0.1){
-//                std::this_thread::sleep_for(std::chrono::milliseconds(50));
-//            }
-            LOG_EVERY_N(INFO, 999) << "process take " << elapse << " seconds";
-            if(dataRemain){
-                frames_.push(frame);
-            }
-    //        LOG(INFO) << "boostSize: " << boostSize;
-            boostSize++;
-            if(boostSize == MAX_THREAD_CNT * 10){
-                waitUntilFramesEmpty();
-                boostSize = 0;
-    //            LOG(INFO) << "waitUntilFramesEmpty";
-            }
-//            if(strategyControl_->getOption().find("frame") != string::npos){
-//                if(boostSize >= 1500){
-//                    break;
-//                }
-//            }
-        }
-        reader_.reset();
-    }
+void MapTask::getRawPointCloud(const StageInfo &stageInfo) {
+  for (const auto &meshDuration : timeDurations_) {
+    if (!reader_) {
+      DevicePosition lidarPos = stageInfo.lidarPosition;
+      PointCloudSource pcSource = stageInfo.pointcloudSource;
+      reader_.reset(
+          new TaskReader(meshDuration, lidarPos, pcSource, paramsServer_));
+    }
+    PointCloud *frame;
+    uint32_t boostSize = 0;
+    bool dataRemain = true;
+    while (dataRemain) {
+      auto start = system_clock::now();
+      dataRemain = reader_->getNextFrame(frame);
+      auto end = system_clock::now();
+      auto duration = duration_cast<milliseconds>(end - start);
+      double elapse = double(duration.count()) / 1000;
+      //            if(elapse > 0.1){
+      //                std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      //            }
+      LOG_EVERY_N(INFO, 999) << "process take " << elapse << " seconds";
+      if (dataRemain) {
+        frames_.push(frame);
+      }
+      //        LOG(INFO) << "boostSize: " << boostSize;
+      boostSize++;
+      if (boostSize == MAX_THREAD_CNT * 10) {
+        waitUntilFramesEmpty();
+        boostSize = 0;
+        //            LOG(INFO) << "waitUntilFramesEmpty";
+      }
+      //            if(strategyControl_->getOption().find("frame") !=
+      //            string::npos){
+      //                if(boostSize >= 1500){
+      //                    break;
+      //                }
+      //            }
+    }
+    reader_.reset();
+  }
 }
 
-void MapTask::configTraj()
-{
-    while(!paramsServer_->getTrajectoryLoded()){
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
-    if(taskTrajectory_.size() == 0){
-        vector<vector<TrajPoint>> poseTraj, ppkTraj;
-        vector<Time_Duration> durations;
-
-        for(const auto &durationvec : timeDurations_){
-            for(const auto &duration : durationvec){
-                durations.push_back(duration);
-            }
-        }
-        if(mapping_traj_){
-            size_t guessTrajIndex = 0;
-            auto basePoseTraj = mapping_traj_->getDurationsTrajs(timeDurations_.front(), guessTrajIndex);
-            guessTrajIndex = 0;
-            poseTraj = mapping_traj_->getDurationsTrajs(durations, guessTrajIndex);
-            LOG(INFO) << "basePoseTraj: " << basePoseTraj.size()
-                      << " poseTraj: " << poseTraj.size();
-            isMerged = basePoseTraj.size() != poseTraj.size();
-            if(MERGE == strategyControl_->getTaskMode() && !isMerged){
-                return;
-            }
-            mapping_mesh_.reset(new Trajectory("", NODE));
-            vector<TrajPoint> meshTraj;
-            for(const auto &traj : poseTraj){
-                if(traj.size() == 0){
-                    continue;
-                }
-                for(const auto &traj_point : traj){
-                    meshTraj.push_back(traj_point);
-                }
-            }
-            sort(meshTraj.begin(), meshTraj.end(),
-                 [](const TrajPoint& query, const TrajPoint& target) {
-                     return query.timestamp < target.timestamp;
-                 });
-            mapping_mesh_->setTrajectory(meshTraj);
-        }
-        if(undistortion_traj_){
-            size_t guessTrajIndex = 0;
-            ppkTraj = undistortion_traj_->getDurationsTrajs(durations, guessTrajIndex);
-            auto traj_mode = strategyControl_->getTrajMode();
-            if(traj_mode == NODE_MODE){
-                undisort_mesh_.reset(new Trajectory("", NODE));
-            }else{
-                undisort_mesh_.reset(new Trajectory("", PPK));
-            }
-//            undisort_mesh_.reset(new Trajectory("", PPK));
-            vector<TrajPoint> meshTraj;
-            for(const auto &traj : ppkTraj){
-                if(traj.size() == 0){
-                    continue;
-                }
-                for(const auto &traj_point : traj){
-                    meshTraj.push_back(traj_point);
-                }
-            }
-            sort(meshTraj.begin(), meshTraj.end(),
-                 [](const TrajPoint& query, const TrajPoint& target) {
-                     return query.timestamp < target.timestamp;
-                 });
-            undisort_mesh_->setTrajectory(meshTraj);
-        }
-        if(mapping_traj_){
-            taskTrajectory_ = poseTraj;
-        }else if(undistortion_traj_){
-            taskTrajectory_ = ppkTraj;
-        }
-        LOG(INFO) << "taskTrajectory_.size(): " << taskTrajectory_.size();
-        uint32_t trajpoint_cnt = 0;
-        double dis = 0;
-        for(const auto &traj : taskTrajectory_){
-            if(traj.size() == 0){
-                continue;
-            }
-            Vector3d last_trans = traj.front().translation;
-            for(const auto &traj_point : traj){
-                local_traj_center_ += traj_point.translation;
-                dis += (traj_point.translation - last_trans).norm();
-                last_trans = traj_point.translation;
-            }
-            trajpoint_cnt += traj.size();
-        }
-        local_traj_center_ /= trajpoint_cnt;
-        LOG(INFO) << "local_traj_center_: " << local_traj_center_.transpose();
-        Vector3d local_center_from_center = local_traj_center_ + paramsServer_->getCenterToSlamStartOffset();
-        Vector3d mod_006_offset = Vector3d::Zero();
-        mod_006_offset.x() = fmod(local_center_from_center.x(), 0.06);
-        mod_006_offset.y() = fmod(local_center_from_center.y(), 0.06);
-        LOG(INFO) << "mod_006_offset: " << mod_006_offset.transpose();
-        local_traj_center_ -= mod_006_offset;
-
-        LOG(INFO) << "dis: " << dis;
-        LOG(INFO) << "trajpoint_cnt - taskTrajectory_.size(): " << trajpoint_cnt - taskTrajectory_.size();
-        avg_dis_ = dis / (trajpoint_cnt - taskTrajectory_.size());
-        if(mapping_traj_->getTrajType() == NODE){
-            setupGridCoord();
-        }
-    }
+void MapTask::configTraj() {
+  while (!paramsServer_->getTrajectoryLoded()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  if (taskTrajectory_.size() == 0) {
+    vector<vector<TrajPoint>> poseTraj, ppkTraj;
+    vector<Time_Duration> durations;
+
+    for (const auto &durationvec : timeDurations_) {
+      for (const auto &duration : durationvec) {
+        durations.push_back(duration);
+      }
+    }
+    if (mapping_traj_) {
+      size_t guessTrajIndex = 0;
+      auto basePoseTraj = mapping_traj_->getDurationsTrajs(
+          timeDurations_.front(), guessTrajIndex);
+      guessTrajIndex = 0;
+      poseTraj = mapping_traj_->getDurationsTrajs(durations, guessTrajIndex);
+      LOG(INFO) << "basePoseTraj: " << basePoseTraj.size()
+                << " poseTraj: " << poseTraj.size();
+      isMerged = basePoseTraj.size() != poseTraj.size();
+      if (MERGE == strategyControl_->getTaskMode() && !isMerged) {
+        return;
+      }
+      mapping_mesh_.reset(new Trajectory("", NODE));
+      vector<TrajPoint> meshTraj;
+      for (const auto &traj : poseTraj) {
+        if (traj.size() == 0) {
+          continue;
+        }
+        for (const auto &traj_point : traj) {
+          meshTraj.push_back(traj_point);
+        }
+      }
+      sort(meshTraj.begin(), meshTraj.end(),
+           [](const TrajPoint &query, const TrajPoint &target) {
+             return query.timestamp < target.timestamp;
+           });
+      mapping_mesh_->setTrajectory(meshTraj);
+    }
+    if (undistortion_traj_) {
+      size_t guessTrajIndex = 0;
+      ppkTraj =
+          undistortion_traj_->getDurationsTrajs(durations, guessTrajIndex);
+      auto traj_mode = strategyControl_->getTrajMode();
+      if (traj_mode == NODE_MODE) {
+        undisort_mesh_.reset(new Trajectory("", NODE));
+      } else {
+        undisort_mesh_.reset(new Trajectory("", PPK));
+      }
+      //            undisort_mesh_.reset(new Trajectory("", PPK));
+      vector<TrajPoint> meshTraj;
+      for (const auto &traj : ppkTraj) {
+        if (traj.size() == 0) {
+          continue;
+        }
+        for (const auto &traj_point : traj) {
+          meshTraj.push_back(traj_point);
+        }
+      }
+      sort(meshTraj.begin(), meshTraj.end(),
+           [](const TrajPoint &query, const TrajPoint &target) {
+             return query.timestamp < target.timestamp;
+           });
+      undisort_mesh_->setTrajectory(meshTraj);
+    }
+    if (mapping_traj_) {
+      taskTrajectory_ = poseTraj;
+    } else if (undistortion_traj_) {
+      taskTrajectory_ = ppkTraj;
+    }
+    LOG(INFO) << "taskTrajectory_.size(): " << taskTrajectory_.size();
+    uint32_t trajpoint_cnt = 0;
+    double dis = 0;
+    for (const auto &traj : taskTrajectory_) {
+      if (traj.size() == 0) {
+        continue;
+      }
+      Vector3d last_trans = traj.front().translation;
+      for (const auto &traj_point : traj) {
+        local_traj_center_ += traj_point.translation;
+        dis += (traj_point.translation - last_trans).norm();
+        last_trans = traj_point.translation;
+      }
+      trajpoint_cnt += traj.size();
+    }
+    local_traj_center_ /= trajpoint_cnt;
+    LOG(INFO) << "local_traj_center_: " << local_traj_center_.transpose();
+    Vector3d local_center_from_center =
+        local_traj_center_ + paramsServer_->getCenterToSlamStartOffset();
+    Vector3d mod_006_offset = Vector3d::Zero();
+    mod_006_offset.x() = fmod(local_center_from_center.x(), 0.06);
+    mod_006_offset.y() = fmod(local_center_from_center.y(), 0.06);
+    LOG(INFO) << "mod_006_offset: " << mod_006_offset.transpose();
+    local_traj_center_ -= mod_006_offset;
+
+    LOG(INFO) << "dis: " << dis;
+    LOG(INFO) << "trajpoint_cnt - taskTrajectory_.size(): "
+              << trajpoint_cnt - taskTrajectory_.size();
+    avg_dis_ = dis / (trajpoint_cnt - taskTrajectory_.size());
+    if (mapping_traj_->getTrajType() == NODE) {
+      setupGridCoord();
+    }
+  }
 }
 
-void MapTask::process()
-{
-    PointCloud* frame;
-    LOG(INFO) << "thread in threadCnt_: " << threadCnt_++;
-    size_t guessUndisortIndex = 0;
-    size_t guessMapIndex = 0;
-    while(!request_to_end_ ){
-        if(!frames_.pop(frame)){
-            std::this_thread::sleep_for(std::chrono::milliseconds(5));
-            LOG_EVERY_N(WARNING, 1000000) << "request_to_end_: " << request_to_end_;
-            continue;
-        }
-        if(nullptr == frame || frame->size() == 0){
-            continue;
-        }
-
-
-//        if(!reader_){
-//            if(frames_->size() == 0){
-//                tempSwitch = true;
-//                pid = std::this_thread::get_id();
-//                LOG(INFO) << "pid: " << pid;
-//            }
-//        }
-        if((stageInfo_.lidarPosition == TopLidar
-                && paramsServer_->getL0SN() == "PA4039CA5B9839CA53") ||
-                (stageInfo_.lidarPosition == BackLidar
-                                && paramsServer_->getL1SN() == "PA4039CA5B9839CA53")){
-            PointCloud removeLine39;
-            for(auto p : frame->points){
-                if(p.ring == 39){
-                    continue;
-                }
-                removeLine39.push_back(p);
-            }
-            frame->swap(removeLine39);
-        }
-        PointCloud* originFrame;
-        if(stageInfo_.hirMissmode == MISS){
-            originFrame = new PointCloud;
-            *originFrame = *frame;
-            for(auto &p : *originFrame){
-                p.x = 0;p.y = 0;p.z = 0;
-            }
-        }
-        buildMappedFrame(frame, stageInfo_, guessUndisortIndex, guessMapIndex);
-
-        if(0 == frame->size()){
-            continue;
-        }
-
-        if(strategyControl_->getOption().size() != 0){
-            continue;
-        }
-
-        if(stageInfo_.hirMissmode == HIT){
-            uint8_t type = 0;
-            if(stageInfo_.pointcloudSource == BAG){
-                type = 1;
-            }
-            conductHits(*frame, stage_, type);
-        }else if(stageInfo_.hirMissmode == MISS){
-            buildMappedFrame(originFrame, stageInfo_, guessUndisortIndex, guessMapIndex);
-
-            conductMisses(frame, originFrame);
-            delete originFrame;
-        }
-//        tempSwitch = false;
-
-        delete frame;
-    }
-    LOG(INFO) << "thread quit threadCnt_: " << threadCnt_--;
+void MapTask::process() {
+  PointCloud *frame;
+  LOG(INFO) << "thread in threadCnt_: " << threadCnt_++;
+  size_t guessUndisortIndex = 0;
+  size_t guessMapIndex = 0;
+  while (!request_to_end_) {
+    if (!frames_.pop(frame)) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(5));
+      LOG_EVERY_N(WARNING, 1000000) << "request_to_end_: " << request_to_end_;
+      continue;
+    }
+    if (nullptr == frame || frame->size() == 0) {
+      continue;
+    }
+
+    //        if(!reader_){
+    //            if(frames_->size() == 0){
+    //                tempSwitch = true;
+    //                pid = std::this_thread::get_id();
+    //                LOG(INFO) << "pid: " << pid;
+    //            }
+    //        }
+    if ((stageInfo_.lidarPosition == TopLidar &&
+         paramsServer_->getL0SN() == "PA4039CA5B9839CA53") ||
+        (stageInfo_.lidarPosition == BackLidar &&
+         paramsServer_->getL1SN() == "PA4039CA5B9839CA53")) {
+      PointCloud removeLine39;
+      for (auto p : frame->points) {
+        if (p.ring == 39) {
+          continue;
+        }
+        removeLine39.push_back(p);
+      }
+      frame->swap(removeLine39);
+    }
+    PointCloud *originFrame = nullptr;
+    if (stageInfo_.hirMissmode == MISS) {
+      originFrame = new PointCloud;
+      *originFrame = *frame;
+      for (auto &p : *originFrame) {
+        p.x = 0;
+        p.y = 0;
+        p.z = 0;
+      }
+    }
+    buildMappedFrame(frame, stageInfo_, guessUndisortIndex, guessMapIndex);
+
+    if (0 == frame->size()) {
+      continue;
+    }
+
+    if (strategyControl_->getOption().size() != 0) {
+      continue;
+    }
+
+    if (stageInfo_.hirMissmode == HIT) {
+      uint8_t type = 0;
+      if (stageInfo_.pointcloudSource == BAG) {
+        type = 1;
+      }
+      conductHits(*frame, stage_, type);
+    } else if (stageInfo_.hirMissmode == MISS) {
+      if (nullptr != originFrame) {
+        buildMappedFrame(originFrame, stageInfo_, guessUndisortIndex,
+                         guessMapIndex);
+        conductMisses(frame, originFrame);
+        delete originFrame;
+      }
+    }
+    //        tempSwitch = false;
+
+    delete frame;
+  }
+  LOG(INFO) << "thread quit threadCnt_: " << threadCnt_--;
 }
 
-void MapTask::establishProcessTrds(vector<ThreadPtr> &trdVec, uint32_t trdCnt)
-{
-    for(size_t i = 0; i < trdCnt; i++){
-        ThreadPtr trd;
-        trd.reset(new boost::thread(boost::bind(&MapTask::process, this)));
-        trdVec.push_back(trd);
-    }
+void MapTask::establishProcessTrds(vector<ThreadPtr> &trdVec, uint32_t trdCnt) {
+  for (size_t i = 0; i < trdCnt; i++) {
+    ThreadPtr trd;
+    trd.reset(new boost::thread(boost::bind(&MapTask::process, this)));
+    trdVec.push_back(trd);
+  }
 }
 
-void MapTask::joinProcessTrds(vector<ThreadPtr> &trdVec)
-{
-    request_to_end_ = true;
-//    for(size_t i = 0; i < trdVec.size(); i++){
-//        frames_->stopQueue();
-//        usleep(10000);
-//    }
-//    LOG(INFO) << "stopQueue";
-
-    for(auto &trd : trdVec){
-        trd->timed_join(boost::posix_time::seconds(1));
-    }
+void MapTask::joinProcessTrds(vector<ThreadPtr> &trdVec) {
+  request_to_end_ = true;
+  //    for(size_t i = 0; i < trdVec.size(); i++){
+  //        frames_->stopQueue();
+  //        usleep(10000);
+  //    }
+  //    LOG(INFO) << "stopQueue";
+
+  for (auto &trd : trdVec) {
+    trd->timed_join(boost::posix_time::seconds(5));
+  }
 }
 
-Matrix4d MapTask::configCalib(const StageInfo &stageInfo, const uint8_t &type)
-{
-    if(type == 1){
-        return paramsServer_->getDeviceCalib(stageInfo.lidarPosition);
-    }else if(type == 2){
-        return paramsServer_->getDeviceCalib(IMU);
-    }else if(type == 3){
-        return paramsServer_->getCalib(stageInfo.lidarPosition);
-    }
-    return Matrix4d::Identity();
+Matrix4d MapTask::configCalib(const StageInfo &stageInfo, const uint8_t &type) {
+  if (type == 1) {
+    return paramsServer_->getDeviceCalib(stageInfo.lidarPosition);
+  } else if (type == 2) {
+    return paramsServer_->getDeviceCalib(IMU);
+  } else if (type == 3) {
+    return paramsServer_->getCalib(stageInfo.lidarPosition);
+  }
+  return Matrix4d::Identity();
 }
 
-void MapTask::conductCalib(PointCloud* &Frame)
-{
-    conductCalib(Frame, calib_);
-}
+void MapTask::conductCalib(PointCloud *&Frame) { conductCalib(Frame, calib_); }
 
-void MapTask::conductCalib(PointCloud* &Frame, const Matrix4d &calib)
-{
-    pcl::transformPointCloud(*Frame, *Frame, calib);
+void MapTask::conductCalib(PointCloud *&Frame, const Matrix4d &calib) {
+  pcl::transformPointCloud(*Frame, *Frame, calib);
 }
 
-bool checkTimeStamp(const double &time,
-                    const double &time_1,
-                    const double &time_2){
-    if(time >= time_1 && time < time_2){
-        return true;
-    }else{
-        return false;
-    }
+bool checkTimeStamp(const double &time, const double &time_1,
+                    const double &time_2) {
+  if (time >= time_1 && time < time_2) {
+    return true;
+  } else {
+    return false;
+  }
 }
 
-void MapTask::conductMappingByPoint(PointCloud* &Frame, size_t &guessTrajIndex)
-{
-    if(0 == Frame->size()){
-        return;
-    }
-    double time_1 = 0, time_2 = 0;
-    bool frame_sensor_odom_set = false;
-
-    PointCloud locatedFrame;
-    Time_Duration duration;
-    double timeBuffer = 0.05;
-    if(NODE_MODE == strategyControl_->getTrajMode()){
-        timeBuffer = 1;
-    }
-    duration.start_sec = Frame->front().timestamp - timeBuffer;
-    duration.end_sec = Frame->back().timestamp + timeBuffer;
-    vector<TrajPoint> trajInFrame = undisort_mesh_->getDurationsTraj(duration, guessTrajIndex);
-    if(trajInFrame.size() < 2){
-        LOG_EVERY_N(WARNING, 99) << setprecision(15) << "trajInFrame.size() < 2: " << Frame->front().timestamp;
-        Frame->clear();
-        return;
+void MapTask::conductMappingByPoint(PointCloud *&Frame,
+                                    size_t &guessTrajIndex) {
+  if (0 == Frame->size()) {
+    return;
+  }
+  double time_1 = 0, time_2 = 0;
+  bool frame_sensor_odom_set = false;
+
+  PointCloud locatedFrame;
+  Time_Duration duration;
+  double timeBuffer = 0.05;
+  if (NODE_MODE == strategyControl_->getTrajMode()) {
+    timeBuffer = 1;
+  }
+  duration.start_sec = Frame->front().timestamp - timeBuffer;
+  duration.end_sec = Frame->back().timestamp + timeBuffer;
+  vector<TrajPoint> trajInFrame =
+      undisort_mesh_->getDurationsTraj(duration, guessTrajIndex);
+  if (trajInFrame.size() < 2) {
+    LOG_EVERY_N(WARNING, 99)
+        << setprecision(15)
+        << "trajInFrame.size() < 2: " << Frame->front().timestamp;
+    Frame->clear();
+    return;
+  }
+  uint32_t lastTrajIndex = 0;
+  TrajPoint currTraj, nextTraj;
+  for (const auto &point : *Frame) {
+    double curr_point_timestamp = point.timestamp;
+    Point transformed_point = point;
+
+    bool found = false;
+    if (!checkTimeStamp(curr_point_timestamp, time_1, time_2)) {
+      for (uint32_t i = lastTrajIndex; i < trajInFrame.size() - 1; i++) {
+        currTraj = trajInFrame.at(i);
+        nextTraj = trajInFrame.at(i + 1);
+        if (checkTimeStamp(curr_point_timestamp, currTraj.timestamp,
+                           nextTraj.timestamp)) {
+          time_1 = currTraj.timestamp;
+          time_2 = nextTraj.timestamp;
+          found = true;
+          break;
+        }
+      }
+      if (!found) {
+        continue;
+      }
     }
-    uint32_t lastTrajIndex = 0;
-    TrajPoint currTraj, nextTraj;
-    for(const auto &point : *Frame){
-        double curr_point_timestamp = point.timestamp;
-        Point transformed_point = point;
-
-        bool found = false;
-        if(!checkTimeStamp(curr_point_timestamp, time_1, time_2)){
-            for(uint32_t i = lastTrajIndex; i < trajInFrame.size() - 1; i++){
-                currTraj = trajInFrame.at(i);
-                nextTraj = trajInFrame.at(i + 1);
-                if(checkTimeStamp(curr_point_timestamp, currTraj.timestamp, nextTraj.timestamp)){
-                    time_1 = currTraj.timestamp;
-                    time_2 = nextTraj.timestamp;
-                    found = true;
-                    break;
-                }
-            }
-            if(!found){
-                continue;
-            }
-        }
-
-        TrajPoint interpolated_traj;
-        double radio = (curr_point_timestamp - currTraj.timestamp) / (nextTraj.timestamp - currTraj.timestamp);
-        interpolated_traj = currTraj;
-        interpolated_traj.translation = currTraj.translation + radio * (nextTraj.translation - currTraj.translation);
-        interpolated_traj.rotation = currTraj.rotation.slerp(radio, nextTraj.rotation);
-        interpolated_traj.timestamp = curr_point_timestamp;
-//        LOG_EVERY_N(INFO, 999) << "interpolated_traj.translation.transpose(): " << interpolated_traj.translation.transpose();
-
-        if(!frame_sensor_odom_set){
 
-
-            locatedFrame.sensor_origin_ = {interpolated_traj.translation.x(),
-                                           interpolated_traj.translation.y(),
-                                           interpolated_traj.translation.z(),
-                                           0};
-            locatedFrame.sensor_orientation_ = interpolated_traj.rotation.cast<float>();
-            frame_sensor_odom_set = true;
-        }
-        Vector3d point_vec(point.x, point.y, point.z);
-
-        Vector3d transformed_vec = interpolated_traj.rotation * point_vec + interpolated_traj.translation;
-        transformed_point.x = transformed_vec.x();
-        transformed_point.y = transformed_vec.y();
-        transformed_point.z = transformed_vec.z();
-        locatedFrame.push_back(transformed_point);
-
-    }
-    Frame->swap(locatedFrame);
+    TrajPoint interpolated_traj;
+    double radio = (curr_point_timestamp - currTraj.timestamp) /
+                   (nextTraj.timestamp - currTraj.timestamp);
+    interpolated_traj = currTraj;
+    interpolated_traj.translation =
+        currTraj.translation +
+        radio * (nextTraj.translation - currTraj.translation);
+    interpolated_traj.rotation =
+        currTraj.rotation.slerp(radio, nextTraj.rotation);
+    interpolated_traj.timestamp = curr_point_timestamp;
+    //        LOG_EVERY_N(INFO, 999) <<
+    //        "interpolated_traj.translation.transpose(): " <<
+    //        interpolated_traj.translation.transpose();
+
+    if (!frame_sensor_odom_set) {
+      locatedFrame.sensor_origin_ = {interpolated_traj.translation.x(),
+                                     interpolated_traj.translation.y(),
+                                     interpolated_traj.translation.z(), 0};
+      locatedFrame.sensor_orientation_ =
+          interpolated_traj.rotation.cast<float>();
+      frame_sensor_odom_set = true;
+    }
+    Vector3d point_vec(point.x, point.y, point.z);
+
+    Vector3d transformed_vec =
+        interpolated_traj.rotation * point_vec + interpolated_traj.translation;
+    transformed_point.x = transformed_vec.x();
+    transformed_point.y = transformed_vec.y();
+    transformed_point.z = transformed_vec.z();
+    locatedFrame.push_back(transformed_point);
+  }
+  Frame->swap(locatedFrame);
 }
 
-void MapTask::conductInverse(PointCloud* &Frame)
-{
-    auto sensor_t = Frame->sensor_origin_;
-    Matrix3f sensor_r = Matrix3f(Frame->sensor_orientation_);
-    Matrix4f frame_T;
-    frame_T.block(0, 0, 3, 3) = sensor_r.inverse();
-    frame_T.block(0, 3, 3, 1) = sensor_r.inverse() * Vector3f(-sensor_t.x(), -sensor_t.y(), -sensor_t.z());
-    pcl::transformPointCloud(*Frame, *Frame, frame_T);
+void MapTask::conductInverse(PointCloud *&Frame) {
+  auto sensor_t = Frame->sensor_origin_;
+  Matrix3f sensor_r = Matrix3f(Frame->sensor_orientation_);
+  Matrix4f frame_T;
+  frame_T.block(0, 0, 3, 3) = sensor_r.inverse();
+  frame_T.block(0, 3, 3, 1) =
+      sensor_r.inverse() *
+      Vector3f(-sensor_t.x(), -sensor_t.y(), -sensor_t.z());
+  pcl::transformPointCloud(*Frame, *Frame, frame_T);
 }
 
-void MapTask::conductMapping(PointCloud* &Frame, size_t &guessTrajIndex)
-{
-    if(Frame->size() == 0){
-        return;
-    }
-
-    PointCloud dummy_frame = *Frame;
-    Frame->clear();
-
-    double curr_frame_timestamp = dummy_frame.front().timestamp;
-    bool isSkip;
-    if(!mapping_mesh_->findFloorTrajPoint(curr_frame_timestamp, guessTrajIndex, isSkip)){
-        return;
-    }
-    if(isSkip){
-        return;
-    }
-    TrajPoint local_traj;
-
-    Isometry3d frameTransform;
-    TrajPoint interpolated_traj;
-    if(!mapping_mesh_->interpolation(curr_frame_timestamp, guessTrajIndex, interpolated_traj)){
-        LOG_EVERY_N(WARNING, 99) << "interpolation fail";
-        return;
-    }
-    local_traj = interpolated_traj;
-    Matrix4d interpolated_node = Trajectory::getTransformFromTrajPoint(local_traj);
-
-    frameTransform.translation() = interpolated_node.block(0, 3, 3, 1);
-    frameTransform.linear() = interpolated_node.block(0, 0, 3, 3);
-
-    frameTransform.translation() -= local_traj_center_;
-    Matrix4d frameMatrix;
-    frameMatrix.block(0, 3, 3, 1) = frameTransform.translation();
-    frameMatrix.block(0, 0, 3, 3) = frameTransform.linear();
-    pcl::transformPointCloud(dummy_frame, dummy_frame, frameMatrix);
-
-    local_traj.translation = frameTransform.translation();
-    StageInfo stageInfo = strategyControl_->getStageInfos()[stage_];
-    if(stageInfo.hirMissmode == HIT && mapping_mesh_->getTrajType() == NODE
-            && strategyControl_->getFilterMode() != DIRECT_OUTPUT_MODE){
-        PointCloud dynamicAccepted_frame;
-        for(auto &point : dummy_frame){
-            double distance = point.distance;
-            int res = dynamicRemover_->grid_coord_->isDynamicallyAccepted(
-                        point.x, point.y, distance, local_traj);
-
-            if(res >= 0) {
-//                point.ring = res;
-                dynamicAccepted_frame.push_back(point);
-            }
-        }
-        dummy_frame.swap(dynamicAccepted_frame);
-    }
-//    double traj_dis = 0;
-//    if(traj_index > 0){
-//        TrajPoint traj_upper = mapping_traj_->getTrajPointByIndex(traj_index + 1);
-//        if(traj_upper.translation.x() != 0){
-//            traj_dis = (traj_at_index.translation - traj_upper.translation).norm();
-//        }
-//    }
-    Frame->swap(dummy_frame);
-    Frame->sensor_origin_ = {local_traj.translation.x(),
-                             local_traj.translation.y(),
-                             local_traj.translation.z(),
-                             0};
-    Frame->sensor_orientation_ = Matrix3f(frameTransform.linear().cast<float>());
+void MapTask::conductMapping(PointCloud *&Frame, size_t &guessTrajIndex) {
+  if (Frame->size() == 0) {
+    return;
+  }
+
+  PointCloud dummy_frame = *Frame;
+  Frame->clear();
+
+  double curr_frame_timestamp = dummy_frame.front().timestamp;
+  bool isSkip;
+  if (!mapping_mesh_->findFloorTrajPoint(curr_frame_timestamp, guessTrajIndex,
+                                         isSkip)) {
+    return;
+  }
+  if (isSkip) {
+    return;
+  }
+  TrajPoint local_traj;
+
+  Isometry3d frameTransform;
+  TrajPoint interpolated_traj;
+  if (!mapping_mesh_->interpolation(curr_frame_timestamp, guessTrajIndex,
+                                    interpolated_traj)) {
+    LOG_EVERY_N(WARNING, 99) << "interpolation fail";
+    return;
+  }
+  local_traj = interpolated_traj;
+  Matrix4d interpolated_node =
+      Trajectory::getTransformFromTrajPoint(local_traj);
+
+  frameTransform.translation() = interpolated_node.block(0, 3, 3, 1);
+  frameTransform.linear() = interpolated_node.block(0, 0, 3, 3);
+
+  frameTransform.translation() -= local_traj_center_;
+  Matrix4d frameMatrix;
+  frameMatrix.block(0, 3, 3, 1) = frameTransform.translation();
+  frameMatrix.block(0, 0, 3, 3) = frameTransform.linear();
+  pcl::transformPointCloud(dummy_frame, dummy_frame, frameMatrix);
+
+  local_traj.translation = frameTransform.translation();
+  StageInfo stageInfo = strategyControl_->getStageInfos()[stage_];
+  if (stageInfo.hirMissmode == HIT && mapping_mesh_->getTrajType() == NODE &&
+      strategyControl_->getFilterMode() != DIRECT_OUTPUT_MODE) {
+    PointCloud dynamicAccepted_frame;
+    for (auto &point : dummy_frame) {
+      double distance = point.distance;
+      int res = dynamicRemover_->grid_coord_->isDynamicallyAccepted(
+          point.x, point.y, distance, local_traj);
+
+      if (res >= 0) {
+        //                point.ring = res;
+        dynamicAccepted_frame.push_back(point);
+      }
+    }
+    dummy_frame.swap(dynamicAccepted_frame);
+  }
+  //    double traj_dis = 0;
+  //    if(traj_index > 0){
+  //        TrajPoint traj_upper = mapping_traj_->getTrajPointByIndex(traj_index
+  //        + 1); if(traj_upper.translation.x() != 0){
+  //            traj_dis = (traj_at_index.translation -
+  //            traj_upper.translation).norm();
+  //        }
+  //    }
+  Frame->swap(dummy_frame);
+  Frame->sensor_origin_ = {local_traj.translation.x(),
+                           local_traj.translation.y(),
+                           local_traj.translation.z(), 0};
+  Frame->sensor_orientation_ = Matrix3f(frameTransform.linear().cast<float>());
 }
 
-void MapTask::conductHits(
-        const PointCloud &Frame, uint8_t stage, uint8_t type)
-{
-//    double radio = Frame->sensor_origin_[3] / avg_dis_;
-    dynamicRemover_->calculateHits(Frame, stage, type);
+void MapTask::conductHits(const PointCloud &Frame, uint8_t stage,
+                          uint8_t type) {
+  //    double radio = Frame->sensor_origin_[3] / avg_dis_;
+  dynamicRemover_->calculateHits(Frame, stage, type);
 }
 
 void MapTask::conductMisses(const PointCloud *mappedFrame,
-                            const PointCloud *mappedOriginFrame)
-{
-//    double radio = mappedFrame->sensor_origin_[3] / avg_dis_;
-    dynamicRemover_->calculateMisses(mappedFrame, mappedOriginFrame, 1);
+                            const PointCloud *mappedOriginFrame) {
+  //    double radio = mappedFrame->sensor_origin_[3] / avg_dis_;
+  dynamicRemover_->calculateMisses(mappedFrame, mappedOriginFrame, 1);
 }
 
-void MapTask::setupGridCoord()
-{
-    auto localTrajactory = taskTrajectory_;
-    for(auto &traj : localTrajactory){
-        for(auto &traj_point : traj){
-            traj_point.translation -= local_traj_center_;
-        }
+void MapTask::setupGridCoord() {
+  auto localTrajactory = taskTrajectory_;
+  for (auto &traj : localTrajactory) {
+    for (auto &traj_point : traj) {
+      traj_point.translation -= local_traj_center_;
     }
+  }
 
-    string path = "/tmp/localTrajactory.txt";
-    std::ofstream ofs(path);
-    if(!ofs){
-        LOG(WARNING) << path << " load fail!";
-    }
-    for(auto& trajectory : localTrajactory){
-        for(auto& pt : trajectory){
-            ofs << setprecision(15) << pt.translation[0] << " " << pt.translation[1] << " "
-                << pt.translation[2] << " " << pt.nodeId << endl;
-        }
+  string path = "/tmp/localTrajactory.txt";
+  std::ofstream ofs(path);
+  if (!ofs) {
+    LOG(WARNING) << path << " load fail!";
+  }
+  for (auto &trajectory : localTrajactory) {
+    for (auto &pt : trajectory) {
+      ofs << setprecision(15) << pt.translation[0] << " " << pt.translation[1]
+          << " " << pt.translation[2] << " " << pt.nodeId << endl;
     }
-    ofs.close();
+  }
+  ofs.close();
 
-    for(auto &traj : localTrajactory){
-        for(auto &traj_point : traj){
-            dynamicRemover_->grid_coord_->updateArea(traj_point);
-        }
+  for (auto &traj : localTrajactory) {
+    for (auto &traj_point : traj) {
+      dynamicRemover_->grid_coord_->updateArea(traj_point);
     }
+  }
 
-    dynamicRemover_->grid_coord_->peakTrajDrationAround();
+  dynamicRemover_->grid_coord_->peakTrajDrationAround();
 
-//    dynamicRemover_->grid_coord_->printGrid();
-    LOG(INFO) << "setupGridCoord done!";
+  //    dynamicRemover_->grid_coord_->printGrid();
+  LOG(INFO) << "setupGridCoord done!";
 }

libs/trajectory/trajectory.cpp

@@ -7,444 +7,436 @@ uint16_t MIN_NODE_LINE_ITEM_CNT = 10;
 float SquaredMinDis = pow(15, 2);
 
 Trajectory::Trajectory(const string &traj_path, const TrajType &trajType)
-    :trajectory_path_(traj_path),
-      trajType_(trajType)
-{
-    switch(trajType_){
+    : trajectory_path_(traj_path), trajType_(trajType) {
+  switch (trajType_) {
     case PPK:
-        frequency_ = 1000;
-        break;
+      frequency_ = 1000;
+      break;
     case NODE:
-        frequency_ = 10;
-        break;
-    }
+      frequency_ = 10;
+      break;
+  }
 }
 
-Trajectory::~Trajectory()
-{
-
-}
+Trajectory::~Trajectory() {}
 
-bool Trajectory::init()
-{
-    string range;
-    switch(trajType_){
+bool Trajectory::init() {
+  string range;
+  switch (trajType_) {
     case PPK:
-        range = ",";
-        break;
+      range = ",";
+      break;
     case NODE:
-        range = " ";
-        break;
-    }
-    LOG(INFO) << "Trajectory set frequency: " << frequency_;
-    io_.reset(new TextIO(trajectory_path_, range));
-    boost::function<void(const vector<string>&)> parse_callback =
-            boost::bind(&Trajectory::parseAndPush, this, _1);
-    io_->setParseCallback(parse_callback);
-
-    if(!io_){
-        LOG(WARNING) << "io_ is nullptr!";
-        return false;
-    }
-    if(!io_->load()){
-        LOG(WARNING) << "io_ load fail!";
+      range = " ";
+      break;
+  }
+  LOG(INFO) << "Trajectory set frequency: " << frequency_;
+  io_.reset(new TextIO(trajectory_path_, range));
+  boost::function<void(const vector<string> &)> parse_callback =
+      boost::bind(&Trajectory::parseAndPush, this, _1);
+  io_->setParseCallback(parse_callback);
+
+  if (!io_) {
+    LOG(WARNING) << "io_ is nullptr!";
+    return false;
+  }
+  if (!io_->load()) {
+    LOG(WARNING) << "io_ load fail!";
+    return false;
+  }
+
+  if (taskTrajInfo_.size()) {
+    setupBaseTrajOctree();
+    onRelativeTraj_ = true;
+    for (auto iter = taskTrajInfo_.begin(); iter != taskTrajInfo_.end();
+         iter++) {
+      const string &taskTrajPath = iter->first;
+      taskTrajPoseDiff_ = iter->second;
+      auto ppk_dir = taskTrajPath + "/raw_trace/ie.txt";
+      auto pose_dir = taskTrajPath + "/slam/cartographer.bag.pbstream.pose";
+      if (PPK == trajType_) {
+        io_.reset(new TextIO(ppk_dir, range));
+      } else if (NODE == trajType_) {
+        io_.reset(new TextIO(pose_dir, range));
+      } else {
         return false;
+      }
+      boost::function<void(const vector<string> &)> parse_callback =
+          boost::bind(&Trajectory::parseAndPush, this, _1);
+      io_->setParseCallback(parse_callback);
+      io_->load();
+      LOG(INFO) << "relative traj " << taskTrajPath << " done";
     }
-
-    if(taskTrajInfo_.size()){
-        setupBaseTrajOctree();
-        onRelativeTraj_ = true;
-        for(auto iter = taskTrajInfo_.begin();
-            iter != taskTrajInfo_.end(); iter++){
-            const string &taskTrajPath = iter->first;
-            taskTrajPoseDiff_ = iter->second;
-            auto ppk_dir = taskTrajPath + "/raw_trace/ie.txt";
-            auto pose_dir = taskTrajPath + "/slam/cartographer.bag.pbstream.pose";
-            if(PPK == trajType_){
-                io_.reset(new TextIO(ppk_dir, range));
-            }else if(NODE == trajType_){
-                io_.reset(new TextIO(pose_dir, range));
-            }else{
-                return false;
-            }
-            boost::function<void(const vector<string>&)> parse_callback =
-                    boost::bind(&Trajectory::parseAndPush, this, _1);
-            io_->setParseCallback(parse_callback);
-            io_->load();
-            LOG(INFO) << "relative traj " << taskTrajPath << " done";
-        }
+  }
+
+  sort(trajectory_.begin(), trajectory_.end(),
+       [](const TrajPoint &query, const TrajPoint &target) {
+         return query.timestamp < target.timestamp;
+       });
+
+  LOG(INFO) << "trajectory_ size: " << trajectory_.size();
+  if (NODE == trajType_) {
+    fullTraj_ = trajectory_;
+    Vector3d lastTranslation;
+    int cnt = 0;
+    for (auto iter = trajectory_.begin(); iter != trajectory_.end();) {
+      if (0 != cnt && (lastTranslation - iter->translation).norm() < 0.39) {
+        iter = trajectory_.erase(iter);
+      } else {
+        lastTranslation = iter->translation;
+        iter++;
+      }
+      cnt++;
     }
+  }
 
-    sort(trajectory_.begin(), trajectory_.end(),
-         [](const TrajPoint& query, const TrajPoint& target) {
-             return query.timestamp < target.timestamp;
-         });
-
-    LOG(INFO) << "trajectory_ size: " << trajectory_.size();
-    if(NODE == trajType_){
-        fullTraj_ = trajectory_;
-        Vector3d lastTranslation;
-        int cnt = 0;
-        for(auto iter = trajectory_.begin(); iter != trajectory_.end();){
-            if(0 != cnt && (lastTranslation - iter->translation).norm() < 0.39){
-                iter = trajectory_.erase(iter);
-            }else{
-                lastTranslation = iter->translation;
-                iter++;
-            }
-            cnt++;
-        }
-    }
-
-    LOG(INFO) << "after filter trajectory_ size: " << trajectory_.size();
-    return true;
+  LOG(INFO) << "after filter trajectory_ size: " << trajectory_.size();
+  return true;
 }
 
-string Trajectory::getTrajPath() const
-{
-    return trajectory_path_;
-}
+string Trajectory::getTrajPath() const { return trajectory_path_; }
 
-TrajType Trajectory::getTrajType() const
-{
-    return trajType_;
-}
+TrajType Trajectory::getTrajType() const { return trajType_; }
 
-bool Trajectory::findFloorTrajPoint(const double &timestamp, size_t &guessTrajIndex, bool& isSkip) const
-{
-    isSkip = false;
-    if(trajectory_.size() < 2){
-        return false;
-    }
-    if(trajectory_.front().timestamp > timestamp){
-        return false;
+bool Trajectory::findFloorTrajPoint(const double &timestamp,
+                                    size_t &guessTrajIndex,
+                                    bool &isSkip) const {
+  isSkip = false;
+  if (trajectory_.size() < 2) {
+    return false;
+  }
+  if (trajectory_.front().timestamp > timestamp) {
+    return false;
+  }
+  for (; guessTrajIndex < trajectory_.size() - 1; guessTrajIndex++) {
+    if (trajectory_.at(guessTrajIndex).timestamp > timestamp) {
+      guessTrajIndex = 0;
+      return findFloorTrajPoint(timestamp, guessTrajIndex, isSkip);
     }
-    for(; guessTrajIndex < trajectory_.size() - 1; guessTrajIndex++){
-        if(trajectory_.at(guessTrajIndex).timestamp > timestamp){
-            guessTrajIndex = 0;
-            return findFloorTrajPoint(timestamp, guessTrajIndex, isSkip);
-        }
-        auto pre = trajectory_.at(guessTrajIndex);
-        auto pro = trajectory_.at(guessTrajIndex + 1);
-
-        if(pre.timestamp <= timestamp && pro.timestamp > timestamp){
-            if((pro.timestamp - pre.timestamp) * frequency_ > 20){
-                isSkip = true;
-            }
-//            LOG_EVERY_N(INFO, 9) << setprecision(15)
-//                                   << "hit!!!! timestamp: " << timestamp
-//                                   << " pre: " << pre.timestamp
-//                                   << " pro: " << pro.timestamp
-//                                   << " guessTrajIndex: " << guessTrajIndex;
-            return true;
-        }
+    auto pre = trajectory_.at(guessTrajIndex);
+    auto pro = trajectory_.at(guessTrajIndex + 1);
+
+    if (pre.timestamp <= timestamp && pro.timestamp > timestamp) {
+      if ((pro.timestamp - pre.timestamp) * frequency_ > 20) {
+        isSkip = true;
+      }
+      //            LOG_EVERY_N(INFO, 9) << setprecision(15)
+      //                                   << "hit!!!! timestamp: " << timestamp
+      //                                   << " pre: " << pre.timestamp
+      //                                   << " pro: " << pro.timestamp
+      //                                   << " guessTrajIndex: " <<
+      //                                   guessTrajIndex;
+      return true;
     }
-    return false;
+  }
+  return false;
 }
 
 int32_t Trajectory::traversalFindTrajPoint(const uint32_t &start_index,
                                            const uint32_t &end_index,
-                                           const double &timestamp) const
-{
-    for(uint32_t index = start_index; index < end_index; index++){
-        if(checkSatisfied(index, timestamp)){
-            return index;
-        }
+                                           const double &timestamp) const {
+  for (uint32_t index = start_index; index < end_index; index++) {
+    if (checkSatisfied(index, timestamp)) {
+      return index;
     }
-    return -1;
+  }
+  return -1;
 }
 
 bool Trajectory::interpolation(const double &timestamp,
                                const int32_t &floor_index,
-                               TrajPoint &res_traj) const
-{
-    int32_t upper_index = floor_index + 1;
-    if((uint32_t)upper_index >= trajectory_.size()){
-        LOG_EVERY_N(WARNING, 99) << "upper_index: " << upper_index
-                                 << "trajectory_.size(): " << trajectory_.size();
-        return false;
-    }
-
-    const TrajPoint &traj_floor = trajectory_[floor_index];
-    const TrajPoint &traj_upper = trajectory_[upper_index];
-
-    double radio = (timestamp - traj_floor.timestamp) / (traj_upper.timestamp - traj_floor.timestamp);
-    res_traj = traj_floor;
-    res_traj.translation = traj_floor.translation + radio * (traj_upper.translation - traj_floor.translation);
-    res_traj.rotation = traj_floor.rotation.slerp(radio, traj_upper.rotation);
-    res_traj.timestamp = timestamp;
-    return true;
+                               TrajPoint &res_traj) const {
+  int32_t upper_index = floor_index + 1;
+  if ((uint32_t)upper_index >= trajectory_.size()) {
+    LOG_EVERY_N(WARNING, 99) << "upper_index: " << upper_index
+                             << "trajectory_.size(): " << trajectory_.size();
+    return false;
+  }
+
+  const TrajPoint &traj_floor = trajectory_[floor_index];
+  const TrajPoint &traj_upper = trajectory_[upper_index];
+
+  double radio = (timestamp - traj_floor.timestamp) /
+                 (traj_upper.timestamp - traj_floor.timestamp);
+  res_traj = traj_floor;
+  res_traj.translation =
+      traj_floor.translation +
+      radio * (traj_upper.translation - traj_floor.translation);
+  res_traj.rotation = traj_floor.rotation.slerp(radio, traj_upper.rotation);
+  res_traj.timestamp = timestamp;
+  return true;
 }
 
-Matrix4d Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint)
-{
-    Matrix4d T;
-    T.block(0, 0, 3, 3) = trajPoint.rotation.toRotationMatrix();
-    T.block(0, 3, 3, 1) = trajPoint.translation;
-    return T;
+Matrix4d Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint) {
+  Matrix4d T;
+  T.block(0, 0, 3, 3) = trajPoint.rotation.toRotationMatrix();
+  T.block(0, 3, 3, 1) = trajPoint.translation;
+  return T;
 }
 
 vector<vector<TrajPoint>> Trajectory::getDurationsTrajs(
-        const vector<Time_Duration> &durations, size_t &guessTrajIndex) const
-{
-    vector<vector<TrajPoint>> ret;
-    for(const auto &duration : durations){
-        vector<TrajPoint> durationsTrajs;
-        bool isSkip;
-        if(!findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip)){
-            LOG(INFO) << setprecision(15) << "findFloorTrajPoint fail:"
-                                             "duration.start_sec: " << duration.start_sec
-                      << " guessTrajIndex: " << guessTrajIndex;
-            guessTrajIndex = 0;
-        }
-        int32_t traj_index = guessTrajIndex;
-        for(uint32_t index = traj_index; index < trajectory_.size(); index++){
-            auto &traj = trajectory_.at(index);
-            if(traj.timestamp < duration.start_sec){
-                continue;
-            }else if(traj.timestamp <= duration.end_sec){
-                durationsTrajs.push_back(traj);
-            }else{
-                break;
-            }
-        }
-        if(durationsTrajs.size() > 1){
-            ret.push_back(durationsTrajs);
-        }
-    }
-    return ret;
-}
-
-vector<TrajPoint> Trajectory::getDurationsTraj(
-        const Time_Duration &duration, size_t &guessTrajIndex) const
-{
-    vector<TrajPoint> ret;
+    const vector<Time_Duration> &durations, size_t &guessTrajIndex) const {
+  vector<vector<TrajPoint>> ret;
+  for (const auto &duration : durations) {
+    vector<TrajPoint> durationsTrajs;
     bool isSkip;
-    int32_t traj_index = findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip);
-    if(isSkip) return ret;
-    if(traj_index < 0){
-        LOG(INFO) << setprecision(15)
-                  << "getDurationsTrajs findFloorTrajPoint fail, error code = " << traj_index
-                  << " duration.start_sec: " << duration.start_sec;
-        traj_index = 0;
+    if (!findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip)) {
+      LOG(INFO) << setprecision(15)
+                << "findFloorTrajPoint fail:"
+                   "duration.start_sec: "
+                << duration.start_sec << " guessTrajIndex: " << guessTrajIndex;
+      guessTrajIndex = 0;
     }
-    for(uint32_t index = traj_index; index < trajectory_.size(); index++){
-        auto &traj = trajectory_.at(index);
-        if(traj.timestamp < duration.start_sec){
-            continue;
-        }else if(traj.timestamp <= duration.end_sec){
-            ret.push_back(traj);
-        }else{
-            break;
-        }
+    int32_t traj_index = guessTrajIndex;
+    for (uint32_t index = traj_index; index < trajectory_.size(); index++) {
+      auto &traj = trajectory_.at(index);
+      if (traj.timestamp < duration.start_sec) {
+        continue;
+      } else if (traj.timestamp <= duration.end_sec) {
+        durationsTrajs.push_back(traj);
+      } else {
+        break;
+      }
     }
-    return ret;
+    if (durationsTrajs.size() > 1) {
+      ret.push_back(durationsTrajs);
+    }
+  }
+  return ret;
 }
 
-TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const
-{
-    if(index < trajectory_.size()){
-        return trajectory_.at(index);
-    }else{
-        return TrajPoint();
+vector<TrajPoint> Trajectory::getDurationsTraj(const Time_Duration &duration,
+                                               size_t &guessTrajIndex) const {
+  vector<TrajPoint> ret;
+  bool isSkip;
+  int32_t traj_index =
+      findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip);
+  if (isSkip) return ret;
+  if (traj_index < 0) {
+    LOG(INFO) << setprecision(15)
+              << "getDurationsTrajs findFloorTrajPoint fail, error code = "
+              << traj_index << " duration.start_sec: " << duration.start_sec;
+    traj_index = 0;
+  }
+  for (uint32_t index = traj_index; index < trajectory_.size(); index++) {
+    auto &traj = trajectory_.at(index);
+    if (traj.timestamp < duration.start_sec) {
+      continue;
+    } else if (traj.timestamp <= duration.end_sec) {
+      ret.push_back(traj);
+    } else {
+      break;
     }
+  }
+  return ret;
 }
 
-TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const
-{
-    for(const TrajPoint &point : fullTraj_){
-        if(point.nodeId == nodeId){
-            return point;
-        }
-    }
+TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const {
+  if (index < trajectory_.size()) {
+    return trajectory_.at(index);
+  } else {
     return TrajPoint();
+  }
 }
 
-vector<TrajPoint> Trajectory::getTrajectory() const
-{
-    return trajectory_;
+TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const {
+  for (const TrajPoint &point : fullTraj_) {
+    if (point.nodeId == nodeId) {
+      return point;
+    }
+  }
+  return TrajPoint();
 }
 
-boost::shared_ptr<LocalCartesian> Trajectory::getProj() const
-{
-    return proj_;
-}
+vector<TrajPoint> Trajectory::getTrajectory() const { return trajectory_; }
 
-void Trajectory::insertRelativeTaskInfo(
-        const string &trajPath, const Isometry3d &taskPoseDiff)
-{
-    taskTrajInfo_.insert(make_pair(trajPath, taskPoseDiff));
+boost::shared_ptr<LocalCartesian> Trajectory::getProj() const { return proj_; }
+
+void Trajectory::insertRelativeTaskInfo(const string &trajPath,
+                                        const Isometry3d &taskPoseDiff) {
+  taskTrajInfo_.insert(make_pair(trajPath, taskPoseDiff));
 }
 
-bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const
-{
-    if(supposed_floor_index < 0){
-        return false;
-    }
-    if((uint32_t)supposed_floor_index >= trajectory_.size() - 2){
-        return false;
-    }
-    auto currTime = trajectory_[supposed_floor_index].timestamp;
-    auto nextTime = trajectory_[supposed_floor_index + 1].timestamp;
-    if(currTime <= timestamp && nextTime >= timestamp &&
-            (nextTime - currTime) * frequency_ < 3){
-        return true;
-    }else{
-        return false;
-    }
+bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index,
+                                const double &timestamp) const {
+  if (supposed_floor_index < 0) {
+    return false;
+  }
+  if ((uint32_t)supposed_floor_index >= trajectory_.size() - 2) {
+    return false;
+  }
+  auto currTime = trajectory_[supposed_floor_index].timestamp;
+  auto nextTime = trajectory_[supposed_floor_index + 1].timestamp;
+  if (currTime <= timestamp && nextTime >= timestamp &&
+      (nextTime - currTime) * frequency_ < 3) {
+    return true;
+  } else {
+    return false;
+  }
 }
 
 double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
-    return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
+  return (315936000 + gps_week * 7 * 24 * 3600 + gps_sec - 18);
 }
 
-void Trajectory::parseAndPush(const vector<string> &vec)
-{
-    switch(trajType_){
-    case PPK:
-    {
-        if(vec.size() < MIN_PPK_LINE_ITEM_CNT){
-            LOG_EVERY_N(WARNING, 99999)
-                    << io_->getPath() << " line item size < " << MIN_PPK_LINE_ITEM_CNT;
-            return;
+void Trajectory::parseAndPush(const vector<string> &vec) {
+  switch (trajType_) {
+    case PPK: {
+      if (vec.size() < MIN_PPK_LINE_ITEM_CNT) {
+        LOG_EVERY_N(WARNING, 99999)
+            << io_->getPath() << " line item size < " << MIN_PPK_LINE_ITEM_CNT;
+        return;
+      }
+      PPK_Raw_Info ppk_raw_info;
+      ppk_raw_info.gps_week = stoi(vec[1]);
+      ppk_raw_info.gps_sec = stod(vec[2]);
+      ppk_raw_info.lat = stod(vec[3]);
+      ppk_raw_info.lng = stod(vec[4]);
+      ppk_raw_info.height = stod(vec[5]);
+      ppk_raw_info.roll = stod(vec[9]);
+      ppk_raw_info.pitch = stod(vec[10]);
+      ppk_raw_info.yaw =
+          -stod(vec[11]);  // ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
+
+      {
+        if (!proj_) {
+          boost::unique_lock<boost::mutex> lock(Mtx_);
+          if (!proj_) {
+            proj_.reset(new LocalCartesian(ppk_raw_info.lat, ppk_raw_info.lng,
+                                           ppk_raw_info.height));
+            LOG(INFO) << setprecision(15) << "LocalCartesian center: "
+                      << Vector3d(ppk_raw_info.lat, ppk_raw_info.lng,
+                                  ppk_raw_info.height)
+                             .transpose();
+          }
         }
-        PPK_Raw_Info ppk_raw_info;
-        ppk_raw_info.gps_week = stoi(vec[1]);
-        ppk_raw_info.gps_sec = stod(vec[2]);
-        ppk_raw_info.lat = stod(vec[3]);
-        ppk_raw_info.lng = stod(vec[4]);
-        ppk_raw_info.height = stod(vec[5]);
-        ppk_raw_info.roll = stod(vec[9]);
-        ppk_raw_info.pitch = stod(vec[10]);
-        ppk_raw_info.yaw = - stod(vec[11]); //ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
-
-        {
-            if(!proj_){
-                boost::unique_lock<boost::mutex> lock(Mtx_);
-                if(!proj_){
-                    proj_.reset(new LocalCartesian(ppk_raw_info.lat,
-                                                   ppk_raw_info.lng,
-                                                   ppk_raw_info.height));
-                    LOG(INFO) << setprecision(15) << "LocalCartesian center: "
-                              << Vector3d(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height).transpose();
-                }
-            }
-        }
-
-        TrajPoint trajPoint;
-        Vector3d translation;
-        proj_->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
-                       translation.x(),
-                       translation.y(),
-                       translation.z());
-        trajPoint.translation = translation;
-        trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw),
-                                             1, 0, 2);
-        if(preTransform_ != Matrix4d::Identity()){
-            trajPoint.rotation = trajPoint.rotation * preTransform_.block(0, 0, 3, 3).inverse();
-        }
-//        trajPoint.rotation = trajPoint.rotation * Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ());
-        trajPoint.nodeId = trajectory_.size();
-        trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
-        if(onRelativeTraj_){
-            trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation * taskTrajPoseDiff_.inverse().linear();
-        }
-        boost::unique_lock<boost::mutex> lock_ppk(Mtx_);
-        trajectory_.push_back(trajPoint);
-
-        break;
+      }
+
+      TrajPoint trajPoint;
+      Vector3d translation;
+      proj_->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
+                     translation.x(), translation.y(), translation.z());
+      trajPoint.translation = translation;
+      trajPoint.rotation = EulerToMatrix3d(
+          Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw), 1,
+          0, 2);
+      if (preTransform_ != Matrix4d::Identity()) {
+        trajPoint.rotation =
+            trajPoint.rotation * preTransform_.block(0, 0, 3, 3).inverse();
+      }
+      //        trajPoint.rotation = trajPoint.rotation *
+      //        Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ());
+      trajPoint.nodeId = trajectory_.size();
+      trajPoint.timestamp =
+          gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
+      if (onRelativeTraj_) {
+        trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation *
+                             taskTrajPoseDiff_.inverse().linear();
+      }
+      boost::unique_lock<boost::mutex> lock_ppk(Mtx_);
+      trajectory_.push_back(trajPoint);
+
+      break;
     }
-    case NODE:
-    {
-        if(vec.size() < MIN_NODE_LINE_ITEM_CNT){
-            LOG_EVERY_N(WARNING, 99999)
-                    << io_->getPath() << " line item size < " << MIN_NODE_LINE_ITEM_CNT;
-            return;
-        }
-
-        NODE_Raw_Info node_raw_info;
-        node_raw_info.node_id = stoi(vec[1]);
-        node_raw_info.time = stod(vec[2]);
-        node_raw_info.x = stod(vec[3]);
-        node_raw_info.y = stod(vec[4]);
-        node_raw_info.z = stod(vec[5]);
-        node_raw_info.w = stod(vec[6]);
-        node_raw_info.qx = stod(vec[7]);
-        node_raw_info.qy = stod(vec[8]);
-        node_raw_info.qz = stod(vec[9]);
-
-//        if(node_raw_info.node_id > 82000 && node_raw_info.node_id < 82100){
-//            return;
-//        }
-//        if(node_raw_info.node_id > 91552 && node_raw_info.node_id < 91744){
-//            return;
-//        }
-//        if(node_raw_info.node_id > 72192 && node_raw_info.node_id < 72256){
-//            return;
-//        }
-//        bool skip = (node_raw_info.node_id > 10907 && node_raw_info.node_id < 13417) ||
-//                (node_raw_info.node_id > 86905 && node_raw_info.node_id < 89253) ||
-//                (node_raw_info.node_id > 3076 && node_raw_info.node_id < 5752) ;
-
-//        skip = !skip;
-
-//        if(skip){
-//            return;
-//        }
-
-        if(trajectory_.size() == 0){
-            trajId_ = stoi(vec[0]);
+    case NODE: {
+      if (vec.size() < MIN_NODE_LINE_ITEM_CNT) {
+        LOG_EVERY_N(WARNING, 99999)
+            << io_->getPath() << " line item size < " << MIN_NODE_LINE_ITEM_CNT;
+        return;
+      }
+
+      NODE_Raw_Info node_raw_info;
+      node_raw_info.node_id = stoi(vec[1]);
+      node_raw_info.time = stod(vec[2]);
+      node_raw_info.x = stod(vec[3]);
+      node_raw_info.y = stod(vec[4]);
+      node_raw_info.z = stod(vec[5]);
+      node_raw_info.w = stod(vec[9]);
+      node_raw_info.qx = stod(vec[6]);
+      node_raw_info.qy = stod(vec[7]);
+      node_raw_info.qz = stod(vec[8]);
+
+      //        if(node_raw_info.node_id > 82000 && node_raw_info.node_id <
+      //        82100){
+      //            return;
+      //        }
+      //        if(node_raw_info.node_id > 91552 && node_raw_info.node_id <
+      //        91744){
+      //            return;
+      //        }
+      //        if(node_raw_info.node_id > 72192 && node_raw_info.node_id <
+      //        72256){
+      //            return;
+      //        }
+      //        bool skip = (node_raw_info.node_id > 10907 &&
+      //        node_raw_info.node_id < 13417) ||
+      //                (node_raw_info.node_id > 86905 && node_raw_info.node_id
+      //                < 89253) || (node_raw_info.node_id > 3076 &&
+      //                node_raw_info.node_id < 5752) ;
+
+      //        skip = !skip;
+
+      //        if(skip){
+      //            return;
+      //        }
+
+      if (trajectory_.size() == 0) {
+        trajId_ = stoi(vec[0]);
+      }
+      TrajPoint trajPoint;
+      trajPoint.nodeId = node_raw_info.node_id;
+      trajPoint.timestamp = node_raw_info.time;
+      trajPoint.translation = {node_raw_info.x, node_raw_info.y,
+                               node_raw_info.z};
+      trajPoint.rotation = {node_raw_info.w, node_raw_info.qx, node_raw_info.qy,
+                            node_raw_info.qz};
+      if (onRelativeTraj_) {
+        trajPoint.translation =
+            taskTrajPoseDiff_.linear() * trajPoint.translation +
+            taskTrajPoseDiff_.translation();
+        int result_index;
+        float sqr_distance;
+        pcl::PointXYZ query;
+        query.x = trajPoint.translation.x();
+        query.y = trajPoint.translation.y();
+        query.z = trajPoint.translation.z();
+        octree->approxNearestSearch(query, result_index, sqr_distance);
+        if (sqr_distance < SquaredMinDis) {
+          return;
         }
-        TrajPoint trajPoint;
-        trajPoint.nodeId = node_raw_info.node_id;
-        trajPoint.timestamp = node_raw_info.time;
-        trajPoint.translation = {node_raw_info.x, node_raw_info.y, node_raw_info.z};
-        trajPoint.rotation = {node_raw_info.w, node_raw_info.qx, node_raw_info.qy, node_raw_info.qz};
-        if(onRelativeTraj_){
-            trajPoint.translation = taskTrajPoseDiff_.linear() * trajPoint.translation
-                    + taskTrajPoseDiff_.translation();
-            int result_index;
-            float sqr_distance;
-            pcl::PointXYZ query;
-            query.x = trajPoint.translation.x();
-            query.y = trajPoint.translation.y();
-            query.z = trajPoint.translation.z();
-            octree->approxNearestSearch(query, result_index, sqr_distance);
-            if(sqr_distance < SquaredMinDis){
-                return;
-            }
-            trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation * taskTrajPoseDiff_.inverse().linear();
-        }
-
+        trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation *
+                             taskTrajPoseDiff_.inverse().linear();
+      }
 
-        //pose yaw take -M_PI/2!!!!!!!!!!!!!!!!!!
-//        trajPoint.rotation = trajPoint.rotation * Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ());
+      // pose yaw take -M_PI/2!!!!!!!!!!!!!!!!!!
+      //        trajPoint.rotation = trajPoint.rotation *
+      //        Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ());
 
-//        Matrix3d tempRot = Matrix3d::Identity() * Eigen::AngleAxisd(-2.032297, Vector3d::UnitZ());
-//        Vector3d tempTrans(-3.7193, 9.0219, 15.2070);
-//        trajPoint.translation = tempRot * trajPoint.translation + (-tempTrans);
-//        trajPoint.rotation = tempRot * trajPoint.rotation;
+      //        Matrix3d tempRot = Matrix3d::Identity() *
+      //        Eigen::AngleAxisd(-2.032297, Vector3d::UnitZ()); Vector3d
+      //        tempTrans(-3.7193, 9.0219, 15.2070); trajPoint.translation =
+      //        tempRot * trajPoint.translation + (-tempTrans);
+      //        trajPoint.rotation = tempRot * trajPoint.rotation;
 
-        boost::unique_lock<boost::mutex> lock_node(Mtx_);
-        trajectory_.push_back(trajPoint);
+      boost::unique_lock<boost::mutex> lock_node(Mtx_);
+      trajectory_.push_back(trajPoint);
 
-        break;
-    }
+      break;
     }
+  }
 }
 
-void Trajectory::setupBaseTrajOctree()
-{
-    octree.reset(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(5));
-    pcl::PointCloud<pcl::PointXYZ> trajCloud;
-    for(const auto& trajPoint : trajectory_){
-        pcl::PointXYZ point;
-        point.x = trajPoint.translation.x();
-        point.y = trajPoint.translation.y();
-        point.z = trajPoint.translation.z();
-        trajCloud.push_back(point);
-    }
-    octree->setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(trajCloud));
-    octree->addPointsFromInputCloud();
+void Trajectory::setupBaseTrajOctree() {
+  octree.reset(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(5));
+  pcl::PointCloud<pcl::PointXYZ> trajCloud;
+  for (const auto &trajPoint : trajectory_) {
+    pcl::PointXYZ point;
+    point.x = trajPoint.translation.x();
+    point.y = trajPoint.translation.y();
+    point.z = trajPoint.translation.z();
+    trajCloud.push_back(point);
+  }
+  octree->setInputCloud(
+      boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(trajCloud));
+  octree->addPointsFromInputCloud();
 }
-