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

Merge pull request #329 from borglab/feature/functorized-factor 

Functorized Factor
release/4.3a0
Varun Agrawal 2020-07-12 22:42:09 -04:00 committed by GitHub
parent cc7f603c04 1f66a73147
commit 05ad89355d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 333 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
gtsam/nonlinear/FunctorizedFactor.h Normal file
View File

@ -0,0 +1,148 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FunctorizedFactor.h
* @date May 31, 2020
* @author Varun Agrawal
**/
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <cmath>
namespace gtsam {
/**
* Factor which evaluates provided unary functor and uses the result to compute
* error with respect to the provided measurement.
*
* Template parameters are
* @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);
*
* /// Functor that takes a matrix and multiplies every element by m
* class MultiplyFunctor {
* double m_; ///< simple multiplier
* public:
* 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;
* }
* };
*
* Matrix measurement = Matrix::Identity(3, 3);
* double multiplier = 2.0;
*
* FunctorizedFactor<Matrix, Matrix> factor(keyX, measurement, model,
* MultiplyFunctor(multiplier));
*/
template <typename R, typename T>
class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
private:
using Base = NoiseModelFactor1<T>;
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:
/** default constructor - only use for serialization */
FunctorizedFactor() {}
/** Construct with given x and the parameters of the basis
*
* @param key: Factor key
* @param z: Measurement object of same type as that returned by functor
* @param model: Noise model
* @param func: The instance of the functor object
*/
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<R, T>(*this)));
}
Vector evaluateError(const T &params,
boost::optional<Matrix &> H = boost::none) const {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
}
/// @name Testable
/// @{
void print(const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl;
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<R, T> *e =
dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
const bool base = Base::equals(*e, tol);
return e && Base::equals(other, tol) &&
traits<R>::Equals(this->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(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
ar &BOOST_SERIALIZATION_NVP(func_);
}
};
/// traits
template <typename R, typename T>
struct traits<FunctorizedFactor<R, T>>
: public Testable<FunctorizedFactor<R, T>> {};
/**
* Helper function to create a functorized factor.
*
* Uses function template deduction to identify return type and functor type, so
* template list only needs the functor argument type.
*/
template <typename T, typename R, typename FUNC>
FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
const SharedNoiseModel &model,
const FUNC func) {
return FunctorizedFactor<R, T>(key, z, model, func);
}
} // namespace gtsam

185
gtsam/nonlinear/tests/testFunctorizedFactor.cpp Normal file
View File

@ -0,0 +1,185 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------1-------------------------------------------
*/
/**
* @file testFunctorizedFactor.cpp
* @date May 31, 2020
* @author Varun Agrawal
* @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>
using namespace std;
using namespace gtsam;
Key key = Symbol('X', 0);
auto model = noiseModel::Isotropic::Sigma(9, 1);
/// Functor that takes a matrix and multiplies every element by m
class MultiplyFunctor {
double m_; ///< simple multiplier
public:
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;
}
};
/* ************************************************************************* */
// Test identity operation for FunctorizedFactor.
TEST(FunctorizedFactor, Identity) {
Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
double multiplier = 1.0;
auto functor = MultiplyFunctor(multiplier);
auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, functor);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test FunctorizedFactor with multiplier value of 2.
TEST(FunctorizedFactor, Multiply2) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model,
MultiplyFunctor(multiplier));
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test equality function for FunctorizedFactor.
TEST(FunctorizedFactor, Equality) {
Matrix measurement = Matrix::Identity(2, 2);
double multiplier = 2.0;
auto factor1 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
MultiplyFunctor(multiplier));
auto factor2 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
MultiplyFunctor(multiplier));
EXPECT(factor1.equals(factor2));
}
/* *************************************************************************** */
// Test Jacobians of FunctorizedFactor.
TEST(FunctorizedFactor, Jacobians) {
Matrix X = Matrix::Identity(3, 3);
Matrix actualH;
double multiplier = 2.0;
auto factor =
MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
Values values;
values.insert<Matrix>(key, X);
// Check Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
// Test print result of FunctorizedFactor.
TEST(FunctorizedFactor, Print) {
Matrix X = Matrix::Identity(2, 2);
double multiplier = 2.0;
auto factor =
MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
// redirect output to buffer so we can compare
stringstream buffer;
streambuf *old = cout.rdbuf(buffer.rdbuf());
factor.print();
// get output string and reset stdout
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";
CHECK_EQUAL(expected, actual);
}
/* ************************************************************************* */
// Test FunctorizedFactor 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);
auto factor =
MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test FunctorizedFactor with a lambda function.
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 = MakeFunctorizedFactor<Matrix>(key, measurement, model, lambda);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */