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

Merge pull request #444 from borglab/feature/nary-expression-factor 

Add N-ary template expression factors
release/4.3a0
Frank Dellaert 2020-07-29 12:45:27 -04:00 committed by GitHub
parent c985701fdc dcd9415ddf
commit 232005a9af
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 329 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
gtsam/nonlinear/ExpressionFactor.h
View File

@ -19,9 +19,11 @@
#pragma once
#include <array>
#include <gtsam/config.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <numeric>
namespace gtsam {
@ -34,6 +36,9 @@ namespace gtsam {
* such instances, the user should declare derived classes from this template,
* implementing expresion(), serialize(), clone(), print(), and defining the
* corresponding `struct traits<NewFactor> : public Testable<NewFactor> {}`.
*
* \tparam T Type for measurements.
*
*/
template<typename T>
class ExpressionFactor: public NoiseModelFactor {
@ -222,26 +227,98 @@ private:
template <typename T>
struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
/**
* N-ary variadic template for ExpressionFactor meant as a base class for N-ary
* factors. Enforces an 'expression' method with N keys.
* Derived class (an N-factor!) needs to call 'initialize'.
*
* Does not provide backward compatible 'evaluateError'.
*
* \tparam T Type for measurements. The rest of template arguments are types
* for the N key-indexed Values.
*
*/
template <typename T, typename... Args>
class ExpressionFactorN : public ExpressionFactor<T> {
public:
static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
/// Destructor
virtual ~ExpressionFactorN() = default;
// Don't provide backward compatible evaluateVector(), due to its problematic
// variable length of optional Jacobian arguments. Vector evaluateError(const
// Args... args,...);
/// Recreate expression from given keys_ and measured_, used in load
/// Needed to deserialize a derived factor
virtual Expression<T> expression(const ArrayNKeys &keys) const {
throw std::runtime_error(
"ExpressionFactorN::expression not provided: cannot deserialize.");
}
protected:
/// Default constructor, for serialization
ExpressionFactorN() = default;
/// Constructor takes care of keys, but still need to call initialize
ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel,
const T &measurement)
: ExpressionFactor<T>(noiseModel, measurement) {
for (const auto &key : keys)
Factor::keys_.push_back(key);
}
private:
/// Return an expression that predicts the measurement given Values
Expression<T> expression() const override {
ArrayNKeys keys;
int idx = 0;
for (const auto &key : Factor::keys_)
keys[idx++] = key;
return expression(keys);
}
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &boost::serialization::make_nvp(
"ExpressionFactorN",
boost::serialization::base_object<ExpressionFactor<T>>(*this));
}
};
/// traits
template <typename T, typename... Args>
struct traits<ExpressionFactorN<T, Args...>>
: public Testable<ExpressionFactorN<T, Args...>> {};
// ExpressionFactorN
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41)
/**
* Binary specialization of ExpressionFactor meant as a base class for binary
* factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'.
* Derived class (a binary factor!) needs to call 'initialize'.
* factors. Enforces an 'expression' method with two keys, and provides
* 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'.
*
* \sa ExpressionFactorN
* \deprecated Prefer the more general ExpressionFactorN<>.
*/
template <typename T, typename A1, typename A2>
class ExpressionFactor2 : public ExpressionFactor<T> {
public:
class ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
public:
/// Destructor
virtual ~ExpressionFactor2() {}
~ExpressionFactor2() override {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Vector evaluateError(const A1 &a1, const A2 &a2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
std::vector<Matrix> H(2);
Vector error = this->unwhitenedError(values, H);
Vector error = ExpressionFactor<T>::unwhitenedError(values, H);
if (H1) (*H1) = H[0];
if (H2) (*H2) = H[1];
return error;
@ -250,35 +327,25 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
/// Recreate expression from given keys_ and measured_, used in load
/// Needed to deserialize a derived factor
virtual Expression<T> expression(Key key1, Key key2) const {
throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
throw std::runtime_error(
"ExpressionFactor2::expression not provided: cannot deserialize.");
}
virtual Expression<T>
expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
const override {
return expression(keys[0], keys[1]);
}
protected:
protected:
/// Default constructor, for serialization
ExpressionFactor2() {}
/// Constructor takes care of keys, but still need to call initialize
ExpressionFactor2(Key key1, Key key2,
const SharedNoiseModel& noiseModel,
const T& measurement)
: ExpressionFactor<T>(noiseModel, measurement) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
private:
/// Return an expression that predicts the measurement given Values
Expression<T> expression() const override {
return expression(this->keys_[0], this->keys_[1]);
}
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
}
ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
const T &measurement)
: ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
};
// ExpressionFactor2
#endif
}// \ namespace gtsam
} // namespace gtsam

