Merge pull request #1808 from borglab/python-localization-example

Author: varunagrawal

gtsam_unstable/gtsam_unstable.i

@@ -671,6 +671,21 @@ class AHRS {
   //void print(string s) const;
 };
 
+#include <gtsam_unstable/slam/PartialPriorFactor.h>
+template <T = {gtsam::Pose2, gtsam::Pose3}>
+virtual class PartialPriorFactor : gtsam::NoiseModelFactor {
+  PartialPriorFactor(gtsam::Key key, size_t idx, double prior,
+                     const gtsam::noiseModel::Base* model);
+  PartialPriorFactor(gtsam::Key key, const std::vector<size_t>& indices,
+                     const gtsam::Vector& prior,
+                     const gtsam::noiseModel::Base* model);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  const gtsam::Vector& prior();
+};
+
 // Tectonic SAM Factors
 
 #include <gtsam_unstable/slam/TSAMFactors.h>

gtsam_unstable/slam/PartialPriorFactor.h

@@ -50,9 +50,6 @@ namespace gtsam {
     Vector prior_;                 ///< Measurement on tangent space parameters, in compressed form.
     std::vector<size_t> indices_;  ///< Indices of the measured tangent space parameters.
 
-    /** default constructor - only use for serialization */
-    PartialPriorFactor() {}
-
     /**
      * constructor with just minimum requirements for a factor - allows more
      * computation in the constructor.  This should only be used by subclasses
@@ -65,7 +62,8 @@ namespace gtsam {
     // Provide access to the Matrix& version of evaluateError:
     using Base::evaluateError;
 
-    ~PartialPriorFactor() override {}
+    /** default constructor - only use for serialization */
+    PartialPriorFactor() {}
 
     /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/
     PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
@@ -85,6 +83,8 @@ namespace gtsam {
       assert(model->dim() == (size_t)prior.size());
     }
 
+    ~PartialPriorFactor() override {}
+
     /// @return a deep copy of this factor
     gtsam::NonlinearFactor::shared_ptr clone() const override {
       return std::static_pointer_cast<gtsam::NonlinearFactor>(

python/CMakeLists.txt

@@ -189,6 +189,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::BinaryMeasurementsPoint3
             gtsam::BinaryMeasurementsUnit3
             gtsam::BinaryMeasurementsRot3
+            gtsam::SimWall2DVector
+            gtsam::SimPolygon2DVector
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
             gtsam::CameraSetCal3Unified

python/gtsam/examples/README.md

@@ -17,7 +17,7 @@
 | InverseKinematicsExampleExpressions.cpp               |        |
 | ISAM2Example_SmartFactor                              |        |
 | ISAM2_SmartFactorStereo_IMU                           |        |
-| LocalizationExample                                   | impossible? |
+| LocalizationExample                                   | :heavy_check_mark:      |
 | METISOrderingExample                                  |        |
 | OdometryExample                                       | :heavy_check_mark:      |
 | PlanarSLAMExample                                     | :heavy_check_mark:      |

python/gtsam_unstable/examples/LocalizationExample.py

@@ -0,0 +1,68 @@
+"""
+A simple 2D pose slam example with "GPS" measurements
+  - The robot moves forward 2 meter each iteration
+  - The robot initially faces along the X axis (horizontal, to the right in 2D)
+  - We have full odometry between pose
+  - We have "GPS-like" measurements implemented with a custom factor
+"""
+import numpy as np
+
+import gtsam
+from gtsam import BetweenFactorPose2, Pose2, noiseModel
+from gtsam_unstable import PartialPriorFactorPose2
+
+
+def main():
+    # 1. Create a factor graph container and add factors to it.
+    graph = gtsam.NonlinearFactorGraph()
+
+    # 2a. Add odometry factors
+    # For simplicity, we will use the same noise model for each odometry factor
+    odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1]))
+
+    # Create odometry (Between) factors between consecutive poses
+    graph.push_back(
+        BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise))
+    graph.push_back(
+        BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise))
+
+    # 2b. Add "GPS-like" measurements
+    # We will use PartialPrior factor for this.
+    unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1,
+                                                      0.1]))  # 10cm std on x,y
+
+    graph.push_back(
+        PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise))
+    graph.push_back(
+        PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise))
+    graph.push_back(
+        PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise))
+    graph.print("\nFactor Graph:\n")
+
+    # 3. Create the data structure to hold the initialEstimate estimate to the solution
+    # For illustrative purposes, these have been deliberately set to incorrect values
+    initialEstimate = gtsam.Values()
+    initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2))
+    initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2))
+    initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1))
+    initialEstimate.print("\nInitial Estimate:\n")
+
+    # 4. Optimize using Levenberg-Marquardt optimization. The optimizer
+    # accepts an optional set of configuration parameters, controlling
+    # things like convergence criteria, the type of linear system solver
+    # to use, and the amount of information displayed during optimization.
+    # Here we will use the default set of parameters.  See the
+    # documentation for the full set of parameters.
+    optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+    result = optimizer.optimize()
+    result.print("Final Result:\n")
+
+    # 5. Calculate and print marginal covariances for all variables
+    marginals = gtsam.Marginals(graph, result)
+    print("x1 covariance:\n", marginals.marginalCovariance(1))
+    print("x2 covariance:\n", marginals.marginalCovariance(2))
+    print("x3 covariance:\n", marginals.marginalCovariance(3))
+
+
+if __name__ == "__main__":
+    main()

python/gtsam_unstable/gtsam_unstable.tpl

@@ -9,6 +9,7 @@
 
 #include <pybind11/eigen.h>
 #include <pybind11/stl_bind.h>
+#include <pybind11/stl.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/functional.h>
 #include <pybind11/iostream.h>