143 lines
5.0 KiB
C++
143 lines
5.0 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 visualSLAM.h
|
|
* @brief Basic typedefs for general VisualSLAM problems. Useful for monocular and stereo systems.
|
|
* @date Jan 14, 2010
|
|
* @author Richard Roberts
|
|
* @author Chris Beall
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/ProjectionFactor.h>
|
|
#include <gtsam/slam/StereoFactor.h>
|
|
#include <gtsam/slam/RangeFactor.h>
|
|
#include <gtsam/nonlinear/Symbol.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/geometry/SimpleCamera.h>
|
|
|
|
|
|
namespace visualSLAM {
|
|
|
|
using namespace gtsam;
|
|
|
|
/// Convenience function for constructing a pose key
|
|
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
|
|
|
|
/// Convenience function for constructing a pose key
|
|
inline Symbol PointKey(Index j) { return Symbol('l', j); }
|
|
|
|
/**
|
|
* Typedefs that make up the visualSLAM namespace.
|
|
*/
|
|
typedef NonlinearEquality<Pose3> PoseConstraint; ///< put a hard constraint on a pose
|
|
typedef NonlinearEquality<Point3> PointConstraint; ///< put a hard constraint on a point
|
|
typedef PriorFactor<Pose3> PosePrior; ///< put a soft prior on a Pose
|
|
typedef PriorFactor<Point3> PointPrior; ///< put a soft prior on a point
|
|
typedef RangeFactor<Pose3, Point3> RangeFactor; ///< put a factor on the range from a pose to a point
|
|
|
|
/// monocular and stereo camera typedefs for general use
|
|
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
|
|
typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
|
|
|
|
/**
|
|
* Non-linear factor graph for vanilla visual SLAM
|
|
*/
|
|
class Graph: public NonlinearFactorGraph {
|
|
|
|
public:
|
|
/// shared pointer to this type of graph
|
|
typedef boost::shared_ptr<Graph> shared_graph;
|
|
|
|
/// default constructor is empty graph
|
|
Graph() {
|
|
}
|
|
|
|
/// print out graph
|
|
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
|
NonlinearFactorGraph::print(s, keyFormatter);
|
|
}
|
|
|
|
/// check if two graphs are equal
|
|
bool equals(const Graph& p, double tol = 1e-9) const {
|
|
return NonlinearFactorGraph::equals(p, tol);
|
|
}
|
|
|
|
/**
|
|
* Add a projection factor measurement (monocular)
|
|
* @param measured the measurement
|
|
* @param model the noise model for the measurement
|
|
* @param poseKey variable key for the camera pose
|
|
* @param pointKey variable key for the landmark
|
|
* @param K shared pointer to calibration object
|
|
*/
|
|
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
|
|
Index poseKey, Index pointKey, const shared_ptrK& K);
|
|
|
|
/**
|
|
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
|
|
* @param key variable key of the camera pose
|
|
* @param p to which pose to constrain it to
|
|
*/
|
|
void addPoseConstraint(Index poseKey, const Pose3& p = Pose3());
|
|
|
|
/**
|
|
* Add a constraint on a point (for now, *must* be satisfied in any Values)
|
|
* @param key variable key of the landmark
|
|
* @param p point around which soft prior is defined
|
|
*/
|
|
void addPointConstraint(Index pointKey, const Point3& p = Point3());
|
|
|
|
/**
|
|
* Add a prior on a pose
|
|
* @param key variable key of the camera pose
|
|
* @param p around which soft prior is defined
|
|
* @param model uncertainty model of this prior
|
|
*/
|
|
void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
|
|
|
|
/**
|
|
* Add a prior on a landmark
|
|
* @param key variable key of the landmark
|
|
* @param p to which point to constrain it to
|
|
* @param model uncertainty model of this prior
|
|
*/
|
|
void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3));
|
|
|
|
/**
|
|
* Add a range prior to a landmark
|
|
* @param poseKey variable key of the camera pose
|
|
* @param pointKey variable key of the landmark
|
|
* @param range approximate range to landmark
|
|
* @param model uncertainty model of this prior
|
|
*/
|
|
void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
|
|
|
|
/**
|
|
* Optimize the graph
|
|
* @param initialEstimate initial estimate of all variables in the graph
|
|
* @param pointKey variable key of the landmark
|
|
* @param range approximate range to landmark
|
|
* @param model uncertainty model of this prior
|
|
*/
|
|
Values optimize(const Values& initialEstimate) {
|
|
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
|
|
}
|
|
|
|
}; // Graph
|
|
|
|
} // namespaces
|