增加loss值显示

lio/src/PoseEstimation.cpp

@@ -76,6 +76,7 @@ LockCloud scan_cloud;
 LockCloud localmap_cloud;
 Eigen::Matrix4d cur_pose;
 double pose_line_length=1.0;
+double final_cost=0;
 
 
 void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
@@ -89,19 +90,16 @@ void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
 }
 
 
-void pubOdometry(const Eigen::Matrix4d& newPose, double& timefullCloud)
-{
-
-}
-
-
 void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
 {
     cur_pose=tranform;
-    //odom_pt=tranform.topRightCorner(3, 1);
-    pubOdometry(tranform, timebase);
 }
 
+void FinalCostCallback(double cost)
+{
+  final_cost=cost;
+  printf(" final cost %.9f\n",cost);
+}
 
 void CloudCallback(TimeCloud tc,int handle)
 {
@@ -248,6 +246,7 @@ int main(int argc, char** argv)
   pMaper->SetOdomCallback(OdometryCallback);
   pMaper->SetCloudMappedCallback(CloudFullCallback);
   pMaper->SetLocalMapCloudCallback(CloudLocalCallback);
+  pMaper->SetFinalCostCallback(FinalCostCallback);
   pMaper->LoadMap(map_dir + "/feature.txt");
 
   double RPI = M_PI / 180.0;
@@ -306,6 +305,7 @@ int main(int argc, char** argv)
   pangolin::Var<std::string> timequeue_string("ui.tq_String", "timequeue_string");
   pangolin::Var<std::string> xyz_string("ui.xyz_String", "xyz");
   pangolin::Var<std::string> rpy_string("ui.rpy_String", "rpy");
+  pangolin::Var<std::string> cost_string("ui.final_cost", "cost");
 
 
 
@@ -385,6 +385,10 @@ int main(int argc, char** argv)
     xyz_string=translation;
     rpy_string=rpy;
 
+    char cost[64]={0};
+    sprintf(cost,"cost:%f",final_cost);
+    cost_string=cost;
+
 
     if (freq_double.GuiChanged())
     {

lio/src/lio/Estimator.cpp

@@ -753,464 +753,465 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
                          const Eigen::Vector3d& gravity)
 {
 
-    int num_corner_map = 0;
-    int num_surf_map = 0;
-
-    static uint32_t frame_count = 0;
-    int windowSize = lidarFrameList.size();
-    Eigen::Matrix4d transformTobeMapped = Eigen::Matrix4d::Identity();
-    Eigen::Matrix3d exRbl = exTlb.topLeftCorner(3, 3).transpose();
-    Eigen::Vector3d exPbl = -1.0 * exRbl * exTlb.topRightCorner(3, 1);
-
-    // store point to line features
-    std::vector<std::vector<FeatureLine>> vLineFeatures(windowSize);
-    for (auto &v : vLineFeatures)
+  int num_corner_map = 0;
+  int num_surf_map = 0;
+
+  static uint32_t frame_count = 0;
+  int windowSize = lidarFrameList.size();
+  Eigen::Matrix4d transformTobeMapped = Eigen::Matrix4d::Identity();
+  Eigen::Matrix3d exRbl = exTlb.topLeftCorner(3, 3).transpose();
+  Eigen::Vector3d exPbl = -1.0 * exRbl * exTlb.topRightCorner(3, 1);
+
+  // store point to line features
+  std::vector<std::vector<FeatureLine>> vLineFeatures(windowSize);
+  for (auto &v : vLineFeatures)
+  {
+    v.reserve(2000);
+  }
+
+  // store point to plan features
+  std::vector<std::vector<FeaturePlanVec>> vPlanFeatures(windowSize);
+  for (auto &v : vPlanFeatures)
+  {
+    v.reserve(2000);
+  }
+
+  std::vector<std::vector<FeatureNon>> vNonFeatures(windowSize);
+  for (auto &v : vNonFeatures)
+  {
+    v.reserve(2000);
+  }
+
+  if (windowSize == SLIDEWINDOWSIZE)
+  {
+    plan_weight_tan = 0.0003;
+    thres_dist = 1.0;
+  }
+  else
+  {
+    plan_weight_tan = 0.0;
+    thres_dist = 25.0;
+  }
+
+  // excute optimize process
+  const int max_iters = 5;
+  for (int iterOpt = 0; iterOpt < max_iters; ++iterOpt)
+  {
+    //用freamelist中每帧的P和V赋值优化参数初值，
+    vector2double(lidarFrameList);
+
+    //create huber loss function
+    ceres::LossFunction *loss_function = NULL;
+    loss_function = new ceres::HuberLoss(0.1 / IMUIntegrator::lidar_m);
+    if (windowSize == SLIDEWINDOWSIZE)
     {
-        v.reserve(2000);
+      loss_function = NULL;
     }
-
-    // store point to plan features
-    std::vector<std::vector<FeaturePlanVec>> vPlanFeatures(windowSize);
-    for (auto &v : vPlanFeatures)
+    else
     {
-        v.reserve(2000);
+      loss_function = new ceres::HuberLoss(0.1 / IMUIntegrator::lidar_m);
     }
 
-    std::vector<std::vector<FeatureNon>> vNonFeatures(windowSize);
-    for (auto &v : vNonFeatures)
+    ceres::Problem::Options problem_options;
+    ceres::Problem problem(problem_options);
+
+    //添加每帧数据的位姿优化变量
+    for (int i = 0; i < windowSize; ++i)
     {
-        v.reserve(2000);
+      //三维P和朝向（旋转向量）
+      problem.AddParameterBlock(para_PR[i], 6);
     }
-
-    if (windowSize == SLIDEWINDOWSIZE)
+    //添加每帧数据的速度优化变量
+    for (int i = 0; i < windowSize; ++i)
     {
-        plan_weight_tan = 0.0003;
-        thres_dist = 1.0;
+      //三维的V，三维的bg，三维的ba
+      problem.AddParameterBlock(para_VBias[i], 9);
     }
-    else
+
+    // add IMU CostFunction
+    for (int f = 1; f < windowSize; ++f)
     {
-        plan_weight_tan = 0.0;
-        thres_dist = 25.0;
+      auto frame_curr = lidarFrameList.begin();
+      std::advance(frame_curr, f);
+      problem.AddResidualBlock(Cost_NavState_PRV_Bias::Create(frame_curr->imuIntegrator,
+                                                              const_cast<Eigen::Vector3d &>(gravity),
+                                                              Eigen::LLT<Eigen::Matrix<double, 15, 15>>
+                                                                      (frame_curr->imuIntegrator.GetCovariance().inverse())
+                                                                      .matrixL().transpose()),
+                               nullptr,
+                               para_PR[f - 1],
+                               para_VBias[f - 1],
+                               para_PR[f],
+                               para_VBias[f]);
     }
 
-    // excute optimize process
-    const int max_iters = 5;
-    for (int iterOpt = 0; iterOpt < max_iters; ++iterOpt)
+    if (last_marginalization_info)
     {
-        //用freamelist中每帧的P和V赋值优化参数初值，
-        vector2double(lidarFrameList);
+      // construct new marginlization_factor
+      auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
+      problem.AddResidualBlock(marginalization_factor, nullptr,
+                               last_marginalization_parameter_blocks);
+    }
 
-        //create huber loss function
-        ceres::LossFunction *loss_function = NULL;
-        loss_function = new ceres::HuberLoss(0.1 / IMUIntegrator::lidar_m);
-        if (windowSize == SLIDEWINDOWSIZE)
-        {
-            loss_function = NULL;
-        }
-        else
-        {
-            loss_function = new ceres::HuberLoss(0.1 / IMUIntegrator::lidar_m);
-        }
+    Eigen::Quaterniond q_before_opti = lidarFrameList.back().Q;
+    Eigen::Vector3d t_before_opti = lidarFrameList.back().P;
 
-        ceres::Problem::Options problem_options;
-        ceres::Problem problem(problem_options);
+    std::vector<std::vector<ceres::CostFunction *>> edgesLine(windowSize);
+    std::vector<std::vector<ceres::CostFunction *>> edgesPlan(windowSize);
+    std::vector<std::vector<ceres::CostFunction *>> edgesNon(windowSize);
+    std::thread threads[3];
+    //添加2帧点云与local地图匹配约束
 
-        //添加每帧数据的位姿优化变量
-        for (int i = 0; i < windowSize; ++i)
-        {
-            //三维P和朝向（旋转向量）
-            problem.AddParameterBlock(para_PR[i], 6);
-        }
-        //添加每帧数据的速度优化变量
-        for (int i = 0; i < windowSize; ++i)
-        {
-            //三维的V，三维的bg，三维的ba
-            problem.AddParameterBlock(para_VBias[i], 9);
-        }
+    for (int f = 0; f < windowSize; ++f)
+    {
 
-        // add IMU CostFunction
-        for (int f = 1; f < windowSize; ++f)
-        {
-            auto frame_curr = lidarFrameList.begin();
-            std::advance(frame_curr, f);
-            problem.AddResidualBlock(Cost_NavState_PRV_Bias::Create(frame_curr->imuIntegrator,
-                                                                    const_cast<Eigen::Vector3d &>(gravity),
-                                                                    Eigen::LLT<Eigen::Matrix<double, 15, 15>>
-                                                                            (frame_curr->imuIntegrator.GetCovariance().inverse())
-                                                                            .matrixL().transpose()),
-                                     nullptr,
-                                     para_PR[f - 1],
-                                     para_VBias[f - 1],
-                                     para_PR[f],
-                                     para_VBias[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();
+    }
 
-        if (last_marginalization_info)
-        {
-            // construct new marginlization_factor
-            auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
-            problem.AddResidualBlock(marginalization_factor, nullptr,
-                                     last_marginalization_parameter_blocks);
-        }
 
-        Eigen::Quaterniond q_before_opti = lidarFrameList.back().Q;
-        Eigen::Vector3d t_before_opti = lidarFrameList.back().P;
-
-        std::vector<std::vector<ceres::CostFunction *>> edgesLine(windowSize);
-        std::vector<std::vector<ceres::CostFunction *>> edgesPlan(windowSize);
-        std::vector<std::vector<ceres::CostFunction *>> edgesNon(windowSize);
-        std::thread threads[3];
-        //添加2帧点云与local地图匹配约束
-
-                  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();
-                  }
-
-
-        int cntSurf = 0;
-        int cntCorner = 0;
-        int cntNon = 0;
-        if (windowSize == SLIDEWINDOWSIZE)
+    int cntSurf = 0;
+    int cntCorner = 0;
+    int cntNon = 0;
+    if (windowSize == SLIDEWINDOWSIZE)
+    {
+      thres_dist = 1.0;
+      if (iterOpt == 0)
+      {
+        for (int f = 0; f < windowSize; ++f)
         {
-            thres_dist = 1.0;
-            if (iterOpt == 0)
+          int cntFtu = 0;
+          for (auto &e : edgesLine[f])
+          {
+            if (std::fabs(vLineFeatures[f][cntFtu].error) > 1e-5)
             {
-                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++;
-                    }
-                }
+              problem.AddResidualBlock(e, loss_function, para_PR[f]);
+              vLineFeatures[f][cntFtu].valid = true;
             }
             else
             {
-                for (int f = 0; f < windowSize; ++f)
-                {
-                    int cntFtu = 0;
-                    for (auto &e : edgesLine[f])
-                    {
-                        if (vLineFeatures[f][cntFtu].valid)
-                        {
-                            problem.AddResidualBlock(e, loss_function, para_PR[f]);
-                        }
-                        cntFtu++;
-                        cntCorner++;
-                    }
-                    cntFtu = 0;
-                    for (auto &e : edgesPlan[f])
-                    {
-                        if (vPlanFeatures[f][cntFtu].valid)
-                        {
-                            problem.AddResidualBlock(e, loss_function, para_PR[f]);
-                        }
-                        cntFtu++;
-                        cntSurf++;
-                    }
-
-                    cntFtu = 0;
-                    for (auto &e : edgesNon[f])
-                    {
-                        if (vNonFeatures[f][cntFtu].valid)
-                        {
-                            problem.AddResidualBlock(e, loss_function, para_PR[f]);
-                        }
-                        cntFtu++;
-                        cntNon++;
-                    }
-                }
+              vLineFeatures[f][cntFtu].valid = false;
             }
-        }
-        else
-        {
-          if (iterOpt == 0)
+            cntFtu++;
+            cntCorner++;
+          }
+
+          cntFtu = 0;
+          for (auto &e : edgesPlan[f])
           {
-            thres_dist = 10.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++;
           }
-          else
+
+          cntFtu = 0;
+          for (auto &e : edgesNon[f])
           {
-            thres_dist = 1.0;
+            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++;
           }
-          for (int f = 0; f < windowSize; ++f)
+        }
+      }
+      else
+      {
+        for (int f = 0; f < windowSize; ++f)
+        {
+          int cntFtu = 0;
+          for (auto &e : edgesLine[f])
           {
-            int cntFtu = 0;
-            for (auto &e : edgesLine[f])
+            if (vLineFeatures[f][cntFtu].valid)
             {
-              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++;
+              problem.AddResidualBlock(e, loss_function, para_PR[f]);
             }
-            cntFtu = 0;
-            for (auto &e : edgesPlan[f])
+            cntFtu++;
+            cntCorner++;
+          }
+          cntFtu = 0;
+          for (auto &e : edgesPlan[f])
+          {
+            if (vPlanFeatures[f][cntFtu].valid)
             {
-              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++;
+              problem.AddResidualBlock(e, loss_function, para_PR[f]);
             }
+            cntFtu++;
+            cntSurf++;
+          }
 
-            cntFtu = 0;
-            for (auto &e : edgesNon[f])
+          cntFtu = 0;
+          for (auto &e : edgesNon[f])
+          {
+            if (vNonFeatures[f][cntFtu].valid)
             {
-              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++;
+              problem.AddResidualBlock(e, loss_function, para_PR[f]);
             }
+            cntFtu++;
+            cntNon++;
           }
         }
+      }
+    }
+    else
+    {
+      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])
+        {
+          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++;
+        }
 
-        ceres::Solver::Options options;
-        options.linear_solver_type = ceres::DENSE_SCHUR;
-        options.trust_region_strategy_type = ceres::DOGLEG;
-        options.max_num_iterations = 10;
-        options.minimizer_progress_to_stdout = false;
-        options.num_threads = 6;
-        ceres::Solver::Summary summary;
+        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::Solve(options, &problem, &summary);
+    ceres::Solver::Options options;
+    options.linear_solver_type = ceres::DENSE_SCHUR;
+    options.trust_region_strategy_type = ceres::DOGLEG;
+    options.max_num_iterations = 10;
+    options.minimizer_progress_to_stdout = false;
+    options.num_threads = 6;
+    ceres::Solver::Summary summary;
 
-        //将优化的结果赋值到lidarFrameList的每一帧数据
-        double2vector(lidarFrameList);
+    ceres::Solve(options, &problem, &summary);
+    final_cost_ = summary.final_cost;
 
-        Eigen::Quaterniond q_after_opti = lidarFrameList.back().Q;
-        Eigen::Vector3d t_after_opti = lidarFrameList.back().P;
-        Eigen::Vector3d V_after_opti = lidarFrameList.back().V;
-        double deltaR = (q_before_opti.angularDistance(q_after_opti)) * 180.0 / M_PI;
-        double deltaT = (t_before_opti - t_after_opti).norm();
+    //将优化的结果赋值到lidarFrameList的每一帧数据
+    double2vector(lidarFrameList);
 
-        if (deltaR < 0.05 && deltaT < 0.05 || (iterOpt + 1) == max_iters)
+    Eigen::Quaterniond q_after_opti = lidarFrameList.back().Q;
+    Eigen::Vector3d t_after_opti = lidarFrameList.back().P;
+    Eigen::Vector3d V_after_opti = lidarFrameList.back().V;
+    double deltaR = (q_before_opti.angularDistance(q_after_opti)) * 180.0 / M_PI;
+    double deltaT = (t_before_opti - t_after_opti).norm();
+
+    if (deltaR < 0.05 && deltaT < 0.05 || (iterOpt + 1) == max_iters)
+    {
+      //printf("Frame: %d\n", frame_count++);
+      if (windowSize != SLIDEWINDOWSIZE) break;  //此处表示IMU未初始化，
+      // apply marginalization
+      auto *marginalization_info = new MarginalizationInfo();
+      if (last_marginalization_info)
+      {
+        std::vector<int> drop_set;
+        for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
         {
-            //printf("Frame: %d\n", frame_count++);
-            if (windowSize != SLIDEWINDOWSIZE) break;  //此处表示IMU未初始化，
-            // apply marginalization
-            auto *marginalization_info = new MarginalizationInfo();
-            if (last_marginalization_info)
-            {
-                std::vector<int> drop_set;
-                for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
-                {
-                    if (last_marginalization_parameter_blocks[i] == para_PR[0] ||
-                            last_marginalization_parameter_blocks[i] == para_VBias[0])
-                        drop_set.push_back(i);
-                }
+          if (last_marginalization_parameter_blocks[i] == para_PR[0] ||
+                  last_marginalization_parameter_blocks[i] == para_VBias[0])
+            drop_set.push_back(i);
+        }
 
-                auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
-                auto *residual_block_info = new ResidualBlockInfo(marginalization_factor, nullptr,
-                                                                  last_marginalization_parameter_blocks,
-                                                                  drop_set);
-                marginalization_info->addResidualBlockInfo(residual_block_info);
-            }
+        auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
+        auto *residual_block_info = new ResidualBlockInfo(marginalization_factor, nullptr,
+                                                          last_marginalization_parameter_blocks,
+                                                          drop_set);
+        marginalization_info->addResidualBlockInfo(residual_block_info);
+      }
 
-            auto frame_curr = lidarFrameList.begin();
-            std::advance(frame_curr, 1);
-            ceres::CostFunction *IMU_Cost = Cost_NavState_PRV_Bias::Create(frame_curr->imuIntegrator,
-                                                                           const_cast<Eigen::Vector3d &>(gravity),
-                                                                           Eigen::LLT<Eigen::Matrix<double, 15, 15>>
-                                                                                   (frame_curr->imuIntegrator.GetCovariance().inverse())
-                                                                                   .matrixL().transpose());
-            auto *residual_block_info = new ResidualBlockInfo(IMU_Cost, nullptr,
-                                                              std::vector<double *>{para_PR[0], para_VBias[0], para_PR[1], para_VBias[1]},
-                                                              std::vector<int>{0, 1});
-            marginalization_info->addResidualBlockInfo(residual_block_info);
-
-            int f = 0;
-            transformTobeMapped = Eigen::Matrix4d::Identity();
-            transformTobeMapped.topLeftCorner(3, 3) = frame_curr->Q * exRbl;
-            transformTobeMapped.topRightCorner(3, 1) = frame_curr->Q * exPbl + frame_curr->P;
-            edgesLine[f].clear();
-            edgesPlan[f].clear();
-            edgesNon[f].clear();
-            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();
-            int cntFtu = 0;
-            for (auto &e : edgesLine[f])
-            {
-                if (vLineFeatures[f][cntFtu].valid)
-                {
-                    auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
-                                                                      std::vector<double *>{para_PR[0]},
-                                                                      std::vector<int>{0});
-                    marginalization_info->addResidualBlockInfo(residual_block_info);
-                }
-                cntFtu++;
-            }
-            cntFtu = 0;
-            for (auto &e : edgesPlan[f])
-            {
-                if (vPlanFeatures[f][cntFtu].valid)
-                {
-                    auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
-                                                                      std::vector<double *>{para_PR[0]},
-                                                                      std::vector<int>{0});
-                    marginalization_info->addResidualBlockInfo(residual_block_info);
-                }
-                cntFtu++;
-            }
+      auto frame_curr = lidarFrameList.begin();
+      std::advance(frame_curr, 1);
+      ceres::CostFunction *IMU_Cost = Cost_NavState_PRV_Bias::Create(frame_curr->imuIntegrator,
+                                                                     const_cast<Eigen::Vector3d &>(gravity),
+                                                                     Eigen::LLT<Eigen::Matrix<double, 15, 15>>
+                                                                             (frame_curr->imuIntegrator.GetCovariance().inverse())
+                                                                             .matrixL().transpose());
+      auto *residual_block_info = new ResidualBlockInfo(IMU_Cost, nullptr,
+                                                        std::vector<double *>{para_PR[0], para_VBias[0], para_PR[1], para_VBias[1]},
+                                                        std::vector<int>{0, 1});
+      marginalization_info->addResidualBlockInfo(residual_block_info);
+
+      int f = 0;
+      transformTobeMapped = Eigen::Matrix4d::Identity();
+      transformTobeMapped.topLeftCorner(3, 3) = frame_curr->Q * exRbl;
+      transformTobeMapped.topRightCorner(3, 1) = frame_curr->Q * exPbl + frame_curr->P;
+      edgesLine[f].clear();
+      edgesPlan[f].clear();
+      edgesNon[f].clear();
+      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();
+      int cntFtu = 0;
+      for (auto &e : edgesLine[f])
+      {
+        if (vLineFeatures[f][cntFtu].valid)
+        {
+          auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
+                                                            std::vector<double *>{para_PR[0]},
+                                                            std::vector<int>{0});
+          marginalization_info->addResidualBlockInfo(residual_block_info);
+        }
+        cntFtu++;
+      }
+      cntFtu = 0;
+      for (auto &e : edgesPlan[f])
+      {
+        if (vPlanFeatures[f][cntFtu].valid)
+        {
+          auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
+                                                            std::vector<double *>{para_PR[0]},
+                                                            std::vector<int>{0});
+          marginalization_info->addResidualBlockInfo(residual_block_info);
+        }
+        cntFtu++;
+      }
 
-            cntFtu = 0;
-            for (auto &e : edgesNon[f])
-            {
-                if (vNonFeatures[f][cntFtu].valid)
-                {
-                    auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
-                                                                      std::vector<double *>{para_PR[0]},
-                                                                      std::vector<int>{0});
-                    marginalization_info->addResidualBlockInfo(residual_block_info);
-                }
-                cntFtu++;
-            }
+      cntFtu = 0;
+      for (auto &e : edgesNon[f])
+      {
+        if (vNonFeatures[f][cntFtu].valid)
+        {
+          auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
+                                                            std::vector<double *>{para_PR[0]},
+                                                            std::vector<int>{0});
+          marginalization_info->addResidualBlockInfo(residual_block_info);
+        }
+        cntFtu++;
+      }
 
-            marginalization_info->preMarginalize();
-            marginalization_info->marginalize();
+      marginalization_info->preMarginalize();
+      marginalization_info->marginalize();
 
-            std::unordered_map<long, double *> addr_shift;
-            for (int i = 1; i < SLIDEWINDOWSIZE; i++)
-            {
-                addr_shift[reinterpret_cast<long>(para_PR[i])] = para_PR[i - 1];
-                addr_shift[reinterpret_cast<long>(para_VBias[i])] = para_VBias[i - 1];
-            }
-            std::vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
+      std::unordered_map<long, double *> addr_shift;
+      for (int i = 1; i < SLIDEWINDOWSIZE; i++)
+      {
+        addr_shift[reinterpret_cast<long>(para_PR[i])] = para_PR[i - 1];
+        addr_shift[reinterpret_cast<long>(para_VBias[i])] = para_VBias[i - 1];
+      }
+      std::vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
 
-            delete last_marginalization_info;
-            last_marginalization_info = marginalization_info;
-            last_marginalization_parameter_blocks = parameter_blocks;
-            break;
-        }
+      delete last_marginalization_info;
+      last_marginalization_info = marginalization_info;
+      last_marginalization_parameter_blocks = parameter_blocks;
+      break;
+    }
 
-        if (windowSize != SLIDEWINDOWSIZE)
-        {
-            for (int f = 0; f < windowSize; ++f)
-            {
-                edgesLine[f].clear();
-                edgesPlan[f].clear();
-                edgesNon[f].clear();
-                vLineFeatures[f].clear();
-                vPlanFeatures[f].clear();
-                vNonFeatures[f].clear();
-            }
-        }
+    if (windowSize != SLIDEWINDOWSIZE)
+    {
+      for (int f = 0; f < windowSize; ++f)
+      {
+        edgesLine[f].clear();
+        edgesPlan[f].clear();
+        edgesNon[f].clear();
+        vLineFeatures[f].clear();
+        vPlanFeatures[f].clear();
+        vNonFeatures[f].clear();
+      }
     }
+  }
 
 }
 void Estimator::MapIncrementLocal(const pcl::PointCloud<PointType>::Ptr& laserCloudCornerStack,

lio/src/lio/Estimator.h

@@ -314,6 +314,7 @@ public:
     pcl::PointCloud<PointType> local_corner_cloud_;
     pcl::PointCloud<PointType> local_surf_cloud_;
     pcl::PointCloud<PointType> local_nonf_cloud_;
+    double final_cost_=0;
 
  private:
 	double para_PR[SLIDEWINDOWSIZE][6];

lio/src/lio/mapper.cpp

@@ -8,19 +8,21 @@
 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;
-    cloudMappedcallback_= nullptr;
-    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>);
-    thread_ = std::thread(&Mapper::process, this);
+  odomcallback_ = nullptr;
+  cloudMappedcallback_ = nullptr;
+  final_cost_callback_ = nullptr;
+  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>);
+  thread_ = std::thread(&Mapper::process, this);
 
 }
+
 Mapper::~Mapper()
 {
     exit_=true;
@@ -50,6 +52,11 @@ void Mapper::InitRT(const Eigen::Matrix3d& R,const Eigen::Vector3d& t)
     exPbl = -1.0 * exRbl * exPlb;
 }
 
+void Mapper::SetFinalCostCallback(CostCallback callback)
+{
+  final_cost_callback_=callback;
+}
+
 void Mapper::SetOdomCallback(odomCallback callback)
 {
     odomcallback_=callback;
@@ -379,297 +386,303 @@ void Mapper::SetInitPose(Eigen::Matrix4d transform)
   */
 void Mapper::process()
 {
-    double time_last_lidar = -1;
-    double time_curr_lidar = -1;
-    Eigen::Matrix3d delta_Rl = Eigen::Matrix3d::Identity();
-    Eigen::Vector3d delta_tl = Eigen::Vector3d::Zero();
-    Eigen::Matrix3d delta_Rb = Eigen::Matrix3d::Identity();
-    Eigen::Vector3d delta_tb = Eigen::Vector3d::Zero();
-    std::vector<ImuData> vimuMsg;
-
-    while (exit_==false)
+  double time_last_lidar = -1;
+  double time_curr_lidar = -1;
+  Eigen::Matrix3d delta_Rl = Eigen::Matrix3d::Identity();
+  Eigen::Vector3d delta_tl = Eigen::Vector3d::Zero();
+  Eigen::Matrix3d delta_Rb = Eigen::Matrix3d::Identity();
+  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>());
+    std::unique_lock<std::mutex> lock_lidar(_mutexLidarQueue);
+    if (!_lidarMsgQueue.empty())
     {
-        auto start = std::chrono::system_clock::now();
-        newfullCloud = false;
-        pcl::PointCloud<PointType>::Ptr cloudFull(new pcl::PointCloud<PointType>());
-        std::unique_lock<std::mutex> lock_lidar(_mutexLidarQueue);
-        if (!_lidarMsgQueue.empty())
-        {
-            // get new lidar msg
-            time_curr_lidar = _lidarMsgQueue.front().timebase;
-            cloudFull=_lidarMsgQueue.front().cloud;
-            _lidarMsgQueue.pop();
-            newfullCloud = true;
-        }
-        lock_lidar.unlock();
+      // get new lidar msg
+      time_curr_lidar = _lidarMsgQueue.front().timebase;
+      cloudFull = _lidarMsgQueue.front().cloud;
+      _lidarMsgQueue.pop();
+      newfullCloud = true;
+    }
+    lock_lidar.unlock();
 
-        if (newfullCloud == false)
+    if (newfullCloud == false)
+    {
+      continue;
+    }
+
+    if (IMU_Mode > 0 && time_last_lidar > 0)
+    {
+      // get IMU msg int the Specified time interval
+      //第二帧雷达数据以后，进入这里,雷达已经初始化，第一帧雷达imu预积分器使用默认值
+      vimuMsg.clear();
+      int countFail = 0;
+      //将上一帧雷达数据时间点到当前时间点的imu数据从queue中取出
+      while (!fetchImuMsgs(time_last_lidar, time_curr_lidar, vimuMsg))
+      {
+        countFail++;
+        if (countFail > 100)
         {
-            continue;
+          break;
         }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+      }
 
-        if (IMU_Mode > 0 && time_last_lidar > 0)
-        {
-            // get IMU msg int the Specified time interval
-            //第二帧雷达数据以后，进入这里,雷达已经初始化，第一帧雷达imu预积分器使用默认值
-            vimuMsg.clear();
-            int countFail = 0;
-            //将上一帧雷达数据时间点到当前时间点的imu数据从queue中取出
-            while (!fetchImuMsgs(time_last_lidar, time_curr_lidar, vimuMsg))
-            {
-                countFail++;
-                if (countFail > 100)
-                {
-                    break;
-                }
-                std::this_thread::sleep_for(std::chrono::milliseconds(1));
-            }
+    }
+    // this lidar frame init
+    Estimator::LidarFrame lidarFrame;
+    lidarFrame.laserCloud = cloudFull;
+    lidarFrame.timeStamp = time_curr_lidar;
+
+    boost::shared_ptr<std::list<Estimator::LidarFrame>> lidar_list;
+    //计算当前雷达数据的imu里程计
+    if (!vimuMsg.empty())
+    {
+      if (!LidarIMUInited)  //imu未初始化（前20帧数据）
+      {
+        // if get IMU msg successfully, use gyro integration to update delta_Rl
+        lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
+        lidarFrame.imuIntegrator.GyroIntegration(time_last_lidar);
+        //imu坐在世界坐标系下的旋转量
+        delta_Rb = lidarFrame.imuIntegrator.GetDeltaQ().toRotationMatrix();
+        //
+        delta_Rl = exTlb.topLeftCorner(3, 3) * delta_Rb * exTlb.topLeftCorner(3, 3).transpose();
+
+        // predict current lidar pose
+        lidarFrame.P = transformAftMapped.topLeftCorner(3, 3) * delta_tb
+                + transformAftMapped.topRightCorner(3, 1);
+        Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3, 3) * delta_Rb;
+        lidarFrame.Q = m3d;
+
+        lidar_list.reset(new std::list<Estimator::LidarFrame>);
+        lidar_list->push_back(lidarFrame);
+      }
+      else   //imu已经初始化，此时WINDOWSIZE=2，lidarFrameList->size=2
+      {
+        // if get IMU msg successfully, use pre-integration to update delta lidar pose
+        //此时的imuIntegrator为默认imuIntegrator，使用上一帧雷达时刻的imu ba和bg预积分
+        lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
+        lidarFrame.imuIntegrator.PreIntegration(lidarFrameList->back().timeStamp,
+                                                lidarFrameList->back().bg,
+                                                lidarFrameList->back().ba);
+
+        //上一时刻的雷达状态，（位姿、速度）
+        const Eigen::Vector3d &Pwbpre = lidarFrameList->back().P;
+        const Eigen::Quaterniond &Qwbpre = lidarFrameList->back().Q;
+        const Eigen::Vector3d &Vwbpre = lidarFrameList->back().V;
+
+        //获取当前帧到上一帧的状态差（dp，dq，dv）
+        const Eigen::Quaterniond &dQ = lidarFrame.imuIntegrator.GetDeltaQ();
+        const Eigen::Vector3d &dP = lidarFrame.imuIntegrator.GetDeltaP();
+        const Eigen::Vector3d &dV = lidarFrame.imuIntegrator.GetDeltaV();
+        double dt = lidarFrame.imuIntegrator.GetDeltaTime();
+
+        //更新当前帧的状态，赋值当前帧的ba和bg为上一帧的ba、bg。
+        lidarFrame.Q = Qwbpre * dQ;
+        lidarFrame.P = Pwbpre + Vwbpre * dt + 0.5 * GravityVector * dt * dt + Qwbpre * (dP);
+        lidarFrame.V = Vwbpre + GravityVector * dt + Qwbpre * (dV);
+        lidarFrame.bg = lidarFrameList->back().bg;
+        lidarFrame.ba = lidarFrameList->back().ba;
+
+        //上一雷达时刻imu位姿
+        Eigen::Quaterniond Qwlpre = Qwbpre * Eigen::Quaterniond(exRbl);
+        Eigen::Vector3d Pwlpre = Qwbpre * exPbl + Pwbpre;
+
+        //当前雷达时刻的imu位姿
+        Eigen::Quaterniond Qwl = lidarFrame.Q * Eigen::Quaterniond(exRbl);
+        Eigen::Vector3d Pwl = lidarFrame.Q * exPbl + lidarFrame.P;
+
+        //两时刻imu位姿差
+        delta_Rl = Qwlpre.conjugate() * Qwl;
+        delta_tl = Qwlpre.conjugate() * (Pwl - Pwlpre);
+        delta_Rb = dQ.toRotationMatrix();
+        delta_tb = dP;
+
+        lidarFrameList->push_back(lidarFrame);
+        lidarFrameList->pop_front();
+        lidar_list = lidarFrameList;
+      }
+    }
+    else   //imu队列无数据(第一帧雷达数据先到）
+    {
+      //imu无数据，但已经初始化过， 表示imu出现中断， break，退出程序
+      if (LidarIMUInited)
+        break;
+      else
+      {   //第一帧数据
+        // predict current lidar pose
+        lidarFrame.P = transformAftMapped.topLeftCorner(3, 3) * delta_tb
+                + transformAftMapped.topRightCorner(3, 1);
+        Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3, 3) * delta_Rb;
+        lidarFrame.Q = m3d;
+
+        lidar_list.reset(new std::list<Estimator::LidarFrame>);
+        lidar_list->push_back(lidarFrame);
+      }
+    }
 
-        }
-        // this lidar frame init
-        Estimator::LidarFrame lidarFrame;
-        lidarFrame.laserCloud = cloudFull;
-        lidarFrame.timeStamp = time_curr_lidar;
-
-        boost::shared_ptr<std::list<Estimator::LidarFrame>> lidar_list;
-        //计算当前雷达数据的imu里程计
-        if (!vimuMsg.empty())
-        {
-            if (!LidarIMUInited)  //imu未初始化（前20帧数据）
-            {
-                // if get IMU msg successfully, use gyro integration to update delta_Rl
-                lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
-                lidarFrame.imuIntegrator.GyroIntegration(time_last_lidar);
-                //imu坐在世界坐标系下的旋转量
-                delta_Rb = lidarFrame.imuIntegrator.GetDeltaQ().toRotationMatrix();
-                //
-                delta_Rl = exTlb.topLeftCorner(3, 3) * delta_Rb * exTlb.topLeftCorner(3, 3).transpose();
-
-                // predict current lidar pose
-                lidarFrame.P = transformAftMapped.topLeftCorner(3, 3) * delta_tb
-                        + transformAftMapped.topRightCorner(3, 1);
-                Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3, 3) * delta_Rb;
-                lidarFrame.Q = m3d;
-
-                lidar_list.reset(new std::list<Estimator::LidarFrame>);
-                lidar_list->push_back(lidarFrame);
-            }
-            else   //imu已经初始化，此时WINDOWSIZE=2，lidarFrameList->size=2
-            {
-                // if get IMU msg successfully, use pre-integration to update delta lidar pose
-                //此时的imuIntegrator为默认imuIntegrator，使用上一帧雷达时刻的imu ba和bg预积分
-                lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
-                lidarFrame.imuIntegrator.PreIntegration(lidarFrameList->back().timeStamp,
-                                                        lidarFrameList->back().bg,
-                                                        lidarFrameList->back().ba);
-
-                //上一时刻的雷达状态，（位姿、速度）
-                const Eigen::Vector3d &Pwbpre = lidarFrameList->back().P;
-                const Eigen::Quaterniond &Qwbpre = lidarFrameList->back().Q;
-                const Eigen::Vector3d &Vwbpre = lidarFrameList->back().V;
-
-                //获取当前帧到上一帧的状态差（dp，dq，dv）
-                const Eigen::Quaterniond &dQ = lidarFrame.imuIntegrator.GetDeltaQ();
-                const Eigen::Vector3d &dP = lidarFrame.imuIntegrator.GetDeltaP();
-                const Eigen::Vector3d &dV = lidarFrame.imuIntegrator.GetDeltaV();
-                double dt = lidarFrame.imuIntegrator.GetDeltaTime();
-
-                //更新当前帧的状态，赋值当前帧的ba和bg为上一帧的ba、bg。
-                lidarFrame.Q = Qwbpre * dQ;
-                lidarFrame.P = Pwbpre + Vwbpre * dt + 0.5 * GravityVector * dt * dt + Qwbpre * (dP);
-                lidarFrame.V = Vwbpre + GravityVector * dt + Qwbpre * (dV);
-                lidarFrame.bg = lidarFrameList->back().bg;
-                lidarFrame.ba = lidarFrameList->back().ba;
-
-                //上一雷达时刻imu位姿
-                Eigen::Quaterniond Qwlpre = Qwbpre * Eigen::Quaterniond(exRbl);
-                Eigen::Vector3d Pwlpre = Qwbpre * exPbl + Pwbpre;
-
-                //当前雷达时刻的imu位姿
-                Eigen::Quaterniond Qwl = lidarFrame.Q * Eigen::Quaterniond(exRbl);
-                Eigen::Vector3d Pwl = lidarFrame.Q * exPbl + lidarFrame.P;
-
-                //两时刻imu位姿差
-                delta_Rl = Qwlpre.conjugate() * Qwl;
-                delta_tl = Qwlpre.conjugate() * (Pwl - Pwlpre);
-                delta_Rb = dQ.toRotationMatrix();
-                delta_tb = dP;
-
-                lidarFrameList->push_back(lidarFrame);
-                lidarFrameList->pop_front();
-                lidar_list = lidarFrameList;
-            }
-        }
-        else   //imu队列无数据(第一帧雷达数据先到）
-        {
-            //imu无数据，但已经初始化过， 表示imu出现中断， break，退出程序
-            if (LidarIMUInited)
-                break;
-            else
-            {   //第一帧数据
-                // predict current lidar pose
-                lidarFrame.P = transformAftMapped.topLeftCorner(3, 3) * delta_tb
-                        + transformAftMapped.topRightCorner(3, 1);
-                Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3, 3) * delta_Rb;
-                lidarFrame.Q = m3d;
-
-                lidar_list.reset(new std::list<Estimator::LidarFrame>);
-                lidar_list->push_back(lidarFrame);
-            }
-        }
+    // 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");
+
+
+    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());
+
+              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>());
+
+
+    Eigen::Matrix4d transformTobeMapped = Eigen::Matrix4d::Identity();
+    transformTobeMapped.topLeftCorner(3, 3) = lidar_list->front().Q * exRbl;
+    transformTobeMapped.topRightCorner(3, 1) = lidar_list->front().Q * exPbl + lidar_list->front().P;
+
+    // update delta transformation
+    delta_Rb = transformAftMapped.topLeftCorner(3, 3).transpose() * lidar_list->front().Q.toRotationMatrix();
+    delta_tb = transformAftMapped.topLeftCorner(3,
+                                                3).transpose() * (lidar_list->front().P - transformAftMapped.topRightCorner(
+            3,
+            1));
+    Eigen::Matrix3d Rwlpre = transformAftMapped.topLeftCorner(3, 3) * exRbl;
+    Eigen::Vector3d Pwlpre = transformAftMapped.topLeftCorner(3, 3) * exPbl + transformAftMapped.topRightCorner(
+            3,
+            1);
+    delta_Rl = Rwlpre.transpose() * transformTobeMapped.topLeftCorner(3, 3);
+    delta_tl = Rwlpre.transpose() * (transformTobeMapped.topRightCorner(3, 1) - Pwlpre);
+    transformAftMapped.topLeftCorner(3, 3) = lidar_list->front().Q.toRotationMatrix();
+    transformAftMapped.topRightCorner(3, 1) = lidar_list->front().P;
+
+    // publish odometry rostopic
+    if (odomcallback_ != nullptr)
+    {
+      odomcallback_(transformTobeMapped, lidar_list->front().timeStamp);
+    }
 
