删除原来的localCloud

config/horizon_config.yaml

@@ -18,4 +18,4 @@ map_skip_frame: 2
 
 ivox_nearby_type: 26 #6  18 26
 max_ivox_node: 500
-ivox_node_solution: 0.2
+ivox_node_solution: 0.5

define/typedef.h

@@ -17,6 +17,7 @@ typedef struct
 
 typedef void(*odomCallback)(Eigen::Matrix4d,double);
 typedef void(*CloudMappedCallback)(pcl::PointCloud<PointType>::Ptr,double);
+typedef void(*CostCallback)(double);
 
 
 typedef struct

lio/src/PoseEstimation.cpp

@@ -295,11 +295,11 @@ int main(int argc, char** argv)
   // Specialisations mean no conversions take place for exact types
   // and conversions between scalar types are cheap.
   pangolin::Var<bool> lvx_checkbox("ui.lvx_Checkbox", false, true);
-  pangolin::Var<std::string> lvx_file("ui.lvx_file_String", "../map/lidardata.lvx");
+  pangolin::Var<std::string> lvx_file("ui.lvx_file_String", "../map/gd.lvx");
 
   pangolin::Var<bool> start_button("ui.start_Button", false, false);
   pangolin::Var<bool> stop_button("ui.stop_Button", false, false);
-  pangolin::Var<double> freq_double("ui.Freq_Double", 5, 1, 15);
+  pangolin::Var<double> freq_double("ui.Freq_Double", 10, 1, 15);
   /*pangolin::Var<int> an_int("ui.An_Int", 2, 0, 5);
   pangolin::Var<double> a_double_log("ui.Log_scale", 3, 1, 1E4, true);*/
   pangolin::Var<std::string> timequeue_string("ui.tq_String", "timequeue_string");
@@ -422,8 +422,6 @@ int main(int argc, char** argv)
       scan_cloud.unlock();
 
       glColor3f(1, 0, 0);
-
-
       localmap_cloud.Lock();
       for (int i = 0; i < localmap_cloud.Get()->size(); ++i)
       {

lio/src/lio/Estimator.cpp

@@ -28,11 +28,11 @@ Estimator::Estimator(const float& filter_corner, const float& filter_surf,
   transformForMap.setIdentity();
 
 
-  for(int i = 0; i < localMapWindowSize; i++){
+  /*for(int i = 0; i < localMapWindowSize; i++){
     localCornerMap[i].reset(new pcl::PointCloud<PointType>);
     localSurfMap[i].reset(new pcl::PointCloud<PointType>);
     localNonFeatureMap[i].reset(new pcl::PointCloud<PointType>);
-  }
+  }*/
 
   downSizeFilterCorner.setLeafSize(filter_corner, filter_corner, filter_corner);
   downSizeFilterSurf.setLeafSize(filter_surf, filter_surf, filter_surf);
@@ -46,8 +46,8 @@ void Estimator::print()
 
 }
 Estimator::~Estimator(){
-}
 
+}
 
 void Estimator::processPointToLine(std::vector<ceres::CostFunction *>& edges,
                                    std::vector<FeatureLine>& vLineFeatures,
@@ -181,7 +181,7 @@ void Estimator::processPointToLine(std::vector<ceres::CostFunction *>& edges,
 
     }
 
-    /*if(local_map_.CornerMapVox()->NumPoints()>20 )
+    if(local_map_.CornerMapVox()->NumPoints()>20 )
     {
         pcl::PointCloud<PointType> nearst;
         local_map_.CornerMapVox()->GetClosestPoint(_pointSel, nearst.points, 5, thres_dist);
@@ -266,7 +266,7 @@ void Estimator::processPointToLine(std::vector<ceres::CostFunction *>& edges,
                 vLineFeatures.back().ComputeError(m4d);
             }
         }
-    }*/
+    }
      
   }
 }
