diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index fc1e69190..bee3e8944 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -76,7 +76,7 @@ public: boost::optional H = boost::none) const { // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -114,7 +114,7 @@ public: boost::optional H = boost::none) const { // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -155,7 +155,7 @@ public: Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) *H2 = eye(3); - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -197,7 +197,7 @@ public: Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) - *H1 = rotated.point3().vector(); + *H1 = rotated.point3(); if (H2) // H2 is 2*2, but we need 3*2 { Matrix H; @@ -206,7 +206,7 @@ public: } if (H3) *H3 = eye(3); - return (hx - measured_).vector(); + return (hx - measured_); } }; diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index c2fdb963d..b5d312a86 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -140,7 +140,7 @@ TEST(ScenarioRunner, Loop) { /* ************************************************************************* */ namespace initial { const Rot3 nRb; -const Point3 P0; +const Point3 P0(0,0,0); const Vector3 V0(0, 0, 0); } @@ -259,7 +259,7 @@ namespace initial3 { // Rotation only // Set up body pointing towards y axis. The body itself has Z axis pointing down const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); -const Point3 P0; +const Point3 P0(0,0,0); const Vector3 V0(0, 0, 0); } diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index da22225e5..e9a117745 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -19,7 +19,7 @@ namespace gtsam { */ class EssentialMatrixFactor: public NoiseModelFactor1 { - Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; @@ -107,9 +107,7 @@ public: */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model) : - Base(model, key1, key2) { - dP1_ = Point3(pA.x(), pA.y(), 1); - pn_ = pB; + Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { f_ = 1.0; } @@ -125,11 +123,8 @@ public: template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2) { - assert(K); - Point2 p1 = K->calibrate(pA); - dP1_ = Point3(p1.x(), p1.y(), 1); // d*P1 = (x,y,1) - pn_ = K->calibrate(pB); + Base(model, key1, key2), dP1_( + EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 07abb557f..48e2e8b2f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -154,7 +154,7 @@ TEST( GeneralSFMFactor, error ) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); EXPECT( assert_equal(((Vector ) Vector2(-3., 0.)), @@ -451,7 +451,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); vector models; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index a349a4992..9fda7d745 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -107,7 +107,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 467aefe91..e441c37ff 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -610,11 +610,11 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-3); - Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + Point3 oldPoint(0,0,0); // this takes the point stored in the factor (we are not interested in this) if (factor1->point()) oldPoint = *(factor1->point()); - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point(values)) expectedPoint = *(factor1->point(values)); @@ -636,10 +636,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { smartGraph.push_back(factor1); double expectedError = factor1->error(values); double expectedErrorGraph = smartGraph.error(values); - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point()) expectedPoint = *(factor1->point()); - // cout << "expectedPoint " << expectedPoint.vector() << endl; // COMMENTS: // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as @@ -677,7 +676,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { smartGraph.push_back(factor1); Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point()) expectedPoint = *(factor1->point()); @@ -720,7 +719,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { // smartGraph.push_back(factor1); // GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values); // -// Point3 expectedPoint; +// Point3 expectedPoint(0,0,0); // if(factor1->point()) // expectedPoint = *(factor1->point()); // @@ -773,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { cameras.push_back(level_camera_right); factor1->error(values); // this is important to have a triangulation of the point - Point3 point; + Point3 point(0,0,0); if (factor1->point()) point = *(factor1->point()); vector Fblocks; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 4ce4d5758..d9674b415 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -108,7 +108,7 @@ private: const Velocity3& v1 = x1.v(), v2 = x2.v(); const Point3& p1 = x1.t(), p2 = x2.t(); - Point3 hx; + Point3 hx(0,0,0); switch(mode) { case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break; case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 879f7095e..042a15108 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -44,7 +44,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0; +static const Point3 microphoneAt0(0,0,0); //***************************************************************************** TEST( TOAFactor, NewWay ) {