diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index 6fbf3ae196ae78568b75a1ef3664fbc11d5f6a73..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
@@ -29,10 +28,11 @@ 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})
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)
+#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
index 8fa710e3807b284298ab601a8b0f80c3dbe30ec3..32230c4cf3029162a340fe55431d818ff6efdef1 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -6,14 +6,7 @@
*/
-#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"
-
+// hello wolf local includes
#include "sensor_range_bearing.h"
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
@@ -21,6 +14,12 @@
#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()
@@ -103,49 +102,31 @@ int main()
// 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";
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string file = wolf_root + "/hello_wolf/hello_wolf_config.yaml";
- // WOLF parser
+ // parse file into params server
parserYAML parser = parserYAML(file);
parser.parse();
paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
- // Wolf problem and solver
- ProblemPtr problem = Problem::autoSetup(server);
+ // Wolf problem
+ ProblemPtr problem = Problem::autoSetup(server);
+// ProblemPtr problem = Problem::autoSetup(file);
+
+ problem->print(4,1,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
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);
+ 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 ===================================================
@@ -176,10 +157,7 @@ int main()
// 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)
+ TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp();
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
@@ -187,37 +165,40 @@ int main()
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)
+ 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);
- sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
+ 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);
- sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
+ cap_rb ->process(); // 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)
+ 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);
- sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+ cap_rb ->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+
problem->print(1,0,1,0);
// SOLVE ================================================================
diff --git a/hello_wolf/hello_wolf_config.yaml b/hello_wolf/hello_wolf_config.yaml
index 58cafcd2d7b62d1ceb15ee56ddce2d5b2f3578c2..13472f71bde49c1e5f46272ea248274f8e77a4aa 100644
--- a/hello_wolf/hello_wolf_config.yaml
+++ b/hello_wolf/hello_wolf_config.yaml
@@ -1,10 +1,12 @@
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]
+ 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:
@@ -13,8 +15,10 @@ config:
name: "odom"
-
- type: "RANGE BEARING"
- name: "rb"
+ type: "RANGE BEARING"
+ name: "rb"
+ noise range metres std: 0.1
+ noise bearing degrees std: 0.5
processors:
-
@@ -30,14 +34,12 @@ config:
unmeasured_perturbation_std: 0.001
-
- type: "RANGE BEARING"
- name: "rb"
- sensor name: "odom"
+ type: "RANGE BEARING"
+ name: "rb"
+ sensor name: "rb"
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"
+# - "/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 e63438750ef6ca1477a7287a03f840dc48ba596a..c379db030ca56f907560bcbb695e297cd77e7cc9 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/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index 74f2c0e46cec59d1be7ba297547f927223d80c94..878f08078fc69a5e20c27c99d2b17744a39db096 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 587259b40fd486015460a9d1e29c7573b589dcb9..0a46b466e5470fcd89cd6874bdfb73dab8f5fae6 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";
}
};