@@ -314,10 +314,6 @@ void Estimator::processPointToPlanVec(std::vector<ceres::CostFunction *>& edges,
     _pointOri = laserCloudSurf->points[i];
       LocalMapIvox::pointAssociateToMap(&_pointOri, &_pointSel, m4d);
 
-    /*int id = map_manager->FindUsedSurfMap(&_pointSel,laserCenWidth_last,laserCenHeight_last,laserCenDepth_last);
-
-    if(id == 5000) continue;*/
-
     if(std::isnan(_pointSel.x) || std::isnan(_pointSel.y) ||std::isnan(_pointSel.z)) continue;
 
     if(m_global_map.laserCloudSurfFromLocal->points.size() > 50) {
@@ -386,7 +382,7 @@ void Estimator::processPointToPlanVec(std::vector<ceres::CostFunction *>& edges,
     }
 
 
-    /*if(local_map_.surfMapVox()->NumPoints() > 20 )
+    if(local_map_.surfMapVox()->NumPoints() > 20 )
     {
         pcl::PointCloud<PointType> nearst;
         local_map_.surfMapVox()->GetClosestPoint(_pointSel, nearst.points, 5, thres_dist);
@@ -452,7 +448,7 @@ void Estimator::processPointToPlanVec(std::vector<ceres::CostFunction *>& edges,
                 vPlanFeatures.back().ComputeError(m4d);
             }
         }
-    }*/
+    }
 
   }
 
@@ -464,19 +460,22 @@ void Estimator::processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
                                      std::vector<FeatureNon>& vNonFeatures,
                                      const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeature,
                                      const Eigen::Matrix4d& exTlb,
-                                     const Eigen::Matrix4d& m4d){
+                                     const Eigen::Matrix4d& m4d)
+{
   Eigen::Matrix4d Tbl = Eigen::Matrix4d::Identity();
-  Tbl.topLeftCorner(3,3) = exTlb.topLeftCorner(3,3).transpose();
-  Tbl.topRightCorner(3,1) = -1.0 * Tbl.topLeftCorner(3,3) * exTlb.topRightCorner(3,1);
-  if(!vNonFeatures.empty()){
-    for(const auto& p : vNonFeatures){
-      auto* e = Cost_NonFeature_ICP::Create(p.pointOri,
+  Tbl.topLeftCorner(3, 3) = exTlb.topLeftCorner(3, 3).transpose();
+  Tbl.topRightCorner(3, 1) = -1.0 * Tbl.topLeftCorner(3, 3) * exTlb.topRightCorner(3, 1);
+  if (!vNonFeatures.empty())
+  {
+    for (const auto &p : vNonFeatures)
+    {
+      auto *e = Cost_NonFeature_ICP::Create(p.pointOri,
                                             p.pa,
                                             p.pb,
                                             p.pc,
                                             p.pd,
                                             Tbl,
-                                            Eigen::Matrix<double, 1, 1>(1/IMUIntegrator::lidar_m));
+                                            Eigen::Matrix<double, 1, 1>(1 / IMUIntegrator::lidar_m));
       edges.push_back(e);
     }
     return;
@@ -488,28 +487,28 @@ void Estimator::processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
   std::vector<int> _pointSearchInd2;
   std::vector<float> _pointSearchSqDis2;
 
-  Eigen::Matrix< double, 5, 3 > _matA0;
+  Eigen::Matrix<double, 5, 3> _matA0;
   _matA0.setZero();
-  Eigen::Matrix< double, 5, 1 > _matB0;
+  Eigen::Matrix<double, 5, 1> _matB0;
   _matB0.setOnes();
   _matB0 *= -1;
-  Eigen::Matrix< double, 3, 1 > _matX0;
+  Eigen::Matrix<double, 3, 1> _matX0;
   _matX0.setZero();
 
   int laserCloudNonFeatureStackNum = laserCloudNonFeature->points.size();
-  for (int i = 0; i < laserCloudNonFeatureStackNum; i++) {
+  for (int i = 0; i < laserCloudNonFeatureStackNum; i++)
+  {
     _pointOri = laserCloudNonFeature->points[i];
-      LocalMapIvox::pointAssociateToMap(&_pointOri, &_pointSel, m4d);
-    /*int id = map_manager->FindUsedNonFeatureMap(&_pointSel,laserCenWidth_last,laserCenHeight_last,laserCenDepth_last);
+    LocalMapIvox::pointAssociateToMap(&_pointOri, &_pointSel, m4d);
+    if (std::isnan(_pointSel.x) || std::isnan(_pointSel.y) || std::isnan(_pointSel.z)) continue;
 
-    if(id == 5000) continue;*/
-
-    if(std::isnan(_pointSel.x) || std::isnan(_pointSel.y) ||std::isnan(_pointSel.z)) continue;
-
-    if(m_global_map.laserCloudNonFeatureFromLocal->points.size() > 100) {
+    if (m_global_map.laserCloudNonFeatureFromLocal->points.size() > 100)
+    {
       m_global_map.kdtreeNonFeatureFromLocal->nearestKSearch(_pointSel, 5, _pointSearchInd, _pointSearchSqDis);
-      if (_pointSearchSqDis[4] < 1 * thres_dist) {
-        for (int j = 0; j < 5; j++) {
+      if (_pointSearchSqDis[4] < 1 * thres_dist)
+      {
+        for (int j = 0; j < 5; j++)
+        {
           _matA0(j, 0) = m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].x;
           _matA0(j, 1) = m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].y;
           _matA0(j, 2) = m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].z;
@@ -528,26 +527,29 @@ void Estimator::processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
         pd /= ps;
 
         bool planeValid = true;
-        for (int j = 0; j < 5; j++) {
+        for (int j = 0; j < 5; j++)
+        {
           if (std::fabs(pa * m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].x +
-                        pb * m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].y +
-                        pc * m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].z + pd) > 0.2) {
+                                pb * m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].y +
+                                pc * m_global_map.laserCloudNonFeatureFromLocal->points[_pointSearchInd[j]].z + pd) > 0.2)
+          {
             planeValid = false;
             break;
           }
         }
 
-        if(planeValid) {
+        if (planeValid)
+        {
 
-          auto* e = Cost_NonFeature_ICP::Create(Eigen::Vector3d(_pointOri.x,_pointOri.y,_pointOri.z),
+          auto *e = Cost_NonFeature_ICP::Create(Eigen::Vector3d(_pointOri.x, _pointOri.y, _pointOri.z),
                                                 pa,
                                                 pb,
                                                 pc,
                                                 pd,
                                                 Tbl,
-                                                Eigen::Matrix<double, 1, 1>(1/IMUIntegrator::lidar_m));
+                                                Eigen::Matrix<double, 1, 1>(1 / IMUIntegrator::lidar_m));
           edges.push_back(e);
-          vNonFeatures.emplace_back(Eigen::Vector3d(_pointOri.x,_pointOri.y,_pointOri.z),
+          vNonFeatures.emplace_back(Eigen::Vector3d(_pointOri.x, _pointOri.y, _pointOri.z),
                                     pa,
                                     pb,
                                     pc,
@@ -557,14 +559,17 @@ void Estimator::processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
           continue;
         }
       }
-    
+
     }
 
-    /*if(local_map_.NonfMapVox()->NumPoints() > 20 ){
-        pcl::PointCloud<PointType> nearst;
-        local_map_.NonfMapVox()->GetClosestPoint(_pointSel,nearst.points,5,thres_dist);
-      if (nearst.size()==5) {
-        for (int j = 0; j < 5; j++) { 
+    if (local_map_.NonfMapVox()->NumPoints() > 20)
+    {
+      pcl::PointCloud<PointType> nearst;
+      local_map_.NonfMapVox()->GetClosestPoint(_pointSel, nearst.points, 5, thres_dist);
+      if (nearst.size() == 5)
+      {
+        for (int j = 0; j < 5; j++)
+        {
           _matA0(j, 0) = nearst.points[j].x;
           _matA0(j, 1) = nearst.points[j].y;
           _matA0(j, 2) = nearst.points[j].z;
@@ -583,26 +588,29 @@ void Estimator::processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
         pd /= ps;
 
         bool planeValid = true;
-        for (int j = 0; j < 5; j++) {
+        for (int j = 0; j < 5; j++)
+        {
           if (std::fabs(pa * nearst.points[j].x +
-                        pb * nearst.points[j].y +
-                        pc * nearst.points[j].z + pd) > 0.2) {
+                                pb * nearst.points[j].y +
+                                pc * nearst.points[j].z + pd) > 0.2)
+          {
             planeValid = false;
             break;
           }
         }
 
-        if(planeValid) {
+        if (planeValid)
+        {
 
-          auto* e = Cost_NonFeature_ICP::Create(Eigen::Vector3d(_pointOri.x,_pointOri.y,_pointOri.z),
+          auto *e = Cost_NonFeature_ICP::Create(Eigen::Vector3d(_pointOri.x, _pointOri.y, _pointOri.z),
                                                 pa,
                                                 pb,
                                                 pc,
                                                 pd,
                                                 Tbl,
-                                                Eigen::Matrix<double, 1, 1>(1/IMUIntegrator::lidar_m));
+                                                Eigen::Matrix<double, 1, 1>(1 / IMUIntegrator::lidar_m));
           edges.push_back(e);
-          vNonFeatures.emplace_back(Eigen::Vector3d(_pointOri.x,_pointOri.y,_pointOri.z),
+          vNonFeatures.emplace_back(Eigen::Vector3d(_pointOri.x, _pointOri.y, _pointOri.z),
                                     pa,
                                     pb,
                                     pc,
@@ -610,7 +618,7 @@ void Estimator::processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
           vNonFeatures.back().ComputeError(m4d);
         }
       }
-    }*/
+    }
   }
 }
 
@@ -664,13 +672,13 @@ void Estimator::EstimateLidarPose(std::list<LidarFrame>& lidarFrameList,
   int laserCloudSurfFromMapNum = m_global_map.laserCloudSurfFromLocal->points.size();
 
 
-  /*local_map_.CornerMapVox()->GetPoints(local_corner_cloud_.points);
+  local_map_.CornerMapVox()->GetPoints(local_corner_cloud_.points);
   local_map_.surfMapVox()->GetPoints(local_surf_cloud_.points);
   local_map_.NonfMapVox()->GetPoints(local_nonf_cloud_.points);
 
 
-  /*int laserCloudCornerFromLocalNum = local_corner_cloud_.points.size();
-  int laserCloudSurfFromLocalNum = local_surf_cloud_.points.size();*/
+  int laserCloudCornerFromLocalNum = local_corner_cloud_.points.size();
+  int laserCloudSurfFromLocalNum = local_surf_cloud_.points.size();
   int stack_count = 0;
 
   //遍历2帧历史数据，将角点，平面点，无特征点，提取出，下采样
@@ -711,8 +719,8 @@ void Estimator::EstimateLidarPose(std::list<LidarFrame>& lidarFrameList,
   }
   //地图有点，执行优化， 更新当前点及历史点的状态
 
-  if (((laserCloudCornerFromMapNum > 0 && laserCloudSurfFromMapNum > 100) /*||
-       (laserCloudCornerFromLocalNum > 0 && laserCloudSurfFromLocalNum > 100)*/))
+  if (((laserCloudCornerFromMapNum > 0 && laserCloudSurfFromMapNum > 100) ||
+       (laserCloudCornerFromLocalNum > 0 && laserCloudSurfFromLocalNum > 100)))
   {
     Estimate(lidarFrameList, exTlb, gravity);
   }
@@ -722,8 +730,7 @@ void Estimator::EstimateLidarPose(std::list<LidarFrame>& lidarFrameList,
   transformTobeMapped.topRightCorner(3, 1) = lidarFrameList.front().Q * exPbl + lidarFrameList.front().P;
 
 
-  /*TimerRecord::Execute(
-          [&,this]() {
+
             //此处加锁 赋值后，地图后台更新线程开始更新地图点
             std::unique_lock<std::mutex> locker(mtx_Map);
             *laserCloudCornerForMap = *laserCloudCornerStack[0];              //下标为0，
@@ -741,10 +748,6 @@ void Estimator::EstimateLidarPose(std::list<LidarFrame>& lidarFrameList,
                               laserCloudNonFeatureForMap,
                               transformTobeMapped);
             locker.unlock();
-          },"LocalMapUpdate"
-  );
-*/
-
 
 }
 
@@ -871,6 +874,7 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
       transformTobeMapped.topLeftCorner(3, 3) = frame_curr->Q * exRbl;
       transformTobeMapped.topRightCorner(3, 1) = frame_curr->Q * exPbl + frame_curr->P;
 
+
       threads[0] = std::thread(&Estimator::processPointToLine, this,
                                std::ref(edgesLine[f]),
                                std::ref(vLineFeatures[f]),
@@ -897,8 +901,8 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
       threads[0].join();
       threads[1].join();
       threads[2].join();
-    }
 
+    }
 
     int cntSurf = 0;
     int cntCorner = 0;
@@ -1120,6 +1124,8 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
       edgesLine[f].clear();
       edgesPlan[f].clear();
       edgesNon[f].clear();
+
+
       threads[0] = std::thread(&Estimator::processPointToLine, this,
                                std::ref(edgesLine[f]),
                                std::ref(vLineFeatures[f]),
@@ -1212,53 +1218,49 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
       }
     }
   }
-
 }
