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

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

@ -151,27 +151,21 @@ TEST(ManifoldEKF_Unit3, Update) {
namespace exampleDynamicMatrix { namespace exampleDynamicMatrix {
// Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. // 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) { 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) // 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 = {}) { 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) { 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->resize(1, p.size());
(*H)(0, 0) = 1.0; // d(trace)/dp00 *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
(*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major)
} }
if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error return p(0, 0) + p(1, 1); // Trace of the matrix
double trace = 0.0;
for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) {
trace += p(i, i);
}
return trace;
} }
} // namespace exampleDynamicMatrix } // namespace exampleDynamicMatrix