29
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_);
}
@ -60,6 +60,21 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
std::cout << s << "BearingFactor" << std::endl;
Base::print(s, kf);
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
private:
friend class boost::serialization::access;

28
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,23 @@ 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]));
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
/// print

61
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,11 +53,25 @@ 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_);
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = Base::unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
/// print
void print(const std::string& s = "",
@ -86,10 +100,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 +114,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,15 +127,29 @@ 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_);
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = Base::unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
/** print contents */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
@ -135,9 +163,12 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
friend class boost::serialization::access;
template <typename ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// **IMPORTANT** We need to (de)serialize parameters before the base class,
// since it calls expression() and we need all parameters ready at that
// point.
ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
ar& boost::serialization::make_nvp(
"Base", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
}
}; // \ RangeFactorWithTransform

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);
//}

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

@ -141,6 +141,27 @@ TEST( RangeFactor, EqualsWithTransform ) {
body_P_sensor_3D);
CHECK(assert_equal(factor3D_1, factor3D_2));
}
/* ************************************************************************* */
TEST( RangeFactor, EqualsAfterDeserializing) {
// Check that the same results are obtained after deserializing:
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
body_P_sensor_3D), factor3D_2;
// check with Equal() trait:
gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
CHECK(assert_equal(factor3D_1, factor3D_2));
const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
const Point3 point(-2.0, 11.0, 1.0);
const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}};
const Vector error_1 = factor3D_1.unwhitenedError(values);
const Vector error_2 = factor3D_2.unwhitenedError(values);
CHECK(assert_equal(error_1, error_2));
}
/* ************************************************************************* */
TEST( RangeFactor, Error2D ) {
@ -152,7 +173,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 +196,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 +215,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 +239,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();
@ -266,8 +287,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 +314,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 +346,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 +377,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 +420,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));
}
/* ************************************************************************* */

97
tests/testExpressionFactor.cpp
View File

@ -630,6 +630,103 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
}
/* ************************************************************************* */
// Test N-ary variadic template
class TestNaryFactor
: public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
gtsam::Rot3, gtsam::Point3,
gtsam::Rot3,gtsam::Point3> {
private:
using This = TestNaryFactor;
using Base =
gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
gtsam::Rot3, gtsam::Point3, gtsam::Rot3, gtsam::Point3>;
public:
/// default constructor
TestNaryFactor() = default;
~TestNaryFactor() override = default;
TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2,
const gtsam::SharedNoiseModel &model, const gtsam::Point3& measured)
: Base({kR1, kV1, kR2, kV2}, model, measured) {
this->initialize(expression({kR1, kV1, kR2, kV2}));
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
// Return measurement expression
gtsam::Expression<gtsam::Point3> expression(
const std::array<gtsam::Key, NARY_EXPRESSION_SIZE> &keys) const override {
gtsam::Expression<gtsam::Rot3> R1_(keys[0]);
gtsam::Expression<gtsam::Point3> V1_(keys[1]);
gtsam::Expression<gtsam::Rot3> R2_(keys[2]);
gtsam::Expression<gtsam::Point3> V2_(keys[3]);
return {gtsam::rotate(R1_, V1_) - gtsam::rotate(R2_, V2_)};
}
/** print */
void print(const std::string &s,
const gtsam::KeyFormatter &keyFormatter =
gtsam::DefaultKeyFormatter) const override {
std::cout << s << "TestNaryFactor("
<< keyFormatter(Factor::keys_[0]) << ","
<< keyFormatter(Factor::keys_[1]) << ","
<< keyFormatter(Factor::keys_[2]) << ","
<< keyFormatter(Factor::keys_[3]) << ")\n";
gtsam::traits<gtsam::Point3>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
bool equals(const gtsam::NonlinearFactor &expected,
double tol = 1e-9) const override {
const This *e = dynamic_cast<const This *>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
gtsam::traits<gtsam::Point3>::Equals(measured_,e->measured_, tol);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &boost::serialization::make_nvp(
"TestNaryFactor",
boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
}
};
TEST(ExpressionFactor, variadicTemplate) {
using gtsam::symbol_shorthand::R;
using gtsam::symbol_shorthand::V;
// Create factor
TestNaryFactor f(R(0),V(0), R(1), V(1), noiseModel::Unit::Create(3), Point3(0,0,0));
// Create some values
Values values;
values.insert(R(0), Rot3::Ypr(0.1, 0.2, 0.3));
values.insert(V(0), Point3(1, 2, 3));
values.insert(R(1), Rot3::Ypr(0.2, 0.5, 0.2));
values.insert(V(1), Point3(5, 6, 7));
// Check unwhitenedError
std::vector<Matrix> H(4);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(4, H.size());
EXPECT(assert_equal(Eigen::Vector3d(-5.63578115, -4.85353243, -1.4801204), actual, 1e-5));
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;