+
 void Estimator::MapIncrementLocal(const pcl::PointCloud<PointType>::Ptr& laserCloudCornerStack,
                                   const pcl::PointCloud<PointType>::Ptr& laserCloudSurfStack,
                                   const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureStack,
                                   const Eigen::Matrix4d& transformTobeMapped)
 {
-    int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
-    int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
-    int laserCloudNonFeatureStackNum = laserCloudNonFeatureStack->points.size();
-    PointType pointSel;
-    PointType pointSel2;
-    size_t Id = localMapID % localMapWindowSize;
     //本地地图特征， 保存50帧最近的点云数据，
-    localCornerMap[Id]->clear();
-    localSurfMap[Id]->clear();
-    localNonFeatureMap[Id]->clear();
-    for (int i = 0; i < laserCloudCornerStackNum; i++)
-    {
-        LocalMapIvox::pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel, transformTobeMapped);
-        localCornerMap[Id]->push_back(pointSel);
-    }
-    for (int i = 0; i < laserCloudSurfStackNum; i++)
-    {
-        LocalMapIvox::pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel2, transformTobeMapped);
-        localSurfMap[Id]->push_back(pointSel2);
-    }
-    for (int i = 0; i < laserCloudNonFeatureStackNum; i++)
-    {
-        LocalMapIvox::pointAssociateToMap(&laserCloudNonFeatureStack->points[i], &pointSel2, transformTobeMapped);
-        localNonFeatureMap[Id]->push_back(pointSel2);
-    }
-
-
     pcl::PointCloud<PointType>::Ptr down_corner(new pcl::PointCloud<PointType>());
     pcl::PointCloud<PointType>::Ptr down_surf(new pcl::PointCloud<PointType>());
     pcl::PointCloud<PointType>::Ptr down_nonf(new pcl::PointCloud<PointType>());