-        // 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");
-
-
-      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());
-
-                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>());
-
-
-        Eigen::Matrix4d transformTobeMapped = Eigen::Matrix4d::Identity();
-        transformTobeMapped.topLeftCorner(3, 3) = lidar_list->front().Q * exRbl;
-        transformTobeMapped.topRightCorner(3, 1) = lidar_list->front().Q * exPbl + lidar_list->front().P;
-
-        // update delta transformation
-        delta_Rb = transformAftMapped.topLeftCorner(3, 3).transpose() * lidar_list->front().Q.toRotationMatrix();
-        delta_tb = transformAftMapped.topLeftCorner(3,
-                                                    3).transpose() * (lidar_list->front().P - transformAftMapped.topRightCorner(
-                3,
-                1));
-        Eigen::Matrix3d Rwlpre = transformAftMapped.topLeftCorner(3, 3) * exRbl;
-        Eigen::Vector3d Pwlpre = transformAftMapped.topLeftCorner(3, 3) * exPbl + transformAftMapped.topRightCorner(
-                3,
-                1);
-        delta_Rl = Rwlpre.transpose() * transformTobeMapped.topLeftCorner(3, 3);
-        delta_tl = Rwlpre.transpose() * (transformTobeMapped.topRightCorner(3, 1) - Pwlpre);
-        transformAftMapped.topLeftCorner(3, 3) = lidar_list->front().Q.toRotationMatrix();
-        transformAftMapped.topRightCorner(3, 1) = lidar_list->front().P;
-
-        // publish odometry rostopic
-        if (odomcallback_!= nullptr)
-        {
-            odomcallback_(transformTobeMapped,lidar_list->front().timeStamp);
-        }
+    // publish lidar points
+    if (cloudMappedcallback_ != nullptr)
+    {
+      int laserCloudFullResNum = lidar_list->front().laserCloud->points.size();
+      pcl::PointCloud<PointType>::Ptr laserCloudAfterEstimate(new pcl::PointCloud<PointType>());
+      laserCloudAfterEstimate->reserve(laserCloudFullResNum);
+      for (int i = 0; i < laserCloudFullResNum; i++)
+      {
+        PointType temp_point;
+        LocalMapIvox::pointAssociateToMap(&lidar_list->front().laserCloud->points[i],
+                                          &temp_point,
+                                          transformTobeMapped);
+        laserCloudAfterEstimate->push_back(temp_point);
+      }
+      cloudMappedcallback_(laserCloudAfterEstimate, lidar_list->front().timeStamp);
+    }
 
