Skip to content
Snippets Groups Projects
Commit fda5bcfe authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix some bugs in Hello-wolf

parent d8bbbc3f
No related branches found
No related tags found
1 merge request!213Hello wolf
...@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0; ...@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
trajectory_ptr_(), trajectory_ptr_(),
state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed. state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
is_removing_(false), is_removing_(false),
frame_id_(++frame_id_count_), frame_id_(++frame_id_count_),
type_(NON_KEY_FRAME), type_(NON_KEY_FRAME),
......
...@@ -27,7 +27,7 @@ ...@@ -27,7 +27,7 @@
#include "ceres_wrapper/ceres_manager.h" #include "ceres_wrapper/ceres_manager.h"
int main() int main()
{ {
/* /*
* ============= PROBLEM DEFINITION ================== * ============= PROBLEM DEFINITION ==================
* *
...@@ -117,12 +117,12 @@ int main() ...@@ -117,12 +117,12 @@ int main()
// sensor odometer 2D // sensor odometer 2D
IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>(); IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>();
intrinsics_odo->k_disp_to_disp = 0.1;
intrinsics_odo->k_rot_to_rot = 0.1;
SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo); SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
// processor odometer 2D // processor odometer 2D
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>(); 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->max_time_span = 999;
params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement
params_odo->angle_turned = 999; params_odo->angle_turned = 999;
...@@ -133,14 +133,16 @@ int main() ...@@ -133,14 +133,16 @@ int main()
// sensor Range and Bearing // sensor Range and Bearing
IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>(); IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
intrinsics_rb->noise_bearing_degrees_std = 1.0;
intrinsics_rb->noise_range_metres_std = 0.1; 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); SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
sensor_rb->fix();
// NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only) // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only)
// sensor_rb->getOPtr()->unfix(); sensor_rb->getOPtr()->unfix();
// processor Range and Bearing // processor Range and Bearing
ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>(); ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
params_rb->voting_active = false;
params_rb->time_tolerance = 0.01; params_rb->time_tolerance = 0.01;
ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb); ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
...@@ -164,10 +166,10 @@ int main() ...@@ -164,10 +166,10 @@ int main()
// STEP 1 -------------------------------------------------------------- // STEP 1 --------------------------------------------------------------
// initialize // initialize
TimeStamp t(0.0); TimeStamp t(0.0); // t : 0.0
Vector3s x(0,0,0); Vector3s x(0,0,0);
Matrix3s P = Matrix3s::Identity() * 0.1; Matrix3s P = Matrix3s::Identity() * 0.1;
problem->setPrior(x, P, t, 0.5); // KF1 problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
// observe lmks // observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1); ids.resize(1); ranges.resize(1); bearings.resize(1);
...@@ -175,14 +177,14 @@ int main() ...@@ -175,14 +177,14 @@ int main()
ranges << 1.0; // see drawing ranges << 1.0; // see drawing
bearings << M_PI/2; bearings << M_PI/2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); sensor_rb ->process(cap_rb); // L1 : (1,2)
// STEP 2 -------------------------------------------------------------- // STEP 2 --------------------------------------------------------------
t += 1.0; t += 1.0; // t : 1.0
// motion // motion
CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov); CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
sensor_odo ->process(cap_motion); // KF2 sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
// observe lmks // observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2); ids.resize(2); ranges.resize(2); bearings.resize(2);
...@@ -190,14 +192,14 @@ int main() ...@@ -190,14 +192,14 @@ int main()
ranges << sqrt(2.0), 1.0; // see drawing ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2; bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
// STEP 3 -------------------------------------------------------------- // STEP 3 --------------------------------------------------------------
t += 1.0; t += 1.0; // t : 2.0
// motion // motion
cap_motion ->setTimeStamp(t); cap_motion ->setTimeStamp(t);
sensor_odo ->process(cap_motion); // KF3 sensor_odo ->process(cap_motion); // KF3 : (2,0,0)
// observe lmks // observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2); ids.resize(2); ranges.resize(2); bearings.resize(2);
...@@ -205,9 +207,7 @@ int main() ...@@ -205,9 +207,7 @@ int main()
ranges << sqrt(2.0), 1.0; // see drawing ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2; bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(4,1,1,1);
// SOLVE ================================================================ // SOLVE ================================================================
...@@ -216,25 +216,30 @@ int main() ...@@ -216,25 +216,30 @@ int main()
WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======") WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1) WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(4,1,1,1); problem->print(4,0,1,1);
// PERTURB initial guess // PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
for (auto sen : problem->getHardwarePtr()->getSensorList()) for (auto sen : problem->getHardwarePtr()->getSensorList())
for (auto sb : sen->getStateBlockVec()) for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed()) if (sb && !sb->isFixed())
sb->setState(VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto kf : problem->getTrajectoryPtr()->getFrameList()) for (auto kf : problem->getTrajectoryPtr()->getFrameList())
kf->setState(Vector3s::Random() * 0.5); // We perturb A LOT ! if (kf->isKey())
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->getMapPtr()->getLandmarkList()) for (auto lmk : problem->getMapPtr()->getLandmarkList())
lmk->getPPtr()->setState(Vector2s::Random()); // We perturb A LOT ! for (auto sb : lmk->getStateBlockVec())
problem->print(4,1,1,1); if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(2,0,1,1);
// SOLVE again // SOLVE again
WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!) WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(4,1,1,1); problem->print(1,1,1,1);
// GET COVARIANCES of all states // GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment