点云匹配初值由预积分提供，增加imu误差分布配置

config/horizon.yaml

@@ -19,12 +19,12 @@ mapping:
                    0, 0, 1]
 IMU:
     freq: 200
-    acc_cov: 0.1
-    gyr_cov: 0.1
-    b_acc_cov: 0.0001
-    b_gyr_cov: 0.0001
-    integration_cov: 0.05
-    integration_length: 30
+    acc_cov: 0.05
+    gyr_cov: 0.05
+    b_acc_cov: 0.001
+    b_gyr_cov: 0.001
+    integration_cov: 0.02
+    integration_length: 20
 
 publish:
     path_en:  true

config/large_velodyne.yaml

@@ -34,12 +34,12 @@ mapping:
 IMU:
     freq: 500
     g: 9.804
-    acc_cov: 3.9939570888238808e-03
-    gyr_cov: 1.5636343949698187e-03
-    b_acc_cov: 6.4356659353532566e-01
-    b_gyr_cov:  3.5640318696367613e-02
-    integration_cov: 0.01
-    integration_length: 50
+    acc_cov: 3.9939570888238808e-04
+    gyr_cov: 1.5636343949698187e-04
+    b_acc_cov: 6.4356659353532566e-05
+    b_gyr_cov:  3.5640318696367613e-05
+    integration_cov: 0.02
+    integration_length: 5
 
 publish:
     path_en:  true

config/velodyne16.yaml

@@ -22,10 +22,7 @@ preprocess:
     blind: 1
 
 mapping:
-    acc_cov: 0.1
-    gyr_cov: 0.1
-    b_acc_cov: 0.0001
-    b_gyr_cov: 0.0001
+    
     fov_degree:    180
     det_range:     100.0
     extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
@@ -33,6 +30,16 @@ mapping:
     extrinsic_R: [ 1, 0, 0, 
                    0, 1, 0, 
                    0, 0, 1]
+                   
+IMU:
+    freq: 500
+    g: 9.804
+    acc_cov: 0.01
+    gyr_cov: 0.01
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    integration_cov: 0.02
+    integration_length: 5
 
 publish:
     path_en:  true

rviz_cfg/loam_livox.rviz

@@ -14,6 +14,7 @@ Panels:
         - /Odometry1/Odometry1/Covariance1
         - /Odometry1/Odometry1/Covariance1/Position1
         - /Odometry1/Odometry1/Covariance1/Orientation1
+        - /kdtreeMap1
         - /Path_isam_update1
         - /loop_clousre1/Namespaces1
       Splitter Ratio: 0.6432291865348816
@@ -156,8 +157,8 @@ Visualization Manager:
           Queue Size: 10
           Shape:
             Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.20000000298023224
+            Axes Length: 3
+            Axes Radius: 0.30000001192092896
             Color: 255; 85; 0
             Head Length: 0
             Head Radius: 0
@@ -169,7 +170,7 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Odometry
-    - Alpha: 1
+    - Alpha: 0.5
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
         Max Value: 10
@@ -348,7 +349,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 581.7444458007812
+      Distance: 808.0084228515625
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -356,9 +357,9 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 89.08528137207031
-        Y: 226.02294921875
-        Z: -41.162208557128906
+        X: 171.46237182617188
+        Y: 100.80445861816406
+        Z: -39.36630630493164
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -366,7 +367,7 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 1.5697963237762451
       Target Frame: global
-      Yaw: 3.1874172687530518
+      Yaw: 6.265603065490723
     Saved: ~
 Window Geometry:
   Displays:
@@ -384,5 +385,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1359
-  X: 158
-  Y: 44
+  X: -36
+  Y: 46

src/laserMapping.cpp

@@ -1520,9 +1520,9 @@ int main(int argc, char **argv) {
   publish_map(pubLaserCloudMap);
   //auto g_ndt = boost::make_shared<pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>>();
   auto g_ndt = boost::make_shared<pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>>();
-  g_ndt->setTransformationEpsilon(0.01);
+  g_ndt->setTransformationEpsilon(0.001);
   g_ndt->setStepSize(0.1);
-  g_ndt->setResolution(2);
+  g_ndt->setResolution(1);
   // Seting max number of registration iterations.
   g_ndt->setMaximumIterations(100);
   g_ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
@@ -1580,6 +1580,17 @@ int main(int argc, char **argv) {
       //ndt 定位
 
       if (has_matched == true) {
+        std::deque<ImuData> imus;
+        for (auto it_imu = Measures.imu.begin(); it_imu != Measures.imu.end(); it_imu++) {
+          ImuData imu;
+          imu.time=(*it_imu)->header.stamp.toSec();
+          imu.acc=gtsam::Vector3((*it_imu)->linear_acceleration.x,(*it_imu)->linear_acceleration.y,
+                                 (*it_imu)->linear_acceleration.z);
+          imu.angular=gtsam::Vector3((*it_imu)->angular_velocity.x,(*it_imu)->angular_velocity.y,
+                                     (*it_imu)->angular_velocity.z);
+          imus.push_back(imu);
+        }
+
         // Setting point cloud to be aligned.
         pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_src (new pcl::PointCloud<pcl::PointXYZI>());
         pcl::copyPointCloud(*feats_down_body, *cloud_src);
@@ -1587,10 +1598,10 @@ int main(int argc, char **argv) {
         g_ndt->setInputSource(cloud_src);
         // Setting point cloud to be aligned to.
         pcl::PointCloud<pcl::PointXYZI>::Ptr unused_result(new pcl::PointCloud<pcl::PointXYZI>());
-        Eigen::Affine3f init = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4],
-                                                      transformTobeMapped[5],
-                                                      transformTobeMapped[0], transformTobeMapped[1],
-                                                      transformTobeMapped[2]);
+        gtsam::Pose3 initPose=odom_opt.Preintegration(imus);
+        Eigen::Affine3f init = pcl::getTransformation(initPose.x(), initPose.y(),initPose.z(),
+                                                      initPose.rotation().roll(), initPose.rotation().pitch(),
+                                                      initPose.rotation().yaw());
         g_ndt->align(*unused_result, init.matrix());
         double icp_t2 = omp_get_wtime();
         float noiseScore;
@@ -1616,26 +1627,13 @@ int main(int argc, char **argv) {
         double opt_time1 = omp_get_wtime();
         gtsam::Pose3 lidar_pose(gtsam::Rot3(q),gtsam::Point3(x,y,z));
 
-        noiseScore=0.3/(1+exp(-64.*(noiseScore-0.1)));
-        double rotNoise=noiseScore*M_PI/180.0;
-        gtsam::noiseModel::Diagonal::shared_ptr lidarPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
-            (gtsam::Vector(6) << noiseScore, noiseScore,noiseScore,rotNoise, rotNoise, rotNoise).finished());
-        std::deque<ImuData> imus;
-        for (auto it_imu = Measures.imu.begin(); it_imu != Measures.imu.end(); it_imu++) {
-          ImuData imu;
-          imu.time=(*it_imu)->header.stamp.toSec();
-          imu.acc=gtsam::Vector3((*it_imu)->linear_acceleration.x,(*it_imu)->linear_acceleration.y,
-                                 (*it_imu)->linear_acceleration.z);
-          imu.angular=gtsam::Vector3((*it_imu)->angular_velocity.x,(*it_imu)->angular_velocity.y,
-                                     (*it_imu)->angular_velocity.z);
-          imus.push_back(imu);
-        }
-        gtsam::Pose3 opt_pose=odom_opt.update(lidar_pose,lidarPoseNoise,imus);
+
+        gtsam::Pose3 opt_pose=odom_opt.update(lidar_pose,noiseScore);
         double opt_time2 = omp_get_wtime();
         gtsam::Pose3 opt_lidar=opt_pose.compose(lidar2imu.inverse());
-        printf(" opt time:%.5f   opt pose : %f %f %f,  %f %f %f\n", opt_time2-opt_time1,
+        /*printf(" opt time:%.5f   opt pose : %f %f %f,  %f %f %f\n", opt_time2-opt_time1,
                opt_lidar.translation().x(),opt_lidar.translation().y(),opt_lidar.translation().z(),
-               opt_lidar.rotation().roll(),opt_lidar.rotation().pitch(),opt_lidar.rotation().yaw());
+               opt_lidar.rotation().roll(),opt_lidar.rotation().pitch(),opt_lidar.rotation().yaw());*/
 
         printf("-----------------------------------------------------------------\n");
         state_point.pos = Eigen::Vector3d(opt_lidar.x(),opt_lidar.y(),opt_lidar.z());

src/odometry.cpp

@@ -70,8 +70,27 @@ bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::Consta
   return false;
 }
 
-gtsam::Pose3 OdometryOpt::update(gtsam::Pose3 lidarPose,gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise,
-                                 std::deque<ImuData>& imuQueue) {
+gtsam::Pose3 OdometryOpt::Preintegration(std::deque<ImuData>& imuQueue){
+  int queue_size=imuQueue.size();
+  double lastTime = -1;
+  while (!imuQueue.empty()) {
+    ImuData imu = imuQueue.front();
+    double dt = (lastTime < 0) ? 1.0 / imu_freq_ : imu.time - lastTime;
+    imu_preint_->integrateMeasurement(imu.acc, imu.angular, dt);
+    lastTime = imu.time;
+    imuQueue.pop_front();
+    /*printf("imu_preint_->integrateMeasurement(gtsam::Vector3(%f,%f,%f),gtsam::Vector3(%f,%f,%f),%f);\n",
+           imu.acc[0],imu.acc[1],imu.acc[2], imu.angular[0],imu.angular[1],imu.angular[2],dt);*/
+    if(dt<1e-6){
+      printf("error >>>>>>>>  dt ===  %f\n",dt);
+    }
+  }
+  gtsam::NavState prevState(prevPose_,prevVel_);
+  imuIntegrateState_ = imu_preint_->predict(prevState, prevBias_);
+  return imuIntegrateState_.pose().compose(lidar2Imu_.inverse());
+}
+
+gtsam::Pose3 OdometryOpt::update(gtsam::Pose3 lidarPose,double ldMatchScore) {
   static int N=0;
   printf(" frame : %d ,  Key :%d >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n",N++,key_);
   if (inited_ == false) { //第一帧
@@ -105,6 +124,7 @@ gtsam::Pose3 OdometryOpt::update(gtsam::Pose3 lidarPose,gtsam::noiseModel::Diago
            prevVel_.x(),prevVel_.y(),prevVel_.z(),
            prevBias_.vector()[0],prevBias_.vector()[1],prevBias_.vector()[2],
            prevBias_.vector()[3],prevBias_.vector()[4],prevBias_.vector()[5]);
+
     graph_.resize(0);
     values_.clear();
     key_ = 1;
@@ -139,31 +159,64 @@ gtsam::Pose3 OdometryOpt::update(gtsam::Pose3 lidarPose,gtsam::noiseModel::Diago
     key_ = 1;
 
   }
-  int queue_size=imuQueue.size();
-  double lastTime = -1;
-  while (!imuQueue.empty()) {
-    ImuData imu = imuQueue.front();
-    double dt = (lastTime < 0) ? 1.0 / imu_freq_ : imu.time - lastTime;
-    imu_preint_->integrateMeasurement(imu.acc, imu.angular, dt);
-    lastTime = imu.time;
-    imuQueue.pop_front();
-    /*printf("imu_preint_->integrateMeasurement(gtsam::Vector3(%f,%f,%f),gtsam::Vector3(%f,%f,%f),%f);\n",
-           imu.acc[0],imu.acc[1],imu.acc[2], imu.angular[0],imu.angular[1],imu.angular[2],dt);*/
-    if(dt<1e-6){
-      printf("error >>>>>>>>  dt ===  %f\n",dt);
+
+
+  double lidarScore=0.02;
+  // insert predicted values
+  gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu_);
+  double RAD=180.0/M_PI;
+  gtsam::Pose3 diff_lidar(gtsam::Rot3::RzRyRx(gtsam::Vector3(curPose.rotation().roll()-prevPose_.rotation().roll(),
+                                                             curPose.rotation().pitch()-prevPose_.rotation().pitch(),
+                                                             curPose.rotation().yaw()-prevPose_.rotation().yaw())),
+                          gtsam::Point3(curPose.x()-prevPose_.x(),curPose.y()-prevPose_.y(),curPose.z()-prevPose_.z()));
+
+  printf(" lidar diff: %.4f %.4f %.4f,%.4f %.4f %.4f, Last Vel: %.3f %.3f %.3f\n",diff_lidar.x(),diff_lidar.y(),diff_lidar.z(),
+         diff_lidar.rotation().roll()*RAD,diff_lidar.rotation().pitch()*RAD,diff_lidar.rotation().yaw()*RAD,
+         prevVel_.x(),prevVel_.y(),prevVel_.z());
+
+  gtsam::Pose3 diff_imu(gtsam::Rot3::RzRyRx(gtsam::Vector3(imuIntegrateState_.pose().rotation().roll()-prevPose_.rotation().roll(),
+                                                           imuIntegrateState_.pose().rotation().pitch()-prevPose_.rotation().pitch(),
+                                                           imuIntegrateState_.pose().rotation().yaw()-prevPose_.rotation().yaw())),
+                        gtsam::Point3(imuIntegrateState_.pose().x()-prevPose_.x(),imuIntegrateState_.pose().y()-prevPose_.y(),imuIntegrateState_.pose().z()-prevPose_.z()));
+
+  printf(" imuPr diff: %.4f %.4f %.4f,%.4f %.4f %.4f\n",diff_imu.x(),diff_imu.y(),diff_imu.z(),
+         diff_imu.rotation().roll()*RAD,diff_imu.rotation().pitch()*RAD,diff_imu.rotation().yaw()*RAD);
+
+  gtsam::Pose3 liadr2imu(gtsam::Rot3::RzRyRx(gtsam::Vector3(imuIntegrateState_.pose().rotation().roll()-curPose.rotation().roll(),
+                                                            imuIntegrateState_.pose().rotation().pitch()-curPose.rotation().pitch(),
+                                                            imuIntegrateState_.pose().rotation().yaw()-curPose.rotation().yaw())),
+                         gtsam::Point3(imuIntegrateState_.pose().x()-curPose.x(),imuIntegrateState_.pose().y()-curPose.y(),imuIntegrateState_.pose().z()-curPose.z()));
+
+
+  double threshxy=0.1,threshR=5*M_PI/180.0;
+  bool ldimuCorrect=(fabs(liadr2imu.x())>threshxy || fabs(liadr2imu.y())>threshxy || fabs(liadr2imu.z())>threshxy ||
+     fabs(liadr2imu.rotation().roll())>threshR || fabs(liadr2imu.rotation().pitch())>threshR||fabs(liadr2imu.rotation().yaw())>threshR);
+  if(ldMatchScore>0.1){ //雷达数据匹配较差
+    if(ldimuCorrect){ //雷达 imu数据接近
+      lidarScore=0.05;
+    }else{
+      lidarScore=0.2;
+    }
+  }else{  //雷达数据较好
+    if(ldimuCorrect){ //雷达 imu数据接近
+      lidarScore=0.005;
+    }else{
+      lidarScore=0.01;
     }
   }
+  lidarScore=0.01;
+    printf(" imu2l diff: %.4f %.4f %.4f,%.4f %.4f %.4f  --ldScore: %f----\n",liadr2imu.x(),liadr2imu.y(),liadr2imu.z(),
+           liadr2imu.rotation().roll()*RAD,liadr2imu.rotation().pitch()*RAD,liadr2imu.rotation().yaw()*RAD,lidarScore);
 
-  // insert predicted values
-  gtsam::NavState prevState(prevPose_,prevVel_);
-  gtsam::NavState propState = imu_preint_->predict(prevState, prevBias_);
+
+  gtsam::noiseModel::Diagonal::shared_ptr lidarPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
+      (gtsam::Vector(6) << lidarScore, lidarScore,lidarScore,lidarScore, lidarScore, lidarScore).finished());
   // add pose factor
-  gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu_);
-  gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key_), curPose, priorPoseNoise);
+  gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key_), curPose, lidarPoseNoise);
   graph_.add(pose_factor);
 
   gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 0.1); // m/s
