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
...@@ -221,19 +221,7 @@ int main() ...@@ -221,19 +221,7 @@ int main()
// PERTURB initial guess // PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
for (auto& sen : problem->getHardware()->getSensorList()) problem->perturb(0.5); // Perturb all state blocks that are not fixed
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->print(1,0,1,0); problem->print(1,0,1,0);
// SOLVE again // SOLVE again
......
...@@ -206,19 +206,7 @@ int main() ...@@ -206,19 +206,7 @@ int main()
// PERTURB initial guess // PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
for (auto& sen : problem->getHardware()->getSensorList()) problem->perturb(0.5); // Perturb all state blocks that are not fixed
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->print(1,0,1,0); problem->print(1,0,1,0);
// SOLVE again // SOLVE again
...@@ -253,9 +241,8 @@ int main() ...@@ -253,9 +241,8 @@ int main()
* *
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as: * IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
* *
* L3 POINT 2d <-- c8 * Lmk3 LandmarkPoint2d <-- Fac9
* Est, x = ( 3 2 ) * P[Est] = ( 1 2 )
* sb: Est
* *
* it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) ! * it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
* *
...@@ -274,79 +261,79 @@ int main() ...@@ -274,79 +261,79 @@ int main()
* The full message is explained below. * The full message is explained below.
* *
* P: wolf tree status --------------------- * P: wolf tree status ---------------------
Hardware Hardware
Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D 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) 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 PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
o: Cap6 - KFrm3 // origin at Capture 6, Frame 3 o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
l: Cap8 - Frm4 // last at Capture 8, Frame 4 l: Cap8 - Frm4 // last at Capture 8, Frame 4
Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing 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) 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 Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
Trajectory Trajectory
KFrm1 <-- Fac4 //KeyFrame 1, constrained by Factor 4 KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
P[Est] = ( -4.4e-12 1.5e-09 ) //Position is estimated P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
O[Est] = ( 2.6e-09 ) //Orientation 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 Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
Ftr1 trk0 prior <-- // Position prior Ftr1 trk0 prior <-- // Position prior
m = ( 0 0 ) m = ( 0 0 )
Fac1 FactorBlockAbsolute --> Abs Fac1 FactorBlockAbsolute --> Abs
Ftr2 trk0 prior <-- // Orientation prior Ftr2 trk0 prior <-- // Orientation prior
m = ( 0 ) m = ( 0 )
Fac2 FactorBlockAbsolute --> Abs Fac2 FactorBlockAbsolute --> Abs
CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1. CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
buffer size : 0 buffer size : 0
Cap4 CaptureRangeBearing -> Sen2 <-- Cap4 CaptureRangeBearing -> Sen2 <--
Ftr3 trk0 FeatureRangeBearing <-- Ftr3 trk0 FeatureRangeBearing <--
m = ( 1 1.6 ) m = ( 1 1.6 )
Fac3 RANGE BEARING --> Lmk1 Fac3 RANGE BEARING --> Lmk1
KFrm2 <-- Fac7 KFrm2 <-- Fac7
P[Est] = ( 1 5.7e-09 ) P[Est] = ( 1 5.7e-09 )
O[Est] = ( 5.7e-09 ) O[Est] = ( 5.7e-09 )
CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <-- // Capture 3 of type motion odom2d from sensor 1. CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
// Its origin is at Capture 2; Frame 1 // Its origin is at Capture 2; Frame 1
buffer size : 2 buffer size : 2
delta preint : (1 0 0) delta preint : (1 0 0)
Ftr4 trk0 ProcessorOdom2d <-- Ftr4 trk0 ProcessorOdom2d <--
m = ( 1 0 0 ) m = ( 1 0 0 )
Fac4 FactorOdom2d --> Frm1 Fac4 FactorOdom2d --> Frm1
Cap7 CaptureRangeBearing -> Sen2 <-- Cap7 CaptureRangeBearing -> Sen2 <--
Ftr5 trk0 FeatureRangeBearing <-- Ftr5 trk0 FeatureRangeBearing <--
m = ( 1.4 2.4 ) m = ( 1.4 2.4 )
Fac5 RANGE BEARING --> Lmk1 Fac5 RANGE BEARING --> Lmk1
Ftr6 trk0 FeatureRangeBearing <-- Ftr6 trk0 FeatureRangeBearing <--
m = ( 1 1.6 ) m = ( 1 1.6 )
Fac6 RANGE BEARING --> Lmk2 Fac6 RANGE BEARING --> Lmk2
KFrm3 <-- KFrm3 <--
P[Est] = ( 2 1.2e-08 ) P[Est] = ( 2 1.2e-08 )
O[Est] = ( 6.6e-09 ) O[Est] = ( 6.6e-09 )
CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <-- CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
buffer size : 2 buffer size : 2
delta preint : (1 0 0) delta preint : (1 0 0)
Ftr7 trk0 ProcessorOdom2d <-- Ftr7 trk0 ProcessorOdom2d <--
m = ( 1 0 0 ) m = ( 1 0 0 )
Fac7 FactorOdom2d --> Frm2 Fac7 FactorOdom2d --> Frm2
Cap9 CaptureRangeBearing -> Sen2 <-- Cap9 CaptureRangeBearing -> Sen2 <--
Ftr8 trk0 FeatureRangeBearing <-- Ftr8 trk0 FeatureRangeBearing <--
m = ( 1.4 2.4 ) m = ( 1.4 2.4 )
Fac8 RANGE BEARING --> Lmk2 Fac8 RANGE BEARING --> Lmk2
Ftr9 trk0 FeatureRangeBearing <-- Ftr9 trk0 FeatureRangeBearing <--
m = ( 1 1.6 ) m = ( 1 1.6 )
Fac9 RANGE BEARING --> Lmk3 Fac9 RANGE BEARING --> Lmk3
Frm4 <-- Frm4 <--
P[Est] = ( 2 0 ) P[Est] = ( 2 0 )
O[Est] = ( 0 ) O[Est] = ( 0 )
CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <-- CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
buffer size : 1 buffer size : 1
delta preint : (0 0 0) delta preint : (0 0 0)
Map Map
Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by facotrs 3 & 5 Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
P[Est] = ( 1 2 ) P[Est] = ( 1 2 )
Lmk2 LandmarkPoint2d <-- Fac6 Fac8 Lmk2 LandmarkPoint2d <-- Fac6 Fac8
P[Est] = ( 2 2 ) P[Est] = ( 2 2 )
Lmk3 LandmarkPoint2d <-- Fac9 Lmk3 LandmarkPoint2d <-- Fac9
P[Est] = ( 3 2 ) P[Est] = ( 3 2 )
----------------------------------------- -----------------------------------------
* *
* ============= GENERAL WOLF NOTES ================== * ============= GENERAL WOLF NOTES ==================
......
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