Fix/plane factor dwisth

[dwisth]

This adds another simplified unit test of related to PR #564 and Issue #561.

The unit test situation is very simple - there are two horizontal planes, p1 and p2, connected to a pose, x0. All the variables have priors on them.

In this toy example, all the factors are "perfect" (i.e. the optimal solution should have zero error), and the initial condition on the planes are "perfect" (the same as their optimal value). The only thing the optimizer needs to do is solve a pose offset - the initial condition is at (100,0,0) and the optimal solution is at (99,0,0).

The Gauss Newton optimizer throws an IndeterminantSystemError exception.

The Hessians associated to each variable are below. It seems that the plane variables are very poorly conditioned.

Hessian P1:
       401          0          0
         0 4.0004e+06     -40000
         0     -40000     400.04
Condition number:
4.99551e+07
Singular values:
4.0008e+06        401   0.080088
SVD U:
         0          1          0
   0.99995          0  0.0099985
-0.0099985          0    0.99995

Hessian P2:
       401          0          0
         0 4.0004e+06     -40000
         0     -40000     400.04
Condition number:
4.99551e+07
Singular values:
4.0008e+06        401   0.080088
SVD U:
         0          1          0
   0.99995          0  0.0099985
-0.0099985          0    0.99995

Hessian X0:
10800     0     0     0     0     0
    0 10800     0     0     0     0
    0     0 10000     0     0     0
    0     0     0 10000     0     0
    0     0     0     0 10000     0
    0     0     0     0     0 10800
Condition number:
1.08
Singular values:
10800 10800 10800 10000 10000 10000
SVD U:
1 0 0 0 0 0
0 1 0 0 0 0
0 0 0 0 0 1
0 0 0 1 0 0
0 0 0 0 1 0
0 0 1 0 0 0

I think there may be a problem with the second element in the OrientedPlane3Factor residual but I don't know enough about how gtsam works out the Hessian to be sure.

Any thoughts on the next step for debugging this?

[varunagrawal]

I believe the Hessian should be approximated as J.T @ J where J is the jacobian, though a deeper dive into the code would be needed to confirm this.

[dellaert]

This is just merging in my fix branch so I'll approve and merge.
I have an idea about at leasts something that might be wrong