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

65 lines
1.9 KiB
C++
Raw Normal View History

3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
/**
* @file pose3SLAM.cpp
* @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert
**/
#include "pose3SLAM.h"
#include "LieConfig-inl.h"
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
// Use pose3SLAM namespace for specific SLAM instance
namespace gtsam {
using namespace pose3SLAM;
INSTANTIATE_LIE_CONFIG(Key, Pose3)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
namespace pose3SLAM {
/* ************************************************************************* */
Definitions of Lie::between, Pose2::compose, expmap, and derivatives were not correct. Fixed, but changes some behavior (iSAM and CitySLAM already fixed), will send email about this change.
2010-01-27 04:00:17 +08:00
Config circle(size_t n, double radius) {
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
Config 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
double theta = 0, dtheta = 2 * M_PI / n;
Definitions of Lie::between, Pose2::compose, expmap, and derivatives were not correct. Fixed, but changes some behavior (iSAM and CitySLAM already fixed), will send email about this change.
2010-01-27 04:00:17 +08:00
// We use aerospace/navlab convention, X forward, Y right, Z down
// First pose will be at (R,0,0)
// ^y ^ X
// | |
// z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
for (size_t i = 0; i < n; i++, theta += dtheta) {
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti);
x.insert(i, gTi);
}
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
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 Pose3& 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));
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +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 Pose3& 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));
3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
2010-01-17 03:37:17 +08:00
push_back(factor);
}
void Graph::addHardConstraint(const Key& i, const Pose3& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
}
/* ************************************************************************* */
} // pose3SLAM
} // gtsam