Merge pull request #1808 from borglab/python-localization-example
Varun Agrawal
GitHub
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