-    downSizeFilterCorner.setInputCloud(localCornerMap[Id]);
+    downSizeFilterCorner.setInputCloud(laserCloudCornerStack);
     downSizeFilterCorner.filter(*down_corner);
 
-    downSizeFilterSurf.setInputCloud(localSurfMap[Id]);
+    downSizeFilterSurf.setInputCloud(laserCloudSurfStack);
     downSizeFilterSurf.filter(*down_surf);
 
-    downSizeFilterNonFeature.setInputCloud(localNonFeatureMap[Id]);
+    downSizeFilterNonFeature.setInputCloud(laserCloudNonFeatureStack);
     downSizeFilterNonFeature.filter(*down_nonf);
 
-    local_map_.MapIncrement(down_corner, down_surf, down_nonf, localMapID);
+  pcl::PointCloud<PointType>::Ptr down_corner_transformed(new pcl::PointCloud<PointType>());
+  pcl::PointCloud<PointType>::Ptr down_surf_transformed(new pcl::PointCloud<PointType>());
+  pcl::PointCloud<PointType>::Ptr down_nonf_transformed(new pcl::PointCloud<PointType>());
+  PointType pointSel;
+  PointType pointSel2;
+  for (int i = 0; i < down_corner->size(); i++)
+  {
+    LocalMapIvox::pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel, transformTobeMapped);
+    down_corner_transformed->push_back(pointSel);
+  }
+  for (int i = 0; i < down_surf->size(); i++)
+  {
+    LocalMapIvox::pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel2, transformTobeMapped);
+    down_surf_transformed->push_back(pointSel2);
+  }
+  for (int i = 0; i < down_nonf->size(); i++)
+  {
+    LocalMapIvox::pointAssociateToMap(&laserCloudNonFeatureStack->points[i], &pointSel2, transformTobeMapped);
+    down_nonf_transformed->push_back(pointSel2);
+  }
+
+
+  local_map_.MapIncrement(down_corner_transformed, down_surf_transformed, down_nonf_transformed, localMapID);
 
 
     //本地地图50个点云的汇总点云，清零，然后再更新

