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

28
gtsam/sam/RangeFactor.h
View File

@ -32,18 +32,18 @@ struct Range;
* @addtogroup SAM
*/
template <typename A1, typename A2 = A1, typename T = double>
class RangeFactor : public ExpressionFactor2<T, A1, A2> {
class RangeFactor : public ExpressionFactorN<T, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;
typedef ExpressionFactor2<T, A1, A2> Base;
typedef ExpressionFactorN<T, A1, A2> Base;
public:
/// default constructor
RangeFactor() {}
RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
: Base(key1, key2, model, measured) {
this->initialize(expression(key1, key2));
: Base({key1, key2}, model, measured) {
this->initialize(expression({key1, key2}));
}
/// @return a deep copy of this factor
@ -53,9 +53,9 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
}
// Return measurement expression
Expression<T> expression(Key key1, Key key2) const override {
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
Expression<A1> a1_(keys[0]);
Expression<A2> a2_(keys[1]);
return Expression<T>(Range<A1, A2>(), a1_, a2_);
}
@ -86,10 +86,10 @@ struct traits<RangeFactor<A1, A2, T> >
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
private:
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
@ -100,8 +100,8 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
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) {
this->initialize(expression(key1, key2));
: Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) {
this->initialize(expression({key1, key2}));
}
virtual ~RangeFactorWithTransform() {}
@ -113,12 +113,12 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
}
// 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> nav_T_body_(key1);
Expression<A1> nav_T_body_(keys[0]);
Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
body_T_sensor__);
Expression<A2> a2_(key2);
Expression<A2> a2_(keys[1]);
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(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);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
@ -104,7 +104,7 @@ TEST(BearingFactor, Serialization3D) {
// values.insert(poseKey, Pose3());
// 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);
// 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(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);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
@ -95,7 +95,7 @@ TEST(BearingRangeFactor, Serialization3D) {
// values.insert(poseKey, Pose3());
// 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);
// 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,
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,
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,
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,
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);
// 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
Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -175,7 +179,7 @@ TEST( RangeFactor, Error2DWithTransform ) {
Point2 point(-4.0, 11.0);
// 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
Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -194,7 +198,7 @@ TEST( RangeFactor, Error3D ) {
Point3 point(-2.0, 11.0, 1.0);
// 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
Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -218,7 +222,7 @@ TEST( RangeFactor, Error3DWithTransform ) {
Point3 point(-2.0, 11.0, 1.0);
// 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
Vector expectedError = (Vector(1) << 0.295630141).finished();
@ -237,8 +241,10 @@ TEST( RangeFactor, Jacobian2D ) {
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
std::vector<Matrix> actualHs(2);
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
Matrix H1Expected, H2Expected;
@ -266,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
std::vector<Matrix> actualHs(2);
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
Matrix H1Expected, H2Expected;
@ -291,8 +299,10 @@ TEST( RangeFactor, Jacobian3D ) {
Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
std::vector<Matrix> actualHs(2);
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
Matrix H1Expected, H2Expected;
@ -321,8 +331,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
Point3 point(-2.0, 11.0, 1.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
std::vector<Matrix> actualHs(2);
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
Matrix H1Expected, H2Expected;
@ -350,7 +362,7 @@ TEST(RangeFactor, Point3) {
Vector expectedError = (Vector(1) << 0.295630141).finished();
// 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();
// 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));
}
/* ************************************************************************* */