merge files from pcl-1 online code

libs/lidar/pcl_point_type.h

@@ -3,8 +3,8 @@
 #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 {
@@ -17,12 +17,13 @@ struct PointInternal {
   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;
@@ -39,12 +40,12 @@ struct PointRos {
   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;
@@ -61,11 +62,12 @@ struct PointExport {
   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;
@@ -80,14 +82,32 @@ struct PointTemp1 {
   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

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,81 +5,78 @@
 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)
-{
+      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 "
+  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;
+                << " duration.end_sec: " << duration.end_sec;
     }
   }
   configTrajectory();
 }
 
-void MapTask::configTrajectory()
-{
+void MapTask::configTrajectory() {
   auto traj_mode = strategyControl_->getTrajMode();
   LOG(INFO) << "traj_mode: " << traj_mode;
-    if(traj_mode == PPK_UNDISORT_NODE_MAP_MODE){
+  if (traj_mode == PPK_UNDISORT_NODE_MAP_MODE) {
     undistortion_traj_ = paramsServer_->getPPKTraj();
     mapping_traj_ = paramsServer_->getNodeTraj();
-    }else if(traj_mode == NODE_MODE){
+  } else if (traj_mode == NODE_MODE) {
     undistortion_traj_ = paramsServer_->getNodeTraj();
     mapping_traj_ = paramsServer_->getNodeTraj();
-    }else if(traj_mode == PPK_MODE){
+  } else if (traj_mode == PPK_MODE) {
     undistortion_traj_ = paramsServer_->getPPKTraj();
     mapping_traj_ = paramsServer_->getPPKTraj();
   }
 }
 
-bool MapTask::addTrajectory(const boost::shared_ptr<Trajectory> &trajectory)
-{
+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_){
+  if (traj_mode == PPK_UNDISORT_NODE_MAP_MODE) {
+    if (trajType == PPK) {
+      if (!undistortion_traj_) {
         undistortion_traj_ = trajectory;
-            }else{
+      } else {
         LOG(INFO) << "undistortion_traj_: " << undistortion_traj_->getTrajPath()
                   << " already set!";
         return false;
       }
-        }else if(trajType == NODE){
-            if(!mapping_traj_){
+    } else if (trajType == NODE) {
+      if (!mapping_traj_) {
         mapping_traj_ = trajectory;
-            }else{
+      } else {
         LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
                   << " already set!";
         return false;
       }
     }
-    }else if(traj_mode == NODE_MODE){
-        if(trajType == NODE){
-            if(!mapping_traj_){
+  } else if (traj_mode == NODE_MODE) {
+    if (trajType == NODE) {
+      if (!mapping_traj_) {
         mapping_traj_ = trajectory;
         undistortion_traj_ = trajectory;
-            }else{
+      } else {
         LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
                   << " already set!";
         return false;
       }
     }
-    }else if(traj_mode == PPK_MODE){
-        if(trajType == PPK){
-            if(!mapping_traj_){
+  } else if (traj_mode == PPK_MODE) {
+    if (trajType == PPK) {
+      if (!mapping_traj_) {
         mapping_traj_ = trajectory;
         undistortion_traj_ = trajectory;
-            }else{
+      } else {
         LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
                   << " already set!";
         return false;
@@ -89,21 +86,21 @@ bool MapTask::addTrajectory(const boost::shared_ptr<Trajectory> &trajectory)
   return true;
 }
 
-void MapTask::build()
-{
+void MapTask::build() {
   configTraj();
-    if(MERGE == strategyControl_->getTaskMode() && !isMerged){
+  if (MERGE == strategyControl_->getTaskMode() && !isMerged) {
     return;
   }
-    if(taskTrajectory_.size() == 0 && strategyControl_->getOption() != "rawframe"){
+  if (taskTrajectory_.size() == 0 &&
+      strategyControl_->getOption() != "rawframe") {
     return;
   }
   vector<StageInfo> stageInfos = strategyControl_->getStageInfos();
   request_to_end_ = false;
-    for(stage_ = 0; stage_ < stageInfos.size(); stage_++){
+  for (stage_ = 0; stage_ < stageInfos.size(); stage_++) {
     stageInfo_ = strategyControl_->getStageInfos()[stage_];
     LOG(INFO) << "stage_ " << stage_;
-        if(0 == stage_){
+    if (0 == stage_) {
       establishProcessTrds(trdVec_);
       LOG(INFO) << "establishProcessTrds Done";
     }
@@ -114,47 +111,47 @@ void MapTask::build()
     waitUntilFramesEmpty();
     LOG(INFO) << "waitUntilFramesEmpty Done";
 
-        if(stageInfos.size() - 1 == stage_){
+    if (stageInfos.size() - 1 == stage_) {
       joinProcessTrds(trdVec_);
       LOG(INFO) << "joinProcessTrds Done";
     }
 
-        if(strategyControl_->getFilterMode() == DIRECT_OUTPUT_MODE){
+    if (strategyControl_->getFilterMode() == DIRECT_OUTPUT_MODE) {
       auto voxeledPointcloud = dynamicRemover_->getVoxeledPointcloud();
-            for(size_t i = 0; i < voxeledPointcloud->size(); i++){
+      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.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_->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"){
+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()){
+    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){
+  if (stageInfo.pointcloudSource == BAG &&
+      strategyControl_->getBagMode() == Disorted) {
     calib_1.block(0, 0, 3, 3) = Matrix3d::Identity();
-//        LOG(INFO) << "calib linear = identity";
+    //        LOG(INFO) << "calib linear = identity";
   }
   Isometry3d T1;
   T1.translation() = calib_1.block(0, 3, 3, 1);
@@ -170,39 +167,37 @@ void MapTask::buildMappedFrame(
   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){
 
+  if ((stageInfo.pointcloudSource != BAG) ||
+      strategyControl_->getBagMode() == Undisorted) {
     conductMappingByPoint(frame, guessUndisortIndex);
 
-        if(0 == frame->size()){
+    if (0 == frame->size()) {
       return;
     }
 
-        if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()){
+    if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()) {
       conductInverse(frame);
     }
   }
 
-    if(strategyControl_->getOption() == "undisortedframe"){
-        if(PPK_UNDISORT_NODE_MAP_MODE != strategyControl_->getTrajMode()){
+  if (strategyControl_->getOption() == "undisortedframe") {
+    if (PPK_UNDISORT_NODE_MAP_MODE != strategyControl_->getTrajMode()) {
       conductInverse(frame);
     }
     boost::unique_lock<boost::mutex> lock(Mtx_);
-        if(frame->size()){
+    if (frame->size()) {
       tempFrames_.push_back(*frame);
     }
     return;
   }
 
-    if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode() &&
+  if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode() &&
       strategyControl_->getMappingLidar() == Fill &&
-            stageInfo.lidarPosition == TopLidar &&
-            stageInfo.hirMissmode == HIT){
+      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){
+    for (const auto &p : *frame) {
+      if (p.z < 0 && p.z > -3 && fabs(p.x) < 10 && fabs(p.y) < 15) {
         topGroundCloud.push_back(p);
       }
     }
@@ -210,61 +205,63 @@ void MapTask::buildMappedFrame(
     topGroundCloud.sensor_origin_ = frame->sensor_origin_;
 
     frame->swap(topGroundCloud);
-//        pcl::io::savePCDFileBinary("/tmp/temp.pcd", *frame);
-//        exit(0);
+    //        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){
+  if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()) {
+    for (auto &p : *frame) {
+      if (p.z < -1.4) {
         p.data[3] = 1.0f;
-            }else{
+      } else {
         p.data[3] = 0.0f;
       }
     }
     conductMapping(frame, guessMapIndex);
-    }else{
-        for(auto &p : *frame){
+  } 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"){
+  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){
+    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()){
+    if (cloud.size()) {
       tempFrames_.push_back(cloud);
     }
     return;
   }
 }
 
-void MapTask::directOutput(vector<PointCloud::Ptr> &Frames)
-{
-    if(Frames.size() == 0){
+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) {
+       [](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){
+  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;
+      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);
     }
   }
@@ -272,160 +269,148 @@ void MapTask::directOutput(vector<PointCloud::Ptr> &Frames)
   export_cloud_->sensor_orientation_ = Frames.front()->sensor_orientation_;
 }
 
-vector<vector<TrajPoint>> MapTask::getTaskTrajectory()
-{
+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()){        
+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_){
+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_));
+      reader_.reset(
+          new TaskReader(meshDuration, lidarPos, pcSource, paramsServer_));
     }
-        PointCloud* frame;
+    PointCloud *frame;
     uint32_t boostSize = 0;
     bool dataRemain = true;
-        while(dataRemain){
+    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));
-//            }
+      //            if(elapse > 0.1){
+      //                std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      //            }
       LOG_EVERY_N(INFO, 999) << "process take " << elapse << " seconds";
-            if(dataRemain){
+      if (dataRemain) {
         frames_.push(frame);
       }
       //        LOG(INFO) << "boostSize: " << boostSize;
       boostSize++;
-            if(boostSize == MAX_THREAD_CNT * 10){
+      if (boostSize == MAX_THREAD_CNT * 10) {
         waitUntilFramesEmpty();
         boostSize = 0;
         //            LOG(INFO) << "waitUntilFramesEmpty";
       }
-//            if(strategyControl_->getOption().find("frame") != string::npos){
-//                if(boostSize >= 1500){
-//                    break;
-//                }
-//            }
+      //            if(strategyControl_->getOption().find("frame") !=
+      //            string::npos){
+      //                if(boostSize >= 1500){
+      //                    break;
+      //                }
+      //            }
     }
     reader_.reset();
   }
 }
 
-void MapTask::configTraj()
-{
-    while(!paramsServer_->getTrajectoryLoded()){
+void MapTask::configTraj() {
+  while (!paramsServer_->getTrajectoryLoded()) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
-    if(taskTrajectory_.size() == 0){
+  if (taskTrajectory_.size() == 0) {
     vector<vector<TrajPoint>> poseTraj, ppkTraj;
     vector<Time_Duration> durations;
 
-        for(const auto &durationvec : timeDurations_){
-            for(const auto &duration : durationvec){
+    for (const auto &durationvec : timeDurations_) {
+      for (const auto &duration : durationvec) {
         durations.push_back(duration);
       }
     }
-        if(mapping_traj_){
+    if (mapping_traj_) {
       size_t guessTrajIndex = 0;
-            auto basePoseTraj = mapping_traj_->getDurationsTrajs(timeDurations_.front(), guessTrajIndex);
+      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){
+      if (MERGE == strategyControl_->getTaskMode() && !isMerged) {
         return;
       }
       mapping_mesh_.reset(new Trajectory("", NODE));
       vector<TrajPoint> meshTraj;
-            for(const auto &traj : poseTraj){
-                if(traj.size() == 0){
+      for (const auto &traj : poseTraj) {
+        if (traj.size() == 0) {
           continue;
         }
-                for(const auto &traj_point : traj){
+        for (const auto &traj_point : traj) {
           meshTraj.push_back(traj_point);
         }
       }
       sort(meshTraj.begin(), meshTraj.end(),
-                 [](const TrajPoint& query, const TrajPoint& target) {
+           [](const TrajPoint &query, const TrajPoint &target) {
              return query.timestamp < target.timestamp;
            });
       mapping_mesh_->setTrajectory(meshTraj);
     }
-        if(undistortion_traj_){
+    if (undistortion_traj_) {
       size_t guessTrajIndex = 0;
-            ppkTraj = undistortion_traj_->getDurationsTrajs(durations, guessTrajIndex);
+      ppkTraj =
+          undistortion_traj_->getDurationsTrajs(durations, guessTrajIndex);
       auto traj_mode = strategyControl_->getTrajMode();
-            if(traj_mode == NODE_MODE){
+      if (traj_mode == NODE_MODE) {
         undisort_mesh_.reset(new Trajectory("", NODE));
-            }else{
+      } else {
         undisort_mesh_.reset(new Trajectory("", PPK));
       }
-//            undisort_mesh_.reset(new Trajectory("", PPK));
+      //            undisort_mesh_.reset(new Trajectory("", PPK));
       vector<TrajPoint> meshTraj;
-            for(const auto &traj : ppkTraj){
-                if(traj.size() == 0){
+      for (const auto &traj : ppkTraj) {
+        if (traj.size() == 0) {
           continue;
         }
-                for(const auto &traj_point : traj){
+        for (const auto &traj_point : traj) {
           meshTraj.push_back(traj_point);
         }
       }
       sort(meshTraj.begin(), meshTraj.end(),
-                 [](const TrajPoint& query, const TrajPoint& target) {
+           [](const TrajPoint &query, const TrajPoint &target) {
              return query.timestamp < target.timestamp;
            });
       undisort_mesh_->setTrajectory(meshTraj);
     }
-        if(mapping_traj_){
+    if (mapping_traj_) {
       taskTrajectory_ = poseTraj;
-        }else if(undistortion_traj_){
+    } 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){
+    for (const auto &traj : taskTrajectory_) {
+      if (traj.size() == 0) {
         continue;
       }
       Vector3d last_trans = traj.front().translation;
-            for(const auto &traj_point : traj){
+      for (const auto &traj_point : traj) {
         local_traj_center_ += traj_point.translation;
         dis += (traj_point.translation - last_trans).norm();
         last_trans = traj_point.translation;
@@ -434,7 +419,8 @@ void MapTask::configTraj()
     }
     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 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);
@@ -442,146 +428,141 @@ void MapTask::configTraj()
     local_traj_center_ -= mod_006_offset;
 
     LOG(INFO) << "dis: " << dis;
-        LOG(INFO) << "trajpoint_cnt - taskTrajectory_.size(): " << trajpoint_cnt - taskTrajectory_.size();
+    LOG(INFO) << "trajpoint_cnt - taskTrajectory_.size(): "
+              << trajpoint_cnt - taskTrajectory_.size();
     avg_dis_ = dis / (trajpoint_cnt - taskTrajectory_.size());
-        if(mapping_traj_->getTrajType() == NODE){
+    if (mapping_traj_->getTrajType() == NODE) {
       setupGridCoord();
     }
   }
 }
 
-void MapTask::process()
-{
-    PointCloud* frame;
+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)){
+  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){
+    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")){
+    //        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){
+      for (auto p : frame->points) {
+        if (p.ring == 39) {
           continue;
         }
         removeLine39.push_back(p);
       }
       frame->swap(removeLine39);
     }
-        PointCloud* originFrame;
-        if(stageInfo_.hirMissmode == MISS){
+    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;
+      for (auto &p : *originFrame) {
+        p.x = 0;
+        p.y = 0;
+        p.z = 0;
       }
     }
     buildMappedFrame(frame, stageInfo_, guessUndisortIndex, guessMapIndex);
 
-        if(0 == frame->size()){
+    if (0 == frame->size()) {
       continue;
     }
 
-        if(strategyControl_->getOption().size() != 0){
+    if (strategyControl_->getOption().size() != 0) {
       continue;
     }
 
-        if(stageInfo_.hirMissmode == HIT){
+    if (stageInfo_.hirMissmode == HIT) {
       uint8_t type = 0;
-            if(stageInfo_.pointcloudSource == BAG){
+      if (stageInfo_.pointcloudSource == BAG) {
         type = 1;
       }
       conductHits(*frame, stage_, type);
-        }else if(stageInfo_.hirMissmode == MISS){
-            buildMappedFrame(originFrame, stageInfo_, guessUndisortIndex, guessMapIndex);
-
+    } else if (stageInfo_.hirMissmode == MISS) {
+      if (nullptr != originFrame) {
+        buildMappedFrame(originFrame, stageInfo_, guessUndisortIndex,
+                         guessMapIndex);
         conductMisses(frame, originFrame);
         delete originFrame;
       }
-//        tempSwitch = false;
+    }
+    //        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++){
+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)
-{
+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(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));
+  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){
+Matrix4d MapTask::configCalib(const StageInfo &stageInfo, const uint8_t &type) {
+  if (type == 1) {
     return paramsServer_->getDeviceCalib(stageInfo.lidarPosition);
-    }else if(type == 2){
+  } else if (type == 2) {
     return paramsServer_->getDeviceCalib(IMU);
-    }else if(type == 3){
+  } 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)
-{
+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){
+bool checkTimeStamp(const double &time, const double &time_1,
+                    const double &time_2) {
+  if (time >= time_1 && time < time_2) {
     return true;
-    }else{
+  } else {
     return false;
   }
 }
 
-void MapTask::conductMappingByPoint(PointCloud* &Frame, size_t &guessTrajIndex)
-{
-    if(0 == Frame->size()){
+void MapTask::conductMappingByPoint(PointCloud *&Frame,
+                                    size_t &guessTrajIndex) {
+  if (0 == Frame->size()) {
     return;
   }
   double time_1 = 0, time_2 = 0;
@@ -590,83 +571,91 @@ void MapTask::conductMappingByPoint(PointCloud* &Frame, size_t &guessTrajIndex)
   PointCloud locatedFrame;
   Time_Duration duration;
   double timeBuffer = 0.05;
-    if(NODE_MODE == strategyControl_->getTrajMode()){
+  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;
+  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){
+  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++){
+    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)){
+        if (checkTimeStamp(curr_point_timestamp, currTraj.timestamp,
+                           nextTraj.timestamp)) {
           time_1 = currTraj.timestamp;
           time_2 = nextTraj.timestamp;
           found = true;
           break;
         }
       }
-            if(!found){
+      if (!found) {
         continue;
       }
     }
 
     TrajPoint interpolated_traj;
-        double radio = (curr_point_timestamp - currTraj.timestamp) / (nextTraj.timestamp - currTraj.timestamp);
+    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.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){
-
+    //        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>();
+                                     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;
+    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)
-{
+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());
+  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){
+void MapTask::conductMapping(PointCloud *&Frame, size_t &guessTrajIndex) {
+  if (Frame->size() == 0) {
     return;
   }
 
@@ -675,22 +664,25 @@ void MapTask::conductMapping(PointCloud* &Frame, size_t &guessTrajIndex)
 
   double curr_frame_timestamp = dummy_frame.front().timestamp;
   bool isSkip;
-    if(!mapping_mesh_->findFloorTrajPoint(curr_frame_timestamp, guessTrajIndex, isSkip)){
+  if (!mapping_mesh_->findFloorTrajPoint(curr_frame_timestamp, guessTrajIndex,
+                                         isSkip)) {
     return;
   }
-    if(isSkip){
+  if (isSkip) {
     return;
   }
   TrajPoint local_traj;
 
   Isometry3d frameTransform;
   TrajPoint interpolated_traj;
-    if(!mapping_mesh_->interpolation(curr_frame_timestamp, guessTrajIndex, 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);
+  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);
@@ -703,80 +695,77 @@ void MapTask::conductMapping(PointCloud* &Frame, size_t &guessTrajIndex)
 
   local_traj.translation = frameTransform.translation();
   StageInfo stageInfo = strategyControl_->getStageInfos()[stage_];
-    if(stageInfo.hirMissmode == HIT && mapping_mesh_->getTrajType() == NODE
-            && strategyControl_->getFilterMode() != DIRECT_OUTPUT_MODE){
+  if (stageInfo.hirMissmode == HIT && mapping_mesh_->getTrajType() == NODE &&
+      strategyControl_->getFilterMode() != DIRECT_OUTPUT_MODE) {
     PointCloud dynamicAccepted_frame;
-        for(auto &point : dummy_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;
+      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();
-//        }
-//    }
+  //    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};
+                           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_;
+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_;
+                            const PointCloud *mappedOriginFrame) {
+  //    double radio = mappedFrame->sensor_origin_[3] / avg_dis_;
   dynamicRemover_->calculateMisses(mappedFrame, mappedOriginFrame, 1);
 }
 
-void MapTask::setupGridCoord()
-{
+void MapTask::setupGridCoord() {
   auto localTrajactory = taskTrajectory_;
-    for(auto &traj : localTrajactory){
-        for(auto &traj_point : traj){
+  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){
+  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;
+  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();
 
-    for(auto &traj : localTrajactory){
-        for(auto &traj_point : traj){
+  for (auto &traj : localTrajactory) {
+    for (auto &traj_point : traj) {
       dynamicRemover_->grid_coord_->updateArea(traj_point);
     }
   }
 
   dynamicRemover_->grid_coord_->peakTrajDrationAround();
 
-//    dynamicRemover_->grid_coord_->printGrid();
+  //    dynamicRemover_->grid_coord_->printGrid();
   LOG(INFO) << "setupGridCoord done!";
 }

libs/trajectory/trajectory.cpp

@@ -7,10 +7,8 @@ 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;
@@ -20,15 +18,11 @@ Trajectory::Trajectory(const string &traj_path, const TrajType &trajType)
   }
 }
 
-Trajectory::~Trajectory()
-{
+Trajectory::~Trajectory() {}
 
-}
-
-bool Trajectory::init()
-{
+bool Trajectory::init() {
   string range;
-    switch(trajType_){
+  switch (trajType_) {
     case PPK:
       range = ",";
       break;
@@ -38,36 +32,36 @@ bool Trajectory::init()
   }
   LOG(INFO) << "Trajectory set frequency: " << frequency_;
   io_.reset(new TextIO(trajectory_path_, range));
-    boost::function<void(const vector<string>&)> parse_callback =
+  boost::function<void(const vector<string> &)> parse_callback =
       boost::bind(&Trajectory::parseAndPush, this, _1);
   io_->setParseCallback(parse_callback);
 
-    if(!io_){
+  if (!io_) {
     LOG(WARNING) << "io_ is nullptr!";
     return false;
   }
-    if(!io_->load()){
+  if (!io_->load()) {
     LOG(WARNING) << "io_ load fail!";
     return false;
   }
 
-    if(taskTrajInfo_.size()){
+  if (taskTrajInfo_.size()) {
     setupBaseTrajOctree();
     onRelativeTraj_ = true;
-        for(auto iter = taskTrajInfo_.begin();
-            iter != taskTrajInfo_.end(); iter++){
+    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_){
+      if (PPK == trajType_) {
         io_.reset(new TextIO(ppk_dir, range));
-            }else if(NODE == trajType_){
+      } else if (NODE == trajType_) {
         io_.reset(new TextIO(pose_dir, range));
-            }else{
+      } else {
         return false;
       }
-            boost::function<void(const vector<string>&)> parse_callback =
+      boost::function<void(const vector<string> &)> parse_callback =
           boost::bind(&Trajectory::parseAndPush, this, _1);
       io_->setParseCallback(parse_callback);
       io_->load();
@@ -76,19 +70,19 @@ bool Trajectory::init()
   }
 
   sort(trajectory_.begin(), trajectory_.end(),
-         [](const TrajPoint& query, const TrajPoint& target) {
+       [](const TrajPoint &query, const TrajPoint &target) {
          return query.timestamp < target.timestamp;
        });
 
   LOG(INFO) << "trajectory_ size: " << trajectory_.size();
-    if(NODE == trajType_){
+  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){
+    for (auto iter = trajectory_.begin(); iter != trajectory_.end();) {
+      if (0 != cnt && (lastTranslation - iter->translation).norm() < 0.39) {
         iter = trajectory_.erase(iter);
-            }else{
+      } else {
         lastTranslation = iter->translation;
         iter++;
       }
@@ -100,42 +94,38 @@ bool Trajectory::init()
   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
-{
+bool Trajectory::findFloorTrajPoint(const double &timestamp,
+                                    size_t &guessTrajIndex,
+                                    bool &isSkip) const {
   isSkip = false;
-    if(trajectory_.size() < 2){
+  if (trajectory_.size() < 2) {
     return false;
   }
-    if(trajectory_.front().timestamp > timestamp){
+  if (trajectory_.front().timestamp > timestamp) {
     return false;
   }
-    for(; guessTrajIndex < trajectory_.size() - 1; guessTrajIndex++){
-        if(trajectory_.at(guessTrajIndex).timestamp > timestamp){
+  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){
+    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;
+      //            LOG_EVERY_N(INFO, 9) << setprecision(15)
+      //                                   << "hit!!!! timestamp: " << timestamp
+      //                                   << " pre: " << pre.timestamp
+      //                                   << " pro: " << pro.timestamp
+      //                                   << " guessTrajIndex: " <<
+      //                                   guessTrajIndex;
       return true;
     }
   }
@@ -144,10 +134,9 @@ bool Trajectory::findFloorTrajPoint(const double &timestamp, size_t &guessTrajIn
 
 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)){
+                                           const double &timestamp) const {
+  for (uint32_t index = start_index; index < end_index; index++) {
+    if (checkSatisfied(index, timestamp)) {
       return index;
     }
   }
@@ -156,10 +145,9 @@ int32_t Trajectory::traversalFindTrajPoint(const uint32_t &start_index,
 
 bool Trajectory::interpolation(const double &timestamp,
                                const int32_t &floor_index,
-                               TrajPoint &res_traj) const
-{
+                               TrajPoint &res_traj) const {
   int32_t upper_index = floor_index + 1;
-    if((uint32_t)upper_index >= trajectory_.size()){
+  if ((uint32_t)upper_index >= trajectory_.size()) {
     LOG_EVERY_N(WARNING, 99) << "upper_index: " << upper_index
                              << "trajectory_.size(): " << trajectory_.size();
     return false;
@@ -168,16 +156,18 @@ bool Trajectory::interpolation(const double &timestamp,
   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);
+  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.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 Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint) {
   Matrix4d T;
   T.block(0, 0, 3, 3) = trajPoint.rotation.toRotationMatrix();
   T.block(0, 3, 3, 1) = trajPoint.translation;
@@ -185,125 +175,114 @@ Matrix4d Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint)
 }
 
 vector<vector<TrajPoint>> Trajectory::getDurationsTrajs(
-        const vector<Time_Duration> &durations, size_t &guessTrajIndex) const
-{
+    const vector<Time_Duration> &durations, size_t &guessTrajIndex) const {
   vector<vector<TrajPoint>> ret;
-    for(const auto &duration : durations){
+  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;
+    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++){
+    for (uint32_t index = traj_index; index < trajectory_.size(); index++) {
       auto &traj = trajectory_.at(index);
-            if(traj.timestamp < duration.start_sec){
+      if (traj.timestamp < duration.start_sec) {
         continue;
-            }else if(traj.timestamp <= duration.end_sec){
+      } else if (traj.timestamp <= duration.end_sec) {
         durationsTrajs.push_back(traj);
-            }else{
+      } else {
         break;
       }
     }
-        if(durationsTrajs.size() > 1){
+    if (durationsTrajs.size() > 1) {
       ret.push_back(durationsTrajs);
     }
   }
   return ret;
 }
 
-vector<TrajPoint> Trajectory::getDurationsTraj(
-        const Time_Duration &duration, size_t &guessTrajIndex) const
-{
+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){
+  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;
+              << "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++){
+  for (uint32_t index = traj_index; index < trajectory_.size(); index++) {
     auto &traj = trajectory_.at(index);
-        if(traj.timestamp < duration.start_sec){
+    if (traj.timestamp < duration.start_sec) {
       continue;
-        }else if(traj.timestamp <= duration.end_sec){
+    } else if (traj.timestamp <= duration.end_sec) {
       ret.push_back(traj);
-        }else{
+    } else {
       break;
     }
   }
   return ret;
 }
 
-TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const
-{
-    if(index < trajectory_.size()){
+TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const {
+  if (index < trajectory_.size()) {
     return trajectory_.at(index);
-    }else{
+  } else {
     return TrajPoint();
   }
 }
 
-TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const
-{
-    for(const TrajPoint &point : fullTraj_){
-        if(point.nodeId == nodeId){
+TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const {
+  for (const TrajPoint &point : fullTraj_) {
+    if (point.nodeId == nodeId) {
       return point;
     }
   }
   return TrajPoint();
 }
 
-vector<TrajPoint> Trajectory::getTrajectory() const
-{
-    return trajectory_;
-}
+vector<TrajPoint> Trajectory::getTrajectory() const { return trajectory_; }
 
-boost::shared_ptr<LocalCartesian> Trajectory::getProj() const
-{
-    return proj_;
-}
+boost::shared_ptr<LocalCartesian> Trajectory::getProj() const { return proj_; }
 
-void Trajectory::insertRelativeTaskInfo(
-        const string &trajPath, const Isometry3d &taskPoseDiff)
-{
+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){
+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){
+  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){
+  if (currTime <= timestamp && nextTime >= timestamp &&
+      (nextTime - currTime) * frequency_ < 3) {
     return true;
-    }else{
+  } 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){
+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;
@@ -316,17 +295,19 @@ void Trajectory::parseAndPush(const vector<string> &vec)
       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!!!!!!!!!!!!!!!!!!!!
+      ppk_raw_info.yaw =
+          -stod(vec[11]);  // ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
 
       {
-            if(!proj_){
+        if (!proj_) {
           boost::unique_lock<boost::mutex> lock(Mtx_);
-                if(!proj_){
-                    proj_.reset(new LocalCartesian(ppk_raw_info.lat,
-                                                   ppk_raw_info.lng,
+          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();
+                      << Vector3d(ppk_raw_info.lat, ppk_raw_info.lng,
+                                  ppk_raw_info.height)
+                             .transpose();
           }
         }
       }
@@ -334,29 +315,31 @@ void Trajectory::parseAndPush(const vector<string> &vec)
       TrajPoint trajPoint;
       Vector3d translation;
       proj_->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
-                       translation.x(),
-                       translation.y(),
-                       translation.z());
+                     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.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();
+      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){
+    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;
@@ -368,41 +351,49 @@ void Trajectory::parseAndPush(const vector<string> &vec)
       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){
+      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();
+      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;
@@ -410,20 +401,22 @@ void Trajectory::parseAndPush(const vector<string> &vec)
         query.y = trajPoint.translation.y();
         query.z = trajPoint.translation.z();
         octree->approxNearestSearch(query, result_index, sqr_distance);
-            if(sqr_distance < SquaredMinDis){
+        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);
@@ -433,18 +426,17 @@ void Trajectory::parseAndPush(const vector<string> &vec)
   }
 }
 
-void Trajectory::setupBaseTrajOctree()
-{
+void Trajectory::setupBaseTrajOctree() {
   octree.reset(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(5));
   pcl::PointCloud<pcl::PointXYZ> trajCloud;
-    for(const auto& trajPoint : trajectory_){
+  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->setInputCloud(
+      boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(trajCloud));
   octree->addPointsFromInputCloud();
 }
-