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

83 lines
2.1 KiB
C
Raw Normal View History

BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
/**
* @file BetweenFactor.h
* @authors Frank Dellaert, Viorela Ila
**/
#pragma once
#include <ostream>
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/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* T is the Lie group type, Config where the T's are gotten from
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
*
* FIXME: This should only need one key type, as we can't have different types
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
*/
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
template<class Config, class Key1, class Key2 = Key1>
class BetweenFactor: public NonlinearFactor2<Config, Key1, Key2> {
public:
typedef typename Key1::Value_t T;
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
private:
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
typedef NonlinearFactor2<Config, Key1, Key2> Base;
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
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
T measured_; /** The measurement */
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
/** Constructor */
make BetweenFactor support two different keys
2010-04-01 01:43:52 +08:00
BetweenFactor(const Key1& key1, const Key2& key2, const T& measured,
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
Base(model, key1, key2), measured_(measured) {
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
}
/** implement functions needed for Testable */
/** print */
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
void print(const std::string& s) const {
Base::print(s);
Pose2Prior is now a typedef, improved some derivatives
2010-01-17 00:46:57 +08:00
measured_.print("measured");
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
}
/** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
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
const BetweenFactor<Config, Key1, Key2> *e =
dynamic_cast<const BetweenFactor<Config, Key1, Key2>*> (&expected);
Pose2Prior is now a typedef, improved some derivatives
2010-01-17 00:46:57 +08:00
return e != NULL && Base::equals(expected) && this->measured_.equals(
e->measured_, tol);
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
}
/** implement functions needed to derive from Factor */
/** vector of errors */
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
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
Removed remaining global lie functions on lie objects and configs, switched the Lie base class to a simple concept check function, fixed build script for examples. ISAM2 and MastSLAM verified as compiling.
2010-08-27 03:55:40 +08:00
T hx = p1.between(p2, H1, H2); // h(x)
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
// manifold equivalent of h(x)-z -> log(z,h(x))
Removed remaining global lie functions on lie objects and configs, switched the Lie base class to a simple concept check function, fixed build script for examples. ISAM2 and MastSLAM verified as compiling.
2010-08-27 03:55:40 +08:00
return measured_.logmap(hx);
Fixed double map lookup
2010-01-13 00:11:24 +08:00
}
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
/** return the measured */
Pose2Prior is now a typedef, improved some derivatives
2010-01-17 00:46:57 +08:00
inline const T measured() const {
return measured_;
}
1) add a compose function that compose all the poses in a factor graph given the spanning tree 2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-13 00:12:25 +08:00
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
/** number of variables attached to this factor */
Pose2Prior is now a typedef, improved some derivatives
2010-01-17 00:46:57 +08:00
inline std::size_t size() const {
return 2;
}
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 23:32:45 +08:00
};
} /// namespace gtsam