-  gtsam::PriorFactor<gtsam::Vector3> velFactor(V(key_),propState.v(),priorVelNoise);
+  gtsam::PriorFactor<gtsam::Vector3> velFactor(V(key_),imuIntegrateState_.v(),priorVelNoise);
   graph_.add(velFactor);
 
   gtsam::Vector noiseModelBetweenBias = (gtsam::Vector(6)
@@ -173,21 +226,22 @@ gtsam::Pose3 OdometryOpt::update(gtsam::Pose3 lidarPose,gtsam::noiseModel::Diago
                                                                 gtsam::noiseModel::Diagonal::Sigmas(
                                                                     imu_preint_->deltaTij() * noiseModelBetweenBias)));
 
+  /*gtsam::Vector noiseModelBetweenVel = (gtsam::Vector(3)
+      << 0.5,0.1,0.1).finished();
+  graph_.add(gtsam::BetweenFactor<gtsam::Vector3>(V(key_ - 1), V(key_), gtsam::Vector3(0,0,0) ,
+                                                                gtsam::noiseModel::Diagonal::Sigmas(
+                                                                    imu_preint_->deltaTij() * noiseModelBetweenVel)));*/
+
   // add imu factor to graph
   const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imu_preint_);
   gtsam::ImuFactor imu_factor(X(key_ - 1), V(key_ - 1), X(key_), V(key_), B(key_ - 1), preint_imu);
   graph_.add(imu_factor);
 
