增加keyframe

config/horizon.yaml

@@ -37,7 +37,7 @@ pcd_save:
 
 
 # voxel filter paprams
-odometrySurfLeafSize: 0.1                     # default: 0.4 - outdoor, 0.2 - indoor
+odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
 mappingCornerLeafSize: 0.1                   # default: 0.2 - outdoor, 0.1 - indoor
 mappingSurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
 
@@ -79,7 +79,7 @@ recontructKdTree: true
 ndtLoopMatch: true  # false -- icp
 locate_mode: false
 locate:
-    init_pos: [-7.129,2.5,0]
+    init_pos: [-7.129,1.2,0]
     init_rpy: [0,0,0.00]
 savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
 savePCDDirectory: "/doc/ros_ws/map/zkxy_livox"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

rviz_cfg/loam_livox.rviz

@@ -110,7 +110,7 @@ Visualization Manager:
           Color: 115; 210; 22
           Color Transformer: Intensity
           Decay Time: 1000
-          Enabled: true
+          Enabled: false
           Invert Rainbow: true
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
@@ -125,7 +125,7 @@ Visualization Manager:
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
+          Value: false
       Enabled: true
       Name: mapping
     - Class: rviz/Group

src/keyFramesManager.cpp

@@ -34,8 +34,10 @@ keyFrame& keyFrame::operator=(const keyFrame& frame){
 keyFrame::~keyFrame() {}
 
 void keyFrame::ResetPose(PointTypePose pose){
+  lock_.lock();
   pose_=pose;
   pose_changed_=true;
+  lock_.unlock();
 }
 
 
@@ -43,6 +45,7 @@ PointCloudXYZI::Ptr keyFrame::world_cloud(Eigen::Isometry3d lidar_2_imu){
   if(pose_changed_==false){
     return cloud_world_;
   }
+  lock_.lock();
   int cloudSize = cloud_->size();
   cloud_world_->resize(cloudSize);
 
@@ -66,6 +69,7 @@ PointCloudXYZI::Ptr keyFrame::world_cloud(Eigen::Isometry3d lidar_2_imu){
     cloud_world_->points[i].intensity = pointFrom.intensity;
   }
   pose_changed_=false;
+  lock_.unlock();
   return cloud_world_;
 }
 

src/keyFramesManager.h

@@ -23,6 +23,7 @@ protected:
     PointCloudXYZI::Ptr cloud_;
     PointCloudXYZI::Ptr cloud_world_;
     bool pose_changed_;
+    std::mutex lock_;
 };
 
 class keyFramesManager {

src/laserMapping.cpp

@@ -245,6 +245,8 @@ gtsam::ISAM2 *isam;
 gtsam::Values isamCurrentEstimate;
 Eigen::MatrixXd poseCovariance;
 
+ros::ServiceServer srvSaveMap;
+ros::ServiceServer srvSavePose;
 ros::Publisher pubLaserCloudSurround;
 ros::Publisher pubOptimizedGlobalMap ;           //   发布最后优化的地图
 
@@ -1937,6 +1939,7 @@ void init_pose_cbk(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg
   double x=msg->pose.pose.position.x;
   double y=msg->pose.pose.position.y;
   printf("  received init pose:%f  %f \n",x,y);
+
   state_ikfom state_inited;
   Eigen::Vector3d pos(x,y, 0);
   Eigen::Quaterniond q(msg->pose.pose.orientation.w,msg->pose.pose.orientation.x,
@@ -1944,6 +1947,7 @@ void init_pose_cbk(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg
   //  更新状态量
   state_inited.pos = pos;
   state_inited.rot =  q;
+  getCurPose(state_inited); //   更新transformTobeMapped
 
   double epsi[23] = {0.005};
   fill(epsi, epsi + 23, 0.005);
@@ -2152,11 +2156,10 @@ int main(int argc, char **argv) {
 
 
   ros::Subscriber sub_init_pose;
+  pcl::NormalDistributionsTransform<PointType, PointType> g_ndt;
   // 回环检测线程
   std::thread *loopthread = nullptr;
   if (locate_mode == false) {
-    ros::ServiceServer srvSaveMap;
-    ros::ServiceServer srvSavePose;
     // saveMap  发布地图保存服务
     srvSaveMap = nh.advertiseService("/save_map", &saveMapService);
     // savePose  发布轨迹保存服务
@@ -2174,7 +2177,12 @@ int main(int argc, char **argv) {
     ikdtree.Build(map_cloud.points);
 
     sub_init_pose = nh.subscribe("/initialpose", 1000, init_pose_cbk);
-
+    transformTobeMapped[0] =init_rpy[0];                //  roll  使用 eulerAngles(2,1,0) 方法时，顺序是 ypr
+    transformTobeMapped[1] = init_rpy[1];                //  pitch
+    transformTobeMapped[2] = init_rpy[2];                //  yaw
+    transformTobeMapped[3] = init_pos[0];          //  x
+    transformTobeMapped[4] = init_pos[1];          //   y
+    transformTobeMapped[5] = init_pos[2];          // z
     state_ikfom state_inited;
     Eigen::Vector3d pos(init_pos[0], init_pos[1], init_pos[2]);
     Eigen::Quaterniond q = EulerToQuat(init_rpy[0], init_rpy[1], init_rpy[2]);
@@ -2190,8 +2198,15 @@ int main(int argc, char **argv) {
     featsFromMap->clear();
     featsFromMap->points = ikdtree.PCL_Storage;
     publish_map(pubLaserCloudMap);
+    g_ndt.setTransformationEpsilon(0.001);
+    g_ndt.setStepSize(0.4);
+    g_ndt.setResolution(1);
+    // Setting max number of registration iterations.
+    g_ndt.setMaximumIterations(100);
+    g_ndt.setInputTarget(featsFromMap);
   }
 
+
   signal(SIGINT, SigHandle);
   ros::Rate rate(5000);
   bool status = ros::ok();
@@ -2233,6 +2248,46 @@ int main(int argc, char **argv) {
         continue;
       }
 
+      downSizeFilterSurf.setInputCloud(feats_undistort);
+      downSizeFilterSurf.filter(*feats_down_body);
+      //ndt 定位
+      if(locate_mode){
+        double ndt1 = omp_get_wtime();
+        // Setting point cloud to be aligned.
+        g_ndt.setInputSource(feats_down_body);
+        // Setting point cloud to be aligned to.
+        pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
+        Eigen::Affine3f init=pcl::getTransformation(transformTobeMapped[3],transformTobeMapped[4],transformTobeMapped[5],
+                               transformTobeMapped[0],transformTobeMapped[1],transformTobeMapped[2]);
+        g_ndt.align(*unused_result, init.matrix());
+        double icp_t2 = omp_get_wtime();
+        float noiseScore;
+        Eigen::Affine3f correctionLidarFrame;
+        noiseScore =g_ndt.getFitnessScore(); //  loop_clousre  noise from ndt
+        correctionLidarFrame = g_ndt.getFinalTransformation();
+        double ndt2 = omp_get_wtime();
+        printf(" ndt time:%f\n",ndt2-ndt1);
+        ikf_lock_.unlock();
+
+        float x, y, z, roll, pitch, yaw;
+        pcl::getTranslationAndEulerAngles(correctionLidarFrame, x, y, z, roll, pitch, yaw);
+        printf(" match pose : %f %f %f,  %f %f %f\n",x,y,z,roll,pitch,yaw);
+        Eigen::AngleAxisd rollAngle(AngleAxisd(roll,Vector3d::UnitX()));
+        Eigen::AngleAxisd pitchAngle(AngleAxisd(pitch,Vector3d::UnitY()));
+        Eigen::AngleAxisd yawAngle(AngleAxisd(yaw,Vector3d::UnitZ()));
+
+        Eigen::AngleAxisd rotation_vector;
+        Eigen::Quaterniond q=yawAngle*pitchAngle*rollAngle;
+
+        state_point.pos=Eigen::Vector3d(x,y,z);
+        state_point.rot=q;
+        getCurPose(state_point); //   更新transformTobeMapped
+
+        if (scan_pub_en) {
+          publish_frame_world(pubLaserCloudFull);        //   发布world系下的点云
+        }
+        continue;
+      }
 
       // 检查当前lidar数据时间，与最早lidar数据时间是否足够,两帧间隔0.1s
       flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true;
@@ -2241,8 +2296,6 @@ int main(int argc, char **argv) {
       if (locate_mode == false)
         lasermap_fov_segment(); // 根据lidar在W系下的位置，重新确定局部地图的包围盒角点，移除远端的点
 
-      downSizeFilterSurf.setInputCloud(feats_undistort);
-      downSizeFilterSurf.filter(*feats_down_body);
       t1 = omp_get_wtime();
       feats_down_size = feats_down_body->points.size(); //当前帧降采样后点数