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

54 lines
1.5 KiB
C++
Raw Normal View History

2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;
2010-01-17 02:01:16 +08:00
/**
* @file pose2SLAM.cpp
* @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert
**/
#include "pose2SLAM.h"
#include "LieConfig-inl.h"
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
// Use pose2SLAM namespace for specific SLAM instance
namespace gtsam {
using namespace pose2SLAM;
INSTANTIATE_LIE_CONFIG(Key, Pose2)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
namespace pose2SLAM {
/* ************************************************************************* */
Config circle(size_t n, double R) {
Config x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}
/* ************************************************************************* */
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 Graph::addPrior(const Key& i, const Pose2& p,
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 Prior(i, p, model));
2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;
2010-01-17 02:01:16 +08:00
push_back(factor);
}
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 Graph::addConstraint(const Key& i, const Key& 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 Constraint(i, j, z, model));
2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;
2010-01-17 02:01:16 +08:00
push_back(factor);
}
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
void Graph::addHardConstraint(const Key& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;
2010-01-17 02:01:16 +08:00
push_back(factor);
}
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;
2010-01-17 02:01:16 +08:00
/* ************************************************************************* */
} // pose2SLAM
} // gtsam