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

new definition for FunctorizedFactor to allow for using std::function and lambdas

release/4.3a0
Varun Agrawal 2020-07-06 17:38:34 -04:00
parent 0bbb39687f
commit 7d0e440293
2 changed files with 97 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -27,14 +27,11 @@ namespace gtsam {
/**
* Factor which evaluates functor and uses the result to compute
* error on provided measurement.
* The provided FUNCTOR should provide two type aliases: `argument_type` which
* corresponds to the type of input it accepts and `return_type` which indicates
* the type of the return value. This factor uses those type values to construct
* the functor.
*
* Template parameters are
* @param FUNCTOR: A class which operates as a functor.
*
* @param R: The return type of the functor after evaluation.
* @param T: The argument type for the functor.
*
* Example:
* Key key = Symbol('X', 0);
* auto model = noiseModel::Isotropic::Sigma(9, 1);
@ -48,58 +45,53 @@ namespace gtsam {
* MultiplyFunctor(double m) : m_(m) {}
* Matrix operator()(const Matrix &X,
* OptionalJacobian<-1, -1> H = boost::none) const {
* if (H) *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols());
* return m_ * X;
* if (H) *H = m_ * Matrix::Identity(X.rows()*X.cols(),
* X.rows()*X.cols()); return m_ * X;
* }
* };
*
* Matrix measurement = Matrix::Identity(3, 3);
* double multiplier = 2.0;
* FunctorizedFactor<MultiplyFunctor> factor(keyX, measurement, model, multiplier);
*
* FunctorizedFactor<Matrix, Matrix> factor(keyX, measurement, model,
* MultiplyFunctor(multiplier));
*/
template <typename FUNCTOR>
class GTSAM_EXPORT FunctorizedFactor
: public NoiseModelFactor1<typename FUNCTOR::argument_type> {
private:
using T = typename FUNCTOR::argument_type;
template <typename R, typename T>
class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
private:
using Base = NoiseModelFactor1<T>;
typename FUNCTOR::return_type
measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
FUNCTOR func_; ///< functor instance
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
std::function<R(T, boost::optional<Matrix &>)> func_; ///< functor instance
public:
public:
/** default constructor - only use for serialization */
FunctorizedFactor() {}
/** Construct with given x and the parameters of the basis
*
* @param Args: Variadic template parameter for functor arguments.
*
* @param key: Factor key
* @param z: Measurement object of type FUNCTOR::return_type
* @param z: Measurement object of type R
* @param model: Noise model
* @param args: Variable number of arguments used to instantiate functor
* @param func: The instance of the functor object
*/
template <typename... Args>
FunctorizedFactor(Key key, const typename FUNCTOR::return_type &z,
const SharedNoiseModel &model, Args &&... args)
: Base(model, key), measured_(z), noiseModel_(model),
func_(std::forward<Args>(args)...) {}
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
const std::function<R(T, boost::optional<Matrix &>)> func)
: Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
virtual ~FunctorizedFactor() {}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor<FUNCTOR>(*this)));
NonlinearFactor::shared_ptr(new FunctorizedFactor<T, R>(*this)));
}
Vector evaluateError(const T &params,
boost::optional<Matrix &> H = boost::none) const {
typename FUNCTOR::return_type x = func_(params, H);
Vector error = traits<typename FUNCTOR::return_type>::Local(measured_, x);
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
}
@ -110,22 +102,21 @@ public:
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl;
traits<typename FUNCTOR::return_type>::Print(measured_, " measurement: ");
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
}
virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
const FunctorizedFactor<FUNCTOR> *e =
dynamic_cast<const FunctorizedFactor<FUNCTOR> *>(&other);
const FunctorizedFactor<T, R> *e =
dynamic_cast<const FunctorizedFactor<T, R> *>(&other);
const bool base = Base::equals(*e, tol);
return e && Base::equals(other, tol) &&
traits<typename FUNCTOR::return_type>::Equals(this->measured_, e->measured_,
tol);
traits<R>::Equals(this->measured_, e->measured_, tol);
}
/// @}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
@ -138,8 +129,8 @@ private:
};
/// traits
template <typename FUNCTOR>
struct traits<FunctorizedFactor<FUNCTOR>>
: public Testable<FunctorizedFactor<FUNCTOR>> {};
template <typename T, typename R>
struct traits<FunctorizedFactor<T, R>>
: public Testable<FunctorizedFactor<T, R>> {};
} // namespace gtsam
} // namespace gtsam

