From 754857e070a67b3c63a828f7e11209812b052f4f Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 30 Apr 2020 15:17:53 +0200
Subject: [PATCH] hotfix set prior in hello wolf
---
demos/hello_wolf/hello_wolf.cpp | 5 ++---
demos/hello_wolf/hello_wolf_autoconf.cpp | 7 +++++--
2 files changed, 7 insertions(+), 5 deletions(-)
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 5986a7eeb..93e14f38c 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 5ab1ad1dd..1b7d7a551 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.
--
GitLab