fixing the problem on the factor lidar map, required both poses to provide gradients. Created a well defined problem wit 3 points and a simple translation

Author: g-ferrer

src/timeCont/examples/test_factor_lidar_map2pose_interpolation.cpp

@@ -121,25 +121,41 @@ int main() {
 
         SE3tc T_origin(state_origin);
         SE3tc T_target(state_target);
-        matData_t t_obs = observation(4);
+        matData_t t_obs = observation(3);
 
         std::cout << "Origin time: " << T_origin.time() << std::endl;
         std::cout << "Target time: " << T_target.time() << std::endl;
         std::cout << "Observation time: " << t_obs << std::endl;//this should be in [target,origin]. it must coincide, so carafull with sweeping this variable.
 
         //to really test it works properly, I think you want to solve this graph
-        graph.add_factor(factor);
-        //graph.print(true);
+        observation << 1.5, 1.0, 0.0,  1.0;
+        map_point << 1.5, 2.0, 0.20;
+        std::shared_ptr<Factor> factor_1 = std::make_shared<FactorLidarMap2PoseInterpolation>(
+            observation, map_point, nodeOrigin, nodeTarget, offset_lidar_imu, obsInf, Factor::robustFactorType::QUADRATIC);
+        graph.add_factor(factor_1);
+        
+        observation << -1.5, 1.0, 0.0,  1.0;
+        map_point << -1.5, 2.0, 0.20;
+        std::shared_ptr<Factor> factor_2 = std::make_shared<FactorLidarMap2PoseInterpolation>(
+            observation, map_point, nodeOrigin, nodeTarget, offset_lidar_imu, obsInf, Factor::robustFactorType::QUADRATIC);
+        graph.add_factor(factor_2);
+
+        observation << 1.5, 2.0, 0.0,  1.0;
+        map_point << 1.5, 3.0, 0.20;
+        std::shared_ptr<Factor> factor_3 = std::make_shared<FactorLidarMap2PoseInterpolation>(
+            observation, map_point, nodeOrigin, nodeTarget, offset_lidar_imu, obsInf, Factor::robustFactorType::QUADRATIC);
+        graph.add_factor(factor_3);
 
         // solve function in fg_solve.cpp. There are defauls params, but not using them
+        graph.print(true);
         auto iters = 
                 graph.solve(FGraphSolve::optimMethod::LM,
                     20,
                     1e-5,
                     1e-6,
                     true);
 
-        std::cout << "\nSolved, chi2 = " << graph.chi2() << std::endl;
+        std::cout << "\nSolved, chi2 = " << graph.chi2() << ", iters = " << iters <<std::endl;
 
         graph.print(true);
 

src/timeCont/factors/factorLidarMap2PoseInterpolation.cpp

@@ -34,7 +34,7 @@ FactorLidarMap2PoseInterpolation::FactorLidarMap2PoseInterpolation(
         const Mat3 &obsInf, 
         Factor::robustFactorType robust_type):
 
-        Factor(3,9,robust_type), obs_(observation), W_(obsInf), map_point_(map_point), point_obs_imu_frame_(Mat31::Zero()),
+        Factor(3,18,robust_type), obs_(observation), W_(obsInf), map_point_(map_point), point_obs_imu_frame_(Mat31::Zero()),
         T_offset_lidar_imu_(offset_lidar_imu), thau_taw_(0.0)
 {
         if (nodeOrigin->get_id() < nodeTarget->get_id())
@@ -62,7 +62,7 @@ void FactorLidarMap2PoseInterpolation::interpolate_pose(const SE3tc &T_origin, c
 {
         matData_t t1 = T_origin.time();
         matData_t t2 = T_target.time();
-        thau_taw_ = (t2 - t_obs)/(t2-t1);
+        thau_taw_ = (t_obs - t1)/(t2-t1);
 
         SE3 T_org = SE3(T_origin.T_SE3());
         SE3 T_tar = SE3(T_target.T_SE3());
@@ -89,8 +89,8 @@ void FactorLidarMap2PoseInterpolation::evaluate_jacobians()
 {
         J_.setZero();
         Mat3 R_taw = T_taw_.R();
-        J_.topLeftCorner<3,3>() = -thau_taw_ * R_taw * hat3(point_obs_imu_frame_);
-        J_.block<3,3>(0,3) =  thau_taw_ * R_taw;
+        J_.block<3,3>(0,9) = -thau_taw_ * R_taw * hat3(point_obs_imu_frame_);
+        J_.block<3,3>(0,9+3) =  thau_taw_ * R_taw;
 }
 
 

src/timeCont/mrob/factors/factorLidarMap2PoseInterpolation.hpp

@@ -66,7 +66,7 @@ class FactorLidarMap2PoseInterpolation : public Factor
     Mat41 obs_; // obs = [X, Y, Z, t] lidar observation
     Mat31 r_; //residual [TP - Z(:3)]
     Mat3 W_;//inverse of observation covariance (information matrix)
-    Mat<3,9> J_;//Joint Jacobian
+    Mat<3,18> J_;//Joint Jacobian
     SE3 T_taw_; // interpolated pose
     Mat31 map_point_, point_obs_imu_frame_; // map point
     SE3 T_offset_lidar_imu_; // offset from lidar to imu XXX: I think it is the other way, from IMU to LiDAR. Plase check