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

61 lines
1.8 KiB
C++
Raw Normal View History

prepend license information on all the codes
2010-10-14 12:54:38 +08:00
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
Fixing directory structure
2009-08-22 06:23:24 +08:00
/**
* @file simulated2D.cpp
* @brief measurement functions and derivatives for simulated 2D robot
* @author Frank Dellaert
*/
Installing headers in subdirectories and converted include directives to match the subdirectory structure (also see email to frankcvs)
2010-08-20 01:23:19 +08:00
#include <gtsam/slam/simulated2D.h>
Renamed LieConfig and TupleConfig to use Values
2010-10-09 11:09:55 +08:00
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
Fixing directory structure
2009-08-22 06:23:24 +08:00
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
namespace gtsam {
SQP tests and implementation now use the new Key system
2010-01-19 13:33:44 +08:00
using namespace simulated2D;
In all nonlinear factors and configs, it is only necessary to specify a key with a typedef Value_t for the associated value. This has been removed from factor template definitions, as it is too easy to use the wrong value. Also, no more string keys or linear symbol keys in nonlinear systems. Updated MastSLAM to work, and ISAM2 works without change
2010-08-24 03:44:17 +08:00
// INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_LIE_CONFIG(PoseKey)
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
SQP tests and implementation now use the new Key system
2010-01-19 13:33:44 +08:00
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
namespace simulated2D {
static Matrix I = gtsam::eye(2);
/* ************************************************************************* */
SQP tests and implementation now use the new Key system
2010-01-19 13:33:44 +08:00
Point2 prior(const Point2& x, boost::optional<Matrix&> H) {
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
if (H) *H = I;
return x;
}
/* ************************************************************************* */
SQP tests and implementation now use the new Key system
2010-01-19 13:33:44 +08:00
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1,
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
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
return x2 - x1;
}
/* ************************************************************************* */
SQP tests and implementation now use the new Key system
2010-01-19 13:33:44 +08:00
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1,
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
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
return l - x;
}
Fixing directory structure
2009-08-22 06:23:24 +08:00
simulated2D now reduced to one .h and .cpp, in its own namespace, better naming, and new-style functions to serve as example
2010-01-14 10:50:06 +08:00
/* ************************************************************************* */
Fixing directory structure
2009-08-22 06:23:24 +08:00
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
} // namespace simulated2D
} // namespace gtsam