diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index b998b6f1c15ccb59d84e0a70ebbb6051a7301220..8d1114b65c1f81eb7fecf443abeeff16c723d6d3 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/hello_wolf/CMakeLists.txt
@@ -13,7 +13,6 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
)
SET(SRCS_HELLOWOLF
-# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp
${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
@@ -24,9 +23,16 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
add_library(hellowolf SHARED ${SRCS_HELLOWOLF})
TARGET_LINK_LIBRARIES(hellowolf ${PROJECT_NAME})
+
ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf)
-add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
-TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
-add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
-TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME} hellowolf)
\ No newline at end of file
+
+ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
+#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME})
+TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME} hellowolf)
+
+#add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
+#TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
+#
+#add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
+#TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME} hellowolf)
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..502c38e2ac48ff2539ab95f8ca21c1b674973b1f
--- /dev/null
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -0,0 +1,381 @@
+/**
+ * \file hello_wolf_autoconf.cpp
+ *
+ * Created on: Jul 16, 2019
+ * \author: jsola
+ */
+
+
+// hello wolf local includes
+#include "sensor_range_bearing.h"
+#include "processor_range_bearing.h"
+#include "capture_range_bearing.h"
+#include "feature_range_bearing.h"
+#include "factor_range_bearing.h"
+#include "landmark_point_2D.h"
+
+// wolf core includes
+#include "core/common/wolf.h"
+#include "core/internal/config.h"
+#include "core/yaml/parser_yaml.hpp"
+#include "core/sensor/sensor_odom_2D.h"
+#include "core/processor/processor_odom_2D.h"
+#include "core/ceres_wrapper/ceres_manager.h"
+
+int main()
+ {
+ /*
+ * ============= PROBLEM DEFINITION ==================
+ *
+ * We have a planar robot with a range-and-bearing sensor 'S' mounted at its front-left corner, looking forward:
+ *
+ * ^ Y
+ * |
+ * ------------------S-> sensor at location (1,1) and orientation 0 degrees, that is, at pose (1,1,0).
+ * | | |
+ * | +--------|--> X robot axes X, Y
+ * | |
+ * -------------------
+ *
+ * The robot performs a straight trajectory with 3 keyframes 'KF', and observes 3 landmarks 'L'.
+ *
+ * The 3 robot keyframes, the 3 resulting sensor poses, and the 3 landmark positions can be sketched as follows
+ *
+ * (1,2) (2,2) (3,2) landmark positions in world frame
+ * L1 L2 L3 LANDMARKS
+ * | \ | \ |
+ * | \ | \ |
+ * | \ | \ |
+ * (1,1,0) (2,1,0) (3,1,0) sensor poses in world frame
+ * S-> S-> S-> SENSOR
+ * (1,1,0) (1,1,0) (1,1,0) sensor poses in robot frame
+ * / / /
+ * / / /
+ * / / /
+ * KF1->---KF2->---KF3-> KEYFRAMES -- robot poses
+ * (0,0,0) (1,0,0) (2,0,0) keyframe poses in world frame
+ * |
+ * * prior Initial robot pose in world frame
+ * (0,0,0)
+ *
+ * where:
+ * the links '--' are the motion factors,
+ * the links '\' and '|' are the landmark measurement factors, and
+ * the links '/' are the robot-to-sensor transforms.
+ *
+ * That is:
+ * - KFs have ids '1', '2', '3'
+ * - All KFs look East, so all theta = 0
+ * - KFs are at poses (0,0, 0), (1,0, 0), and (2,0, 0)
+ * - We set a prior at (0,0,0) on KF1 to render the system observable
+ * - The range-and-bearing sensor is mounted at pose (1,1, 0) w.r.t. the robot's origin
+ * - Lmks have ids '1', '2', '3'
+ * - Lmks are at positions (1,2), (2,2), (3,2)
+ * - Observations have ranges 1 or sqrt(2)
+ * - Observations have bearings pi/2 or 3pi/4
+ *
+ * The robot starts at (0,0,0) with a map with no previously known landmarks.
+ * At each keyframe, it does:
+ * - Create a motion factor to the previous keyframe
+ * - Measure some landmarks. For each measurement,
+ * - If the landmark is unknown, add it to the map and create an observation factor
+ * - If the landmark is known, create an observation factor
+ *
+ * The landmarks observed at each keyframe are as follows:
+ * - KF1: lmk 1
+ * - KF2: lmks 1 and 2
+ * - KF3: lmks 2 and 3
+ *
+ * After 3 keyframes, the problem is solved and the robot poses and landmark positions are optimized:
+ * - First, using the exact values as initial guess for the solver
+ * - Second, using random values
+ * Both solutions must produce the same exact values as in the sketches above.
+ *
+ * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 151)
+ *
+ * (c) 2017 Joan Sola @ IRI-CSIC
+ */
+
+ // SET PROBLEM =======================================================
+
+ using namespace wolf;
+
+
+ WOLF_TRACE("======== CONFIGURE PROBLEM =======")
+
+ // Config file to parse. Here is where all the problem is defined:
+ std::string file = std::string(_WOLF_ROOT_DIR) + "/hello_wolf/hello_wolf_config.yaml";
+
+ // parse file into params server: each param will be retrievable from this params server:
+ ParserYAML parser = ParserYAML(file);
+ parser.parse();
+ ParamsServer server = ParamsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+
+ // Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
+ ProblemPtr problem = Problem::autoSetup(server);
+
+ // Print problem to see its status before processing any sensor data
+ problem->print(4,0,1,0);
+
+ // recover sensor pointers for later use (access by sensor name)
+ SensorBasePtr sensor_odo = problem->getSensor("odom");
+ SensorBasePtr sensor_rb = problem->getSensor("rb");
+
+ // Solver. Configure a Ceres solver
+ ceres::Solver::Options options;
+ options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
+ CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
+
+
+
+ // SELF CALIBRATION ===================================================
+ // These few lines control whether we calibrate some sensor parameters or not.
+
+ // NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
+ // Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
+ sensor_rb->getO()->unfix();
+
+ // NOTE: SELF-CALIBRATION OF SENSOR POSITION
+ // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
+ // sensor_rb->getP()->unfix();
+
+ // CONFIGURE input data (motion and measurements) ==============================================
+
+ // Motion data
+ Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+ Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
+
+ // landmark observations data
+ VectorXi ids;
+ VectorXs ranges, bearings;
+
+ // SET OF EVENTS: make things happen =======================================================
+ std::cout << std::endl;
+ WOLF_TRACE("======== BUILD PROBLEM =======")
+
+ // We'll do 3 steps of motion and landmark observations.
+
+ // STEP 1 --------------------------------------------------------------
+
+ // initialize
+ TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp();
+
+ // observe lmks
+ ids.resize(1); ranges.resize(1); bearings.resize(1);
+ ids << 1; // will observe Lmk 1
+ ranges << 1.0; // see drawing
+ bearings << M_PI/2;
+ CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb->process(); // L1 : (1,2)
+
+ // STEP 2 --------------------------------------------------------------
+ t += 1.0; // t : 1.0
+
+ // motion
+ CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
+ cap_motion ->process(); // KF2 : (1,0,0)
+
+ // observe lmks
+ ids.resize(2); ranges.resize(2); bearings.resize(2);
+ ids << 1, 2; // will observe Lmks 1 and 2
+ ranges << sqrt(2.0), 1.0; // see drawing
+ bearings << 3*M_PI/4, M_PI/2;
+
+ cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb ->process(); // L1 : (1,2), L2 : (2,2)
+
+ // STEP 3 --------------------------------------------------------------
+ t += 1.0; // t : 2.0
+
+ // motion
+ cap_motion ->setTimeStamp(t);
+ cap_motion ->process(); // KF3 : (2,0,0)
+
+ // observe lmks
+ ids.resize(2); ranges.resize(2); bearings.resize(2);
+ ids << 2, 3; // will observe Lmks 2 and 3
+ ranges << sqrt(2.0), 1.0; // see drawing
+ bearings << 3*M_PI/4, M_PI/2;
+
+ cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb ->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+
+ problem->print(1,0,1,0);
+
+ // SOLVE ================================================================
+
+ // SOLVE with exact initial guess
+ WOLF_TRACE("======== 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)
+ problem->print(1,0,1,0);
+
+ // PERTURB initial guess
+ WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
+ for (auto sen : problem->getHardware()->getSensorList())
+ for (auto sb : sen->getStateBlockVec())
+ if (sb && !sb->isFixed())
+ sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ for (auto kf : problem->getTrajectory()->getFrameList())
+ if (kf->isKeyOrAux())
+ for (auto sb : kf->getStateBlockVec())
+ if (sb && !sb->isFixed())
+ sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ for (auto sb : lmk->getStateBlockVec())
+ if (sb && !sb->isFixed())
+ sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ problem->print(1,0,1,0);
+
+ // SOLVE again
+ WOLF_TRACE("======== 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!)
+ problem->print(1,0,1,0);
+
+ // GET COVARIANCES of all states
+ WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
+ ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ for (auto kf : problem->getTrajectory()->getFrameList())
+ if (kf->isKeyOrAux())
+ {
+ Eigen::MatrixXs cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ Eigen::MatrixXs cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
+ std::cout << std::endl;
+
+ WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
+ problem->print(4,1,1,1);
+
+ /*
+ * ============= FIRST COMMENT ON THE RESULTS ==================
+ *
+ * IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
+ *
+ * L3 POINT 2D <-- c8
+ * Est, x = ( 3 2 )
+ * sb: Est
+ *
+ * it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
+ *
+ * Side notes:
+ *
+ * - Observe that all other KFs and Lmks are correct.
+ *
+ * - Try self-calibrating the sensor orientation by uncommenting line 151 (well, around 151)
+ *
+ */
+
+ /*
+ * ============= DETAILED DESCRIPTION OF THE PRINTED RESULT ==================
+ *
+ * The line problem->print(4,1,1,1) produces a printout of the status of the WOLF problem.
+ * The full message is explained below.
+ *
+ * P: wolf tree status ---------------------
+ Hardware
+ S1 ODOM 2D // Sensor 1, type ODOMETRY 2D.
+ Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
+ Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
+ pm1 ODOM 2D // Processor 1, type ODOMETRY 2D
+ o: C7 - F3 // origin at Capture 7, Frame 3
+ l: C10 - F4 // last at Capture 10, frame 4
+ S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
+ Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
+ Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
+ pt2 RANGE BEARING // Processor 2: type Range and Bearing
+ Trajectory
+ KF1 <-- c3 // KeyFrame 1, constrained by Factor 3
+ Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector
+ sb: Est Est // State's pos and orient are estimated
+ C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute
+ f1 FIX <-- // Feature 1, type Fix
+ m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
+ c1 FIX --> A // Factor 1, type FIX, it is Absolute
+ CM2 ODOM 2D -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
+ C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
+ f2 RANGE BEARING <-- // Feature 2, type R+B
+ m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
+ c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1
+ KF2 <-- c6
+ Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
+ sb: Est Est
+ CM3 ODOM 2D -> S1 [Sta, Sta] <--
+ f3 ODOM 2D <--
+ m = ( 1 0 0 )
+ c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
+ C9 RANGE BEARING -> S2 [Sta, Sta] <--
+ f4 RANGE BEARING <--
+ m = ( 1.41 2.36 )
+ c4 RANGE BEARING --> L1
+ f5 RANGE BEARING <--
+ m = ( 1 1.57 )
+ c5 RANGE BEARING --> L2
+ KF3 <--
+ Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
+ sb: Est Est
+ CM7 ODOM 2D -> S1 [Sta, Sta] <--
+ f6 ODOM 2D <--
+ m = ( 1 0 0 )
+ c6 ODOM 2D --> F2
+ C12 RANGE BEARING -> S2 [Sta, Sta] <--
+ f7 RANGE BEARING <--
+ m = ( 1.41 2.36 )
+ c7 RANGE BEARING --> L2
+ f8 RANGE BEARING <--
+ m = ( 1 1.57 )
+ c8 RANGE BEARING --> L3
+ F4 <--
+ Est, ts=2, x = ( 0.11 -0.045 0.26 )
+ sb: Est Est
+ CM10 ODOM 2D -> S1 [Sta, Sta] <--
+ Map
+ L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
+ Est, x = ( 1 2 ) // L4 state is estimated, state vector
+ sb: Est // L4 has 1 state block estimated
+ L2 POINT 2D <-- c5 c7
+ Est, x = ( 2 2 )
+ sb: Est
+ L3 POINT 2D <-- c8
+ Est, x = ( 3 2 )
+ sb: Est
+ -----------------------------------------
+ *
+ * ============= GENERAL WOLF NOTES ==================
+ *
+ * Explanatory notes, general to the Wolf architecture design:
+ *
+ * (1): Sensor params (extrinsics and intrinsics) can be declared Dynamic or Static
+ * Static: they do not change with time --> stored in the Sensor
+ * Dynamic: they change with time --> stored in each Capture
+ *
+ * (2): General params can be declared Fixed or Estimated
+ * Fixed: they are used as constant values, never estimated
+ * Estimated: they are estimated by the solver iteratively
+ *
+ * Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This produces 4 combinations:
+ *
+ * 1 Fixed + Static : general case of calibrated sensor.
+ * Example: rigidly fixed sensor with calibrated parameters
+ * This is the case for the current example (the SensorRangeBearing we use is like this)
+ *
+ * 2 Estimated + Static : Wolf performs self-calibration.
+ * Example: extrinsics self-calibration of a camera
+ *
+ * 3 Fixed + Dynamic : calibrated but variable parameters.
+ * Example: pan and tilt camera extrinsics, known at every time through precise encoders
+ *
+ * 4 Estimated + Dynamic : Wolf will track those sensor parameters that evolve with time.
+ * Example: IMU bias
+ *
+ */
+
+ return 0;
+}
diff --git a/hello_wolf/hello_wolf_config.yaml b/hello_wolf/hello_wolf_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..13472f71bde49c1e5f46272ea248274f8e77a4aa
--- /dev/null
+++ b/hello_wolf/hello_wolf_config.yaml
@@ -0,0 +1,45 @@
+config:
+
+ problem:
+ frame structure: "PO"
+ dimension: 2
+ prior timestamp: 0.0
+ prior state: [0,0,0]
+ prior cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ prior time tolerance: 0.1
+
+
+ sensors:
+ -
+ type: "ODOM 2D"
+ name: "odom"
+
+ -
+ type: "RANGE BEARING"
+ name: "rb"
+ noise range metres std: 0.1
+ noise bearing degrees std: 0.5
+
+ processors:
+ -
+ type: "ODOM 2D"
+ name: "odom"
+ sensor name: "odom"
+ voting active: true
+ time tolerance: 0.1
+ max time span: 999
+ dist_traveled: 0.95
+ angle_turned: 999
+ cov_det: 999
+ unmeasured_perturbation_std: 0.001
+
+ -
+ type: "RANGE BEARING"
+ name: "rb"
+ sensor name: "rb"
+
+
+files:
+# - "/Users/jsola/dev/wolf_lib/core/lib/libhellowolf.dylib"
+
+
\ No newline at end of file
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 7cf2c04facbba1b1523d533a05f83ddba3e6a853..54bbb3aec498cacc736321f77abc127a24414106 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -52,7 +52,6 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
- // kf->addCapture(capture_rb);
capture_rb->link(kf);
// 3. explore all observations in the capture
@@ -76,40 +75,33 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
{
// new landmark:
// - create landmark
- // lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
- // getProblem()->getMap()->addLandmark(lmk);
// - add to known landmarks
known_lmks.emplace(id, lmk);
}
// 5. create feature
Vector2s rb(range,bearing);
- auto ftr = std::make_shared<FeatureRangeBearing>(rb,
- getSensor()->getNoiseCov());
- // capture_rb->addFeature(ftr);
- FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov());
+ auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
+ rb,
+ getSensor()->getNoiseCov());
// 6. create factor
auto prc = shared_from_this();
- // auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
- // lmk,
- // prc,
- // false,
- // FAC_ACTIVE);
- auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb,
+ auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
+ capture_rb,
lmk,
prc,
false,
FAC_ACTIVE);
- // ftr->addFactor(ctr);
- // lmk->addConstrainedBy(ctr);
}
}
-ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
+ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr _sen_ptr)
{
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr);
ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
@@ -124,8 +116,8 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
}
ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name,
- const ParamsServer& _server,
- const SensorBasePtr _sensor_ptr)
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr)
{
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr);
ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>();
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index ff99fddfba05ce919949b115c74c6b73a429b633..2bc4767c14f0c9619534d3bd6f6ae1dc5d4f506b 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -22,19 +22,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing);
struct ProcessorParamsRangeBearing : public ProcessorParamsBase
{
// We do not need special parameters, but in case you need they should be defined here.
- ProcessorParamsRangeBearing()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
- ProcessorParamsRangeBearing(std::string _unique_name, const ParamsServer& _server):
- ProcessorParamsBase(_unique_name, _server)
- {
- //
- }
- std::string print()
- {
- return "\n" + ProcessorParamsBase::print();
- }
+ ProcessorParamsRangeBearing()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ProcessorParamsRangeBearing(std::string _unique_name, const ParamsServer& _server) :
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ //
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print();
+ }
};
using namespace Eigen;
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index 7be1171b9b6cf3fdbd46104bad76e509f96ddf82..964b909338dfd1062012c73e1aaf0a7b33999295 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/hello_wolf/sensor_range_bearing.cpp
@@ -45,10 +45,10 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, //
const ParamsServer& _server)
{
- Eigen::Vector2s noise_std(0.1,1.0);
+ IntrinsicsRangeBearing intr(_unique_name, _server);
+ Eigen::Vector2s noise_std(intr.noise_range_metres_std, intr.noise_bearing_degrees_std);
SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std);
sensor->setName(_unique_name);
- // sensor->node_name_ = _unique_name;
return sensor;
}
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index f3ed332fffdbab37fd51072eb0b58cee82f43d05..161818df65a56f5b2df02d9a66327a71e80a63a0 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -26,12 +26,13 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
IntrinsicsRangeBearing(std::string _unique_name, const ParamsServer& _server):
IntrinsicsBase(_unique_name, _server)
{
- noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std", "0.05");
- noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std", "0.5");
+ noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise range metres std", "0.05");
+ noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise bearing degrees std", "0.5");
}
std::string print()
{
- return "\n" + IntrinsicsBase::print() + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ return "" + IntrinsicsBase::print()
+ + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
}
};
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 5bb0732f9a6e46acdc53658ebc881786ff48f2e2..7c7c1cd0a431d4c4c0053598fa9567854cc09742 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -21,18 +21,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
struct ProcessorParamsOdom2D : public ProcessorParamsMotion
{
- Scalar cov_det = 1.0; // 1 rad^2
- ProcessorParamsOdom2D() = default;
- ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server):
- ProcessorParamsMotion(_unique_name, _server)
- {
- cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
- }
- std::string print()
- {
- return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n";
- }
+ Scalar cov_det = 1.0; // 1 rad^2
+ ProcessorParamsOdom2D() = default;
+ ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ProcessorParamsMotion(_unique_name, _server)
+ {
+ cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n";
+ }
};
+
class ProcessorOdom2D : public ProcessorMotion
{
public:
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
index 34d2f8734625a32bca587d45a34844a2a898e5c7..94e8f1a8468a89749685ad471ee9b234e392f84d 100644
--- a/include/core/utils/params_server.hpp
+++ b/include/core/utils/params_server.hpp
@@ -1,20 +1,26 @@
#ifndef PARAMS_SERVER_HPP
#define PARAMS_SERVER_HPP
+
+#include "core/utils/converter.h"
+//#include "core/yaml/parser_yaml.hpp"
+
#include <vector>
#include <regex>
#include <map>
-#include "core/utils/converter.h"
+
namespace wolf{
class ParamsServer{
struct ParamsInitSensor{
std::string _type;
std::string _name;
};
+
struct ParamsInitProcessor{
std::string _type;
std::string _name;
std::string _name_assoc_sensor;
};
+
std::map<std::string, std::string> _params;
std::map<std::string,ParamsInitSensor> _paramsSens;
std::map<std::string,ParamsInitProcessor> _paramsProc;
@@ -42,21 +48,26 @@ public:
~ParamsServer(){
//
}
+
void print(){
for(auto it : _params)
std::cout << it.first << "~~" << it.second << std::endl;
}
+
void addInitParamsSensor(std::string type, std::string name){
ParamsInitSensor params = {type, name};
_paramsSens.insert(std::pair<std::string, ParamsInitSensor>(type + "/" + name + "/", params));
}
+
void addInitParamsProcessor(std::string type, std::string name, std::string name_assoc_sensor){
ParamsInitProcessor params = {type, name, name_assoc_sensor};
_paramsProc.insert(std::pair<std::string, ParamsInitProcessor>(type + "/" + name + "/", params));
}
+
void addParam(std::string key, std::string value){
_params.insert(std::pair<std::string, std::string>(key, value));
}
+
template<typename T>
T getParam(std::string key, std::string def_value) const {
if(_params.find(key) != _params.end()){
@@ -65,6 +76,7 @@ public:
return converter<T>::convert(def_value);
}
}
+
template<typename T>
T getParam(std::string key) const {
if(_params.find(key) != _params.end()){
@@ -73,16 +85,20 @@ public:
throw std::runtime_error("The following key: '" + key + "' has not been found in the parameters server and no default value was provided.");
}
}
+
std::vector<ParamsInitSensor> getSensors(){
std::vector<ParamsInitSensor> rtn = std::vector<ParamsInitSensor>();
std::transform(this->_paramsSens.begin(), this->_paramsSens.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitSensor> v){return v.second;});
return rtn;
}
+
std::vector<ParamsInitProcessor> getProcessors(){
std::vector<ParamsInitProcessor> rtn = std::vector<ParamsInitProcessor>();
std::transform(this->_paramsProc.begin(), this->_paramsProc.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitProcessor> v){return v.second;});
return rtn;
}
};
+
}
-#endif
\ No newline at end of file
+
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 226d3cca16e7eb31ebb4a294c003500c86b51dbb..3af5c1630da3e1bbeba9ec0ec4c6cc8ab5c63cfb 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -1,8 +1,11 @@
#ifndef PARSER_YAML_HPP
#define PARSER_YAML_HPP
-#include "yaml-cpp/yaml.h"
+
#include "core/utils/converter.h"
#include "core/common/wolf.h"
+
+#include "yaml-cpp/yaml.h"
+
#include <vector>
#include <regex>
#include <map>
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 4565bb62293ec1ba1f1df56b74f32dc3e0124200..eab55cc02cf4d12045fddf7813c4d7cdf87f35b4 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -73,14 +73,19 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
p->setup();
return p->shared_from_this();
}
+
ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
- std::string frame_structure = _server.getParam<std::string>("problem/frame structure", "PO");
- int dim = _server.getParam<int>("problem/dimension", "2");
- auto p = Problem::create(frame_structure, dim);
+ // Problem structure and dimension
+ std::string frame_structure = _server.getParam<std::string> ("problem/frame structure", "PO");
+ int dim = _server.getParam<int> ("problem/dimension", "2");
+ auto problem = Problem::create(frame_structure, dim);
// cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
+ WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
+
+ // Load plugins
auto loaders = std::vector<Loader*>();
for(auto it : _server.getParam<std::vector<std::string>>("files")) {
WOLF_TRACE("Loading file " + it)
@@ -88,27 +93,37 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
l->load();
loaders.push_back(l);
}
- WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension {" + std::to_string(dim) + "}");
- Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior state");
- Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior cov");
- Scalar time_tolerance = _server.getParam<Scalar>("problem/time tolerance");
- auto ts = TimeStamp();
- WOLF_TRACE("prior timestamp:\n",ts);
- WOLF_TRACE("prior state:\n", prior_state.transpose());
- WOLF_TRACE("prior covariance:\n", prior_cov);
- WOLF_TRACE("prior time tolerance:\n", time_tolerance);
- p->setPrior(prior_state, prior_cov, ts, time_tolerance);
+
+ // Install sensors and processors
auto sensorMap = std::map<std::string, SensorBasePtr>();
auto procesorMap = std::map<std::string, ProcessorBasePtr>();
- for(auto s : _server.getSensors()){
- sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, _server)));
+
+ for(auto sen : _server.getSensors()){
+ sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen._name, problem->installSensor(sen._type, sen._name, _server)));
}
- for(auto s : _server.getProcessors()){
- procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, _server)));
+
+ for(auto prc : _server.getProcessors()){
+ procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc._name, problem->installProcessor(prc._type, prc._name, prc._name_assoc_sensor, _server)));
}
- return p;
+
+ // Prior
+ Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior state");
+ Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior cov");
+ Scalar prior_time_tolerance = _server.getParam<Scalar>("problem/prior time tolerance");
+ Scalar prior_ts = _server.getParam<Scalar>("problem/prior timestamp");
+
+ WOLF_TRACE("prior timestamp:\n" , prior_ts);
+ WOLF_TRACE("prior state:\n" , prior_state.transpose());
+ WOLF_TRACE("prior covariance:\n" , prior_cov);
+ WOLF_TRACE("prior time tolerance:\n", prior_time_tolerance);
+
+ problem->setPrior(prior_state, prior_cov, prior_ts, prior_time_tolerance);
+
+ // Done
+ return problem;
}
+
Problem::~Problem()
{
// WOLF_DEBUG("destructed -P");