解决点云数据赋值问题

lio/src/PoseEstimation.cpp

@@ -2,7 +2,7 @@
 
 #include <csignal>
 
-#include "segment/LidarFeatureExtractor.h"
+
 #include "lio/Estimator.h"
 #include "lio/mapper.h"
 
@@ -21,14 +21,11 @@
 #include <pangolin/display/widgets.h>
 #include <pangolin/display/default_font.h>
 #include <pangolin/handler/handler.h>
-
-
+#include "segment/LidarFeatureExtractor.h"
 
 LidarFeatureExtractor* lidarFeatureExtractor;
+
 pcl::PointCloud<PointType>::Ptr laserCloud;
-pcl::PointCloud<PointType>::Ptr laserConerCloud;
-pcl::PointCloud<PointType>::Ptr laserSurfCloud;
-pcl::PointCloud<PointType>::Ptr laserNonFeatureCloud;
 
 int Lidar_Type=0;
 int N_SCANS = 6;
@@ -81,7 +78,6 @@ Eigen::Matrix4d cur_pose;
 double pose_line_length=1.0;
 
 
-
 void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
 {
     scan_cloud.Set(cloud);
@@ -105,20 +101,7 @@ void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
     //odom_pt=tranform.topRightCorner(3, 1);
     pubOdometry(tranform, timebase);
 }
