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

51 lines
1.3 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 Simulated3D.cpp
* @brief measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/
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/Simulated3D.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 {
Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.
2010-08-25 01:26:56 +08:00
using namespace simulated3D;
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)
Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.
2010-08-25 01:26:56 +08:00
Large gtsam refactoring To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors. The following are the biggest changes: 1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function. 2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like if (H) *H = Matrix_(3,6,....); 3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'> 4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
namespace simulated3D {
Fixing directory structure
2009-08-22 06:23:24 +08:00
Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.
2010-08-25 01:26:56 +08:00
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
if (H) *H = eye(3);
Fixing directory structure
2009-08-22 06:23:24 +08:00
return x;
}
Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.
2010-08-25 01:26:56 +08:00
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
Fixing directory structure
2009-08-22 06:23:24 +08:00
return x2 - x1;
}
Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.
2010-08-25 01:26:56 +08:00
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
Fixing directory structure
2009-08-22 06:23:24 +08:00
return l - 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
}} // namespace gtsam::simulated3D