preserving the marginalization function when relocalizing

CMakeLists.txt

@@ -114,7 +114,7 @@ include_directories(
 ## Declare a C++ library
 
 # Main LIO
-add_executable(${PROJECT_NAME}_estimator src/Estimator.cpp include/ikdTree/ikd_Tree.cpp src/mySolver.cpp src/PointToMapAssoc.cpp)
+add_executable(${PROJECT_NAME}_estimator src/Estimator.cpp include/ikdTree/ikd_Tree.cpp src/tmnSolver.cpp src/PointToMapAssoc.cpp)
 add_dependencies(${PROJECT_NAME}_estimator ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
 target_compile_options(${PROJECT_NAME}_estimator PRIVATE ${OpenMP_CXX_FLAGS})
 target_link_libraries(${PROJECT_NAME}_estimator ${catkin_LIBRARIES} ${CERES_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} UFO::Map Sophus::Sophus)

config/mcdviral_atv.yaml

@@ -130,7 +130,7 @@ fuse_lidar:         1
 fuse_imu:           1
 fuse_poseprop:      0
 fuse_velprop:       0
-fuse_marg:          0
+fuse_marg:          1
 
 regularize_imu:     0
 imu_init_time:      0.5

launch/run_mcdviral_slamprior.launch

@@ -20,7 +20,7 @@
       <!-- <arg  name="bag_file"  default="$(arg data_path)/ntu_day_02/*.bag"/> -->
       <!-- <arg  name="bag_file"  default="$(arg data_path)/ntu_day_10/*.bag"/> -->
       <!-- <arg  name="bag_file"  default="$(arg data_path)/ntu_night_04/*.bag"/> -->
-      <!-- <arg  name="bag_file"  default="$(arg data_path)/ntu_night_08/*.bag"/> -->
+      <arg  name="bag_file"  default="$(arg data_path)/ntu_night_08/*.bag"/>
       <!-- <arg  name="bag_file"  default="$(arg data_path)/ntu_night_13/*.bag"/> -->
       <!-- <arg  name="bag_file"  default="$(arg data_path)/kth_day_06/*.bag"/> -->
       <!-- <arg  name="bag_file"  default="$(arg data_path)/kth_day_09/*.bag"/> -->

scripts/run_mcdviral_slamprior.sh

@@ -49,9 +49,9 @@ SEQUENCES=(
             $DATASET_UNPUBLISHED/ntu_night_12
           )
 
-EPOC_DIR=/media/$USER/mySataSSD1/DATASETS/MCDVIRAL/Experiment/slict_slamprior
+EPOC_DIR=/media/$USER/mySataSSD1/DATASETS/MCDVIRAL/Experiment/slict_slamprior_2
 
-for n in {1..1}; do
+for n in {1..3}; do
 
   EPOC_DIR_N=${EPOC_DIR}/try_$n
 

src/Estimator.cpp

@@ -107,7 +107,7 @@
 #include <ikdTree/ikd_Tree.h>
 
 // myGN solver
-#include "mySolver.h"
+#include "tmnSolver.h"
 #include "factor/GyroAcceBiasFactorTMN.hpp"
 #include "factor/Point2PlaneFactorTMN.hpp"
 
@@ -189,6 +189,9 @@ class Estimator
 
     bool use_ceres = true;
 
+    // Custom solver
+    tmnSolver* mySolver = NULL;
+
     // Sliding window data (prefixed "Sw" )
     struct TimeSegment
     {
@@ -1900,6 +1903,9 @@ class Estimator
                     SwDepVsAssoc[i].clear();
                 }
 
+                // Update the priors of the prior factor
+                mySolver->RelocalizePrior(tf_Lprior_L0.getSE3());
+
                 // Change the map
                 {
                     lock_guard<mutex> lg(map_mtx);
@@ -2666,16 +2672,18 @@ class Estimator
         Vector3d XBIG(sfBig.back().back());
         Vector3d XBIA(sfBia.back().back());
 
-        // Create a solver
-        static mySolver ms(nh_ptr);
+        // Create a custom solver
+        if(mySolver == NULL)
+            mySolver = new tmnSolver(nh_ptr);
+
         string iekf_report = "";
         bool ms_success = false;
 
         // Solve the least square problem
         if (!use_ceres)
-            ms_success = ms.Solve(traj, XBIG, XBIA, prev_knot_x, curr_knot_x, swNextBase, iter,
-                                  SwImuBundle, SwCloudDskDS, SwLidarCoef,
-                                  imuSelected, featureSelected, iekf_report, report, tlog);
+            ms_success = mySolver->Solve(traj, XBIG, XBIA, prev_knot_x, curr_knot_x, swNextBase, iter,
+                                          SwImuBundle, SwCloudDskDS, SwLidarCoef,
+                                          imuSelected, featureSelected, iekf_report, report, tlog);
 
         if (ms_success)
         {

src/mySolver.cpp → src/tmnSolver.cpp

@@ -1,4 +1,4 @@
-#include "mySolver.h"
+#include "tmnSolver.h"
 
 // Define the size of the params, residual, and Jacobian
 #define XROT_SIZE  3                                        // Size of a control rot
@@ -76,10 +76,10 @@ bool GetBoolParam(ros::NodeHandlePtr &nh, string param, bool default_value)
 }
 
 // Destructor
-mySolver::~mySolver(){};
+tmnSolver::~tmnSolver(){};
 
 // Constructor
-mySolver::mySolver(ros::NodeHandlePtr &nh_) : nh(nh_)
+tmnSolver::tmnSolver(ros::NodeHandlePtr &nh_) : nh(nh_)
 {
     // IMU noise
     nh->getParam("/GYR_N", GYR_N);
@@ -126,7 +126,7 @@ mySolver::mySolver(ros::NodeHandlePtr &nh_) : nh(nh_)
 };
 
 // Change the size of variable dimension and observation dimension
-void mySolver::UpdateDimensions(int &imu_factors, int &ldr_factors, int knots)
+void tmnSolver::UpdateDimensions(int &imu_factors, int &ldr_factors, int knots)
 {
     // Local sizes of states
     XROT_BASE  = 0;
@@ -153,7 +153,7 @@ void mySolver::UpdateDimensions(int &imu_factors, int &ldr_factors, int knots)
 }
 
 // Evaluate the imu factors to update residual and jacobian.
-void mySolver::EvaluateImuFactors
+void tmnSolver::EvaluateImuFactors
 (
     PoseSplineX &traj, vector<SO3d> &xr, vector<Vector3d> &xp, Vector3d &xbg, Vector3d &xba,
     deque<deque<ImuSequence>> &SwImuBundle, vector<ImuIdx> &imuSelected, ImuBias imuBiasPrior,
@@ -207,7 +207,7 @@ void mySolver::EvaluateImuFactors
 }
 
 // Evaluate the lidar factors to update residual and jacobian
-void mySolver::EvaluateLdrFactors
+void tmnSolver::EvaluateLdrFactors
 (
     PoseSplineX &traj, vector<SO3d> &xr, vector<Vector3d> &xp,
     deque<CloudXYZIPtr> &SwCloudDskDS,
@@ -280,7 +280,7 @@ void mySolver::EvaluateLdrFactors
         *cost = pow(r.block(RESLDR_GBASE, 0, RESLDR_GSIZE, 1).norm(), 2);
 }
 
-void mySolver::EvaluatePriFactors
+void tmnSolver::EvaluatePriFactors
 (
     int                  &iter,
     map<int, int>        &prev_knot_x,
@@ -416,7 +416,7 @@ void mySolver::EvaluatePriFactors
     }
 }
 
-void mySolver::HbToJr(const MatrixXd &H, const VectorXd &b, MatrixXd &J, VectorXd &r)
+void tmnSolver::HbToJr(const MatrixXd &H, const VectorXd &b, MatrixXd &J, VectorXd &r)
 {
     Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
     Eigen::VectorXd S = Eigen::VectorXd((saes.eigenvalues().array() > 0).select(saes.eigenvalues().array(), 0));
@@ -429,8 +429,17 @@ void mySolver::HbToJr(const MatrixXd &H, const VectorXd &b, MatrixXd &J, VectorX
     r = S_inv_sqrt.asDiagonal() * saes.eigenvectors().transpose() * b;
 }
 
+
+void tmnSolver::RelocalizePrior(SE3d tf)
+{
+    for (auto &se3 : xse3_keep)
+    {
+        se3 = tf*se3;
+    }
+}
+
 // Solving the problem
-bool mySolver::Solve
+bool tmnSolver::Solve
 (
     PoseSplineX               &traj,
     Vector3d                  &BIG,

src/mySolver.h → src/tmnSolver.h

@@ -37,15 +37,15 @@ struct lidarFeaIdx
     int wdidx; int pointidx; int depth; int absidx;
 };
 
-class mySolver
+class tmnSolver
 {
 
 public:
 
     // Constructor
-    mySolver(ros::NodeHandlePtr &nh_);
+    tmnSolver(ros::NodeHandlePtr &nh_);
     // Destructor
-   ~mySolver();
+   ~tmnSolver();
 
     // Update the dimenstions of the optimization problem.
     void UpdateDimensions(int &imu_factors, int &ldr_factors, int knots);
@@ -105,8 +105,12 @@ class mySolver
         slict::TimeLog            &tlog
     );
 
+    // Utility to convert between prior forms
     void HbToJr(const MatrixXd &H, const VectorXd &b, MatrixXd &J, VectorXd &r);
 
+    // Update the priors when there is a relocalization event
+    void RelocalizePrior(SE3d tf);
+
 private:
 
     // Node handle to get information needed