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

fixes, better tests, docs

release/4.3a0
Varun Agrawal 2020-06-01 19:52:50 -04:00
parent 1db0f441bc
commit 131213a983
2 changed files with 78 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -11,6 +11,7 @@
/**
* @file FunctorizedFactor.h
* @date May 31, 2020
* @author Varun Agrawal
**/
@ -26,13 +27,35 @@ namespace gtsam {
/**
* Factor which evaluates functor and uses the result to compute
* error on provided measurement.
* The provided FUNCTOR should provide two definitions: `argument_type` which
* 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.
*
* 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:
* using argument_type = Matrix;
* using return_type = Matrix;
* 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<MultiplyFunctor> factor(keyX, measurement, model, multiplier);
*/
template <typename FUNCTOR>
class GTSAM_EXPORT FunctorizedFactor
@ -82,26 +105,23 @@ public:
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream &
operator<<(std::ostream &os, const FunctorizedFactor<FUNCTOR> &f) {
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
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<typename FUNCTOR::return_type>::Print(measured_, " measurement: ");
std::cout << *this << std::endl;
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
}
virtual bool equals(const NonlinearFactor &other, double tol = 1e-9)
const {
virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
const FunctorizedFactor<FUNCTOR> *e =
dynamic_cast<const FunctorizedFactor<FUNCTOR>*>(&other);
dynamic_cast<const FunctorizedFactor<FUNCTOR> *>(&other);
const bool base = Base::equals(*e, tol);
return e != nullptr && base;
return e && Base::equals(other, tol) &&
traits<typename FUNCTOR::return_type>::Equals(this->measured_, e->measured_,
tol);
}
/// @}
@ -117,7 +137,9 @@ private:
}
};
// TODO(Varun): Include or kill?
// template <> struct traits<Functorized> : public Testable<ImuFactor2> {};
/// traits
template <typename FUNCTOR>
struct traits<FunctorizedFactor<FUNCTOR>>
: public Testable<FunctorizedFactor<FUNCTOR>> {};
} // namespace gtsam

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

@ -20,14 +20,15 @@
#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;
Key keyX = Symbol('X', 0);
auto model = noiseModel::Isotropic::Sigma(3, 1);
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 {
@ -41,60 +42,73 @@ 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());
return m_ * X;
}
};
TEST(FunctorizedFactor, Identity) {
Matrix X = Matrix::Identity(3, 3);
Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
double multiplier = 1.0;
FunctorizedFactor<MultiplyFunctor> factor(keyX, X, model, multiplier);
FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model,
multiplier);
Values values;
values.insert<Matrix>(keyX, X);
Matrix error = factor.evaluateError(X);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
TEST(FunctorizedFactor, Multiply2) {
Matrix X = Matrix::Identity(3, 3);
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
FunctorizedFactor<MultiplyFunctor> factor(keyX, X, model, multiplier);
FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model, multiplier);
Values values;
values.insert<Matrix>(keyX, X);
Vector error = factor.evaluateError(X);
Matrix error = factor.evaluateError(X);
Matrix expected = Matrix::Identity(3, 3);
expected.resize(9, 1);
EXPECT(assert_equal(expected, error, 1e-9));
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
TEST(FunctorizedFactor, Equality) {
Matrix X = Matrix::Identity(2, 2);
Matrix measurement = Matrix::Identity(2, 2);
double multiplier = 2.0;
FunctorizedFactor<MultiplyFunctor> factor1(keyX, X, model, multiplier);
FunctorizedFactor<MultiplyFunctor> factor2(keyX, X, model, multiplier);
FunctorizedFactor<MultiplyFunctor> factor1(key, measurement, model,
multiplier);
FunctorizedFactor<MultiplyFunctor> factor2(key, measurement, model,
multiplier);
EXPECT(factor1.equals(factor2));
}
//******************************************************************************
TEST(FunctorizedFactor, Jacobians) {
Matrix X = Matrix::Identity(3, 3);
Matrix actualH;
double multiplier = 2.0;
FunctorizedFactor<MultiplyFunctor> factor(key, X, model, multiplier);
Values values;
values.insert<Matrix>(key, X);
// Check 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(keyX, X, model, multiplier);
FunctorizedFactor<MultiplyFunctor> factor(key, X, model, multiplier);
// redirect output to buffer so we can compare
stringstream buffer;
@ -107,13 +121,13 @@ TEST(FunctorizedFactor, Print) {
cout.rdbuf(old);
string expected = " keys = { X0 }\n"
" noise model: unit (3) \n"
" noise model: unit (9) \n"
"FunctorizedFactor(X0)\n"
" measurement: [\n"
" 1, 0;\n"
" 0, 1\n"
"]\n"
" noise model sigmas: 1 1 1\n";
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
CHECK_EQUAL(expected, actual);
}