Functorized Factor #329

varunagrawal · 2020-06-01T02:09:37Z

This PR adds a new type of factor which allows for more dynamic computation.

Consider the case that the input type to a factor's evaluateError method is different from the measurement, and the factor requires some intermediate processing of the input to match the type (both syntactically and semantically) of the measurement. The FunctorizedFactor accepts the functor argument type as a template argument and operates on the input, allowing for considerable reuse.

E.g. if we wish to compute the error from a simple linear matrix operation Ax = b, we could define a factor like:

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 X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
double multiplier = 1.0;

// `multiplier` will be passed in to the functor constructor
auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, multiplier);

// This factor now evaluates `m*A` and computes the error to `b` -> `mA - b`
Matrix error = factor.evaluateError(X);

Thus the factor is completely determined by the functor passed in, thus providing a great amount of simplicity.

The factor also supports std::function and C++ lambda types.

This change is

dellaert · 2020-06-01T17:46:38Z

gtsam/linear/NoiseModel.cpp

@@ -619,6 +619,7 @@ void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
 // Unit
 /* ************************************************************************* */
 void Unit::print(const std::string& name) const {
+  //TODO(Varun): Do we need that space at the end?


remove from this PR

dellaert · 2020-06-01T17:47:03Z

gtsam/nonlinear/FunctorizedFactor.h

+/**
+ * Factor which evaluates functor and uses the result to compute
+ * error on provided measurement.
+ * The provided FUNCTOR should provide two definitions: `argument_type` which


type aliases

dellaert · 2020-06-01T17:47:49Z

gtsam/nonlinear/FunctorizedFactor.h

+ * 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.
+ *


Add the example from the description after you fix it (constructor has no key, jacobian is ignored although it would be simple in your example)

dellaert · 2020-06-01T17:48:30Z

gtsam/nonlinear/FunctorizedFactor.h

+  /// @{
+  GTSAM_EXPORT friend std::ostream &
+  operator<<(std::ostream &os, const FunctorizedFactor<FUNCTOR> &f) {
+    os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();


Stream operator is not part of Testable. Move it somewhere else? Or, in fact remove it. Adding streaming is a big design exercise, large refactor we might not want to tackle now

Just as a note, there are factors that have the stream operator as a part of Testable.

dellaert · 2020-06-01T17:50:10Z

gtsam/nonlinear/FunctorizedFactor.h

+    const {
+      const FunctorizedFactor<FUNCTOR> *e =
+          dynamic_cast<const FunctorizedFactor<FUNCTOR>*>(&other);
+      const bool base = Base::equals(*e, tol);


will fail if e == nullptr

dellaert · 2020-06-01T17:50:31Z

gtsam/nonlinear/FunctorizedFactor.h

+  }
+};
+
+// TODO(Varun): Include or kill?


if factors have it, include. But do they?

They do. Included.

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

dellaert · 2020-06-01T17:52:08Z

Awesome. Some comments...

gchenfc · 2020-06-01T22:30:52Z

gtsam/nonlinear/FunctorizedFactor.h

+ */
+template <typename FUNCTOR>
+class GTSAM_EXPORT FunctorizedFactor
+    : public NoiseModelFactor1<typename FUNCTOR::argument_type> {


Is it possible to allow this to accept more variables? It seems like it should be reasonably common to have

|h(x1, x2, ...) - z|

where we define a functor for h(x1, x2, ...).

Yup it takes any number of arguments. That's why you have the template <typename... Args> on the constructor.

Sorry to clarify, I meant rather than NoiseModelFactor1, also be able to have NoiseModelFactor2, 3, etc

So right now from the constructor templating it's able to do something like

h(x, theta1, theta2, ...)

where theta are fixed parameters, but I was wondering about multiple variables

Ah I see. Yeah that's phase 2. Now that we have this factor working, it should be straightforward to extend it to multiple keys.

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

dellaert

Sorry, this is just not ready yet - since this introduces a new core factor we need to think harder.

dellaert · 2020-06-19T13:38:41Z

gtsam/nonlinear/FunctorizedFactor.h

+/**
+ * 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


Now I’m wondering whether this is just any std::function? And whether we could use it with a lambda? https://en.cppreference.com/w/cpp/utility/functional/function

I propose you add two unit tests that exercise those cases and we can see if and how it leads to a change in your requirements. It could also be much simpler to just say that the Functor has to satisfy the standard function concept

In fact, reading more, we probably have to:

https://stackoverflow.com/questions/35907454/why-stdfunctionargument-type-has-been-deprecated

“Now that we have decltype that can be used to deduce a callable's return type, variadic templates and perfect forwarding to forward arbitrary number of arguments to functions, etc. the pre-C++11 mechanisms are way too cumbersome to use.”

https://en.cppreference.com/w/cpp/language/decltype

The following post seems to hint that we might not want to know the argument types though: https://stackoverflow.com/questions/6512019/can-we-get-the-type-of-a-lambda-argument

I actually thought about this at first and had some initial difficulty which I don't remember right now. Then again, my understanding of functionals in C++ has improved quite a bit since then so I'll tackle this today.

So I've not had a lot of success with this without over-complicating the code. Something to consider would be just removing the need for argument_type and return_type from the FUNCTOR and making it match the traits of std::function so that they can be used interchangeably.

Yes. And it should also do lambdas. Did you make the two unit tests I proposed?

I did, but I've been having trouble getting the code to compile.

dellaert · 2020-06-19T13:44:04Z

gtsam/nonlinear/FunctorizedFactor.h

+ *   double multiplier = 2.0;
+ *   FunctorizedFactor<MultiplyFunctor> factor(keyX, measurement, model, multiplier);
+ */
+template <typename FUNCTOR>


To address my other comment, perhaps we should instead template on T. And then a Functor/lambda/std::function passed needs to take (T, jacobian). Not necessarily dynamic jacobian, btw...

The problem here is that we won't know what the return type is since we don't have anything to call decltype on. We need the return type to define the measurement type.

dellaert · 2020-06-19T13:44:41Z

gtsam/nonlinear/FunctorizedFactor.h

+   * @param model: Noise model
+   * @param args: Variable number of arguments used to instantiate functor
+   */
+  template <typename... Args>


This would only be one constructor. The other takes a lambda or function object...

… and lambdas

By adding the helper function MakeFunctorizedFactor, we now only need to provide the argument type in the template parameter list. This considerably simplifies the factor declaration, while removing the need for argument type and return type in the functor definition. Also added tests for std::function and lambda functions.

varunagrawal · 2020-07-07T01:56:54Z

So lots of good stuff thanks to my discussion with @dellaert. Declaring the FunctorizedFactor is crazy simple now (with lots of nice function template deduction happening behind the scenes), and we have eliminated the need for functors to define argument_type and return_type in their definitions.

varunagrawal · 2020-07-13T02:42:03Z

Merging since this PR doesn't introduce any breaking changes

Added FunctorizedFactor and corresponding tests

1db0f44

varunagrawal added enhancement Improvement to GTSAM feature New proposed feature design Design choices labels Jun 1, 2020

varunagrawal requested review from ProfFan, jlblancoc, dellaert, Alescontrela, gchenfc and step13489mcc June 1, 2020 02:09

varunagrawal self-assigned this Jun 1, 2020

dellaert reviewed Jun 1, 2020

View reviewed changes

gchenfc reviewed Jun 1, 2020

View reviewed changes

fixes, better tests, docs

131213a

varunagrawal requested review from dellaert and gchenfc June 1, 2020 23:53

varunagrawal added 2 commits June 1, 2020 19:54

remove TODO

ea8b319

improved documentation

0bbb396

varunagrawal mentioned this pull request Jun 18, 2020

4.0.3 Release Tracking Issue #357

Closed

26 tasks

dellaert requested changes Jun 19, 2020

View reviewed changes

varunagrawal added 2 commits July 6, 2020 17:38

new definition for FunctorizedFactor to allow for using std::function…

7d0e440

… and lambdas

varunagrawal requested a review from dellaert July 7, 2020 01:59

Merge branch 'develop' into feature/functorized-factor

1f66a73

varunagrawal added this to the GTSAM 4.1 milestone Jul 12, 2020

dellaert approved these changes Jul 13, 2020

View reviewed changes

varunagrawal merged commit 05ad893 into develop Jul 13, 2020

varunagrawal deleted the feature/functorized-factor branch July 13, 2020 02:42

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Functorized Factor #329

Functorized Factor #329

varunagrawal commented Jun 1, 2020 •

edited

Loading

dellaert Jun 1, 2020

dellaert Jun 1, 2020

dellaert Jun 1, 2020

dellaert Jun 1, 2020

varunagrawal Jun 1, 2020

dellaert Jun 1, 2020

dellaert Jun 1, 2020

varunagrawal Jun 1, 2020

dellaert commented Jun 1, 2020

gchenfc Jun 1, 2020

varunagrawal Jun 1, 2020

gchenfc Jun 2, 2020

varunagrawal Jun 2, 2020

dellaert left a comment

dellaert Jun 19, 2020

varunagrawal Jul 5, 2020

varunagrawal Jul 6, 2020

dellaert Jul 6, 2020

varunagrawal Jul 6, 2020

dellaert Jun 19, 2020

varunagrawal Jul 6, 2020

dellaert Jun 19, 2020

varunagrawal commented Jul 7, 2020

varunagrawal commented Jul 13, 2020

Functorized Factor #329

Functorized Factor #329

Conversation

varunagrawal commented Jun 1, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dellaert commented Jun 1, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

varunagrawal commented Jul 7, 2020

varunagrawal commented Jul 13, 2020

varunagrawal commented Jun 1, 2020 •

edited

Loading