diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 06f32890a..02664af41 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -155,7 +155,7 @@ public: // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d - // We then convert to second camera by P2 = 1R2Õ*(P1-1T2) + // We then convert to second camera by P2 = 1R2�*(P1-1T2) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) // where we multiplied with d which yields equivalent homogeneous coordinates. diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 69e78c410..6d85685d5 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // Check evaluation - Point3 P1 = data.tracks[i].p; - const Point2 pi = camera2.project(P1); + Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); + const Point2 pi = SimpleCamera::project_to_camera(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector();