diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index b998b6f1c15ccb59d84e0a70ebbb6051a7301220..6fbf3ae196ae78568b75a1ef3664fbc11d5f6a73 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/hello_wolf/CMakeLists.txt
@@ -24,9 +24,15 @@ 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_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
+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)
\ No newline at end of file
+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..8fa710e3807b284298ab601a8b0f80c3dbe30ec3
--- /dev/null
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -0,0 +1,399 @@
+/**
+ * \file hello_wolf_autoconf.cpp
+ *
+ * Created on: Jul 16, 2019
+ * \author: jsola
+ */
+
+
+#include "core/common/wolf.h"
+
+#include "core/utils/params_server.hpp"
+#include "core/yaml/parser_yaml.hpp"
+
+#include "core/sensor/sensor_odom_2D.h"
+#include "core/processor/processor_odom_2D.h"
+
+#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"
+
+#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;
+
+
+ // Config file to parse
+ std::string file = "/Users/jsola/dev/wolf_lib/core/hello_wolf/hello_wolf_config.yaml";
+ // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/yaml/params_demo_quim.yaml";
+
+ // WOLF parser
+ parserYAML parser = parserYAML(file);
+ parser.parse();
+ paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+
+
+ // Wolf problem and solver
+ ProblemPtr problem = Problem::autoSetup(server);
+
+ 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);
+
+ // sensor odometer 2D
+ IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>();
+ SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
+
+ // processor odometer 2D
+ ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
+ params_odo->voting_active = true;
+ params_odo->time_tolerance = 0.1;
+ params_odo->max_time_span = 999;
+ params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement
+ params_odo->angle_turned = 999;
+ params_odo->cov_det = 999;
+ params_odo->unmeasured_perturbation_std = 0.001;
+ ProcessorBasePtr processor = problem->installProcessor("ODOM 2D", "processor odo", sensor_odo, params_odo);
+ ProcessorOdom2DPtr processor_odo = std::static_pointer_cast<ProcessorOdom2D>(processor);
+
+ // sensor Range and Bearing
+ IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
+ intrinsics_rb->noise_range_metres_std = 0.1;
+ intrinsics_rb->noise_bearing_degrees_std = 1.0;
+ SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
+
+ // processor Range and Bearing
+ ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
+ params_rb->voting_active = false;
+ params_rb->time_tolerance = 0.01;
+ ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
+
+ // SELF CALIBRATION ===================================================
+
+ // 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 ==========================================================
+
+ // 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 =======================================================
+ std::cout << std::endl;
+ WOLF_TRACE("======== BUILD PROBLEM =======")
+
+ // We'll do 3 steps of motion and landmark observations.
+
+ // STEP 1 --------------------------------------------------------------
+
+ // initialize
+ TimeStamp t(0.0); // t : 0.0
+ Vector3s x(0,0,0);
+ Matrix3s P = Matrix3s::Identity() * 0.1;
+ problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
+
+ // 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);
+ sensor_rb ->process(cap_rb); // 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);
+ sensor_odo ->process(cap_motion); // 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);
+ sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
+
+ // STEP 3 --------------------------------------------------------------
+ t += 1.0; // t : 2.0
+
+ // motion
+ cap_motion ->setTimeStamp(t);
+ sensor_odo ->process(cap_motion); // 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);
+ sensor_rb ->process(cap_rb); // 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..58cafcd2d7b62d1ceb15ee56ddce2d5b2f3578c2
--- /dev/null
+++ b/hello_wolf/hello_wolf_config.yaml
@@ -0,0 +1,43 @@
+config:
+
+ problem:
+ frame structure: "PO"
+ dimension: 2
+ prior state: [0,0,0]
+ prior cov: [[3,3],1,0,0,0,1,0,0,0,1]
+
+
+ sensors:
+ -
+ type: "ODOM 2D"
+ name: "odom"
+
+ -
+ type: "RANGE BEARING"
+ name: "rb"
+
+ 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: "odom"
+
+
+files:
+ - "/Users/jsola/dev/wolf_lib/core/lib/libhellowolf.dylib"
+ - "/Users/jsola/dev/wolf_lib/core/lib/librange_bearing.dylib"
+ - "/Users/jsola/dev/wolf_lib/core/lib/libsensor_odom.dylib"
+
+
\ No newline at end of file