[WIP bugged] checking buffer integration

src/motion_buffer.cpp

@@ -18,11 +18,11 @@ Motion::Motion(const TimeStamp& _ts,
         calib_size_(_jac_calib.cols()),
         ts_(_ts),
         data_(_data),
-        data_cov_(_delta_cov),
+        data_cov_(_data_cov),
         delta_(_delta),
         delta_cov_(_delta_cov),
         delta_integr_(_delta_integr),
-        delta_integr_cov_(_delta_cov),
+        delta_integr_cov_(_delta_integr_cov),
         jacobian_delta_(_jac_delta),
         jacobian_delta_integr_(_jac_delta_int),
         jacobian_calib_(_jac_calib)
@@ -117,29 +117,29 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before
 
 MatrixXs MotionBuffer::integrateCovariance() const
 {
-    Eigen::MatrixXs cov(cov_size_, cov_size_);
-    cov.setZero();
+    Eigen::MatrixXs delta_integr_cov(cov_size_, cov_size_);
+    delta_integr_cov.setZero();
     for (Motion mot : container_)
     {
-        cov = mot.jacobian_delta_integr_ * cov * mot.jacobian_delta_integr_.transpose()
-                + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
+        delta_integr_cov = mot.jacobian_delta_integr_ * delta_integr_cov * mot.jacobian_delta_integr_.transpose()
+                         + mot.jacobian_delta_        * mot.delta_cov_   * mot.jacobian_delta_.transpose();
     }
-    return cov;
+    return delta_integr_cov;
 }
 
 MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts) const
 {
-    Eigen::MatrixXs cov(cov_size_, cov_size_);
-    cov.setZero();
+    Eigen::MatrixXs delta_integr_cov(cov_size_, cov_size_);
+    delta_integr_cov.setZero();
     for (Motion mot : container_)
     {
         if (mot.ts_ > _ts)
             break;
 
-        cov = mot.jacobian_delta_integr_ * cov * mot.jacobian_delta_integr_.transpose()
-                + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
+        delta_integr_cov = mot.jacobian_delta_integr_ * delta_integr_cov * mot.jacobian_delta_integr_.transpose()
+                         + mot.jacobian_delta_        * mot.delta_cov_   * mot.jacobian_delta_.transpose();
     }
-    return cov;
+    return delta_integr_cov;
 }
 
 MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts_1, const TimeStamp _ts_2) const
@@ -158,12 +158,12 @@ MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts_1, const TimeSta
     return cov;
 }
 
-void MotionBuffer::print(bool show_delta, bool show_delta_cov, bool show_delta_int, bool show_delta_int_cov)
+void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs)
 {
     using std::cout;
     using std::endl;
 
-    if (!show_delta && !show_delta_cov && !show_delta_int && !show_delta_int_cov)
+    if (!show_data && !show_delta && !show_delta_int && !show_jacs)
     {
         cout << "Buffer state [" << container_.size() << "] : <";
         for (Motion mot : container_)
@@ -178,14 +178,28 @@ void MotionBuffer::print(bool show_delta, bool show_delta_cov, bool show_delta_i
             cout << "-- Motion (" << mot.ts_ << ")" << endl;
 //            if (show_ts)
 //                cout << "   ts: " << mot.ts_ << endl;
+            if (show_data)
+            {
+                cout << "   data: " << mot.data_.transpose() << endl;
+                cout << "   data cov: \n" << mot.data_cov_ << endl;
+            }
             if (show_delta)
+            {
                 cout << "   delta: " << mot.delta_.transpose() << endl;
-            if (show_delta_cov)
                 cout << "   delta cov: \n" << mot.delta_cov_ << endl;
+            }
             if (show_delta_int)
+            {
                 cout << "   delta integrated: " << mot.delta_integr_.transpose() << endl;
-            if (show_delta_int_cov)
-                cout << "   delta integrated cov: \n" << integrateCovariance(mot.ts_) << endl;
+                cout << "   delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
+            }
+            if (show_jacs)
+            {
+                cout << "   Jac delta: \n" << mot.jacobian_delta_ << endl;
+                cout << "   Jac delta integr: \n" << mot.jacobian_delta_integr_ << endl;
+                cout << "   Jac calib: \n" << mot.jacobian_calib_ << endl;
+
+            }
         }
     }
 }

src/motion_buffer.h

@@ -87,7 +87,7 @@ class MotionBuffer{
         MatrixXs integrateCovariance() const; // Integrate all buffer
         MatrixXs integrateCovariance(const TimeStamp& _ts) const; // Integrate up to time stamp (included)
         MatrixXs integrateCovariance(const TimeStamp& _ts_1, const TimeStamp _ts_2) const; // integrate between time stamps (both included)
-        void print(bool show_delta = 0, bool show_delta_cov = 0, bool show_delta_int = 0, bool show_delta_int_cov = 0);
+        void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0);
 
     private:
         std::list<Motion> container_;

src/problem.cpp

@@ -421,7 +421,7 @@ void Problem::removeConstraintPtr(ConstraintBasePtr _constraint_ptr)
     }
     // Remove addition notification
     if (ctr_found_it != constraint_notification_list_.end())
-        constraint_notification_list_.erase(ctr_found_it); // FIXME see if the shared_ptr is still active and messing up
+        constraint_notification_list_.erase(ctr_found_it);
     // Add remove notification
     else
         constraint_notification_list_.push_back(ConstraintNotification({REMOVE, nullptr, _constraint_ptr->id()}));

