diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 919e6f941e7bd4e830927ff7854109bd1cd2d746..59252fbbdc6a0ec80f1bb3ed91c02c5f8b51a44d 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -110,7 +110,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
for(auto sb2 : fr_ptr->getStateBlockVec())
- if (sb)
+ if (sb2)
{
state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index 81351162ead4b06d53f2170e66f6c986855f2f88..20ff4e7934a11437869a286e59affc82a359fff4 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -27,7 +27,7 @@
#include "ceres_wrapper/ceres_manager.h"
int main()
-{
+ {
/*
* ============= PROBLEM DEFINITION ==================
*
@@ -100,7 +100,7 @@ int main()
* - 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 141)
+ * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 142)
*
* (c) 2017 Joan Sola @ IRI-CSIC
*/
@@ -117,12 +117,12 @@ int main()
// sensor odometer 2D
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);
// 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;
@@ -133,16 +133,21 @@ int main()
// sensor Range and Bearing
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_bearing_degrees_std = 1.0;
SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
- // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
+ // 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->getOPtr()->unfix();
- // sensor_rb->getPPtr()->unfix(); // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
+
+ // 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->getPPtr()->unfix();
// 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);
@@ -167,10 +172,10 @@ int main()
// STEP 1 --------------------------------------------------------------
// initialize
- TimeStamp t(0.0);
+ 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
+ problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
@@ -178,14 +183,14 @@ 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);
+ sensor_rb ->process(cap_rb); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
- t += 1.0;
+ 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
+ sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -193,14 +198,14 @@ int main()
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);
+ sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
- t += 1.0;
+ t += 1.0; // t : 2.0
// motion
cap_motion ->setTimeStamp(t);
- sensor_odo ->process(cap_motion); // KF3
+ sensor_odo ->process(cap_motion); // KF3 : (2,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -209,7 +214,6 @@ int main()
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);
@@ -226,9 +230,12 @@ int main()
for (auto sen : problem->getHardwarePtr()->getSensorList())
for (auto sb : sen->getStateBlockVec())
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())
- 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 sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
@@ -269,7 +276,7 @@ int main()
*
* - Observe that all other KFs and Lmks are correct.
*
- * - Try self-calibrating the sensor orientation by uncommenting line 141 (well, around 141)
+ * - Try self-calibrating the sensor orientation by uncommenting line 142 (well, around 142)
*
*/