-        // publish lidar points
-        if(cloudMappedcallback_!= nullptr)
-        {
-            int laserCloudFullResNum = lidar_list->front().laserCloud->points.size();
-            pcl::PointCloud<PointType>::Ptr laserCloudAfterEstimate(new pcl::PointCloud<PointType>());
-            laserCloudAfterEstimate->reserve(laserCloudFullResNum);
-            for (int i = 0; i < laserCloudFullResNum; i++)
-            {
-                PointType temp_point;
-                LocalMapIvox::pointAssociateToMap(&lidar_list->front().laserCloud->points[i],
-                                                 &temp_point,
-                                                 transformTobeMapped);
-                laserCloudAfterEstimate->push_back(temp_point);
-            }
-            cloudMappedcallback_(laserCloudAfterEstimate,lidar_list->front().timeStamp);
-        }
+    if (cloudLocalMappcallback_ != nullptr)
+    {
 
-        if( cloudLocalMappcallback_!= nullptr)
-        {
+      pcl::PointCloud<PointType> all;
 
-            pcl::PointCloud<PointType> all;
+      all += estimator->local_corner_cloud_;
+      all += estimator->local_surf_cloud_;
+      all += estimator->local_nonf_cloud_;
 
-            all+=estimator->local_corner_cloud_;
-            all+=estimator->local_surf_cloud_;
-            all+=estimator->local_nonf_cloud_;
+      cloudLocalMappcallback_(all.makeShared(), lidar_list->front().timeStamp);
+    }
 
-            cloudLocalMappcallback_(all.makeShared(),lidar_list->front().timeStamp);
-        }
+    if(final_cost_callback_!= nullptr)
+    {
+      final_cost_callback_(estimator->final_cost_);
+    }
 
-        // if tightly coupled IMU message, start IMU initialization
-        if (IMU_Mode > 1 && !LidarIMUInited)
+    // if tightly coupled IMU message, start IMU initialization
+    if (IMU_Mode > 1 && !LidarIMUInited)
+    {
+      // update lidar frame pose
+      lidarFrame.P = transformTobeMapped.topRightCorner(3, 1);
+      Eigen::Matrix3d m3d = transformTobeMapped.topLeftCorner(3, 3);
+      lidarFrame.Q = m3d;
+
+      // static int pushCount = 0;
+      if (pushCount == 0)
+      {
+        lidarFrameList->push_back(lidarFrame);
+        lidarFrameList->back().imuIntegrator.Reset();
+        if (lidarFrameList->size() > WINDOWSIZE)
+          lidarFrameList->pop_front();
+      }
+      else
+      {
+        lidarFrameList->back().laserCloud = lidarFrame.laserCloud;
+        lidarFrameList->back().imuIntegrator.PushIMUMsg(vimuMsg);
+        lidarFrameList->back().timeStamp = lidarFrame.timeStamp;
+        lidarFrameList->back().P = lidarFrame.P;
+        lidarFrameList->back().Q = lidarFrame.Q;
+      }
+      pushCount++;
+      if (pushCount >= 3)
+      {
+        pushCount = 0;
+        if (lidarFrameList->size() > 1)
         {
-            // update lidar frame pose
-            lidarFrame.P = transformTobeMapped.topRightCorner(3, 1);
-            Eigen::Matrix3d m3d = transformTobeMapped.topLeftCorner(3, 3);
-            lidarFrame.Q = m3d;
-
-            // static int pushCount = 0;
-            if (pushCount == 0)
-            {
-                lidarFrameList->push_back(lidarFrame);
-                lidarFrameList->back().imuIntegrator.Reset();
-                if (lidarFrameList->size() > WINDOWSIZE)
-                    lidarFrameList->pop_front();
-            }
-            else
-            {
-                lidarFrameList->back().laserCloud = lidarFrame.laserCloud;
-                lidarFrameList->back().imuIntegrator.PushIMUMsg(vimuMsg);
-                lidarFrameList->back().timeStamp = lidarFrame.timeStamp;
-                lidarFrameList->back().P = lidarFrame.P;
-                lidarFrameList->back().Q = lidarFrame.Q;
-            }
-            pushCount++;
-            if (pushCount >= 3)
-            {
-                pushCount = 0;
-                if (lidarFrameList->size() > 1)
-                {
-                    auto iterRight = std::prev(lidarFrameList->end());
-                    auto iterLeft = std::prev(std::prev(lidarFrameList->end()));
-                    iterRight->imuIntegrator.PreIntegration(iterLeft->timeStamp, iterLeft->bg, iterLeft->ba);
-                }
-                //记录前2/3 windowsize的数据时间点， 此时间点之前的数据不参与imu初始化，初始化完成后置WINDOWSIE=2
-                if (lidarFrameList->size() == int(WINDOWSIZE / 1.5))
-                {
-                    startTime = lidarFrameList->back().timeStamp;
-                }
-
-                if (!LidarIMUInited && lidarFrameList->size() == WINDOWSIZE && lidarFrameList->front().timeStamp >= startTime)
-                {
-                    std::cout << "**************Start MAP Initialization!!!******************" << std::endl;
-                    if (TryMAPInitialization())
-                    {
-                        LidarIMUInited = true;
-                        pushCount = 0;
-                        startTime = 0;
-                    }
-                    std::cout << "**************Finish MAP Initialization!!!******************" << std::endl;
-                }
-
-            }
+          auto iterRight = std::prev(lidarFrameList->end());
+          auto iterLeft = std::prev(std::prev(lidarFrameList->end()));
+          iterRight->imuIntegrator.PreIntegration(iterLeft->timeStamp, iterLeft->bg, iterLeft->ba);
+        }
+        //记录前2/3 windowsize的数据时间点， 此时间点之前的数据不参与imu初始化，初始化完成后置WINDOWSIE=2
+        if (lidarFrameList->size() == int(WINDOWSIZE / 1.5))
+        {
+          startTime = lidarFrameList->back().timeStamp;
         }
-        time_last_lidar = time_curr_lidar;
-        auto end = std::chrono::system_clock::now();
-        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;
 
+        if (!LidarIMUInited && lidarFrameList->size() == WINDOWSIZE && lidarFrameList->front().timeStamp >= startTime)
+        {
+          std::cout << "**************Start MAP Initialization!!!******************" << std::endl;
+          if (TryMAPInitialization())
+          {
+            LidarIMUInited = true;
+            pushCount = 0;
+            startTime = 0;
+          }
+          std::cout << "**************Finish MAP Initialization!!!******************" << std::endl;
+        }
 
+      }
     }
