implemented new evaluate() and evaluateAndJacobians() #30

rdube · 2014-09-15T13:15:47Z

Implemented new functions for making real adaptive factors (no switch case). The rest of the solution will be in AdaptiveOdometryFactor::unwhitenedError() on the trajectory_maintainer gitrepo :

https://github.com/ethz-asl/continuous-trajectory-scanmatching

The old evaluate() and evaluateAndJacobians() were kept until the adaptive factor architecture is validated.

Thanks for commenting.

…ptive factor

rdube · 2014-09-15T14:31:08Z

Thanks for your comments Mike! The type of chainRule was changed to Eigen::MatrixXd.

I understand that using unordered_map might not be the most efficient. The change to use vectors instead should not be much work.

Before we do the changes, does someone disagree to use std::vector as the containers for coefficients and jacobian pointers which are passed to the evaluator eval() function? @gawela @furgalep

furgalep · 2014-09-15T16:30:05Z

Are we sure that using an unordered map is too heavy?

At some point we have to map a coefficient's Jacobian to some place in the std::vector<Matrix> Jacobian

Before implementing, can someone explain how it would work with a vector?

furgalep · 2014-09-15T16:38:32Z

@mikebosse, here is the function that we have to overload when making a factor:

  /**
   * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
   * Override this method to finish implementing an N-way factor.
   * If any of the optional Matrix reference arguments are specified, it should compute
   * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
   */
  virtual Vector unwhitenedError(const Values& x, boost::optional< std::vector< Matrix > &> H = boost::none) const = 0;

The Jacobians go out in a std::vector. The ordering of this vector is specified by the factor. See the factor header here:
https://bitbucket.org/gtborg/gtsam/src/bcab483574f2bd636dcd8171cf1b62fdfe15b6d0/gtsam/inference/Factor.h?at=develop

Values is something like an unordered map:
https://bitbucket.org/gtborg/gtsam/src/bcab483574f2bd636dcd8171cf1b62fdfe15b6d0/gtsam/nonlinear/Values.h?at=develop

Values is using a map underneath:

    // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
    // below) to clone and deallocate the Value objects, and a boost
    // fast_pool_allocator to allocate map nodes.  In this way, all memory is
    // allocated in a boost memory pool.
    typedef boost::ptr_map<
        Key,
        Value,
        std::less<Key>,
        ValueCloneAllocator,
        boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;

For the first argument of the evaluator, we could make an adapter that wraps the Values object and provides map-like access.

But somehow we have to compute these Jacobians (even if there are duplicates) and apply the chain rule and then put the Jacobians back in this standard vector.

For the key-->index mapping, isn't a map the right data structure? Why do we think maps are heavy or slow?

furgalep · 2014-09-15T16:40:19Z

curves/include/curves/Evaluator.hpp

+
+  virtual ValueType evaluateAndJacobians(const boost::unordered_map<Key, Coefficient>& keyCoefficient,
+                                         const boost::unordered_map<Key, Eigen::MatrixXd*>& keyJacobian,
+                                         const Eigen::MatrixXd& chainRule) const = 0;


Put keyJacobian last as it is an output variable.

furgalep · 2014-09-15T16:49:09Z

@rdube, @gawela Thanks for working on this! We will get there.

rdube · 2014-09-15T16:53:49Z

@furgalep @mikebosse Thanks to you guys for the comments and help. We are learning a lot while having fun!

mikebosse · 2014-09-15T18:41:07Z

I think that using a map just to pass matrices from the evaluator to the
factor will be to expensive since a new map will need to be created for
every factor, and there is no good way to preallocate and keep using the
same map.
Since unordered maps are hash tables underneath, they are a bit too heavy
for uses when there are < 10 items.
the coefficient map should just be adapted from GTSAMs value map since it
is already there.
when a factor is created, it can store the position of each evaluator's
keys in its list of unique keys that it needs to setup for gtsam. Then
when its unwhitened error is called it can pass a std::vector of jacobian
pointers to each eval for pointing to the appropiate matrix in its Jacobain
vector. Then there is no need for extra key lookups or unordered_map
generation.

On Mon, Sep 15, 2014 at 6:30 PM, Paul Furgale notifications@github.com
wrote:

Are we sure that using an unordered map is too heavy?

At some point we have to map a coefficient's Jacobian to some place in the std::vector
Jacobian

Before implementing, can someone explain how it would work with a vector?

—
Reply to this email directly or view it on GitHub
#30 (comment).

furgalep · 2014-09-15T19:40:22Z

Can you give some pseudocode for what this would look like? Let's have, as an example, a factor with two evaluators where the evaluators could have some of the same keys.

mikebosse · 2014-09-15T20:04:25Z

Here is some really rough pseudo code for constructor and unwhitened error:

RelativePoseFactor::RelativePoseFactor( curve, timeA, timeB) {

  evalA = curve.getEvaluator(timeA);
  evalB = curve.getEvaluator(timeB);
  vector<int> JacOrderA, JacOrderB;
  map<key, index> unique_keys;

  for i and each key in evalA {
    if( !unique_keys.has(key)) {
     unique_keys[key] = unique_keys.size();
    }
    JacOrderA[i] = unique_keys[key];

  // repeat same for EvalB ....
  for i and each key in evalB {
     if(!unique_keys.has(key)){
       unique_keys[key] = unique_keys.size();
    }
   JacOrderB[i] = unique_keys[key];
  }
}

RelativePoseFactor::UnwhitenedError(Values, JacobianVec) {

  Matrix chainRuleMat = Jacobian for A
  vector<Matrix *> jacobianPtrs;
  for each i in JacOrderA.size() {
    jacobianPtrs[i] = &JacobianVec[JacOrderA[i]];
  }
  poseA = evalA.eval(Values,chainRuleMat,jacobianPtrs);

  Matrix chainRuleMat = Jacobian for B;
  jacobianPtrs.clear();
  for each i in JacOrderB.size() {
    jacobianPtrs.push_back(&JacobianVec[JacOrderB[i]]);
  }
  poseB = evalA.eval(Values,chainRuleMat,jacobianPtrs);

  return error of (poseB-poseA);
}

furgalep · 2014-09-16T07:42:04Z

@mikebosse, Yes, that is a nice approach. Some small things: we shouldn't use map.size() as it is not guaranteed to be constant time. This can be done easily with a counter, though.

Cool! Let's do it like that.

rdube · 2014-09-16T07:49:09Z

Cool! Looks clean! This will be done today.

…rgs of evaluate functions

rdube · 2014-09-16T20:31:03Z

This last commits integrate the use of std::vector for passing the coefficients and jacobians. Profiling results of an example with 1000 coefficients and 100000 relative factor are found in the trajectory maintainer repo for both vector and unordered_maps approaches. See:

https://github.com/ethz-asl/continuous-trajectory-scanmatching/tree/master/profiling

Thanks!

furgalep · 2014-09-16T20:44:37Z

Hi Renaud,
As a general rule, please don't check binary files into GitHub. They are (nearly) impossible to remove and cause a lot of headaches. Files like that can be posted to issues of the repo. Admittedly, this is hard with PDFs but images work great.

rdube · 2014-09-16T20:47:28Z

Thank, lesson learned! Will try to remove them.

mikebosse · 2014-09-16T21:02:42Z

it looks like almost half the time is being spent in the eigeny matrix
constructor. are lots of jacobians being copied? or, @furgalep, is this normal
for eigen?

furgalep · 2014-09-17T04:37:39Z

No idea.

furgalep · 2014-09-17T04:39:24Z

Okay, make this compile and you are good to merge

HannesSommer · 2014-09-17T05:38:11Z

@rdube , do you know how to remove the pdf from the history - so not only commit a delete? Otherwise I can show you. It is better to have that removed from the pull request before merge, to keep the mess small..

HannesSommer · 2014-09-17T07:00:24Z

Oh, I thought the PDFs were in the PR but they are in the master of https://github.com/ethz-asl/continuous-trajectory-scanmatching. Hm, shall we still remove the PDFs from the history? I guess not many people have that branch tracked, yet.

furgalep · 2014-09-17T07:49:36Z

Can you try?

HannesSommer · 2014-09-17T07:56:39Z

Yes of course, the problem is not removing the files - the problem is for the people that have this branch locally. They might have to reset or rebase at the latest before they push next time (exactly when they have this commit already in a local branch). So I guess only @rdube , @gawela and @mikebosse could be in that situation. So are you okay with me rewriting the master's history? I can quickly come by to whoever is unsure how to deal with that on his machine..

furgalep · 2014-09-17T08:05:25Z

Hi Renaud,
Please don't merge PRs until they compile on the build server. This one didn't compile:

http://129.132.38.183:8080/job/curves/32/

And now the master doesn't compile:

http://129.132.38.183:8080/job/curves/33/

Can you make a new branch and try to fix this?

rdube · 2014-09-17T08:34:33Z

Ok it builds fine on my computer but I will check to fix this.

HannesSommer · 2014-09-17T13:43:20Z

That was a unrelated jenkins issue. Something wired with updating catkin_simple (it got conflicts by pulling - I'm on it). I reset that rep and it is fine now : http://129.132.38.183:8080/job/curves/36/.

HannesSommer · 2014-09-17T13:46:06Z

Ah, and the profiling PDFs in https://github.com/ethz-asl/continuous-trajectory-scanmatching are in the "never committed" state, as they should be ;).

furgalep · 2014-09-17T14:11:03Z

Regarding the Jenkins issue, the general rule is: don't merge until you

have addressed all the code review points, and
get the green light that the merge build is successful.

It is true that sometimes this is a Jenkins issue, but in that case, do your best to debug, then escalate to one of the admins (hannes, me, simon, mike).

OK, thanks for all of your hard work!

Thanks @HannesSommer for sorting that all out!

rdube added 2 commits September 15, 2014 15:11

implemented new evaluate() and evaluateAndJacobians() for the new ada…

8f7fc7c

…ptive factor

changed the type of chainRule to Eigen::MatrixXd

f839484

furgalep reviewed Sep 15, 2014
View reviewed changes

integrated the approach to use vectors instead of unordered_maps as a…

b05e734

…rgs of evaluate functions

rdube merged commit b05e734 into master Sep 17, 2014

HannesSommer deleted the fix/evaluator branch September 17, 2014 13:44

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

implemented new evaluate() and evaluateAndJacobians() #30

implemented new evaluate() and evaluateAndJacobians() #30

rdube commented Sep 15, 2014

rdube commented Sep 15, 2014

furgalep commented Sep 15, 2014

furgalep commented Sep 15, 2014

furgalep Sep 15, 2014

furgalep commented Sep 15, 2014

rdube commented Sep 15, 2014

mikebosse commented Sep 15, 2014

furgalep commented Sep 15, 2014

mikebosse commented Sep 15, 2014

furgalep commented Sep 16, 2014

rdube commented Sep 16, 2014

rdube commented Sep 16, 2014

furgalep commented Sep 16, 2014

rdube commented Sep 16, 2014

mikebosse commented Sep 16, 2014

furgalep commented Sep 17, 2014

furgalep commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

furgalep commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

furgalep commented Sep 17, 2014

rdube commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

furgalep commented Sep 17, 2014

implemented new evaluate() and evaluateAndJacobians() #30

implemented new evaluate() and evaluateAndJacobians() #30

Conversation

rdube commented Sep 15, 2014

rdube commented Sep 15, 2014

furgalep commented Sep 15, 2014

furgalep commented Sep 15, 2014

furgalep Sep 15, 2014

Choose a reason for hiding this comment

furgalep commented Sep 15, 2014

rdube commented Sep 15, 2014

mikebosse commented Sep 15, 2014

furgalep commented Sep 15, 2014

mikebosse commented Sep 15, 2014

furgalep commented Sep 16, 2014

rdube commented Sep 16, 2014

rdube commented Sep 16, 2014

furgalep commented Sep 16, 2014

rdube commented Sep 16, 2014

mikebosse commented Sep 16, 2014

furgalep commented Sep 17, 2014

furgalep commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

furgalep commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

furgalep commented Sep 17, 2014

rdube commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

HannesSommer commented Sep 17, 2014

furgalep commented Sep 17, 2014