-/*
-ImuData msg2Imudata(sensor_msgs::ImuConstPtr msg)
-{
-    ImuData  imu;
-    imu.timebase=msg->header.stamp.toSec();
-    imu.angular_velocity.x=msg->angular_velocity.x;
-    imu.angular_velocity.y=msg->angular_velocity.y;
-    imu.angular_velocity.z=msg->angular_velocity.z;
-    imu.linear_acceleration.x=msg->linear_acceleration.x;
-    imu.linear_acceleration.y=msg->linear_acceleration.y;
-    imu.linear_acceleration.z=msg->linear_acceleration.z;
-    return imu;
-}
-*/
+
 
 void CloudCallback(TimeCloud tc,int handle)
 {
@@ -130,8 +113,8 @@ void CloudCallback(TimeCloud tc,int handle)
     for(const auto& p : tc.cloud->points)
     {
 
-
-        if (point.normal_y > N_SCANS - 1) continue;
+        //std::cout<<point.normal_y<<"   "<<point.normal_x<<"   "<<timeSpan<<std::endl;
+        if (p.normal_y > N_SCANS - 1) continue;
         if (p.x < 0.01) continue;
         if (!pcl_isfinite(p.x) ||
                 !pcl_isfinite(p.y) ||
@@ -139,16 +122,17 @@ void CloudCallback(TimeCloud tc,int handle)
         {
             continue;
         }
-        point.normal_x = point.normal_x / timeSpan; //normao_x  time
+        point.x=p.x;
+        point.y=p.y;
+        point.z=p.z;
+        point.normal_x = p.normal_x / timeSpan; //normao_x  time
+        point.intensity=p.intensity;
+        int line=int(p.normal_y);
+        point.normal_y=LidarFeatureExtractor::_int_as_float(line);
         laserCloud->push_back(point);
-    }
-
-
-    if(Use_seg){
-        lidarFeatureExtractor->FeatureExtract_with_segment( laserCloud, laserConerCloud, laserSurfCloud, laserNonFeatureCloud,N_SCANS);
-    }
-    else{
-        lidarFeatureExtractor->FeatureExtract(laserCloud, laserConerCloud, laserSurfCloud,N_SCANS);
+      /*std::cout<<" x:"<<point.x<<" y:"<<point.y<<" z:"<<point.z
+               <<" intnesity:"<<point.intensity<<" normal_x:"<<point.normal_x
+               <<" normal_y:"<<point.normal_y<<std::endl;*/
     }
 
     tc.cloud=laserCloud;
@@ -215,9 +199,6 @@ int main(int argc, char** argv)
 
 
   laserCloud.reset(new pcl::PointCloud<PointType>);
-  laserConerCloud.reset(new pcl::PointCloud<PointType>);
-  laserSurfCloud.reset(new pcl::PointCloud<PointType>);
-  laserNonFeatureCloud.reset(new pcl::PointCloud<PointType>);
 
 
   lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,
@@ -230,6 +211,7 @@ int main(int argc, char** argv)
                                                     LidarNearestDis,
                                                     KdTreeCornerOutlierDis);
 
+
   //map 参数
   std::string map_dir="../map/";
   float filter_parameter_corner = 0.2;
@@ -262,7 +244,7 @@ int main(int argc, char** argv)
   else
     WINDOWSIZE = 20;
 
-  pMaper = new Mapper(WINDOWSIZE, filter_parameter_corner, filter_parameter_surf,
+  pMaper = new Mapper(WINDOWSIZE, lidarFeatureExtractor,Use_seg,filter_parameter_corner, filter_parameter_surf,
                       ivox_nearby_type, max_ivox_node, ivox_resolution);
   pMaper->InitRT(R, t);
   pMaper->SetOdomCallback(OdometryCallback);

lio/src/lio/Estimator.cpp

@@ -1,5 +1,5 @@
 #include "Estimator.h"
-#include "utils/TimerRecord.h"
+
 
 Estimator::Estimator(const float& filter_corner, const float& filter_surf,
                      int ivox_nearby_type,int max_ivox_node,float resolution){
@@ -43,7 +43,7 @@ Estimator::Estimator(const float& filter_corner, const float& filter_surf,
 
 void Estimator::print()
 {
-    TimerRecord::PrintAll();
+
 }
 Estimator::~Estimator(){
 }
@@ -714,10 +714,7 @@ void Estimator::EstimateLidarPose(std::list<LidarFrame>& lidarFrameList,
   if (((laserCloudCornerFromMapNum > 0 && laserCloudSurfFromMapNum > 100) /*||
        (laserCloudCornerFromLocalNum > 0 && laserCloudSurfFromLocalNum > 100)*/))
   {
-    TimerRecord::Execute(
-            [&, this]() {
-              Estimate(lidarFrameList, exTlb, gravity);
-            }, "Estimate all");
+    Estimate(lidarFrameList, exTlb, gravity);
   }
 //根据优化结果重新赋值队列第一帧imu状态
   transformTobeMapped = Eigen::Matrix4d::Identity();
@@ -864,45 +861,43 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
         std::vector<std::vector<ceres::CostFunction *>> edgesNon(windowSize);
         std::thread threads[3];
         //添加2帧点云与local地图匹配约束
-        TimerRecord::Execute(
-                [&,this]() {
+
                   for (int f = 0; f < windowSize; ++f)
                   {
 
-                      auto frame_curr = lidarFrameList.begin();
-                      std::advance(frame_curr, f);
-                      transformTobeMapped = Eigen::Matrix4d::Identity();
-                      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]),
-                                               std::ref(laserCloudCornerStack[f]),
-                                               std::ref(exTlb),
-                                               std::ref(transformTobeMapped));
-
-
-                      threads[1] = std::thread(&Estimator::processPointToPlanVec, this,
-                                               std::ref(edgesPlan[f]),
-                                               std::ref(vPlanFeatures[f]),
-                                               std::ref(laserCloudSurfStack[f]),
-                                               std::ref(exTlb),
-                                               std::ref(transformTobeMapped));
-
-
-                      threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
-                                               std::ref(edgesNon[f]),
-                                               std::ref(vNonFeatures[f]),
-                                               std::ref(laserCloudNonFeatureStack[f]),
-                                               std::ref(exTlb),
-                                               std::ref(transformTobeMapped));
-
-                      threads[0].join();
-                      threads[1].join();
-                      threads[2].join();
+                    auto frame_curr = lidarFrameList.begin();
+                    std::advance(frame_curr, f);
+                    transformTobeMapped = Eigen::Matrix4d::Identity();
+                    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]),
+                                             std::ref(laserCloudCornerStack[f]),
+                                             std::ref(exTlb),
+                                             std::ref(transformTobeMapped));
+
+
+                    threads[1] = std::thread(&Estimator::processPointToPlanVec, this,
+                                             std::ref(edgesPlan[f]),
+                                             std::ref(vPlanFeatures[f]),
+                                             std::ref(laserCloudSurfStack[f]),
+                                             std::ref(exTlb),
+                                             std::ref(transformTobeMapped));
+
+
+                    threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
+                                             std::ref(edgesNon[f]),
+                                             std::ref(vNonFeatures[f]),
+                                             std::ref(laserCloudNonFeatureStack[f]),
+                                             std::ref(exTlb),
+                                             std::ref(transformTobeMapped));
+
+                    threads[0].join();
+                    threads[1].join();
+                    threads[2].join();
                   }
-                },"Estimator::feature match");
 
 
         int cntSurf = 0;
