Difference between Analytical and Numerical Jacobian for BetweenFactor
jukindle opened this issue · 1 comments
Hello!
I basically tried to compare the analytical and numerical jacobians for the BetweenFactor and notices they differ. I think I am using the correct manifold update (from right) given that I am using the numerical derivative function from GTSAM itself. I would like to understand why they differ, i.e. at which point an approximation is made.
Does the difference come from the fact that for the analytical jacobian derivation we are not multiplying with the right jacobian, i.e.
Here's the example code I am using
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
// Define error function for BetweenFactor
gtsam::Vector betweenFactorError(const gtsam::Pose3 &T1, const gtsam::Pose3 &T2, const gtsam::BetweenFactor<gtsam::Pose3> &factor)
{
return factor.evaluateError(T1, T2);
}
int main()
{
// Create a random BetweenFactor
gtsam::Key key1 = gtsam::Symbol('T1', 0);
gtsam::Key key2 = gtsam::Symbol('T2', 0);
gtsam::Pose3 measuredDifference = gtsam::Pose3::Expmap(gtsam::Vector6::Random());
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(6, 0.1); // Assuming 6D error
gtsam::BetweenFactor<gtsam::Pose3> factor(key1, key2, measuredDifference, model);
// Sample random poses
gtsam::Pose3 T1 = gtsam::Pose3::Expmap(gtsam::Vector6::Random());
gtsam::Pose3 T2 = gtsam::Pose3::Expmap(gtsam::Vector6::Random());
// Compute the analytical Jacobians
gtsam::Matrix H1_analytical, H2_analytical;
factor.evaluateError(T1, T2, H1_analytical, H2_analytical);
// Compute the numerical Jacobians
gtsam::Matrix H1_numerical = gtsam::numericalDerivative11<gtsam::Vector, gtsam::Pose3>(boost::bind(&betweenFactorError, _1, T2, factor), T1);
gtsam::Matrix H2_numerical = gtsam::numericalDerivative11<gtsam::Vector, gtsam::Pose3>(boost::bind(&betweenFactorError, T1, _1, factor), T2);
// Compare the Jacobians
std::cout << "Analytic H_WA:\n"
<< H1_analytical << std::endl;
std::cout << "Numeric H_WA:\n"
<< H1_numerical << std::endl;
std::cout << std::endl;
std::cout << "Analytic H_AB:\n"
<< H2_analytical << std::endl;
std::cout << "Numeric H_AB:\n"
<< H2_numerical << std::endl;
return 0;
}
with the resulting output
Analytic H_WA:
-0.158304 -0.913121 -0.3757 -0 -0 -0
0.792649 -0.344409 0.503081 -0 -0 -0
0.588768 0.218158 -0.778305 -0 -0 -0
0.180432 -0.0921831 0.14802 -0.158304 -0.913121 -0.3757
0.4953 0.396486 -0.508955 0.792649 -0.344409 0.503081
-0.618301 0.240098 -0.40043 0.588768 0.218158 -0.778305
Numeric H_WA:
-0.529653 -0.713478 -0.528738 0 0 0
0.85708 -0.621987 -0.0196841 0 0 0
0.264351 0.393483 -0.909937 0 0 0
-0.361997 0.000131522 0.224496 -0.529653 -0.713478 -0.528738
0.132429 0.0729057 -0.83948 0.85708 -0.621987 -0.0196841
-0.828156 0.207672 -0.163757 0.264351 0.393483 -0.909937
Analytic H_AB:
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
Numeric H_AB:
0.933985 -0.440099 -0.0559737 0 0 0
0.439665 0.883679 0.38425 0 0 0
-0.0592823 -0.383754 0.949692 0 0 0
-0.135217 -0.45016 -0.0199266 0.933985 -0.440099 -0.0559737
0.436924 -0.154545 0.0758573 0.439665 0.883679 0.38425
-0.12027 -0.0602826 -0.0194418 -0.0592823 -0.383754 0.949692
It seems like the multiplication from left with the right jacobian is exactly the missing bit left away for efficiency reasons and which can be enabled by compiling with GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR:
gtsam/gtsam/slam/BetweenFactor.h
Line 115 in b5d19fb