diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index 5986a7eeb4f057b36db436584a8905d48c55a85d..93e14f38c41ad1639c09810b4a26631e0ddc61e5 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -141,9 +141,8 @@ int main() TimeStamp t(0.0); // t : 0.0 Vector3d x(0,0,0); Matrix3d P = Matrix3d::Identity() * 0.1; - //FrameBasePtr KF1 = problem->setPriorFactor(x, P, 0.5); // KF1 : (0,0,0) - //std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1); - problem->setPriorOptions("factor", 0.5, x, P); + FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0) + std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1); // SELF CALIBRATION =================================================== diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 5ab1ad1dd20a6b03411ea5cc458bde99b2b1fca8..1b7d7a551b1de4ba0a1fed374a24da716fce29df 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -9,6 +9,7 @@ // wolf core includes #include "core/common/wolf.h" #include "core/capture/capture_odom_2d.h" +#include "core/processor/processor_motion.h" #include "core/yaml/parser_yaml.h" #include "core/ceres_wrapper/ceres_manager.h" @@ -118,9 +119,11 @@ int main() // recover sensor pointers and other stuff for later use (access by sensor name) SensorBasePtr sensor_odo = problem->getSensor("sen odom"); SensorBasePtr sensor_rb = problem->getSensor("sen rb"); - TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp(); - + // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN =================================================== + TimeStamp t(0.0); + FrameBasePtr KF1 = problem->applyPriorOptions(t); + std::static_pointer_cast<ProcessorMotion>(problem->getProcessorIsMotion())->setOrigin(KF1); // SELF CALIBRATION =================================================== // These few lines control whether we calibrate some sensor parameters or not.