Merge branch 'master' into sensors_api

src/capture_base.h

@@ -92,6 +92,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void fixIntrinsics();
         void unfixIntrinsics();
 
+        bool hasCalibration() {return calib_size_ > 0;}
         Size getCalibSize() const;
         virtual Eigen::VectorXs getCalibration() const;
         void setCalibration(const Eigen::VectorXs& _calib);

src/problem.cpp

@@ -849,24 +849,21 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                                 cout << endl;
                             }
 
-                    if (C->isMotion() && metric)
+                    if (C->isMotion() )
                     {
-                        try
+                        CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
+                        cout << "      buffer size  :  " << CM->getBuffer().get().size() << endl;
+                        if ( metric && ! CM->getBuffer().get().empty())
                         {
-                            CaptureMotionPtr CM = std::static_pointer_cast<CaptureMotion>(C);
-                            cout << "      buffer size  :  " << CM->getBuffer().get().size() << endl;
-                            if ( CM->getCalibSize() > 0 && ! CM->getBuffer().get().empty())
+                            cout << "      delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
+                            if (CM->hasCalibration())
                             {
-                                cout << "      delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
                                 cout << "      calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
                                 cout << "      jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
                                 cout << "      calib current: (" << CM->getCalibration().transpose() << ")" << endl;
                                 cout << "      delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
                             }
                         }
-                        catch  (std::runtime_error& e)
-                        {
-                        }
                     }
 
                     if (depth >= 3)

src/processor_motion.cpp

@@ -404,11 +404,8 @@ void ProcessorMotion::integrateOneStep()
                    jacobian_delta_);
 
     // integrate Jacobian wrt calib
-    if (calib_size_ > 0)
-    {
+    if ( hasCalibration() )
         jacobian_calib_ = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_;
-        // WOLF_TRACE("jac calib: ", jacobian_calib_.row(0));
-    }
 
     // Integrate covariance
     delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
@@ -461,7 +458,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
                        motion_it->jacobian_delta_);
 
         // integrate Jacobian wrt calib
-        if (calib_size_ > 0)
+        if ( hasCalibration() )
             motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
 
         // Integrate covariance

src/processor_motion.h

@@ -397,6 +397,8 @@ class ProcessorMotion : public ProcessorBase
 
         Motion motionZero(const TimeStamp& _ts);
 
+        bool hasCalibration() {return calib_size_ > 0;}
+
     public:
         CaptureMotionPtr getOriginPtr();
         CaptureMotionPtr getLastPtr();