12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix bug in Debug

release/4.3a0
Frank Dellaert 2025-05-10 10:29:47 -04:00
parent da09c4a2c3
commit 87797c687e
2 changed files with 16 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/navigation/tests/testLieGroupEKF.cpp
View File

@ -126,19 +126,17 @@ namespace exampleLieGroupDynamicMatrix {
return kFixedVelocityTangent;
}
// Measurement function h(X, H_p)
// H_p is D(h)/D(X_local)
double h(const Matrix& p, OptionalJacobian<-1, -1> H_p = {}) {
if (p.size() == 0) {
if (H_p) H_p->resize(1, 0); // Jacobian dh/dX_local is 1x0
return 0.0; // Define trace of empty matrix as 0 for consistency
// Measurement function h(X, H)
double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
// Specialized for a 2x2 matrix!
if (p.rows() != 2 || p.cols() != 2) {
throw std::invalid_argument("Matrix must be 2x2.");
}
// This test specifically uses 2x2 matrices (size 4)
if (H_p) {
H_p->resize(1, p.size()); // p.size() is 4
(*H_p) << 1.0, 0.0, 0.0, 1.0; // d(trace)/d(p_flat) for p_flat = [p00,p10,p01,p11]
if (H) {
H->resize(1, p.size());
*H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
}
return p(0, 0) + p(1, 1); // trace
return p(0, 0) + p(1, 1); // Trace of the matrix
}
} // namespace exampleLieGroupDynamicMatrix

20
gtsam/navigation/tests/testManifoldEKF.cpp
View File

@ -151,27 +151,21 @@ TEST(ManifoldEKF_Unit3, Update) {
namespace exampleDynamicMatrix {
// Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt.
// For Matrix (a VectorSpace type), Retract(M, v) is typically M + v.
Matrix f(const Matrix& p, const Vector& vTangent, double dt) {
return traits<Matrix>::Retract(p, vTangent * dt);
return traits<Matrix>::Retract(p, vTangent * dt); // +
}
// Define a measurement model: measure the trace of the Matrix (assumed 2x2 here)
// H is the Jacobian dh/d(flatten(p))
double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
// Specialized for a 2x2 matrix!
if (p.rows() != 2 || p.cols() != 2) {
throw std::invalid_argument("Matrix must be 2x2.");
}
if (H) {
// The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size().
// For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1].
H->resize(1, p.size());
(*H)(0, 0) = 1.0; // d(trace)/dp00
(*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major)
*H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
}
if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error
double trace = 0.0;
for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) {
trace += p(i, i);
}
return trace;
return p(0, 0) + p(1, 1); // Trace of the matrix
}
} // namespace exampleDynamicMatrix