Improve case of singular cov

src/feature_base.cpp

@@ -79,9 +79,14 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
 
 void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
 {
-    Eigen::MatrixXs eps = Eigen::MatrixXs::Identity(_meas_cov.rows(), _meas_cov.cols()) * 1e-10;
+    if (_meas_cov.determinant() < Constants::EPS_SMALL)
+    {
+        Eigen::MatrixXs eps = Eigen::MatrixXs::Identity(_meas_cov.rows(), _meas_cov.cols()) * 1e-10;
+        measurement_covariance_ = _meas_cov + eps; // avoid singular covariance
+    }
+    else
+        measurement_covariance_ = _meas_cov;
 
-    measurement_covariance_ = _meas_cov + eps; // avoid singular covariance
 	measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_cov);
 }
 

src/hello_wolf/processor_range_bearing.cpp

@@ -24,12 +24,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 {
     CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture);
 
-//    // 1. create KF
-//    VectorXs    x(3);
-//    TimeStamp   ts;
-//    getProblem()->getCurrentStateAndStamp(x, ts);
-//    FrameBasePtr kf = getProblem()->emplaceFrame(KEY_FRAME, x, ts);
-//    getProblem()->keyFrameCallback(kf, shared_from_this(), 0.1);
+//    // 1. get KF
     FrameBasePtr kf = getProblem()->getLastKeyFramePtr();
 
     // 2. create Capture