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

Define and partially specify Range and Bearing functors

release/4.3a0
Frank Dellaert 2015-07-12 15:51:49 -07:00
parent 3bad6fea67
commit 03db69117a
13 changed files with 311 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam.h
View File

@ -2272,20 +2272,16 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/sam/BearingFactor.h>
template<POSE, POINT, MEASURED>
template<POSE, POINT, BEARING>
virtual class BearingFactor : gtsam::NoiseModelFactor {
BearingFactor(size_t key1, size_t key2, const MEASURED& measured, const gtsam::noiseModel::Base* noiseModel);
BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
@ -2295,9 +2291,9 @@ typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFa
#include <gtsam/slam/BearingRangeFactor.h>
template<POSE, POINT, MEASURED>
template<POSE, POINT, BEARING>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const MEASURED& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
BearingRangeFactor(size_t poseKey, size_t pointKey, const BEARING& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
pair<MEASURED, double> measured() const;

35
gtsam/geometry/CalibratedCamera.h
View File

@ -318,7 +318,7 @@ public:
}
/// @}
/// @name Transformations and mesaurement functions
/// @name Transformations and measurement functions
/// @{
/**
@ -393,5 +393,38 @@ struct traits<const CalibratedCamera> : public internal::Manifold<
CalibratedCamera> {
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <>
struct Range<CalibratedCamera, Point3> {
typedef double result_type;
double operator()(const CalibratedCamera& camera, const Point3& point,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return camera.range(point, H1, H2);
}
};
template <>
struct Range<CalibratedCamera, Pose3> {
typedef double result_type;
double operator()(const CalibratedCamera& camera, const Pose3& pose,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera.range(pose, H1, H2);
}
};
template <>
struct Range<CalibratedCamera, CalibratedCamera> {
typedef double result_type;
double operator()(const CalibratedCamera& camera1,
const CalibratedCamera& camera2,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera1.range(camera2, H1, H2);
}
};
}

58
gtsam/geometry/PinholeCamera.h
View File

@ -263,24 +263,22 @@ public:
template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
boost::optional<Matrix&> Dother = boost::none) const {
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block<1, 6>(0, 0) = Dother_;
Dother->template block<1, 6>(0, 0) = Dother_;
}
return result;
}
/**
* Calculate range to another camera
* Calculate range to a calibrated camera
* @param camera Other camera
* @return range (double)
*/
@ -314,4 +312,52 @@ struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
} // \ gtsam
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, Point3> {
typedef double result_type;
typedef PinholeCamera<Calibration> Camera;
double operator()(const Camera& camera, const Point3& point,
OptionalJacobian<1, Camera::dimension> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return camera.range(point, H1, H2);
}
};
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, Pose3> {
typedef double result_type;
typedef PinholeCamera<Calibration> Camera;
double operator()(const Camera& camera, const Pose3& pose,
OptionalJacobian<1, Camera::dimension> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera.range(pose, H1, H2);
}
};
template <typename CalibrationA, typename CalibrationB>
struct Range<PinholeCamera<CalibrationA>, PinholeCamera<CalibrationB> > {
typedef double result_type;
typedef PinholeCamera<CalibrationA> CameraA;
typedef PinholeCamera<CalibrationB> CameraB;
double operator()(const CameraA& camera1, const CameraB& camera2,
OptionalJacobian<1, CameraA::dimension> H1 = boost::none,
OptionalJacobian<1, CameraB::dimension> H2 = boost::none) {
return camera1.range(camera2, H1, H2);
}
};
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, CalibratedCamera> {
typedef double result_type;
typedef PinholeCamera<Calibration> Camera;
double operator()(const Camera& camera, const CalibratedCamera& cc,
OptionalJacobian<1, Camera::dimension> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera.range(cc, H1, H2);
}
};
} // \ gtsam

18
gtsam/geometry/Point3.h
View File

@ -22,6 +22,7 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
@ -199,4 +200,19 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
}
template <typename A1, typename A2>
struct Range;
template <>
struct Range<Point3, Point3> {
typedef double result_type;
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return p.distance(q, H1, H2);
}
};
} // namespace gtsam

47
gtsam/geometry/Pose2.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
namespace gtsam {
@ -295,5 +296,51 @@ struct traits<Pose2> : public internal::LieGroup<Pose2> {};
template<>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
// Define Bearing functor specializations that are used in BearingFactor
template <typename A1, typename A2> struct Bearing;
template <>
struct Bearing<Pose2, Point2> {
typedef Rot2 result_type;
Rot2 operator()(const Pose2& pose, const Point2& point,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
return pose.bearing(point, H1, H2);
}
};
template <>
struct Bearing<Pose2, Pose2> {
typedef Rot2 result_type;
Rot2 operator()(const Pose2& pose1, const Pose2& pose2,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return pose1.bearing(pose2, H1, H2);
}
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <>
struct Range<Pose2, Point2> {
typedef double result_type;
double operator()(const Pose2& pose, const Point2& point,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
return pose.range(point, H1, H2);
}
};
template <>
struct Range<Pose2, Pose2> {
typedef double result_type;
double operator()(const Pose2& pose1, const Pose2& pose2,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
} // namespace gtsam

41
gtsam/geometry/Pose3.h
View File

@ -331,11 +331,46 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;
template<>
template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template<>
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
// Define Bearing functor specializations that are used in BearingFactor
template <typename A1, typename A2> struct Bearing;
} // namespace gtsam
template <>
struct Bearing<Pose3, Point3> {
typedef Unit3 result_type;
Unit3 operator()(const Pose3& pose, const Point3& point,
OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) {
return pose.bearing(point, H1, H2);
}
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <>
struct Range<Pose3, Point3> {
typedef double result_type;
double operator()(const Pose3& pose, const Point3& point,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return pose.range(point, H1, H2);
}
};
template <>
struct Range<Pose3, Pose3> {
typedef double result_type;
double operator()(const Pose3& pose1, const Pose3& pose2,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
} // namespace gtsam

22
gtsam/geometry/concepts.h
View File

@ -59,24 +59,6 @@ private:
}
};
/**
* Range measurement concept
* Given a pair of Lie variables, there must exist a function to calculate
* range with derivatives.
*/
template<class V1, class V2>
class RangeMeasurementConcept {
private:
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;
return x.range(p, H1, H2);
}
};
} // \namespace gtsam
/**
@ -92,7 +74,3 @@ private:
#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T;
/** Range Measurement macros */
#define GTSAM_CONCEPT_RANGE_MEASUREMENT_INST(V1,V2) template class gtsam::RangeMeasurementConcept<V1,V2>;
#define GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(V1,V2) typedef gtsam::RangeMeasurementConcept<V1,V2> _gtsam_RangeMeasurementConcept##V1##V2;

28
gtsam/sam/BearingFactor.h
View File

@ -11,8 +11,7 @@
/**
* @file BearingFactor.h
* @brief Serializable factor induced by a bearing measurement of a point from
*a given pose
* @brief Serializable factor induced by a bearing measurement
* @date July 2015
* @author Frank Dellaert
**/
@ -20,17 +19,23 @@
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/geometry/concepts.h>
namespace gtsam {
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
/**
* Binary factor for a bearing measurement
* Works for any two types A1,A2 for which the functor Bearing<A1,A2>() is
* defined
* @addtogroup SAM
*/
template <typename POSE, typename POINT, typename T>
struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
typedef SerializableExpressionFactor2<T, POSE, POINT> Base;
template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public SerializableExpressionFactor2<T, A1, A2> {
typedef SerializableExpressionFactor2<T, A1, A2> Base;
/// default constructor
BearingFactor() {}
@ -44,8 +49,9 @@ struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
// Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const {
return Expression<T>(Expression<POSE>(key1), &POSE::bearing,
Expression<POINT>(key2));
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
}
/// print
@ -57,8 +63,8 @@ struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
}; // BearingFactor
/// traits
template <class POSE, class POINT, class T>
struct traits<BearingFactor<POSE, POINT, T> >
: public Testable<BearingFactor<POSE, POINT, T> > {};
template <typename A1, typename A2, typename T>
struct traits<BearingFactor<A1, A2, T> >
: public Testable<BearingFactor<A1, A2, T> > {};
} // namespace gtsam

64
gtsam/sam/RangeFactor.h
View File

@ -11,8 +11,7 @@
/**
* @file RangeFactor.h
* @brief Serializable factor induced by a range measurement between two points
*and/or poses
* @brief Serializable factor induced by a range measurement
* @date July 2015
* @author Frank Dellaert
**/
@ -20,30 +19,30 @@
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/geometry/concepts.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {
// forward declaration of Range functor, assumed partially specified
template <typename A1, typename A2>
struct Range;
/**
* Binary factor for a range measurement
* Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined
* @addtogroup SAM
*/
template <class A1, class A2 = A1>
struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactor : public SerializableExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;
typedef SerializableExpressionFactor2<double, A1, A2> Base;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
typedef SerializableExpressionFactor2<T, A1, A2> Base;
public:
/// default constructor
RangeFactor() {}
RangeFactor(Key key1, Key key2, double measured,
const SharedNoiseModel& model)
RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
: Base(key1, key2, model, measured) {
this->initialize(expression(key1, key2));
}
@ -55,10 +54,10 @@ struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
}
// Return measurement expression
virtual Expression<double> expression(Key key1, Key key2) const {
Expression<A1> t1_(key1);
Expression<A2> t2_(key2);
return Expression<double>(t1_, &A1::range, t2_);
virtual Expression<T> expression(Key key1, Key key2) const {
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
return Expression<T>(Range<A1, A2>(), a1_, a2_);
}
/// print
@ -70,30 +69,29 @@ struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
}; // \ RangeFactor
/// traits
template <class A1, class A2>
struct traits<RangeFactor<A1, A2> > : public Testable<RangeFactor<A1, A2> > {};
template <typename A1, typename A2, typename T>
struct traits<RangeFactor<A1, A2, T> >
: public Testable<RangeFactor<A1, A2, T> > {};
/**
* Binary factor for a range measurement, with a transform applied
* @addtogroup SAM
*/
template <class A1, class A2 = A1>
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform
: public SerializableExpressionFactor2<double, A1, A2> {
: public SerializableExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactorWithTransform<A1, A2> This;
typedef SerializableExpressionFactor2<double, A1, A2> Base;
typedef SerializableExpressionFactor2<T, A1, A2> Base;
A1 body_T_sensor_; ///< The pose of the sensor in the body frame
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
public:
//// Default constructor
RangeFactorWithTransform() {}
RangeFactorWithTransform(Key key1, Key key2, double measured,
RangeFactorWithTransform(Key key1, Key key2, T measured,
const SharedNoiseModel& model,
const A1& body_T_sensor)
: Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) {
@ -109,13 +107,13 @@ class RangeFactorWithTransform
}
// Return measurement expression
virtual Expression<double> expression(Key key1, Key key2) const {
virtual Expression<T> expression(Key key1, Key key2) const {
Expression<A1> body_T_sensor__(body_T_sensor_);
Expression<A1> nav_T_body_(key1);
Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
body_T_sensor__);
Expression<A2> t2_(key2);
return Expression<double>(nav_T_sensor_, &A1::range, t2_);
Expression<A2> a2_(key2);
return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
}
/** print contents */
@ -128,8 +126,8 @@ class RangeFactorWithTransform
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
friend typename boost::serialization::access;
template <typename ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Base", boost::serialization::base_object<Base>(*this));
@ -138,8 +136,8 @@ class RangeFactorWithTransform
}; // \ RangeFactorWithTransform
/// traits
template <class A1, class A2>
struct traits<RangeFactorWithTransform<A1, A2> >
: public Testable<RangeFactorWithTransform<A1, A2> > {};
template <typename A1, typename A2, typename T>
struct traits<RangeFactorWithTransform<A1, A2, T> >
: public Testable<RangeFactorWithTransform<A1, A2, T> > {};
} // \ namespace gtsam

4
gtsam/sam/tests/testBearingFactor.cpp
View File

@ -32,12 +32,12 @@ using namespace gtsam;
Key poseKey(1);
Key pointKey(2);
typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
typedef BearingFactor<Pose2, Point2> BearingFactor2D;
double measurement2D(10.0);
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
typedef BearingFactor<Pose3, Point3> BearingFactor3D;
Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values!
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);

52
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -344,6 +344,58 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
// Do a test with Point3
TEST(RangeFactor, Point3) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor<Point3> factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Point3 pose(1.0, 2.0, 00);
Point3 point(-4.0, 11.0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error
CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
}
/* ************************************************************************* */
// Do a test with non GTSAM types
namespace gtsam{
template <>
struct Range<Vector3, Vector3> {
typedef double result_type;
double operator()(const Vector3& v1, const Vector3& v2,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) {
return (v2 - v1).norm();
}
};
}
TEST(RangeFactor, NonGTSAM) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor<Vector3> factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Vector3 pose(1.0, 2.0, 00);
Vector3 point(-4.0, 11.0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error
CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

1
gtsam/slam/BearingRangeFactor.h
View File

@ -47,7 +47,6 @@ namespace gtsam {
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
GTSAM_CONCEPT_POSE_TYPE(Pose)
public:

13
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -170,4 +170,17 @@ private:
template<>
struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <>
struct Range<PoseRTV, PoseRTV> {
typedef double result_type;
double operator()(const PoseRTV& pose1, const PoseRTV& pose2,
OptionalJacobian<1, 9> H1 = boost::none,
OptionalJacobian<1, 9> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
} // \namespace gtsam