diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 6ba1c65f79007633d68d9cef636ff6bf4c9de2c6..de2bf5f14e06cac0338a36e76e130fc074748437 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/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); /*