Skip to content

Commit

Permalink
Merge pull request #1808 from borglab/python-localization-example
Browse files Browse the repository at this point in the history
varunagrawal authored Aug 25, 2024
2 parents 46765ec + c57d5a0 commit 14a7e06
Showing 6 changed files with 91 additions and 5 deletions.
15 changes: 15 additions & 0 deletions gtsam_unstable/gtsam_unstable.i
Original file line number Diff line number Diff line change
@@ -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>
8 changes: 4 additions & 4 deletions gtsam_unstable/slam/PartialPriorFactor.h
Original file line number Diff line number Diff line change
@@ -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>(
2 changes: 2 additions & 0 deletions python/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
2 changes: 1 addition & 1 deletion python/gtsam/examples/README.md
Original file line number Diff line number Diff line change
@@ -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: |
68 changes: 68 additions & 0 deletions python/gtsam_unstable/examples/LocalizationExample.py
Original file line number Diff line number Diff line change
@@ -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()
1 change: 1 addition & 0 deletions python/gtsam_unstable/gtsam_unstable.tpl
Original file line number Diff line number Diff line change
@@ -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>

0 comments on commit 14a7e06

Please sign in to comment.