lio/src/lio/Estimator.h

@@ -25,6 +25,9 @@ class Estimator{
             kdtreeCornerFromLocal.reset(new pcl::KdTreeFLANN<PointType>);
             kdtreeSurfFromLocal.reset(new pcl::KdTreeFLANN<PointType>);
             kdtreeNonFeatureFromLocal.reset(new pcl::KdTreeFLANN<PointType>);
+          downSizeFilterCorner.setLeafSize(0.1, 0.1, 0.1);
+          downSizeFilterSurf.setLeafSize(0.2, 0.2, 0.2);
+          downSizeFilterNonFeature.setLeafSize(0.4, 0.4, 0.4);
         }
         void reset(pcl::PointCloud<PointType>::Ptr corner,pcl::PointCloud<PointType>::Ptr surf,
                    pcl::PointCloud<PointType>::Ptr nonf)
@@ -78,42 +81,59 @@ class Estimator{
 
         bool load_map(std::string cloud_file)
         {
-            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-            read_txt_cloud(cloud,cloud_file);
-
-            for(int i=0;i<cloud->size();++i)
+          pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+          read_txt_cloud(cloud, cloud_file);
+          pcl::PointCloud<PointType>::Ptr Corner(new pcl::PointCloud<PointType>);
+          pcl::PointCloud<PointType>::Ptr Surf(new pcl::PointCloud<PointType>);
+          pcl::PointCloud<PointType>::Ptr Nonf(new pcl::PointCloud<PointType>);
+
+
+          for (int i = 0; i < cloud->size(); ++i)
+          {
+            pcl::PointXYZRGB pt = cloud->points[i];
+            PointType pt_temp;
+            pt_temp.x = pt.x;
+            pt_temp.y = pt.y;
+            pt_temp.z = pt.z;
+            if (pt.r == 255)
             {
-                pcl::PointXYZRGB pt=cloud->points[i];
-                PointType pt_temp;
-                pt_temp.x=pt.x;
-                pt_temp.y=pt.y;
-                pt_temp.z=pt.z;
-                if(pt.r==255)
-                {
-                    laserCloudCornerFromLocal->push_back(pt_temp);
-                }
-                else if(pt.g==255)
-                {
-                    laserCloudSurfFromLocal->push_back(pt_temp);
-                }
-                else
-                {
-                    laserCloudNonFeatureFromLocal->push_back(pt_temp);
-                }
+              Corner->push_back(pt_temp);
             }
-            printf(" load map   corner:%d  surf:%d  nonf:%d\n",laserCloudCornerFromLocal->size(),
-                   laserCloudSurfFromLocal->size(),laserCloudNonFeatureFromLocal->size());
-            kdtreeCornerFromLocal->setInputCloud(laserCloudCornerFromLocal);
-            kdtreeSurfFromLocal->setInputCloud(laserCloudSurfFromLocal);
-            kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);
-            return true;
+            else if (pt.g == 255)
+            {
+              Surf->push_back(pt_temp);
+            }
+            else
+            {
+              Nonf->push_back(pt_temp);
+            }
+          }
+
+          downSizeFilterCorner.setInputCloud(Corner);
+          downSizeFilterCorner.filter(*laserCloudCornerFromLocal);
+
+          downSizeFilterSurf.setInputCloud(Surf);
+          downSizeFilterSurf.filter(*laserCloudSurfFromLocal);
+
+          downSizeFilterNonFeature.setInputCloud(Nonf);
+          downSizeFilterNonFeature.filter(*laserCloudNonFeatureFromLocal);
+          printf(" load map   corner:%d  surf:%d  nonf:%d\n", laserCloudCornerFromLocal->size(),
+                 laserCloudSurfFromLocal->size(), laserCloudNonFeatureFromLocal->size());
+          kdtreeCornerFromLocal->setInputCloud(laserCloudCornerFromLocal);
+          kdtreeSurfFromLocal->setInputCloud(laserCloudSurfFromLocal);
+          kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);
+          return true;
         }
         /*pcl::PointCloud<PointType>::Ptr& corner_cloud(){return laserCloudCornerFromLocal;}
         pcl::PointCloud<PointType>::Ptr& surf_cloud(){return laserCloudSurfFromLocal;}
         pcl::PointCloud<PointType>::Ptr& nonf_cloud(){return laserCloudNonFeatureFromLocal;}*/
 
      public:
+        pcl::VoxelGrid<PointType> downSizeFilterCorner;
+        pcl::VoxelGrid<PointType> downSizeFilterSurf;
+        pcl::VoxelGrid<PointType> downSizeFilterNonFeature;
+
         pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromLocal;
         pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromLocal;
         pcl::KdTreeFLANN<PointType>::Ptr kdtreeNonFeatureFromLocal;
@@ -343,9 +363,6 @@ public:
 
 	static const int localMapWindowSize = 50;
 	int localMapID = 0;
-	pcl::PointCloud<PointType>::Ptr localCornerMap[localMapWindowSize];
-	pcl::PointCloud<PointType>::Ptr localSurfMap[localMapWindowSize];
-	pcl::PointCloud<PointType>::Ptr localNonFeatureMap[localMapWindowSize];
 
 	int map_update_ID = 0;
 

lio/src/lio/mapper.cpp

@@ -38,7 +38,7 @@ void Mapper::completed()
 {
     exit_=true;
     thread_.join();
-    TimerRecord::PrintAll();
+  TimerRecord::PrintAll();
 
 }
 
@@ -532,33 +532,35 @@ void Mapper::process()
     pcl::PointCloud<PointType>::Ptr laserSurfCloud(new pcl::PointCloud<PointType>);
     pcl::PointCloud<PointType>::Ptr laserNonFeatureCloud(new pcl::PointCloud<PointType>);
     TimerRecord::Execute(
-            [&, this]() {
-              if (Use_seg_)
-              {
-                lidarFeatureExtractor_->FeatureExtract_with_segment(cloudFull,
-                                                                    laserConerCloud,
-                                                                    laserSurfCloud,
-                                                                    laserNonFeatureCloud,
-                                                                    N_SCANS);
-              }
-              else
-              {
-                lidarFeatureExtractor_->FeatureExtract(cloudFull, laserConerCloud, laserSurfCloud, N_SCANS);
-              }
-            }, "feature extra");
+            [&,this](){
 
 
-    TimerRecord::Execute(
-            [&, this]() {
-              RemoveLidarDistortion(cloudFull, delta_Rl, delta_tl);
+    if (Use_seg_)
+    {
+      lidarFeatureExtractor_->FeatureExtract_with_segment(cloudFull,
+                                                          laserConerCloud,
+                                                          laserSurfCloud,
+                                                          laserNonFeatureCloud,
+                                                          N_SCANS);
+    }
+    else
+    {
+      lidarFeatureExtractor_->FeatureExtract(cloudFull, laserConerCloud, laserSurfCloud, N_SCANS);
+    }
+            },"Feature Extra"
+    );
 
-              // optimize current lidar pose with IMU,
-              //printf("window size :%d  lidar list size %d---\n",WINDOWSIZE,lidar_list->size());
 
-              estimator->EstimateLidarPose(*lidar_list, exTlb, GravityVector);
-            }, "EstimateLidarPose");
+    RemoveLidarDistortion(cloudFull, delta_Rl, delta_tl);
+
+    TimerRecord::Execute(
+            [&,this](){
 
 
+    estimator->EstimateLidarPose(*lidar_list, exTlb, GravityVector);
+
+            },"ceres"
+    );
     pcl::PointCloud<PointType>::Ptr laserCloudCornerMap(new pcl::PointCloud<PointType>());
     pcl::PointCloud<PointType>::Ptr laserCloudSurfMap(new pcl::PointCloud<PointType>());
 
@@ -617,7 +619,7 @@ void Mapper::process()
       cloudLocalMappcallback_(all.makeShared(), lidar_list->front().timeStamp);
     }
 
-    if(final_cost_callback_!= nullptr)
+    if (final_cost_callback_ != nullptr)
     {
       final_cost_callback_(estimator->final_cost_);
     }
@@ -681,7 +683,6 @@ void Mapper::process()
     auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
     frame_tm_ = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
 
-
   }
 }