some improvements in tdcp factors

include/gnss/factor/factor_gnss_tdcp_3d.h

@@ -19,7 +19,12 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
 
     public:
 
-        FactorGnssTdcp3d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+        FactorGnssTdcp3d(const FeatureBasePtr& _ftr_ptr,
+                         const FrameBasePtr& _frame_other_ptr,
+                         const SensorGnssPtr& _sensor_gnss_ptr,
+                         const ProcessorBasePtr& _processor_ptr,
+                         bool _apply_loss_function,
+                         FactorStatus _status = FAC_ACTIVE) :
             FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d",
                                                                         TOP_GEOM,
                                                                         _ftr_ptr,
@@ -40,6 +45,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
                                                                         _sensor_gnss_ptr->getEnuMapYaw()),
             sensor_gnss_ptr_(_sensor_gnss_ptr)
         {
+            assert(_ftr_ptr->getMeasurement().size() == 3 && "FactorGnssTdcp3d uses 3d measurements. For FeatureGnssTdcp with also delta clock, use FactorGnssTdcpBatch instead");
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
         }
 

include/gnss/factor/factor_gnss_tdcp_batch.h

@@ -46,6 +46,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
                                                                                  _sensor_gnss_ptr->getEnuMapYaw()),
             sensor_gnss_ptr_(_sensor_gnss_ptr)
         {
+            assert(_ftr_ptr->getMeasurement().size() == 4 && "FactorGnssTdcpBatch uses 4d measurements (pos.displacement, delta clock). For FeatureGnssTdcp with only displacement, use FactorGnssTdcp3d instead");
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
         }
 
@@ -84,22 +85,23 @@ inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
     Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE2(_x2);
     Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE2(_o2);
     Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
+    Eigen::Map<Eigen::Matrix<T,3,1> > disp_residuals(_residuals);
     Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals);
 
     Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().cast<T>();
     Eigen::Matrix<T,3,3> R_ENU_MAP = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
 
-    // Expected displacement
-    Eigen::Matrix<T,3,1> expected_ECEF = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+    // Expected d
+    Eigen::Matrix<T,4,1> exp;
 
-    // position error
-    Eigen::Matrix<T,3,1> error_ECEF = expected_ECEF - getMeasurement().head<3>().cast<T>();
+    // Expected displacement in ECEF
+    exp.head(3) = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
 
     // clock error
-    T error_clock = _t2 - _t1;
+    exp(3) = *_t2 - *_t1;
 
     // Compute residual
-    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_ECEF - getMeasurement().cast<T>());
+    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (exp - getMeasurement().cast<T>());
 
     //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
     //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;