Add example SAM 3D with self-calibration (#104)

joansola · artivis · commit 70066d0c4516 · 2019-08-26T09:55:56.000-04:00
* SAM 3D selfcalib works!
* const auto&amp;
* Update README.md
* small fix readme
* Fix printing pose results
diff --git a/README.md b/README.md
@@ -96,8 +96,8 @@ $ catkin build manif --cmake-args -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON
 
 ###### Generate the documentation
 ```terminal
-cd manif
-doxygen .doxygen.txt
+$ cd manif
+$ doxygen .doxygen.txt
 ```
 
 #### Use **manif** in your project
@@ -208,8 +208,8 @@ Some more general documentation and tips on the use of the library is available
 To generate the documentation on your machine, type in the terminal
 
 ```terminal
-cd manif
-doxygen .doxygen.txt
+$ cd manif
+$ doxygen .doxygen.txt
 ```
 
 and find it at `manif/doc/html/index.html`.
@@ -220,13 +220,14 @@ find in the section <a href="#publications">Publications</a>.
 ## Tutorials and application demos
 
 We provide some self-contained and self-explained executables implementing some real problems.
-Their source code is located in `[manif]/examples/`.
+Their source code is located in `manif/examples/`.
 These demos are:
 
 -   [`se2_localization.cpp`](examples/se2_localization.cpp): 2D robot localization based on fixed landmarks using SE2 as robot poses. This implements the example V.A in the paper.
 -   [`se3_localization.cpp`](examples/se3_localization.cpp): 3D robot localization based on fixed landmarks using SE3 as robot poses. This re-implements the example above but in 3D.
 -   [`se2_sam.cpp`](examples/se2_sam.cpp): 2D smoothing and mapping (SAM) with simultaneous estimation of robot poses and landmark locations, based on SE2 robot poses. This implements a the example V.B in the paper.
 -   [`se3_sam.cpp`](examples/se3_sam.cpp): 3D smoothing and mapping (SAM) with simultaneous estimation of robot poses and landmark locations, based on SE3 robot poses. This implements a 3D version of the example V.B in the paper.
+-   [`se3_sam_selfcalib.cpp`](examples/se3_sam_selfcalib.cpp): 3D smoothing and mapping (SAM) with self-calibration, with simultaneous estimation of robot poses, landmark locations and sensor parameters, based on SE3 robot poses. This implements a 3D version of the example V.C in the paper.
 
 ## Publications
 
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
@@ -8,6 +8,7 @@ add_executable(se2_sam se2_sam.cpp)
 
 add_executable(se3_localization se3_localization.cpp)
 add_executable(se3_sam se3_sam.cpp)
+add_executable(se3_sam_selfcalib se3_sam_selfcalib.cpp)
 
 set(CXX_11_EXAMPLE_TARGETS
 
@@ -24,6 +25,7 @@ set(CXX_11_EXAMPLE_TARGETS
   # SE3
   se3_localization
   se3_sam
+  se3_sam_selfcalib
 )
 
 # Link to manif
diff --git a/examples/se2_sam.cpp b/examples/se2_sam.cpp
@@ -358,7 +358,7 @@ int main()
     for (int i = 0; i < NUM_POSES; ++i)
     {
         // make measurements
-        for (auto k : pairs[i])
+        for (const auto& k : pairs[i])
         {
             // simulate measurement
             b       = landmarks_simu[k];              // lmk coordinates in world frame
@@ -393,7 +393,7 @@ int main()
     // DEBUG INFO
     cout << "prior" << std::showpos << endl;
     for (const auto& X : poses)
-        cout << "pose: " << X.log() << endl;
+        cout << "pose  : " << X.translation().transpose() << " " << X.angle() << endl;
     for (const auto& b : landmarks)
         cout << "lmk : " << b.transpose() << endl;
     cout << "-----------------------------------------------" << endl;
@@ -487,10 +487,10 @@ int main()
                 Xj = poses[j];
                 u  = controls[i];
 
-                // expectation
+                // expectation (use right-minus since motion measurements are local)
                 d  = Xj.rminus(Xi, J_d_xj, J_d_xi); // expected motion = Xj (-) Xi
 
-                // residual (use right-minus since motion measurements are local)
+                // residual
                 r.segment<DoF>(row)         = W * (d - u).coeffs(); // residual
 
                 // Jacobian of residual wrt first pose
@@ -506,7 +506,7 @@ int main()
             }
 
             // 3. evaluate measurement factors ---------------------------
-            for (auto k : pairs[i])
+            for (const auto& k : pairs[i])
             {
                 // recover related states and data
                 Xi = poses[i];
@@ -517,7 +517,7 @@ int main()
                 e       = Xi.inverse(J_ix_x).act(b, J_e_ix, J_e_b); // expected measurement = Xi.inv * bj
                 J_e_x   = J_e_ix * J_ix_x;                          // chain rule
 
-                // residual (use right-minus since sensor measurements are local)
+                // residual
                 r.segment<Dim>(row)         = S * (e - y);
 
                 // Jacobian of residual wrt pose
@@ -542,7 +542,7 @@ int main()
         dX = - (J.transpose() * J).inverse() * J.transpose() * r;
 
         // update all poses
-        for (int i = 0; i < 3; ++i)
+        for (int i = 0; i < NUM_POSES; ++i)
         {
             // we go very verbose here
             int row            = i * DoF;
@@ -578,15 +578,15 @@ int main()
     // solved problem
     cout << "posterior" << std::showpos << endl;
     for (const auto& X : poses)
-        cout << "pose: " << X.log() << endl;
+        cout << "pose  : " << X.translation().transpose() << " " << X.angle() << endl;
     for (const auto& b : landmarks)
         cout << "lmk : " << b.transpose() << endl;
     cout << "-----------------------------------------------" << endl;
 
     // ground truth
     cout << "ground truth1" << std::showpos << endl;
     for (const auto& X : poses_simu)
-        cout << "pose: " << X.log() << endl;
+        cout << "pose  : " << X.translation().transpose() << " " << X.angle() << endl;
     for (const auto& b : landmarks_simu)
         cout << "lmk : " << b.transpose() << endl;
     cout << "-----------------------------------------------" << endl;
diff --git a/examples/se3_sam.cpp b/examples/se3_sam.cpp
@@ -358,7 +358,7 @@ int main()
     for (int i = 0; i < NUM_POSES; ++i)
     {
         // make measurements
-        for (auto k : pairs[i])
+        for (const auto& k : pairs[i])
         {
             // simulate measurement
             b       = landmarks_simu[k];              // lmk coordinates in world frame
@@ -393,7 +393,7 @@ int main()
     // DEBUG INFO
     cout << "prior" << std::showpos << endl;
     for (const auto& X : poses)
-        cout << "pose: " << X.log() << endl;
+        cout << "pose  : " << X.translation().transpose() << " " << X.asSO3().log() << endl;
     for (const auto& b : landmarks)
         cout << "lmk : " << b.transpose() << endl;
     cout << "-----------------------------------------------" << endl;
@@ -487,10 +487,10 @@ int main()
                 Xj = poses[j];
                 u  = controls[i];
 
-                // expectation
+                // expectation (use right-minus since motion measurements are local)
                 d  = Xj.rminus(Xi, J_d_xj, J_d_xi); // expected motion = Xj (-) Xi
 
-                // residual (use right-minus since motion measurements are local)
+                // residual
                 r.segment<DoF>(row)         = W * (d - u).coeffs(); // residual
 
                 // Jacobian of residual wrt first pose
@@ -506,7 +506,7 @@ int main()
             }
 
             // 3. evaluate measurement factors ---------------------------
-            for (auto k : pairs[i])
+            for (const auto& k : pairs[i])
             {
                 // recover related states and data
                 Xi = poses[i];
@@ -517,7 +517,7 @@ int main()
                 e       = Xi.inverse(J_ix_x).act(b, J_e_ix, J_e_b); // expected measurement = Xi.inv * bj
                 J_e_x   = J_e_ix * J_ix_x;                          // chain rule
 
-                // residual (use right-minus since sensor measurements are local)
+                // residual
                 r.segment<Dim>(row)         = S * (e - y);
 
                 // Jacobian of residual wrt pose
@@ -542,7 +542,7 @@ int main()
         dX = - (J.transpose() * J).inverse() * J.transpose() * r;
 
         // update all poses
-        for (int i = 0; i < 3; ++i)
+        for (int i = 0; i < NUM_POSES; ++i)
         {
             // we go very verbose here
             int row             = i * DoF;
@@ -578,15 +578,15 @@ int main()
     // solved problem
     cout << "posterior" << std::showpos << endl;
     for (const auto& X : poses)
-        cout << "pose: " << X.log() << endl;
+        cout << "pose  : " << X.translation().transpose() << " " << X.asSO3().log() << endl;
     for (const auto& b : landmarks)
         cout << "lmk : " << b.transpose() << endl;
     cout << "-----------------------------------------------" << endl;
 
     // ground truth
     cout << "ground truth" << std::showpos << endl;
     for (const auto& X : poses_simu)
-        cout << "pose: " << X.log() << endl;
+        cout << "pose  : " << X.translation().transpose() << " " << X.asSO3().log() << endl;
     for (const auto& b : landmarks_simu)
         cout << "lmk : " << b.transpose() << endl;
     cout << "-----------------------------------------------" << endl;
diff --git a/examples/se3_sam_selfcalib.cpp b/examples/se3_sam_selfcalib.cpp
