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

92 lines
2.3 KiB
C
Raw Normal View History

Fixing directory structure
2009-08-22 06:23:24 +08:00
/**
* @file VSLAMFactor.h
* @brief A Nonlinear Factor, specialized for visual SLAM
* @author Alireza Fathi
*/
#pragma once
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
#include <boost/optional.hpp>
Fixing directory structure
2009-08-22 06:23:24 +08:00
#include "NonlinearFactor.h"
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
#include "SimpleCamera.h"
#include "VSLAMConfig.h"
Fixing directory structure
2009-08-22 06:23:24 +08:00
#include "Cal3_S2.h"
VSLAMFactor Testable
2009-11-12 05:09:43 +08:00
'''BIG CHANGE''': avoid converting back and to FGConfigs by templating on configuration type. Details: * Factors are now templated on the configuration type. Factor Graphs are now templated on the factor type and configuration type. * LinearFactor is a factor on an FGConfig. * LinearFactorGraph uses LinearFactor and FGConfig. * NonLinearFactor is still templated on Config. * NonLinearFactorGraph uses NonLinearFactors, but is still templated on Config. * Tests and VSLAMFactor have been updated to reflect those changes.
2009-10-07 02:25:04 +08:00
namespace gtsam {
PairConfig is implemented, VSLAMConfig is now a typedef!
2010-01-14 10:58:29 +08:00
typedef NonlinearFactor2<VSLAMConfig,
VSLAMPoseKey, Pose3, VSLAMPointKey, Point3> VSLAMFactorBase;
Added default constructor and Serialization
2009-11-17 07:49:04 +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
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
Added default constructor and Serialization
2009-11-17 07:49:04 +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
class VSLAMFactor: public VSLAMFactorBase , Testable<VSLAMFactor> {
private:
// Keep a copy of measurement and calibration for I/O
Point2 z_;
boost::shared_ptr<Cal3_S2> K_;
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VSLAMFactor> shared_ptr;
/**
* Default constructor
*/
VSLAMFactor();
/**
* Constructor
* @param z is the 2 dimensional location of point in image (the measurement)
* @param sigma is the standard deviation
* @param cameraFrameNumber is basically the frame number
* @param landmarkNumber is the index of the landmark
* @param K the constant calibration
*/
VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber,
int landmarkNumber, const shared_ptrK & K);
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "VSLAMFactor") const;
/**
* equals
*/
bool equals(const VSLAMFactor&, double tol = 1e-9) const;
/** h(x) */
Point2 predict(const Pose3& pose, const Point3& point) const {
return SimpleCamera(*K_, pose).project(point);
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
SimpleCamera camera(*K_, pose);
if (H1) *H1=Dproject_pose(camera,point);
if (H2) *H2=Dproject_point(camera,point);
Point2 reprojectionError(project(camera, point) - z_);
return reprojectionError.vector();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
PairConfig is implemented, VSLAMConfig is now a typedef!
2010-01-14 10:58:29 +08:00
//ar & BOOST_SERIALIZATION_NVP(key1_);
//ar & BOOST_SERIALIZATION_NVP(key2_);
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
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
Fixing directory structure
2009-08-22 06:23:24 +08:00
'''BIG CHANGE''': avoid converting back and to FGConfigs by templating on configuration type. Details: * Factors are now templated on the configuration type. Factor Graphs are now templated on the factor type and configuration type. * LinearFactor is a factor on an FGConfig. * LinearFactorGraph uses LinearFactor and FGConfig. * NonLinearFactor is still templated on Config. * NonLinearFactorGraph uses NonLinearFactors, but is still templated on Config. * Tests and VSLAMFactor have been updated to reflect those changes.
2009-10-07 02:25:04 +08:00
}