12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/cpp/planarSLAM.h

57 lines
1.7 KiB
C
Raw Normal View History

svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
/**
* @file planarSLAM.h
* @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert
**/
#pragma once
added BearingRangeFactor which captures both BearingFactor and RangeFactor
2010-04-02 06:02:31 +08:00
#include "BearingRangeFactor.h"
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
#include "TupleConfig.h"
#include "NonlinearEquality.h"
created addPrior
2010-02-19 10:48:47 +08:00
#include "PriorFactor.h"
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
#include "BetweenFactor.h"
#include "NonlinearFactorGraph.h"
#include "NonlinearOptimizer.h"
// We use gtsam namespace for generally useful factors
namespace gtsam {
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
// Keys and Config
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
define two additional types
2010-02-20 08:28:10 +08:00
typedef LieConfig<PoseKey, Pose2> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig;
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
// Factors
typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;
created addPrior
2010-02-19 10:48:47 +08:00
typedef PriorFactor<Config, PoseKey, Pose2> Prior;
NOISE MODEL. This is a big edit but with no templates involed, so it should not be a big deal. New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable. Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests.
2010-01-18 13:38:53 +08:00
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
typedef RangeFactor<Config, PoseKey, PointKey> Range;
added BearingRangeFactor which captures both BearingFactor and RangeFactor
2010-04-02 06:02:31 +08:00
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
// Graph
struct Graph: public NonlinearFactorGraph<Config> {
created addPrior
2010-02-19 10:48:47 +08:00
void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model);
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
void addPoseConstraint(const PoseKey& i, const Pose2& p);
NOISE MODEL. This is a big edit but with no templates involed, so it should not be a big deal. New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable. Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests.
2010-01-18 13:38:53 +08:00
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
case change: SharedGaussian and SharedDiagonal are now classes with their own header file. Needed for MATLAB TORO hail Mary
2010-01-23 01:36:57 +08:00
const SharedGaussian& model);
NOISE MODEL. This is a big edit but with no templates involed, so it should not be a big deal. New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable. Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests.
2010-01-18 13:38:53 +08:00
void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
case change: SharedGaussian and SharedDiagonal are now classes with their own header file. Needed for MATLAB TORO hail Mary
2010-01-23 01:36:57 +08:00
const SharedGaussian& model);
NOISE MODEL. This is a big edit but with no templates involed, so it should not be a big deal. New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable. Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests.
2010-01-18 13:38:53 +08:00
void addRange(const PoseKey& i, const PointKey& j, double z,
case change: SharedGaussian and SharedDiagonal are now classes with their own header file. Needed for MATLAB TORO hail Mary
2010-01-23 01:36:57 +08:00
const SharedGaussian& model);
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
};
// Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer;
} // planarSLAM
} // namespace gtsam