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

port ExpressionFactor{2->N}

release/4.3a0
Jose Luis Blanco Claraco 2020-07-28 01:01:37 +02:00
parent c9bd7ef8a7
commit 6b630effd0
No known key found for this signature in database
GPG Key ID: D443304FBD70A641
6 changed files with 62 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/sam/BearingFactor.h
View File

@ -34,8 +34,8 @@ struct Bearing;
*/ */
template <typename A1, typename A2, template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type> typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public ExpressionFactor2<T, A1, A2> { struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
typedef ExpressionFactor2<T, A1, A2> Base; typedef ExpressionFactorN<T, A1, A2> Base;
/// default constructor /// default constructor
BearingFactor() {} BearingFactor() {}
@ -43,14 +43,14 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
/// primary constructor /// primary constructor
BearingFactor(Key key1, Key key2, const T& measured, BearingFactor(Key key1, Key key2, const T& measured,
const SharedNoiseModel& model) const SharedNoiseModel& model)
: Base(key1, key2, model, measured) { : Base({key1, key2}, model, measured) {
this->initialize(expression(key1, key2)); this->initialize(expression({key1, key2}));
} }
// Return measurement expression // Return measurement expression
Expression<T> expression(Key key1, Key key2) const override { Expression<T> expression(const typename Base::ArrayNKeys &keys) const override {
Expression<A1> a1_(key1); Expression<A1> a1_(keys[0]);
Expression<A2> a2_(key2); Expression<A2> a2_(keys[1]);
return Expression<T>(Bearing<A1, A2>(), a1_, a2_); return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
} }

14
gtsam/sam/BearingRangeFactor.h
View File

@ -33,10 +33,10 @@ template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type, typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type> typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor class BearingRangeFactor
: public ExpressionFactor2<BearingRange<A1, A2>, A1, A2> { : public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
private: private:
typedef BearingRange<A1, A2> T; typedef BearingRange<A1, A2> T;
typedef ExpressionFactor2<T, A1, A2> Base; typedef ExpressionFactorN<T, A1, A2> Base;
typedef BearingRangeFactor<A1, A2> This; typedef BearingRangeFactor<A1, A2> This;
public: public:
@ -48,8 +48,8 @@ class BearingRangeFactor
/// primary constructor /// primary constructor
BearingRangeFactor(Key key1, Key key2, const B& measuredBearing, BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
const R& measuredRange, const SharedNoiseModel& model) const R& measuredRange, const SharedNoiseModel& model)
: Base(key1, key2, model, T(measuredBearing, measuredRange)) { : Base({key1, key2}, model, T(measuredBearing, measuredRange)) {
this->initialize(expression(key1, key2)); this->initialize(expression({key1, key2}));
} }
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}
@ -61,9 +61,9 @@ class BearingRangeFactor
} }
// Return measurement expression // Return measurement expression
Expression<T> expression(Key key1, Key key2) const override { Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
return Expression<T>(T::Measure, Expression<A1>(key1), return Expression<T>(T::Measure, Expression<A1>(keys[0]),
Expression<A2>(key2)); Expression<A2>(keys[1]));
} }
/// print /// print

28
gtsam/sam/RangeFactor.h
View File

@ -32,18 +32,18 @@ struct Range;
* @addtogroup SAM * @addtogroup SAM
*/ */
template <typename A1, typename A2 = A1, typename T = double> template <typename A1, typename A2 = A1, typename T = double>
class RangeFactor : public ExpressionFactor2<T, A1, A2> { class RangeFactor : public ExpressionFactorN<T, A1, A2> {
private: private:
typedef RangeFactor<A1, A2> This; typedef RangeFactor<A1, A2> This;
typedef ExpressionFactor2<T, A1, A2> Base; typedef ExpressionFactorN<T, A1, A2> Base;
public: public:
/// default constructor /// default constructor
RangeFactor() {} RangeFactor() {}
RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model) RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
: Base(key1, key2, model, measured) { : Base({key1, key2}, model, measured) {
this->initialize(expression(key1, key2)); this->initialize(expression({key1, key2}));
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -53,9 +53,9 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
} }
// Return measurement expression // Return measurement expression
Expression<T> expression(Key key1, Key key2) const override { Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
Expression<A1> a1_(key1); Expression<A1> a1_(keys[0]);
Expression<A2> a2_(key2); Expression<A2> a2_(keys[1]);
return Expression<T>(Range<A1, A2>(), a1_, a2_); return Expression<T>(Range<A1, A2>(), a1_, a2_);
} }
@ -86,10 +86,10 @@ struct traits<RangeFactor<A1, A2, T> >
*/ */
template <typename A1, typename A2 = A1, template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type> typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> { class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
private: private:
typedef RangeFactorWithTransform<A1, A2> This; typedef RangeFactorWithTransform<A1, A2> This;
typedef ExpressionFactor2<T, A1, A2> Base; typedef ExpressionFactorN<T, A1, A2> Base;
A1 body_T_sensor_; ///< The pose of the sensor in the body frame A1 body_T_sensor_; ///< The pose of the sensor in the body frame
@ -100,8 +100,8 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
RangeFactorWithTransform(Key key1, Key key2, T measured, RangeFactorWithTransform(Key key1, Key key2, T measured,
const SharedNoiseModel& model, const SharedNoiseModel& model,
const A1& body_T_sensor) const A1& body_T_sensor)
: Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) { : Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) {
this->initialize(expression(key1, key2)); this->initialize(expression({key1, key2}));
} }
virtual ~RangeFactorWithTransform() {} virtual ~RangeFactorWithTransform() {}
@ -113,12 +113,12 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
} }
// Return measurement expression // Return measurement expression
Expression<T> expression(Key key1, Key key2) const override { Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
Expression<A1> body_T_sensor__(body_T_sensor_); Expression<A1> body_T_sensor__(body_T_sensor_);
Expression<A1> nav_T_body_(key1); Expression<A1> nav_T_body_(keys[0]);
Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_, Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
body_T_sensor__); body_T_sensor__);
Expression<A2> a2_(key2); Expression<A2> a2_(keys[1]);
return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_); return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
} }

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

@ -68,7 +68,7 @@ TEST(BearingFactor, 2D) {
values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0)); values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
values, 1e-7, 1e-5); values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
} }
@ -104,7 +104,7 @@ TEST(BearingFactor, Serialization3D) {
// values.insert(poseKey, Pose3()); // values.insert(poseKey, Pose3());
// values.insert(pointKey, Point3(1, 0, 0)); // values.insert(pointKey, Point3(1, 0, 0));
// //
// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), // EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
// values, 1e-7, 1e-5); // values, 1e-7, 1e-5);
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
//} //}

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

@ -67,7 +67,7 @@ TEST(BearingRangeFactor, 2D) {
values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0)); values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
values, 1e-7, 1e-5); values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
} }
@ -95,7 +95,7 @@ TEST(BearingRangeFactor, Serialization3D) {
// values.insert(poseKey, Pose3()); // values.insert(poseKey, Pose3());
// values.insert(pointKey, Point3(1, 0, 0)); // values.insert(pointKey, Point3(1, 0, 0));
// //
// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), // EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
// values, 1e-7, 1e-5); // values, 1e-7, 1e-5);
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
//} //}

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

@ -47,25 +47,29 @@ double measurement(10.0);
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point, Vector factorError2D(const Pose2& pose, const Point2& point,
const RangeFactor2D& factor) { const RangeFactor2D& factor) {
return factor.evaluateError(pose, point); const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorError3D(const Pose3& pose, const Point3& point, Vector factorError3D(const Pose3& pose, const Point3& point,
const RangeFactor3D& factor) { const RangeFactor3D& factor) {
return factor.evaluateError(pose, point); const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
const RangeFactorWithTransform2D& factor) { const RangeFactorWithTransform2D& factor) {
return factor.evaluateError(pose, point); const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
const RangeFactorWithTransform3D& factor) { const RangeFactorWithTransform3D& factor) {
return factor.evaluateError(pose, point); const auto &keys = factor.keys();
return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -152,7 +156,7 @@ TEST( RangeFactor, Error2D ) {
Point2 point(-4.0, 11.0); Point2 point(-4.0, 11.0);
// Use the factor to calculate the error // Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished(); Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -175,7 +179,7 @@ TEST( RangeFactor, Error2DWithTransform ) {
Point2 point(-4.0, 11.0); Point2 point(-4.0, 11.0);
// Use the factor to calculate the error // Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished(); Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -194,7 +198,7 @@ TEST( RangeFactor, Error3D ) {
Point3 point(-2.0, 11.0, 1.0); Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the error // Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished(); Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -218,7 +222,7 @@ TEST( RangeFactor, Error3DWithTransform ) {
Point3 point(-2.0, 11.0, 1.0); Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the error // Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished(); Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -237,8 +241,10 @@ TEST( RangeFactor, Jacobian2D ) {
Point2 point(-4.0, 11.0); Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual; std::vector<Matrix> actualHs(2);
factor.evaluateError(pose, point, H1Actual, H2Actual); factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
const Matrix& H1Actual = actualHs.at(0);
const Matrix& H2Actual = actualHs.at(1);
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
@ -266,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
Point2 point(-4.0, 11.0); Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual; std::vector<Matrix> actualHs(2);
factor.evaluateError(pose, point, H1Actual, H2Actual); factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
const Matrix& H1Actual = actualHs.at(0);
const Matrix& H2Actual = actualHs.at(1);
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
@ -291,8 +299,10 @@ TEST( RangeFactor, Jacobian3D ) {
Point3 point(-2.0, 11.0, 1.0); Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual; std::vector<Matrix> actualHs(2);
factor.evaluateError(pose, point, H1Actual, H2Actual); factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
const Matrix& H1Actual = actualHs.at(0);
const Matrix& H2Actual = actualHs.at(1);
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
@ -321,8 +331,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
Point3 point(-2.0, 11.0, 1.0); Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual; std::vector<Matrix> actualHs(2);
factor.evaluateError(pose, point, H1Actual, H2Actual); factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
const Matrix& H1Actual = actualHs.at(0);
const Matrix& H2Actual = actualHs.at(1);
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
@ -350,7 +362,7 @@ TEST(RangeFactor, Point3) {
Vector expectedError = (Vector(1) << 0.295630141).finished(); Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error // Verify we get the expected error
CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9)); CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -393,7 +405,7 @@ TEST(RangeFactor, NonGTSAM) {
Vector expectedError = (Vector(1) << 0.295630141).finished(); Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error // Verify we get the expected error
CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9)); CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */