Merge pull request #1808 from borglab/python-localization-example
						commit
						14a7e06327
					
				|  | @ -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> | ||||
|  |  | |||
|  | @ -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>( | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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:      | | ||||
|  |  | |||
|  | @ -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() | ||||
|  | @ -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> | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue