diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 6a0f15a90885f5438c18c2cf01ed714c488ba599..b590ff73fa5724f30908ebe134524826d6c376b3 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -12,6 +12,7 @@
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
#include "core/processor/is_motion.h"
+#include "core/factor/factor_block_difference.h"
#include "core/common/time_stamp.h"
#include "core/utils/params_server.h"
@@ -457,6 +458,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+ /** \brief emplace a factor accounting for the drift of calibration/intrinsics parameters
+ * \param _cap_motion: capture containing an origin capture, the factor will be created between biases at
+ * _cap_motion time and its origin capture time
+ */
+ virtual void emplaceCalibrationDriftFactor(CaptureMotionPtr _cap_motion);
+
virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
Motion motionZero(const TimeStamp& _ts) const;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index b1572a010a6594473b51aceb949f5b21c26e8eb6..1f457b1838b4475fccbf796d99ebf6daedae636d 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -103,6 +103,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
protected:
Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise
+ Eigen::MatrixXd calib_drift_cov_; // cov matrix of calibration drift in continuous time (in [parameter unit]^2/s)
void setProblem(ProblemPtr _problem) override final;
@@ -233,6 +234,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
SizeEigen getCalibSize() const;
Eigen::VectorXd getCalibration() const;
+ Eigen::MatrixXd getCalibrationDriftCov() const;
+ void setCalibrationDriftCov(const Eigen::MatrixXd & _calib_drift_cov);
void setNoiseStd(const Eigen::VectorXd & _noise_std);
void setNoiseCov(const Eigen::MatrixXd & _noise_std);
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 8238139c3a2bf85238a9d4bacc7ac02994e987dd..05180f90df967ce5ba12665f7d693198f442e2b7 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -25,6 +25,11 @@ struct ParamsSensorDiffDrive : public ParamsSensorBase
Eigen::Vector3d prior_cov_diag;
double ticks_cov_factor;
+ // drift standard deviation
+ double radius_left_rate_stdev = 1;
+ double radius_right_rate_stdev = 1;
+ double wheel_separation_rate_stdev = 1;
+
ParamsSensorDiffDrive() = default;
ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server)
@@ -36,17 +41,25 @@ struct ParamsSensorDiffDrive : public ParamsSensorBase
set_intrinsics_prior = _server.getParam<bool>(prefix + _unique_name + "/set_intrinsics_prior");
prior_cov_diag = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag");
ticks_cov_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_cov_factor");
+
+ radius_left_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/radius_left_rate_stdev");
+ radius_right_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/radius_right_rate_stdev");
+ wheel_separation_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wheel_separation_rate_stdev");
+
}
std::string print() const
{
- return "\n" + ParamsSensorBase::print() + "\n"
+ return "\n" + ParamsSensorBase::print() + "\n"
+ "radius_left: " + std::to_string(radius_left) + "\n"
+ "radius_right: " + std::to_string(radius_right) + "\n"
+ "wheel_separation: " + std::to_string(wheel_separation) + "\n"
+ "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n"
+ "set_intrinsics_prior: " + std::to_string(set_intrinsics_prior) + "\n"
+ "prior_cov_diag: to_string not implemented yet! \n"
- + "ticks_cov_factor: " + std::to_string(ticks_cov_factor) + "\n";
+ + "ticks_cov_factor: " + std::to_string(ticks_cov_factor) + "\n";
+ + "radius_left_rate_stdev: " + std::to_string(radius_left_rate_stdev) + "\n";
+ + "radius_right_rate_stdev: " + std::to_string(radius_right_rate_stdev) + "\n";
+ + "wheel_separation_rate_stdev: " + std::to_string(wheel_separation_rate_stdev) + "\n";
}
};
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d18ae7c312f4e19a98ed1ae9522b2b06aad2fd41..5e35be115a93cdcf8543fcfcc59cc5c80ce552d1 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -86,6 +86,10 @@ void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
// emplace new feature and factor
auto new_feature = emplaceFeature(cap_target);
emplaceFactor(new_feature, cap_prev->getOriginCapture());
+
+ // emplace calibration drift factor if sensor intrinsics are dynamic
+ if (getSensor()->hasStateBlock('I') and getSensor()->isStateBlockDynamic('I'))
+ emplaceCalibrationDriftFactor(cap_target);
}
}
@@ -272,6 +276,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create motion factor and add it to the feature, and constrain to the other capture (origin)
emplaceFactor(feature_new, capture_origin );
+
+ // emplace calibration drift factor if sensor intrinsics are dynamic
+ if (getSensor()->hasStateBlock('I') and getSensor()->isStateBlockDynamic('I'))
+ emplaceCalibrationDriftFactor(capture_for_keyframe_callback);
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
@@ -283,6 +291,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto new_feature_existing = emplaceFeature(capture_existing);
emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
+ // emplace calibration drift factor if sensor intrinsics are dynamic
+ if (getSensor()->hasStateBlock('I') and getSensor()->isStateBlockDynamic('I'))
+ emplaceCalibrationDriftFactor(capture_existing);
+
// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
//
// // Modify existing feature --------
@@ -367,6 +379,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create motion factor and add it to the feature, and constrain to the other capture (origin)
emplaceFactor(feature_for_keyframe_callback, capture_origin );
+ // emplace calibration drift factor if sensor intrinsics are dynamic
+ if (getSensor()->hasStateBlock('I') and getSensor()->isStateBlockDynamic('I'))
+ emplaceCalibrationDriftFactor(capture_for_keyframe_callback);
+
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
@@ -427,7 +443,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto key_feature = emplaceFeature(last_ptr_);
// create motion factor and link it to parent feature and other frame (which is origin's frame)
- auto factor = emplaceFactor(key_feature, origin_ptr_);
+ emplaceFactor(key_feature, origin_ptr_);
+
+ // emplace calibration drift factor if sensor intrinsics are dynamic
+ if (getSensor()->hasStateBlock('I') and getSensor()->isStateBlockDynamic('I'))
+ emplaceCalibrationDriftFactor(last_ptr_);
// create a new frame
auto frame_new = FrameBase::createNonKeyFrame<FrameBase>(getTimeStamp(),
@@ -439,8 +459,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
key_frame->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- getCalibration(last_ptr_),
- getCalibration(last_ptr_),
+ getCalibration(origin_ptr_),
+ getCalibration(origin_ptr_),
last_ptr_);
// reset the new buffer
capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
@@ -942,6 +962,27 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
return nullptr;
}
+void ProcessorMotion::emplaceCalibrationDriftFactor(CaptureMotionPtr _cap_motion){
+ MatrixXd cov_continuous = getSensor()->getCalibrationDriftCov();
+ if (cov_continuous.size() == 0){
+ WOLF_TRACE("SensorBase calib_drift_cov_ not initialized")
+ }
+ else {
+ CaptureBasePtr cap_origin = _cap_motion->getOriginCapture();
+ double dt_drift = _cap_motion->getTimeStamp() - cap_origin->getTimeStamp();
+ // covariance resulting from the integration of a continuous white gaussian
+ Eigen::MatrixXd cov_drift = cov_continuous * dt_drift;
+ Eigen::VectorXd zero_drift(getSensor()->getCalibSize()); zero_drift.setZero();
+ FeatureBasePtr feat_diff = FeatureBase::emplace<FeatureBase>(_cap_motion, "ComBiasDrift", zero_drift, cov_drift,
+ FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ StateBlockPtr sb_pbc1 = cap_origin->getSensorIntrinsic();
+ StateBlockPtr sb_pbc2 = _cap_motion->getSensorIntrinsic();
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ feat_diff, feat_diff, sb_pbc1, sb_pbc2, nullptr, cap_origin
+ );
+ }
+}
+
bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
{
return true;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 82fbe684558bd9791763b70c0bbedab68514b8f0..a4792242079212faa7aeb119a602f828b98942c2 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -23,7 +23,8 @@ SensorBase::SensorBase(const std::string& _type,
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size)
+ noise_cov_(_noise_size, _noise_size),
+ calib_drift_cov_(Matrix<double,0,0>())
{
assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
@@ -214,6 +215,17 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
noise_cov_ = _noise_cov;
}
+Eigen::MatrixXd SensorBase::getCalibrationDriftCov() const
+{
+ return calib_drift_cov_;
+}
+
+void SensorBase::setCalibrationDriftCov(const Eigen::MatrixXd & _calib_drift_cov)
+{
+ calib_drift_cov_.resize(_calib_drift_cov.rows(), _calib_drift_cov.cols());
+ calib_drift_cov_ = _calib_drift_cov;
+}
+
CaptureBasePtr SensorBase::lastCapture(void) const
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index afdc43e02a8085feb99c4ae0ac1a9a4cdefe2162..258231e70b0456459e60684cc0dfe7d60e429acb 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -32,7 +32,11 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
setNoiseStd(noise_sigma);
-
+
+ // initialize continuous calibration drift covariance
+ Vector3d std_vec_drift; std_vec_drift << params_diff_drive_->radius_left_rate_stdev, params_diff_drive_->radius_right_rate_stdev, params_diff_drive_->wheel_separation_rate_stdev;
+ Matrix3d cov = std_vec_drift.array().square().matrix().asDiagonal();
+ setCalibrationDriftCov(cov);
}
SensorDiffDrive::~SensorDiffDrive()