Merge branch 'master' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf

src/problem.cpp

@@ -12,6 +12,7 @@
 #include "processor_motion.h"
 
 #include "processor_tracker.h"
+//#include "processors/processor_tracker_feature_trifocal.h"
 #include "capture_pose.h"
 
 // IRI libs includes
@@ -769,6 +770,13 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                         ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
                         if (pt)
                         {
+//                            ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
+//                            if (ptt)
+//                            {
+//                                if (ptt->getPrevOriginPtr())
+//                                    cout << "      p: C" << ptt->getPrevOriginPtr()->id() << " - " << (ptt->getPrevOriginPtr()->getFramePtr()->isKey() ? "  KF" : "  F")
+//                                    << ptt->getPrevOriginPtr()->getFramePtr()->id() << endl;
+//                            }
                             if (pt->getOriginPtr())
                                 cout << "      o: C" << pt->getOriginPtr()->id() << " - " << (pt->getOriginPtr()->getFramePtr()->isKey() ? "  KF" : "  F")
                                 << pt->getOriginPtr()->getFramePtr()->id() << endl;

src/processor_IMU.cpp

@@ -299,7 +299,7 @@ void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
     assert(_x.size() == 10 && "Wrong _x vector size");
     assert(_delta.size() == 10 && "Wrong _delta vector size");
     assert(_x_plus_delta.size() == 10 && "Wrong _x_plus_delta vector size");
-    assert(_dt >= 0 && "Time interval _Dt is negative!");
+    assert(_dt >= 0 && "Time interval _dt is negative!");
 
     _x_plus_delta = imu::composeOverState(_x, _delta, _dt);
 }

src/processor_motion.cpp

@@ -382,6 +382,7 @@ void ProcessorMotion::integrateOneStep()
 {
     // Set dt
     dt_ = updateDt();
+    assert(dt_ >= 0 && "Time interval _dt is negative!");
 
     // get vector of parameters to calibrate
     calib_ = getBuffer().getCalibrationPreint();

src/test/gtest_constraint_autodiff_distance_3D.cpp

@@ -50,7 +50,7 @@ class ConstraintAutodiffDistance3D_Test : public testing::Test
             pose2  << pos2, vquat2;
 
             dist = Vector1s(sqrt(2.0));
-            dist_cov(0.01);
+            dist_cov = Matrix1s(0.01);
 
             problem = Problem::create("PO 3D");
             ceres_manager = std::make_shared<CeresManager>(problem);
@@ -79,7 +79,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, ground_truth)
     ASSERT_NEAR(res, 0.0, 1e-8);
 }
 
-TEST_F(ConstraintAutodiffDistance3D_Test, inexact)
+TEST_F(ConstraintAutodiffDistance3D_Test, expected_residual)
 {
     Scalar measurement = 1.400;
 
@@ -93,6 +93,17 @@ TEST_F(ConstraintAutodiffDistance3D_Test, inexact)
     ASSERT_NEAR(res, res_expected, 1e-8);
 }
 
+TEST_F(ConstraintAutodiffDistance3D_Test, solve)
+{
+    Scalar measurement = 1.400;
+    f2->setMeasurement(Vector1s(measurement));
+
+    std::string report = ceres_manager->solve(2);
+
+    // Check distance between F1 and F2 positions -- must match the measurement
+    ASSERT_NEAR( (F1->getPPtr()->getState() - F2->getPPtr()->getState()).norm(), measurement, 1e-10);
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);

src/test/gtest_processor_tracker_feature_trifocal.cpp

@@ -133,7 +133,6 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
         proc_trk->process(capt_trk);
 
         CaptureBasePtr prev = proc_trk->getPrevOriginPtr();
-        WOLF_INFO("PTrifocal prev: C", (prev ? prev->id() : 0), " KF", (prev ? prev->getFramePtr()->id(): 0));
         problem->print(2,0,0,0);
 
         // Only odom creating KFs