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

Merge branch 'devel' into 287-tris

parents 8f585337 b7ffd080
No related branches found
No related tags found
1 merge request!358WIP: Resolve "Complete state vector new data structure?"
Pipeline #5351 failed
Hide whitespace changes
Inline Side-by-side
demos/hello_wolf/hello_wolf.cpp
+ 1
− 13
View file @ 11f89cfe
......@@ -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
......
demos/hello_wolf/hello_wolf_autoconf.cpp
+ 75
− 88
View file @ 11f89cfe
......@@ -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 ==================
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment