Fixed more default constructor calls

gtsam/navigation/MagFactor.h

@@ -76,7 +76,7 @@ public:
       boost::optional<Matrix&> H = boost::none) const {
     // measured bM = nRb<52> * nM + b
     Point3 hx = unrotate(nRb, nM_, H) + bias_;
-    return (hx - measured_).vector();
+    return (hx - measured_);
   }
 };
 
@@ -114,7 +114,7 @@ public:
       boost::optional<Matrix&> H = boost::none) const {
     // measured bM = nRb<52> * 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_);
   }
 };
 

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);
 }
 

gtsam/slam/EssentialMatrixFactor.h

@@ -19,7 +19,7 @@ namespace gtsam {
  */
 class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
 
-  Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates
+  Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates
 
   typedef NoiseModelFactor1<EssentialMatrix> 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<class CALIBRATION>
   EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
       const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> 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());
   }
 

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<SharedNoiseModel> models;

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)));
 }

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<Matrix29> Fblocks;

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;

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 ) {