Question about OrientedPlaneFactor implementation

[clams-casino]

Hi, I'm trying to implement a slam program based on plane landmarks and the OrientedPlaneFactor. I have a question about the way the jacobians are computed in the evaluateError() method in the factor.

virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
          boost::none) const {
    OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2);
    Vector err(3);
    err << predicted_plane.error(measured_p_);
    return (err);
}

So the jacobians for the predicted plane with respect to the pose and plane variables are computed, but the jacobians for the error with respect to the predicted plane is not.

There are two methods defined for differencing two OrientedPlane3s, error() and errorVector() which also computes jacobians.

  /** Computes the error between two planes.
   *  The error is a norm 1 difference in tangent space.
   * @param the other plane
   */
  Vector3 error(const OrientedPlane3& plane) const;

  /** Computes the error between the two planes, with derivatives.
   *  This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
   *  Unit3::localCoordinates. This one has correct derivatives.
   *  NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
   * @param the other plane
   */
  Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
                      OptionalJacobian<3, 3> H2 = boost::none) const;

Why does the OrientedPlaneFactor's evaluateError() not use the errorVector() which also gives the jacobian of the error with respect to the predicted plane?

[dellaert]

Yes, if the comment is correct and .error has incorrect derivatives this should probably be changed. I don't want to break any existing code someone might have though, so this should probably only be changed if the unit tests still work and the convergence behavior is the same (or better). Feel free to investigate and do a PR, if you're up for it.

[jlblancoc]

Maybe related: in #183 , unit test thresholds for OrientedPlane3 had to be raised from 1e-9 to 1e-5 for them to pass in x32 builds. I thought it was just a low-level numerical accuracy thing, but might be something else if you found an inconsistency in the formulation...

[dwisth]

I'm also interested in the correctness of the OrientedPlane3Factor.

I added a small unit test to testOrientedPlane3Factor.cpp (branch) to check that the calculated Jacobians match the numerical ones. It seems to indicate that part of the Jacobians are wrong (the top left 2x2 block of both matrices). Below are the results from the test:

/home/david/git/gtsam/gtsam/slam/tests/testOrientedPlane3Factor.cpp:156: Failure: "assert_equal(numericalH1, actualH1, 1e-8)" 
not equal:
expected = [
	 0.866566, -0.316936,         0;
 	 0.112075, -0.685689,         0;
 	  15.0571,  -88.6789,         1
]
actual = [
	 0.661798, -0.749682,         0;
 	 0.749682,  0.661798,         0;
 	  15.0571,  -88.6789,         1
]
actual - expected = [
	   -0.204768,    -0.432747,            0;
 	    0.637607,      1.34749,            0;
 	 2.22066e-10, -1.41527e-09,  3.78578e-11
]
/home/david/git/gtsam/gtsam/slam/tests/testOrientedPlane3Factor.cpp:157: Failure: "assert_equal(numericalH2, actualH2, 1e-8)" 
not equal:
expected = [
	   0.92137,  0.0904696,  0.0846706,          0,          0,          0;
 	-0.0114131,   0.510599,  -0.854871,          0,          0,          0;
 	         0,          0,          0,  -0.130342,    0.85043,   0.509687
]
actual = [
	    0.991469,     0.111801,    0.0670054,            0,            0,            0;
 	-7.07943e-20,     0.514072,    -0.857747,            0,            0,            0;
 	           0,            0,            0,    -0.130342,      0.85043,     0.509687
]
actual - expected = [
	  0.0700988,   0.0213311,  -0.0176653,           0,           0,           0;
 	  0.0114131,  0.00347302, -0.00287617,           0,           0,           0;
 	          0,           0,           0, 3.73396e-10, 6.80116e-11, 1.13806e-11
]

Please let me know if I'm not testing the factor behaviour correctly.

[dellaert]

I'm taking a look...

[dwisth]

I proposed a solution in #362 which passes the unit tests using numericalDerivative.

If anyone knows the details of OrientedPlane3 and Unit3 I might need a hand to calculate the analytical derivatives for OrientedPlane3::error and Unit3::localCoordinates.

[varunagrawal]

Resolved via #362