diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index 93e14f38c41ad1639c09810b4a26631e0ddc61e5..dce04f5eff7d55bf2726075b07064a666af51f5f 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -221,19 +221,7 @@ int main() // 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() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT ! - for (auto& kf : problem->getTrajectory()->getFrameList()) - if (kf->isKeyOrAux()) - for (auto& pair_key_sb : kf->getStateBlockMap()) - if (!pair_key_sb.second->isFixed()) - pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT ! - for (auto lmk : problem->getMap()->getLandmarkList()) - for (auto& pair_key_sb : lmk->getStateBlockMap()) - if (!pair_key_sb.second->isFixed()) - pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT ! + problem->perturb(0.5); // Perturb all state blocks that are not fixed problem->print(1,0,1,0); // SOLVE again diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 39d5558eb7e2925352a78694531aefdea16ee6f1..bff8afd133c97f83f16f35fbcd35ab3db011f17f 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -206,19 +206,7 @@ int main() // 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() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT ! - for (auto& kf : problem->getTrajectory()->getFrameList()) - if (kf->isKeyOrAux()) - for (auto& pair_key_sb : kf->getStateBlockMap()) - if (pair_key_sb.second && !pair_key_sb.second->isFixed()) - pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT ! - for (auto lmk : problem->getMap()->getLandmarkList()) - for (auto& pair_key_sb : lmk->getStateBlockMap()) - if (!pair_key_sb.second->isFixed()) - pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT ! + problem->perturb(0.5); // Perturb all state blocks that are not fixed problem->print(1,0,1,0); // SOLVE again @@ -253,9 +241,8 @@ int main() * * 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 + * Lmk3 LandmarkPoint2d <-- Fac9 + * P[Est] = ( 1 2 ) * * it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) ! * @@ -274,79 +261,79 @@ int main() * The full message is explained below. * * P: wolf tree status --------------------- - Hardware - Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D - sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below) - PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D - o: Cap6 - KFrm3 // origin at Capture 6, Frame 3 - l: Cap8 - Frm4 // last at Capture 8, Frame 4 - Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing - sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below) - Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type rande and bearing - Trajectory - KFrm1 <-- Fac4 //KeyFrame 1, constrained by Factor 4 - P[Est] = ( -4.4e-12 1.5e-09 ) //Position is estimated - O[Est] = ( 2.6e-09 ) //Orientation is estimated - Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior - Ftr1 trk0 prior <-- // Position prior - m = ( 0 0 ) - Fac1 FactorBlockAbsolute --> Abs - Ftr2 trk0 prior <-- // Orientation prior - m = ( 0 ) - Fac2 FactorBlockAbsolute --> Abs - CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1. - buffer size : 0 - Cap4 CaptureRangeBearing -> Sen2 <-- - Ftr3 trk0 FeatureRangeBearing <-- - m = ( 1 1.6 ) - Fac3 RANGE BEARING --> Lmk1 - KFrm2 <-- Fac7 - P[Est] = ( 1 5.7e-09 ) - O[Est] = ( 5.7e-09 ) - CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <-- // Capture 3 of type motion odom2d from sensor 1. + Hardware + Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D + sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below) + PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D + o: Cap6 - KFrm3 // origin at Capture 6, Frame 3 + l: Cap8 - Frm4 // last at Capture 8, Frame 4 + Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing + sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below) + Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing + Trajectory + KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4 + P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated + O[Est] = ( 2.6e-09 ) // Orientation is estimated + Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior + Ftr1 trk0 prior <-- // Position prior + m = ( 0 0 ) + Fac1 FactorBlockAbsolute --> Abs + Ftr2 trk0 prior <-- // Orientation prior + m = ( 0 ) + Fac2 FactorBlockAbsolute --> Abs + CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1. + buffer size : 0 + Cap4 CaptureRangeBearing -> Sen2 <-- + Ftr3 trk0 FeatureRangeBearing <-- + m = ( 1 1.6 ) + Fac3 RANGE BEARING --> Lmk1 + KFrm2 <-- Fac7 + P[Est] = ( 1 5.7e-09 ) + O[Est] = ( 5.7e-09 ) + CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1. // Its origin is at Capture 2; Frame 1 - buffer size : 2 - delta preint : (1 0 0) - Ftr4 trk0 ProcessorOdom2d <-- - m = ( 1 0 0 ) - Fac4 FactorOdom2d --> Frm1 - Cap7 CaptureRangeBearing -> Sen2 <-- - Ftr5 trk0 FeatureRangeBearing <-- - m = ( 1.4 2.4 ) - Fac5 RANGE BEARING --> Lmk1 - Ftr6 trk0 FeatureRangeBearing <-- - m = ( 1 1.6 ) - Fac6 RANGE BEARING --> Lmk2 - KFrm3 <-- - P[Est] = ( 2 1.2e-08 ) - O[Est] = ( 6.6e-09 ) - CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <-- - buffer size : 2 - delta preint : (1 0 0) - Ftr7 trk0 ProcessorOdom2d <-- - m = ( 1 0 0 ) - Fac7 FactorOdom2d --> Frm2 - Cap9 CaptureRangeBearing -> Sen2 <-- - Ftr8 trk0 FeatureRangeBearing <-- - m = ( 1.4 2.4 ) - Fac8 RANGE BEARING --> Lmk2 - Ftr9 trk0 FeatureRangeBearing <-- - m = ( 1 1.6 ) - Fac9 RANGE BEARING --> Lmk3 - Frm4 <-- - P[Est] = ( 2 0 ) - O[Est] = ( 0 ) - CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <-- - buffer size : 1 - delta preint : (0 0 0) - Map - Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by facotrs 3 & 5 - P[Est] = ( 1 2 ) - Lmk2 LandmarkPoint2d <-- Fac6 Fac8 - P[Est] = ( 2 2 ) - Lmk3 LandmarkPoint2d <-- Fac9 - P[Est] = ( 3 2 ) - ----------------------------------------- + buffer size : 2 + delta preint : (1 0 0) + Ftr4 trk0 ProcessorOdom2d <-- + m = ( 1 0 0 ) + Fac4 FactorOdom2d --> Frm1 + Cap7 CaptureRangeBearing -> Sen2 <-- + Ftr5 trk0 FeatureRangeBearing <-- + m = ( 1.4 2.4 ) + Fac5 RANGE BEARING --> Lmk1 + Ftr6 trk0 FeatureRangeBearing <-- + m = ( 1 1.6 ) + Fac6 RANGE BEARING --> Lmk2 + KFrm3 <-- + P[Est] = ( 2 1.2e-08 ) + O[Est] = ( 6.6e-09 ) + CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <-- + buffer size : 2 + delta preint : (1 0 0) + Ftr7 trk0 ProcessorOdom2d <-- + m = ( 1 0 0 ) + Fac7 FactorOdom2d --> Frm2 + Cap9 CaptureRangeBearing -> Sen2 <-- + Ftr8 trk0 FeatureRangeBearing <-- + m = ( 1.4 2.4 ) + Fac8 RANGE BEARING --> Lmk2 + Ftr9 trk0 FeatureRangeBearing <-- + m = ( 1 1.6 ) + Fac9 RANGE BEARING --> Lmk3 + Frm4 <-- + P[Est] = ( 2 0 ) + O[Est] = ( 0 ) + CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <-- + buffer size : 1 + delta preint : (0 0 0) + Map + Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5 + P[Est] = ( 1 2 ) + Lmk2 LandmarkPoint2d <-- Fac6 Fac8 + P[Est] = ( 2 2 ) + Lmk3 LandmarkPoint2d <-- Fac9 + P[Est] = ( 3 2 ) + ----------------------------------------- * * ============= GENERAL WOLF NOTES ==================