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

Added a "range" concept check for a measurement type - trying to add to generic factor types, but does not compile

release/4.3a0
Alex Cunningham 2011-10-15 18:38:44 +00:00
parent c0ecb8aa92
commit 0cb5c258df
3 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/concepts.h
View File

@ -38,4 +38,25 @@ struct PoseConcept {
/** Instantiation macro */ /** Instantiation macro */
#define GTSAM_CONCEPT_POSE(T) template class PoseConcept<T>; #define GTSAM_CONCEPT_POSE(T) template class PoseConcept<T>;
/**
* Range measurement concept
* Given a pair of Lie variables, there must exist a function to calculate
* range with derivatives.
*/
template<class V1, class V2>
struct RangeMeasurementConcept {
static double checkRangeMeasurement(const V1& x, const V2& p) {
return x.range(p);
}
static double checkRangeMeasurementDerivatives(const V1& x, const V2& p) {
boost::optional<Matrix&> H1, H2;
// FIXME: verify the dimensions of the derivative functions
return x.range(p, H1, H2);
}
};
/** Instantiation macro */
#define GTSAM_CONCEPT_RANGE_MEASUREMENT(V1,V2) template class RangeMeasurementConcept<V1,V2>;
} // \namespace gtsam } // \namespace gtsam

4
gtsam/slam/BearingRangeFactor.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -40,6 +41,9 @@ namespace gtsam {
Rot bearing_; Rot bearing_;
double range_; double range_;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT(Pose, Point)
public: public:
BearingRangeFactor() {} /* Default constructor */ BearingRangeFactor() {} /* Default constructor */

7
gtsam/slam/RangeFactor.h
View File

@ -17,6 +17,7 @@
#pragma once #pragma once
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -33,6 +34,12 @@ namespace gtsam {
typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This; typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
typedef typename POSEKEY::Value Pose;
typedef typename POINTKEY::Value Point;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT(Pose, Point)
public: public:
RangeFactor() {} /* Default constructor */ RangeFactor() {} /* Default constructor */