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

49 lines
1.3 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.cpp
* @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert
**/
#include "planarSLAM.h"
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "TupleConfig-inl.h"
// Use planarSLAM namespace for specific SLAM instance
namespace gtsam {
using namespace planarSLAM;
Added an explicit instantiation to fix a link error that was only detected on Linux
2010-01-18 10:23:10 +08:00
INSTANTIATE_LIE_CONFIG(PointKey, Point2)
svn restored from 1733. this commit updates gtsam to version 1774, which now appears as 1734.
2010-01-16 09:16:59 +08:00
INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
namespace planarSLAM {
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p));
push_back(factor);
}
void Graph::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
sharedFactor factor(new Odometry(i, j, z, 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
push_back(factor);
}
void Graph::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
sharedFactor factor(new Bearing(i, j, z, 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
push_back(factor);
}
void Graph::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) {
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
sharedFactor factor(new Range(i, j, z, 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
push_back(factor);
}
} // planarSLAM
} // gtsam