@@ -1004,63 +999,63 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
         }
         else
         {
-            if (iterOpt == 0)
+          if (iterOpt == 0)
+          {
+            thres_dist = 10.0;
+          }
+          else
+          {
+            thres_dist = 1.0;
+          }
+          for (int f = 0; f < windowSize; ++f)
+          {
+            int cntFtu = 0;
+            for (auto &e : edgesLine[f])
             {
-                thres_dist = 10.0;
+              if (std::fabs(vLineFeatures[f][cntFtu].error) > 1e-5)
+              {
+                problem.AddResidualBlock(e, loss_function, para_PR[f]);
+                vLineFeatures[f][cntFtu].valid = true;
+              }
+              else
+              {
+                vLineFeatures[f][cntFtu].valid = false;
+              }
+              cntFtu++;
+              cntCorner++;
             }
-            else
+            cntFtu = 0;
+            for (auto &e : edgesPlan[f])
             {
-                thres_dist = 1.0;
+              if (std::fabs(vPlanFeatures[f][cntFtu].error) > 1e-5)
+              {
+                problem.AddResidualBlock(e, loss_function, para_PR[f]);
+                vPlanFeatures[f][cntFtu].valid = true;
+              }
+              else
+              {
+                vPlanFeatures[f][cntFtu].valid = false;
+              }
+              cntFtu++;
+              cntSurf++;
             }
-            for (int f = 0; f < windowSize; ++f)
-            {
-                int cntFtu = 0;
-                for (auto &e : edgesLine[f])
-                {
-                    if (std::fabs(vLineFeatures[f][cntFtu].error) > 1e-5)
-                    {
-                        problem.AddResidualBlock(e, loss_function, para_PR[f]);
-                        vLineFeatures[f][cntFtu].valid = true;
-                    }
-                    else
-                    {
-                        vLineFeatures[f][cntFtu].valid = false;
-                    }
-                    cntFtu++;
-                    cntCorner++;
-                }
-                cntFtu = 0;
-                for (auto &e : edgesPlan[f])
-                {
-                    if (std::fabs(vPlanFeatures[f][cntFtu].error) > 1e-5)
-                    {
-                        problem.AddResidualBlock(e, loss_function, para_PR[f]);
-                        vPlanFeatures[f][cntFtu].valid = true;
-                    }
-                    else
-                    {
-                        vPlanFeatures[f][cntFtu].valid = false;
-                    }
-                    cntFtu++;
-                    cntSurf++;
-                }
 
-                cntFtu = 0;
-                for (auto &e : edgesNon[f])
-                {
-                    if (std::fabs(vNonFeatures[f][cntFtu].error) > 1e-5)
-                    {
-                        problem.AddResidualBlock(e, loss_function, para_PR[f]);
-                        vNonFeatures[f][cntFtu].valid = true;
-                    }
-                    else
-                    {
-                        vNonFeatures[f][cntFtu].valid = false;
-                    }
-                    cntFtu++;
-                    cntNon++;
-                }
+            cntFtu = 0;
+            for (auto &e : edgesNon[f])
+            {
+              if (std::fabs(vNonFeatures[f][cntFtu].error) > 1e-5)
+              {
+                problem.AddResidualBlock(e, loss_function, para_PR[f]);
+                vNonFeatures[f][cntFtu].valid = true;
+              }
+              else
+              {
+                vNonFeatures[f][cntFtu].valid = false;
+              }
+              cntFtu++;
+              cntNon++;
             }
+          }
         }
 
         ceres::Solver::Options options;
@@ -1071,10 +1066,7 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
         options.num_threads = 6;
         ceres::Solver::Summary summary;
 
-        TimerRecord::Execute(
-                [&,this]() {
-                  ceres::Solve(options, &problem, &summary);
-                },"Estimate::ceres solve");
+        ceres::Solve(options, &problem, &summary);
 
         //将优化的结果赋值到lidarFrameList的每一帧数据
         double2vector(lidarFrameList);

lio/src/lio/mapper.cpp

@@ -3,9 +3,9 @@
 //
 
 #include "mapper.h"
+#include "utils/TimerRecord.h"
 
-
-Mapper::Mapper(int wndsize,float filter_coner,float filter_surf,
+Mapper::Mapper(int wndsize,LidarFeatureExtractor* feature_extractor,int Use_seg,float filter_coner,float filter_surf,
                int ivox_nearby_type,int max_ivox_node,float resolution)
 {
     odomcallback_= nullptr;
@@ -13,6 +13,8 @@ Mapper::Mapper(int wndsize,float filter_coner,float filter_surf,
     WINDOWSIZE=wndsize;
     exit_=false;
 
+  lidarFeatureExtractor_=feature_extractor;
+  Use_seg_=Use_seg;
     estimator = new Estimator(filter_parameter_corner, filter_parameter_surf,
             ivox_nearby_type,max_ivox_node,resolution);
     lidarFrameList.reset(new std::list<Estimator::LidarFrame>);
@@ -34,7 +36,7 @@ void Mapper::completed()
 {
     exit_=true;
     thread_.join();
-    estimator->print();
+    TimerRecord::PrintAll();
 
 }
 
@@ -144,7 +146,6 @@ void Mapper::RemoveLidarDistortion(pcl::PointCloud<PointType>::Ptr& cloud,
     }
 }
 
-
 bool Mapper::TryMAPInitialization()
 {
 //雷达数据达到三帧
@@ -386,11 +387,8 @@ void Mapper::process()
     Eigen::Vector3d delta_tb = Eigen::Vector3d::Zero();
     std::vector<ImuData> vimuMsg;
 
-
-
     while (exit_==false)
     {
-
         auto start = std::chrono::system_clock::now();
         newfullCloud = false;
         pcl::PointCloud<PointType>::Ptr cloudFull(new pcl::PointCloud<PointType>());
@@ -522,14 +520,36 @@ void Mapper::process()
 
         // remove lidar distortion
         //根据imu里程计（当前帧与上一帧的差），消除点云运动畸变
+      //特征提取
+      pcl::PointCloud<PointType>::Ptr laserConerCloud(new pcl::PointCloud<PointType>);
+      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");
 
 
-        RemoveLidarDistortion(cloudFull, delta_Rl, delta_tl);
+      TimerRecord::Execute(
+              [&, this]() {
+                RemoveLidarDistortion(cloudFull, delta_Rl, delta_tl);
 
-        // optimize current lidar pose with IMU,
-        //printf("window size :%d  lidar list size %d---\n",WINDOWSIZE,lidar_list->size());
+                // 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);
+                estimator->EstimateLidarPose(*lidar_list, exTlb, GravityVector);
+              },"EstimateLidarPose");
 
         pcl::PointCloud<PointType>::Ptr laserCloudCornerMap(new pcl::PointCloud<PointType>());
         pcl::PointCloud<PointType>::Ptr laserCloudSurfMap(new pcl::PointCloud<PointType>());

lio/src/lio/mapper.h

@@ -8,12 +8,12 @@
 #include "Estimator.h"
 
 #include "typedef.h"
-
+#include "segment/LidarFeatureExtractor.h"
 
 class Mapper
 {
  public:
-    Mapper(int wndsize,float filter_coner,float filter_surf,
+    Mapper(int wndsize,LidarFeatureExtractor* feature_extractor,int Use_seg,float filter_coner,float filter_surf,
            int ivox_nearby_type,int max_ivox_node,float resolution);
     ~Mapper();
     void InitRT(const Eigen::Matrix3d& R,const Eigen::Vector3d& T);
@@ -56,6 +56,7 @@ class Mapper
 
     bool LidarIMUInited = false;
     boost::shared_ptr<std::list<Estimator::LidarFrame>> lidarFrameList;
+    LidarFeatureExtractor* lidarFeatureExtractor_;
     Estimator* estimator;
 
     bool newfullCloud = false;
@@ -76,6 +77,8 @@ class Mapper
     int IMU_Mode = 2;
     int pushCount = 0;
     double startTime = 0;
+    int N_SCANS=6;
+    int Use_seg_=true;
 
 };
 

livox/livox_driver/lddc.cpp

@@ -110,14 +110,16 @@ void Lddc::FillPointsToCustomMsg(TimeCloud& tCloud, \
   LivoxPointXyzrtl* point_xyzrtl = (LivoxPointXyzrtl*)src_point;
   for (uint32_t i = 0; i < num; i++) {
     PointType point;
+    int64_t tm=0;
     if (echo_num > 1) { /** dual return mode */
-      point.normal_x = offset_time + (i / echo_num) * point_interval;
+      tm = (offset_time + (i / echo_num) * point_interval);
     } else {
-      point.normal_x = offset_time + i * point_interval;
+      tm = (offset_time + i * point_interval);
     }
     point.x = point_xyzrtl->x;
     point.y = point_xyzrtl->y;
     point.z = point_xyzrtl->z;
+    point.normal_x=double(tm)/1000000000.0;
     point.intensity = point_xyzrtl->reflectivity;
     //point.tag = point_xyzrtl->tag;
     point.normal_y = point_xyzrtl->line;
@@ -274,7 +276,7 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
     return;
   }
 
-  while (!QueueIsEmpty(p_queue)) {
+  while (!QueueIsEmpty(p_queue)&&!exit_) {
     uint32_t used_size = QueueUsedSize(p_queue);
     uint32_t onetime_publish_packets = lidar->onetime_publish_packets;
     if (used_size < onetime_publish_packets) {
@@ -291,7 +293,7 @@ void Lddc::PollingLidarImuData(uint8_t handle, LidarDevice *lidar) {
     return;
   }
 
-  while (!QueueIsEmpty(p_queue)) {
+  while (!QueueIsEmpty(p_queue)&&!exit_) {
     PublishImuData(p_queue, 1, handle);
   }
 }