+    time_last_lidar = time_curr_lidar;
+    auto end = std::chrono::system_clock::now();
+    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;
+
+
+  }
 }
 
 void Mapper::LoadMap(std::string file)

lio/src/lio/mapper.h

@@ -23,6 +23,7 @@ class Mapper
 
     void SetOdomCallback(odomCallback callback);
     void SetCloudMappedCallback(CloudMappedCallback callback);
+    void SetFinalCostCallback(CostCallback callback);
 
     void SetLocalMapCloudCallback(CloudMappedCallback callback);
 
@@ -52,6 +53,7 @@ class Mapper
     odomCallback odomcallback_;
     CloudMappedCallback cloudMappedcallback_;
     CloudMappedCallback cloudLocalMappcallback_;
+    CostCallback final_cost_callback_;
 
 
     bool LidarIMUInited = false;

livox/livox_driver/lds.cpp

@@ -660,7 +660,8 @@ void Lds::UpdateLidarInfoByEthPacket(LidarDevice *p_lidar,
   }
 }
 
-void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
+void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet)
+{
   LidarDevice *p_lidar = &lidars_[handle];
   LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
   LdsStamp cur_timestamp;
@@ -669,17 +670,21 @@ void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
   memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
          sizeof(cur_timestamp));
   timestamp = RawLdsStampToNs(cur_timestamp, eth_packet->timestamp_type);
-  if (timestamp >= kRosTimeMax) {
+  if (timestamp >= kRosTimeMax)
+  {
     printf("Raw EthPacket time out of range Lidar[%d]\n", handle);
     return;
   }
 
-  if (kImu != eth_packet->data_type) {
+  if (kImu != eth_packet->data_type)
+  {
     UpdateLidarInfoByEthPacket(p_lidar, eth_packet);
-    if (eth_packet->timestamp_type == kTimestampTypePps) {
+    if (eth_packet->timestamp_type == kTimestampTypePps)
+    {
       /** Whether a new sync frame */
       if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
-          (cur_timestamp.stamp < kPacketTimeGap)) {
+              (cur_timestamp.stamp < kPacketTimeGap))
+      {
         auto cur_time = std::chrono::high_resolution_clock::now();
         int64_t sync_time = cur_time.time_since_epoch().count();
         /** used receive time as timebase */
@@ -689,29 +694,37 @@ void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
     packet_statistic->last_timestamp = cur_timestamp.stamp;
 
     LidarDataQueue *p_queue = &p_lidar->data;
-    if (nullptr == p_queue->storage_packet) {
+    if (nullptr == p_queue->storage_packet)
+    {
       uint32_t queue_size = CalculatePacketQueueSize(
-          buffer_time_ms_, p_lidar->info.type, eth_packet->data_type);
+              buffer_time_ms_, p_lidar->info.type, eth_packet->data_type);
       InitQueue(p_queue, queue_size);
       printf("Lidar[%d][%s] storage queue size : %d %d\n", p_lidar->handle,
              p_lidar->info.broadcast_code, queue_size, p_queue->size);
     }
-    if (!QueueIsFull(p_queue)) {
-      QueuePushAny(p_queue, (uint8_t *)eth_packet,
-          GetEthPacketLen(eth_packet->data_type),
-          packet_statistic->timebase,
-          GetPointsPerPacket(eth_packet->data_type));
-      if (QueueUsedSize(p_queue) > p_lidar->onetime_publish_packets) {
-        if (semaphore_.GetCount() <= 0) {
+    if (!QueueIsFull(p_queue))
+    {
+      QueuePushAny(p_queue, (uint8_t *) eth_packet,
+                   GetEthPacketLen(eth_packet->data_type),
+                   packet_statistic->timebase,
+                   GetPointsPerPacket(eth_packet->data_type));
+      if (QueueUsedSize(p_queue) > p_lidar->onetime_publish_packets)
+      {
+        if (semaphore_.GetCount() <= 0)
+        {
           semaphore_.Signal();
         }
       }
     }
-  } else {
-    if (eth_packet->timestamp_type == kTimestampTypePps) {
+  }
+  else    //imu data
+  {
+    if (eth_packet->timestamp_type == kTimestampTypePps)
+    {
       /** Whether a new sync frame */
       if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) &&
-          (cur_timestamp.stamp < kPacketTimeGap)) {
+              (cur_timestamp.stamp < kPacketTimeGap))
+      {
         auto cur_time = std::chrono::high_resolution_clock::now();
         int64_t sync_time = cur_time.time_since_epoch().count();
         /** used receive time as timebase */
@@ -721,17 +734,19 @@ void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
     packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
 
     LidarDataQueue *p_queue = &p_lidar->imu_data;
-    if (nullptr == p_queue->storage_packet) {
+    if (nullptr == p_queue->storage_packet)
+    {
       uint32_t queue_size = 256;  /* fixed imu data queue size */
       InitQueue(p_queue, queue_size);
       printf("Lidar[%d][%s] imu storage queue size : %d %d\n", p_lidar->handle,
              p_lidar->info.broadcast_code, queue_size, p_queue->size);
     }
-    if (!QueueIsFull(p_queue)) {
-      QueuePushAny(p_queue, (uint8_t *)eth_packet,
-          GetEthPacketLen(eth_packet->data_type),
-          packet_statistic->imu_timebase,
-          GetPointsPerPacket(eth_packet->data_type));
+    if (!QueueIsFull(p_queue))
+    {
+      QueuePushAny(p_queue, (uint8_t *) eth_packet,
+                   GetEthPacketLen(eth_packet->data_type),
+                   packet_statistic->imu_timebase,
+                   GetPointsPerPacket(eth_packet->data_type));
     }
   }
 }