src/processor_imu.h

@@ -529,10 +529,10 @@ inline FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_mot
     // CaptureIMUPtr capt_imu = std::static_pointer_cast<CaptureIMU>(_capture_motion);
     FrameIMUPtr key_frame_ptr = std::static_pointer_cast<FrameIMU>(_related_frame);
     // create motion feature and add it to the key_capture
-    MatrixXs delta_integr_cov(integrateBufferCovariance(_capture_motion->getBuffer()));
+//    MatrixXs delta_integr_cov(integrateBufferCovariance(_capture_motion->getBuffer()));
     FeatureIMUPtr key_feature_ptr = std::make_shared<FeatureIMU>(
             _capture_motion->getBuffer().get().back().delta_integr_,
-            delta_integr_cov,
+            _capture_motion->getBuffer().get().back().delta_integr_cov_,
             key_frame_ptr->getAccBiasPtr()->getState(),
             key_frame_ptr->getGyroBiasPtr()->getState(),
             _capture_motion->getBuffer().get().back().jacobian_calib_);

src/processor_motion.cpp

@@ -258,7 +258,11 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
         new_capture->getBuffer().get().push_back(motion_interpolated);
     }
 
+
+    reintegrateBuffer(new_capture);
+
     Eigen::MatrixXs new_covariance = integrateBufferCovariance(new_capture->getBuffer());
+//    Eigen::MatrixXs new_covariance = new_capture->getBuffer().get().back().delta_integr_cov_;
 
     // check for very small covariances and fix
     // FIXME: This situation means no motion. Therefore,
@@ -336,23 +340,27 @@ void ProcessorMotion::integrateOneStep()
                           + jacobian_delta_        * delta_cov_                                 * jacobian_delta_.transpose();
 
     // push all into buffer
-    getBuffer().get().push_back(Motion( incoming_ptr_->getTimeStamp(),
-                                        incoming_ptr_->getData(),
-                                        incoming_ptr_->getDataCovariance(),
-                                        delta_,
-                                        delta_cov_,
-                                        delta_integrated_,
-                                        delta_integrated_cov_,
-                                        jacobian_delta_,
-                                        jacobian_delta_preint_,
-                                        jacobian_calib_));
+    getBuffer().get().push_back(Motion(incoming_ptr_->getTimeStamp(),
+                                   incoming_ptr_->getData(),
+                                   incoming_ptr_->getDataCovariance(),
+                                   delta_,
+                                   delta_cov_,
+                                   delta_integrated_,
+                                   delta_integrated_cov_,
+                                   jacobian_delta_,
+                                   jacobian_delta_preint_,
+                                   jacobian_calib_));
 }
 
 void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
 {
 
+    _capture_ptr->getBuffer().print(0,0,1,0);
+
     // start with empty motion
     _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFramePtr()->getTimeStamp()));
+
+    // Iterate all the buffer
     auto motion_it = _capture_ptr->getBuffer().get().begin();
     auto prev_motion_it = motion_it;
     motion_it++;
@@ -361,16 +369,30 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
         // get dt
         const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
 
+        // re-convert data to delta with the new calibration parameters
+        VectorXs calib = getCalibration();
+
+        data2delta(motion_it->data_, motion_it->data_cov_, dt, motion_it->delta_, motion_it->delta_cov_, calib, jacobian_delta_calib_);
+
         // integrate delta into delta_integr, and rewrite the buffer
         deltaPlusDelta(prev_motion_it->delta_integr_, motion_it->delta_, dt, motion_it->delta_integr_,
                        motion_it->jacobian_delta_integr_, motion_it->jacobian_delta_);
 
-        motion_it->jacobian_calib_ = jacobian_calib_;
+        // integrate Jacobian wrt calib
+        if (calib_size_ > 0)
+            motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
+
+        // Integrate covariance
+        motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * prev_motion_it->delta_integr_cov_ * motion_it->jacobian_delta_integr_.transpose()
+                                     + motion_it->jacobian_delta_        * motion_it->delta_cov_             * motion_it->jacobian_delta_.transpose();
 
         // advance in buffer
         motion_it++;
         prev_motion_it++;
     }
+
+    _capture_ptr->getBuffer().print(0,0,1,0);
+
 }
 
 Eigen::MatrixXs ProcessorMotion::integrateBufferCovariance(const MotionBuffer& _motion_buffer)

src/processor_odom_2D.h

@@ -243,9 +243,15 @@ inline FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_
 {
     // create motion feature and add it to the key_capture
     Eigen::MatrixXs delta_integr_cov(integrateBufferCovariance(_capture_motion->getBuffer()));
+    Eigen::MatrixXs delta_integr_cov_2 = _capture_motion->getBuffer().get().back().delta_integr_cov_;
+
+    WOLF_TRACE("Cov pre-integrated: \n", delta_integr_cov_2);
+    WOLF_TRACE("Cov  re-integrated: \n", delta_integr_cov);
+
     FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>(
             "MOTION",
             _capture_motion->getBuffer().get().back().delta_integr_,
+//            _capture_motion->getBuffer().get().back().delta_integr_cov_);
             delta_integr_cov.determinant() > 0 ?
             delta_integr_cov : Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_) * 1e-8);
     _capture_motion->addFeature(key_feature_ptr);