GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices.
When I use GTSAM, build the factors that optimize for Pose3. Taking the variant of GPSFactor as an example, the residual is as follows.
residual = R * p.translation() – nT_;
I'm going to change GPSFactor to NoiseModelFactor2, and I'm going to be evaluating Pose3 and the coefficient R, and I should have two Jacobians, but I don't find the Jacobian for Pose3. I only found Jacobian for Pose2 in the website.
I guess the Jacobian for Pose3 would look something like this:
Jacobian << Z_3x3 , R * p.rotation().matrix();
But running my code produced the wrong result.
When I use GTSAM, build the factors that optimize for Pose3. Taking the variant of GPSFactor as an example, the residual is as follows.
residual = R * p.translation() – nT_;
I'm going to change GPSFactor to NoiseModelFactor2, and I'm going to be evaluating Pose3 and the coefficient R, and I should have two Jacobians, but I don't find the Jacobian for Pose3. I only found Jacobian for Pose2 in the website. I guess the Jacobian for Pose3 would look something like this:Jacobian << Z_3x3 , R * p.rotation().matrix();
But running my code produced the wrong result.