Fix OrientedPlane3Factor jacobian #362

dwisth · 2020-06-22T04:55:01Z

This PR fixes the derivative of the OrientedPlane3Factor. Previously, the calculated Jacobians didn't account for the second function in evaluateError which meant that the Jacobian was not correct.

I have added a numericalDerivative to calculate the Jacobian of the second calculation and then applied the chain rule. I have also added a unit test to check that this is correct.

TODO: Ideally OrientedPlane3::error() should calculate analytical derivatives itself instead of using numericalDerivatives. Would anyone be able to help with this?

@mcamurri

This change is

dellaert

Awesome. I will think about the analytical error. Maybe @akshay-krishnan would not mind thinking about it, either: he just went deep into this for lines - I'm a bit rusty.

In the meantime I do have a number of style comments (and performance!) that will need to be addressed in the PR before we could merge. Mainly: move as much as possible in cpp files, and move the responsibility of derivatives from the factor to the OrientedPane itself. Finally, use fixed-size matrices allocated on the stack as malloc is evil :-)

dellaert · 2020-06-22T14:01:54Z

gtsam/geometry/OrientedPlane3.h

@@ -114,6 +114,10 @@ class GTSAM_EXPORT OrientedPlane3 {
   */
  Vector3 error(const OrientedPlane3& plane) const;

+  static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) {


I don't think we should add this. Instead, add 2 OptionalJacobian arguments to method above (H1 for this, H2 for plane)

gtsam/slam/OrientedPlane3Factor.h

dellaert · 2020-06-22T14:03:45Z

gtsam/slam/OrientedPlane3Factor.h

+      OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1);
+      err << predicted_plane.error(measured_p_);
+      // Numerically calculate the derivative since this function doesn't provide one.
+      auto f = boost::bind(&OrientedPlane3::Error, _1, _2);


move this logic into OrientedPlane.cpp, to calculate the optional derivatives of the error method, then call

Matrix33 error_H_predicted, error_H_measured; err << predicted.error(measured_p_, error_H_predicted, error_H_measured);

gtsam/slam/OrientedPlane3Factor.h

gtsam/slam/tests/testOrientedPlane3Factor.cpp

akshay-krishnan · 2020-06-23T05:52:02Z

Yes, I can help with the analytical derivative. Looks like we will need to define the derivatives for Unit3’s localCoordinates, since OrientedPlane3’s error function makes use of it.

About the error method in OrientedPlane3, I’m not sure what the norm 1 error is, but if this is the L1 norm, should it not be the absolute value? Are we missing an std::abs here?

dwisth · 2020-06-23T09:06:57Z

Thanks for all the detailed feedback @dellaert! I've incorporated almost all the changes you mentioned into the latest commit. Once we fix the derivatives we can remove OrientedPlane3::Error
method.

@akshay-krishnan - Thanks for your help :) The call to numericalDerivative is now in OrientedPlane3::error, which we will remove once Unit3::localCoordinates can output it's derivatives.

Also, I'm not sure about the comment about the L1 norm..

dellaert

Another style comment. Also, did you read the comment on the errorVector method ? If there a reason we don’t just switch to errorVector in the factor? That already is supposed to have the right derivatives.

gtsam/slam/OrientedPlane3Factor.cpp

dwisth · 2020-06-23T23:39:07Z

I updated the style (as suggested by Frank) and changed from OrientedPlane3::error to OrientedPlane3::errorVector. If I change this the OrientedPlane3Factor, lm_rotation_error unit test fails. It seems that the two functions give very different error values and derivatives.

E.g. Here is the error and derivatives for OrientedPlane3::error():

Error:
-0.0996687
        -0
         0
H1:
         0          0          1          0          0          0
 -0.099174    0.99174          0          0          0          0
         0          0          0  -0.995037 -0.0995037          0
H2:
       1        0        0
       0 0.996687        0
       0        0        1

And here is the error and derivatives for OrientedPlane3::errorVector() with the same inputs:

Error:
0.0995037
        0
        0
H1:
         0          0  -0.995037          0          0          0
 0.0990099  -0.990099          0          0          0          0
         0          0          0  -0.995037 -0.0995037          0
H2:
-0.995037         0         0
        0 -0.995037         0
        0         0         1

I'm not sure where to go from here - @akshay-krishnan any ideas?

akshay-krishnan · 2020-06-24T01:15:06Z

The error and errorVector functions do not do the same thing. One is the negative of the other. I believe this was designed to be this way. If you look at the source, errorVector computes (this - other) where as error computes (other - this) in tangential space (like localCoordinates).

I am still confused about the fact that neither of them have an absolute value.

dellaert · 2020-06-24T01:43:42Z

So, any factor calculates a vector of doubles, which GTSAM then takes the norm of to calculate the error. So, the sign of the individual error entries does not matter.

jlblancoc · 2020-06-24T06:19:13Z

My two cents: negative numbers are perfectly OK in an error vector. But I think both formulas, the one used to error evaluation and the one used to derive the Jacobians, should be the same (with same sign: both "a-b", not "a-b" vs "b-a"), or the nonlinear optimizer may be misled...

dellaert · 2020-06-24T11:57:36Z

Hmmm. The way you used errorVector seems correct. Can you confirm that (a) the errorVector derivative unit test succeeds, and (b) print out the output of the failing evaluateError test?

dellaert · 2020-06-24T12:02:37Z

My two cents: negative numbers are perfectly OK in an error vector. But I think both formulas, the one used to error evaluation and the one used to derive the Jacobians, should be the same (with same sign: both "a-b", not "a-b" vs "b-a"), or the nonlinear optimizer may be misled...

I think David is doing this. Is there a specific line you think is wrong?

jlblancoc · 2020-06-24T15:35:18Z

My two cents: negative numbers are perfectly OK in an error vector. But I think both formulas, the one used to error evaluation and the one used to derive the Jacobians, should be the same (with same sign: both "a-b", not "a-b" vs "b-a"), or the nonlinear optimizer may be misled...

I think David is doing this. Is there a specific line you think is wrong?

@dellaert : Honestly I didn't reviewed the code, I was just answering to:

The error and errorVector functions do not do the same thing. One is the negative of the other. I believe this was designed to be this way. If you look at the source, errorVector computes (this - other) where as error computes (other - this) in tangential space (like localCoordinates).

which seemed suspicious at first sight :-)

dellaert · 2020-07-09T19:24:03Z

So, what's the status on this now? @dwisth @akshay-krishnan

akshay-krishnan · 2020-07-10T06:11:28Z

So, what's the status on this now? @dwisth @akshay-krishnan

Sorry, I have not found time to work on the Jacobians for Unit3::error. Could @dwisth confirm if the errorVector unit tests worked?

dellaert · 2020-07-13T12:22:12Z

If you want this in 4.0.3, last checkpoint before 4.1, we need to clean up this PR now. It seems that switching to the other error made things work @dwisth ?

dwisth · 2020-07-14T22:30:35Z

Yes, seemed that switching error terms made it work. I'm going to take another look at this today / tomorrow and try to finish it off.

…r Unit3::localCoordinates.

dellaert · 2020-07-15T12:58:13Z

Thanks for your latest commit @dwisth . But now that you switched error functions, do we still need the analytical Jacobian for localCoordinates? If not I'd rather remove the numerical versions and their unit tests in this PR.

dwisth · 2020-07-16T01:19:33Z

To summarise the issue:

I first noticed that the Jacobians for the OrientedPlane3Factor were incorrect.
I then fixed the Jacobians but noticed that the unit test OrientedPlane3Factor, lm_rotation_error failed. This was using OrientedPlane3::errorVector() to calculate the residual.
I then noticed that if I switched the residual from OrientedPlane3::errorVector() to OrientedPlane3::error() all the unit tests passed. I tested this with a numerical derivative on Unit3::localCoordinates.
The only remaining thing to solve is the analytical derivative for Unit3::localCoordinates().

I'm a bit stuck on this last point and if anyone can help or give me some pointers it would be greatly appreciated.

dellaert · 2020-07-16T12:44:55Z

The only remaining thing to solve is the analytical derivative for Unit3::localCoordinates().
I'm a bit stuck on this last point and if anyone can help or give me some pointers it would be greatly appreciated.

Awesome. But, again: now that you switched error functions, do we still need the analytical Jacobian for localCoordinates? If not I'd rather remove the numerical versions and their unit tests in this PR.

dwisth · 2020-07-16T23:45:55Z

Awesome. But, again: now that you switched error functions, do we still need the analytical Jacobian for localCoordinates? If not I'd rather remove the numerical versions and their unit tests in this PR.

Yes we do. In the end I found the the original error function worked best (OrientedPlane3::error()) but we just need to fix the Jacobian. This requires the derivative for Unit3::localCoordinates.

varunagrawal · 2020-12-06T22:36:01Z

So here are my thoughts from looking into this issue:

Both error & errorVector for OrientedPlane3 are equivalent. error uses localCoordinates which is other - this, and errorVector uses n_.errorVector which seems to be this - other. We should be using localCoordinates in both ideally.
The Unit3 class has different definitions for error/errorVector and localCoordinates. I am not sure why this is the case since localCoordinates effectively computes other - this in the tangent space. The unit tests for localCoordinates make sense e.g. localCoordinates((1, 0, 0), (-1, 0, 0)) gives pi, 0 which is the angle and magnitude difference. error((1, 0, 0), (-1, 0, 0)) gives us 0, 0 which doesn't make sense to me because these are not the same unit vectors.

varunagrawal · 2020-12-06T22:38:39Z

I believe if we can figure out Unit3's localCoordinate jacobians (which are actually easy to compute) and resolve the ambiguity with error/errorVector we should be pretty much done.

Also, Unit3::error has been marked deprecated, but IMHO we should just have the one error function which has the signature and functionality of errorVector.

dellaert · 2021-01-21T15:44:51Z

This was never merged. I propose I merge it into the other branch and do all plane-related stuff at once.

dellaert

Merging into other branch to consolidate

varunagrawal · 2021-01-21T16:53:58Z

@dellaert be sure to keep track of my comments. We need the Unit3 jacobians implemented. :)

dellaert · 2021-01-22T04:54:32Z

@dellaert be sure to keep track of my comments. We need the Unit3 jacobians implemented. :)

Maybe. But I'll keep your comments in mind.

dwisth added 2 commits June 16, 2020 11:30

Add unit test for oriented plane 3 factor jacobian

df5ecac

Fix OrientedPlane3Factor jacobian using numericalDerivative

75eb859

dwisth mentioned this pull request Jun 22, 2020

Question about OrientedPlaneFactor implementation #283

Closed

dellaert requested changes Jun 22, 2020

View reviewed changes

Address Frank's comments and clean up changes

58a0f82

dellaert reviewed Jun 23, 2020

View reviewed changes

gtsam/slam/OrientedPlane3Factor.cpp Outdated Show resolved Hide resolved

Update style and switch to errorVector()

795380d

dellaert mentioned this pull request Jul 7, 2020

4.0.3 Release Tracking Issue #357

Closed

26 tasks

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

dellaert modified the milestones: GTSAM 4.1, GTSAM 4.1.1 Jul 14, 2020

Fix all unit tests. Only remaining item is the analytical Jacobian fo…

a897ee7

…r Unit3::localCoordinates.

mcamurri mentioned this pull request Oct 10, 2020

Incremental Smoothing with Plane Factor occasionally fails #561

Closed

varunagrawal added 2 commits December 6, 2020 17:51

Merge branch 'develop' into fix/oriented-plane3-factor-jacobian

edb3aea

formatting and small fixes

534fa6b

dellaert changed the base branch from develop to fix/planeFactor January 21, 2021 15:45

dellaert changed the base branch from fix/planeFactor to develop January 21, 2021 15:47

dellaert changed the base branch from develop to fix/planeFactor January 21, 2021 15:48

dellaert approved these changes Jan 21, 2021

View reviewed changes

Merge branch 'fix/planeFactor' into fix/oriented-plane3-factor-jacobian

fd8575b

dellaert merged commit 564c8bf into borglab:fix/planeFactor Jan 21, 2021

dellaert mentioned this pull request Jan 21, 2021

Fix/plane factor #564

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix OrientedPlane3Factor jacobian #362

Fix OrientedPlane3Factor jacobian #362

dwisth commented Jun 22, 2020 •

edited by dellaert

dellaert left a comment

dellaert Jun 22, 2020

dellaert Jun 22, 2020

akshay-krishnan commented Jun 23, 2020

dwisth commented Jun 23, 2020

dellaert left a comment

dwisth commented Jun 23, 2020

akshay-krishnan commented Jun 24, 2020

dellaert commented Jun 24, 2020

jlblancoc commented Jun 24, 2020

dellaert commented Jun 24, 2020

dellaert commented Jun 24, 2020

jlblancoc commented Jun 24, 2020

dellaert commented Jul 9, 2020

akshay-krishnan commented Jul 10, 2020

dellaert commented Jul 13, 2020

dwisth commented Jul 14, 2020

dellaert commented Jul 15, 2020

dwisth commented Jul 16, 2020 •

edited

dellaert commented Jul 16, 2020

dwisth commented Jul 16, 2020

varunagrawal commented Dec 6, 2020 •

edited

varunagrawal commented Dec 6, 2020

dellaert commented Jan 21, 2021

dellaert left a comment

varunagrawal commented Jan 21, 2021

dellaert commented Jan 22, 2021

Fix OrientedPlane3Factor jacobian #362

Fix OrientedPlane3Factor jacobian #362

Conversation

dwisth commented Jun 22, 2020 • edited by dellaert

dellaert left a comment

Choose a reason for hiding this comment

dellaert Jun 22, 2020

Choose a reason for hiding this comment

dellaert Jun 22, 2020

Choose a reason for hiding this comment

akshay-krishnan commented Jun 23, 2020

dwisth commented Jun 23, 2020

dellaert left a comment

Choose a reason for hiding this comment

dwisth commented Jun 23, 2020

akshay-krishnan commented Jun 24, 2020

dellaert commented Jun 24, 2020

jlblancoc commented Jun 24, 2020

dellaert commented Jun 24, 2020

dellaert commented Jun 24, 2020

jlblancoc commented Jun 24, 2020

dellaert commented Jul 9, 2020

akshay-krishnan commented Jul 10, 2020

dellaert commented Jul 13, 2020

dwisth commented Jul 14, 2020

dellaert commented Jul 15, 2020

dwisth commented Jul 16, 2020 • edited

dellaert commented Jul 16, 2020

dwisth commented Jul 16, 2020

varunagrawal commented Dec 6, 2020 • edited

varunagrawal commented Dec 6, 2020

dellaert commented Jan 21, 2021

dellaert left a comment

Choose a reason for hiding this comment

varunagrawal commented Jan 21, 2021

dellaert commented Jan 22, 2021

dwisth commented Jun 22, 2020 •

edited by dellaert

dwisth commented Jul 16, 2020 •

edited

varunagrawal commented Dec 6, 2020 •

edited