I am having trouble getting a Cost Function (analytical derivatives) to work with a local parameterization. I have a basic example to illustrate:
I have a SE3 pose I wish to minimize with regards to another SE3 pose. Each pose has 7 parameters (3 for translation and 4 for the rotation quaternion).
The residual is taken in tangent space, so it has dimension of 6 (3 for translation, 3 for rotation)
I know I have to provide a 6x7 jacobian in the cost function. Since the 7x6 local param jacobian will convert that jacobian into the final 6x6 form.
I am analytically calculating the 6x7 jacobian, and associating a local parameterization to the parameter block, which implements ComputeJacobian analytically and returns the 7x6 matrix.
This same local parameterization works just fine with an autodiff cost function, and i've verified that the derivatives i'm providing are correct.
Is there possibly some problem in the way I'm formulating the problem? The number if unsuccesfull steps leads me to believe the derivatives w.r.t. the global parameters are incorrect but I have verified those.
Here is a summary:
Original Reduced
Parameter blocks 1 1
Parameters 7 7
Effective parameters 6 6
Residual blocks 1 1
Residual 6 6
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_SCHUR DENSE_SCHUR
Threads 1 1
Linear solver threads 1 1
Linear solver ordering AUTOMATIC 1
Cost:
Initial 8.149899e+02
Final 8.467852e+01
Change 7.303114e+02
Minimizer iterations 25
Successful steps 9
Unsuccessful steps 16
Time (in seconds):
Preprocessor 0.0001
Residual evaluation 0.0000
Jacobian evaluation 0.0000
Linear solver 0.0002
Minimizer 0.0006
Postprocessor 0.0000
Total 0.0007
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 9.896785e-07 <= 1.000000e-06)
final norm:
13.0137