-  gtsam::Pose3 lidar_preint=propState.pose().compose(lidar2Imu_.inverse());
+  gtsam::Pose3 lidar_preint=imuIntegrateState_.pose().compose(lidar2Imu_.inverse());
 
-  printf(" frame %d init pose:%f %f %f,%f %f %f, V:%f %f %f, \nBias : %f %f %f,%f %f %f\n",N,
-         propState.pose().x(),propState.pose().y(),propState.pose().z(),
-         propState.pose().rotation().roll(), propState.pose().rotation().pitch(), propState.pose().rotation().yaw(),
-         propState.v().x(),propState.v().y(),propState.v().z(),
-         prevBias_.vector()[0],prevBias_.vector()[1],prevBias_.vector()[2],
-         prevBias_.vector()[3],prevBias_.vector()[4],prevBias_.vector()[5]);
-  values_.insert(X(key_), propState.pose());
-  values_.insert(V(key_), propState.v());
+
+  values_.insert(X(key_), imuIntegrateState_.pose());
+  values_.insert(V(key_), imuIntegrateState_.v());
   values_.insert(B(key_), prevBias_);
 
   // optimize

src/odometry.h

@@ -30,8 +30,8 @@ public:
     OdometryOpt(int length);
     ~OdometryOpt();
     void init(gtsam::Pose3 lidar2imu,double imu_freq,double accN,double gryN,double integrationN,double imuAccBias,double imuGryBisa);
-    gtsam::Pose3 update(gtsam::Pose3 lidarPose,gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise,
-                        std::deque<ImuData>& imuQueue);
+    gtsam::Pose3 Preintegration(std::deque<ImuData>& imuQueue);
+    gtsam::Pose3 update(gtsam::Pose3 lidarPose,double ldMatchScore);
 protected:
     void reset_opt();
     static bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur);
@@ -50,6 +50,7 @@ protected:
     gtsam::Vector3 prevVel_;
     gtsam::NavState prevState_;
     gtsam::imuBias::ConstantBias prevBias_;
+    gtsam::NavState imuIntegrateState_;
 
     double imu_freq_;
     double imuAccN_;