Update hello_wolf_autoconf.cpp

demos/hello_wolf/hello_wolf_autoconf.cpp

@@ -116,7 +116,7 @@ int main()
     using namespace wolf;
 
 
-    WOLF_TRACE("======== CONFIGURE PROBLEM =======");
+    WOLF_INFO("======== CONFIGURE PROBLEM =======");
 
     // Config file to parse. Here is where all the problem is defined:
     std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
@@ -144,7 +144,7 @@ int main()
     // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
     TimeStamp     t(0.0);
     FrameBasePtr KF1 = problem->applyPriorOptions(t);
-    std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
+//    std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
 
     // SELF CALIBRATION ===================================================
     // These few lines control whether we calibrate some sensor parameters or not.
@@ -169,12 +169,16 @@ int main()
 
     // SET OF EVENTS: make things happen =======================================================
     std::cout << std::endl;
-    WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
+    WOLF_INFO("======== START ROBOT MOVE AND SLAM =======")
 
     // We'll do 3 steps of motion and landmark observations.
 
     // STEP 1 --------------------------------------------------------------
 
+    // move zero motion to accept the first keyframe and initialize the processor
+    CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0*motion_data, 0*motion_cov);
+    cap_motion  ->process();      // KF1 : (0,0,0)
+
     // observe lmks
     ids.resize(1); ranges.resize(1); bearings.resize(1);
     ids         << 1;                       // will observe Lmk 1
@@ -187,7 +191,7 @@ int main()
     t += 1.0;                     // t : 1.0
 
     // motion
-    CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
+    cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
     cap_motion  ->process();      // KF2 : (1,0,0)
 
     // observe lmks
@@ -220,40 +224,40 @@ int main()
     // SOLVE ================================================================
 
     // SOLVE with exact initial guess
-    WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
+    WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
     std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
-    WOLF_TRACE(report);                     // should show a very low iteration number (possibly 1)
+    WOLF_INFO(report);                     // should show a very low iteration number (possibly 1)
     problem->print(1,0,1,0);
 
     // PERTURB initial guess
-    WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
+    WOLF_INFO("======== PERTURB PROBLEM PRIORS =======")
     problem->perturb(0.5);                  // Perturb all state blocks that are not fixed
     problem->print(1,0,1,0);
 
     // SOLVE again
-    WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
+    WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
     report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
-    WOLF_TRACE(report);                     // should show a very high iteration number (more than 10, or than 100!)
+    WOLF_INFO(report);                     // should show a very high iteration number (more than 10, or than 100!)
     problem->print(1,0,1,0);
 
     // GET COVARIANCES of all states
-    WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
+    WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
     for (auto& kf : *problem->getTrajectory())
     {
         Eigen::MatrixXd cov;
         kf->getCovariance(cov);
-        WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+        WOLF_INFO("KF", kf->id(), "_cov = \n", cov);
     }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
         Eigen::MatrixXd cov;
         lmk->getCovariance(cov);
-        WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+        WOLF_INFO("L", lmk->id(), "_cov = \n", cov);
     }
     std::cout << std::endl;
 
-    WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
+    WOLF_INFO("======== FINAL PRINT FOR INTERPRETATION =======")
     problem->print(4,1,1,1);
 
     /*