77 lines
2.1 KiB
C++
77 lines
2.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file pose2SLAM.h
|
|
* @brief: 2D Pose SLAM
|
|
* @authors Frank Dellaert
|
|
**/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/base/LieVector.h>
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/nonlinear/LieValues.h>
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include <gtsam/nonlinear/Key.h>
|
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
|
|
|
namespace gtsam {
|
|
|
|
// Use pose2SLAM namespace for specific SLAM instance
|
|
namespace pose2SLAM {
|
|
|
|
// Keys and Values
|
|
typedef TypedSymbol<Pose2, 'x'> Key;
|
|
typedef LieValues<Key> Values;
|
|
|
|
/**
|
|
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
|
|
* @param n number of poses
|
|
* @param R radius of circle
|
|
* @param c character to use for keys
|
|
* @return circle of n 2D poses
|
|
*/
|
|
Values circle(size_t n, double R);
|
|
|
|
// Factors
|
|
typedef PriorFactor<Values, Key> Prior;
|
|
typedef BetweenFactor<Values, Key> Constraint;
|
|
typedef NonlinearEquality<Values, Key> HardConstraint;
|
|
|
|
// Graph
|
|
struct Graph: public NonlinearFactorGraph<Values> {
|
|
typedef BetweenFactor<Values, Key> Constraint;
|
|
typedef Pose2 Pose;
|
|
void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model);
|
|
void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model);
|
|
void addHardConstraint(const Key& i, const Pose2& p);
|
|
};
|
|
|
|
// Optimizer
|
|
typedef NonlinearOptimizer<Graph, Values> Optimizer;
|
|
|
|
} // pose2SLAM
|
|
|
|
/**
|
|
* Backwards compatibility
|
|
*/
|
|
typedef pose2SLAM::Values Pose2Values;
|
|
typedef pose2SLAM::Prior Pose2Prior;
|
|
typedef pose2SLAM::Constraint Pose2Factor;
|
|
typedef pose2SLAM::Graph Pose2Graph;
|
|
|
|
} // namespace gtsam
|
|
|