implemented new evaluate() and evaluateAndJacobians()

[rdube]

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.

[rdube]

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]

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]

@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]

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

[rdube]

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

[mikebosse]

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 https://github.com/ethz-asl/curves/pull/30#issuecomment-55617065.

[furgalep]

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]

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]

@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]

Cool! Looks clean! This will be done today.

[rdube]

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]

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]

Thank, lesson learned! Will try to remove them.

[mikebosse]

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]

No idea.

[furgalep]

Okay, make this compile and you are good to merge

[HannesSommer]

@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]

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]

Can you try?

[HannesSommer]

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]

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]

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

[HannesSommer]

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]

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]

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

1. have addressed all the code review points, and
2. 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!