89
gtsam/nonlinear/tests/testFunctorizedFactor.cpp
View File

@ -17,13 +17,12 @@
* @brief unit tests for FunctorizedFactor class
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/FunctorizedFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
@ -32,9 +31,9 @@ auto model = noiseModel::Isotropic::Sigma(9, 1);
/// Functor that takes a matrix and multiplies every element by m
class MultiplyFunctor {
double m_; ///< simple multiplier
double m_; ///< simple multiplier
public:
public:
using argument_type = Matrix;
using return_type = Matrix;
@ -42,32 +41,33 @@ public:
Matrix operator()(const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) const {
if (H)
*H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
return m_ * X;
}
};
/* ************************************************************************* */
TEST(FunctorizedFactor, Identity) {
Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
double multiplier = 1.0;
FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model,
multiplier);
FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
MultiplyFunctor(multiplier));
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
TEST(FunctorizedFactor, Multiply2) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model, multiplier);
FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
MultiplyFunctor(multiplier));
Vector error = factor.evaluateError(X);
@ -79,10 +79,10 @@ TEST(FunctorizedFactor, Equality) {
double multiplier = 2.0;
FunctorizedFactor<MultiplyFunctor> factor1(key, measurement, model,
multiplier);
FunctorizedFactor<MultiplyFunctor> factor2(key, measurement, model,
multiplier);
FunctorizedFactor<Matrix, Matrix> factor1(key, measurement, model,
MultiplyFunctor(multiplier));
FunctorizedFactor<Matrix, Matrix> factor2(key, measurement, model,
MultiplyFunctor(multiplier));
EXPECT(factor1.equals(factor2));
}
@ -94,7 +94,8 @@ TEST(FunctorizedFactor, Jacobians) {
double multiplier = 2.0;
FunctorizedFactor<MultiplyFunctor> factor(key, X, model, multiplier);
FunctorizedFactor<Matrix, Matrix> factor(key, X, model,
MultiplyFunctor(multiplier));
Values values;
values.insert<Matrix>(key, X);
@ -103,12 +104,14 @@ TEST(FunctorizedFactor, Jacobians) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FunctorizedFactor, Print) {
Matrix X = Matrix::Identity(2, 2);
double multiplier = 2.0;
FunctorizedFactor<MultiplyFunctor> factor(key, X, model, multiplier);
FunctorizedFactor<Matrix, Matrix> factor(key, X, model,
MultiplyFunctor(multiplier));
// redirect output to buffer so we can compare
stringstream buffer;
@ -120,18 +123,56 @@ TEST(FunctorizedFactor, Print) {
string actual = buffer.str();
cout.rdbuf(old);
string expected = " keys = { X0 }\n"
" noise model: unit (9) \n"
"FunctorizedFactor(X0)\n"
" measurement: [\n"
" 1, 0;\n"
" 0, 1\n"
"]\n"
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
string expected =
" keys = { X0 }\n"
" noise model: unit (9) \n"
"FunctorizedFactor(X0)\n"
" measurement: [\n"
" 1, 0;\n"
" 0, 1\n"
"]\n"
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
CHECK_EQUAL(expected, actual);
}
/* ************************************************************************* */
// Test factor using a std::function type.
TEST(FunctorizedFactor, Functional) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
MultiplyFunctor(multiplier);
FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model, functional);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
TEST(FunctorizedFactor, Lambda) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
auto lambda = [multiplier](const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) {
if (H)
*H = multiplier *
Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
return multiplier * X;
};
// FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
auto factor = FunctorizedFactor<Matrix>(key, measurement, model, lambda);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* *************************************************************************
*/
int main() {