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

Merge branch 'devel' into 287-tris

parents c8907d60 8c691915
No related branches found
No related tags found
1 merge request!358WIP: Resolve "Complete state vector new data structure?"
Pipeline #5344 failed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    demos/hello_wolf/hello_wolf_autoconf.cpp
    + 68
    − 61
    View file @ f5980d6c
    ......@@ -275,72 +275,79 @@ int main()
    *
    * P: wolf tree status ---------------------
    Hardware
    S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
    Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
    Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
    pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
    o: C7 - F3 // origin at Capture 7, Frame 3
    l: C10 - F4 // last at Capture 10, frame 4
    S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
    Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
    Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
    pt2 RANGE BEARING // Processor 2: type Range and Bearing
    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
    KF1 <-- c3 // KeyFrame 1, constrained by Factor 3
    Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector
    sb: Est Est // State's pos and orient are estimated
    C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute
    f1 FIX <-- // Feature 1, type Fix
    m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
    c1 FIX --> A // Factor 1, type FIX, it is Absolute
    CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
    C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
    f2 RANGE BEARING <-- // Feature 2, type R+B
    m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
    c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1
    KF2 <-- c6
    Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
    sb: Est Est
    CM3 ODOM 2d -> S1 [Sta, Sta] <--
    f3 ODOM 2d <--
    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 )
    c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
    C9 RANGE BEARING -> S2 [Sta, Sta] <--
    f4 RANGE BEARING <--
    m = ( 1.41 2.36 )
    c4 RANGE BEARING --> L1
    f5 RANGE BEARING <--
    m = ( 1 1.57 )
    c5 RANGE BEARING --> L2
    KF3 <--
    Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
    sb: Est Est
    CM7 ODOM 2d -> S1 [Sta, Sta] <--
    f6 ODOM 2d <--
    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 )
    c6 ODOM 2d --> F2
    C12 RANGE BEARING -> S2 [Sta, Sta] <--
    f7 RANGE BEARING <--
    m = ( 1.41 2.36 )
    c7 RANGE BEARING --> L2
    f8 RANGE BEARING <--
    m = ( 1 1.57 )
    c8 RANGE BEARING --> L3
    F4 <--
    Est, ts=2, x = ( 0.11 -0.045 0.26 )
    sb: Est Est
    CM10 ODOM 2d -> S1 [Sta, Sta] <--
    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
    L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
    Est, x = ( 1 2 ) // L4 state is estimated, state vector
    sb: Est // L4 has 1 state block estimated
    L2 POINT 2d <-- c5 c7
    Est, x = ( 2 2 )
    sb: Est
    L3 POINT 2d <-- c8
    Est, x = ( 3 2 )
    sb: Est
    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 )
    -----------------------------------------
    *
    * ============= 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