Chasing down more vector() and Point2 default constructor uses

gtsam/geometry/CameraSet.h

@@ -56,8 +56,7 @@ protected:
     // Project and fill error vector
     Vector b(ZDim * m);
     for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
-      Z e = predicted[i] - measured[i];
+      b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]);
-      b.segment<ZDim>(row) = e.vector();
     }
     return b;
   }

gtsam/geometry/Rot3.h

@@ -179,7 +179,7 @@ namespace gtsam {
      */
     static Rot3 AxisAngle(const Point3& axis, double angle) {
 #ifdef GTSAM_USE_QUATERNIONS
-      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
+      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
 #else
       return Rot3(SO3::AxisAngle(axis,angle));
 #endif

gtsam/geometry/tests/testPinholeSet.cpp

@@ -84,7 +84,7 @@ TEST(PinholeSet, Pinhole) {
   EXPECT(!set.equals(set2));
 
   // Check measurements
-  Point2 expected;
+  Point2 expected(0,0);
   ZZ z = set.project2(p);
   EXPECT(assert_equal(expected, z[0]));
   EXPECT(assert_equal(expected, z[1]));

gtsam/geometry/triangulation.h

@@ -471,7 +471,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
         if (params.dynamicOutlierRejectionThreshold > 0) {
           const Point2& zi = measured.at(i);
           Point2 reprojectionError(camera.project(point) - zi);
-          totalReprojError += reprojectionError.vector().norm();
+          totalReprojError += reprojectionError.norm();
         }
         i += 1;
       }

gtsam/slam/EssentialMatrixFactor.h

@@ -139,7 +139,7 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
-        << dP1_.transpose() << ")' and (" << pn_.vector().transpose()
+        << dP1_.transpose() << ")' and (" << pn_.transpose()
         << ")'" << std::endl;
   }
 
@@ -195,7 +195,7 @@ public:
 
     }
     Point2 reprojectionError = pn - pn_;
-    return f_ * reprojectionError.vector();
+    return f_ * reprojectionError;
   }
 
 };

gtsam/slam/GeneralSFMFactor.h

@@ -122,8 +122,7 @@ public:
   Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
       boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
     try {
-      Point2 reprojError(camera.project2(point,H1,H2) - measured_);
+      return camera.project2(point,H1,H2) - measured_;
-      return reprojError.vector();
     }
     catch( CheiralityException& e) {
       if (H1) *H1 = JacobianC::Zero();
@@ -145,8 +144,7 @@ public:
     try {
       const CAMERA& camera = values.at<CAMERA>(key1);
       const LANDMARK& point = values.at<LANDMARK>(key2);
-      Point2 reprojError(camera.project2(point, H1, H2) - measured());
+      b = measured() - camera.project2(point, H1, H2);
-      b = -reprojError.vector();
     } catch (CheiralityException& e) {
       H1.setZero();
       H2.setZero();
@@ -262,8 +260,7 @@ public:
   {
     try {
       Camera camera(pose3,calib);
-      Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
+      return camera.project(point, H1, H2, H3) - measured_;
-      return reprojError.vector();
     }
     catch( CheiralityException& e) {
       if (H1) *H1 = Matrix::Zero(2, 6);

gtsam/slam/ProjectionFactor.h

@@ -56,7 +56,9 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
 
     /// Default constructor
-    GenericProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
+  GenericProjectionFactor() :
+      measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
+  }
 
     /**
      * Constructor
@@ -134,16 +136,14 @@ namespace gtsam {
             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
             Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
             *H1 = *H1 * H0;
-            return reprojectionError.vector();
+            return reprojectionError;
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
+            return camera.project(point, H1, H2, boost::none) - measured_;
-            return reprojectionError.vector();
           }
         } else {
           PinholeCamera<CALIBRATION> camera(pose, *K_);
-          Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
+          return camera.project(point, H1, H2, boost::none) - measured_;
-          return reprojectionError.vector();
         }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam/slam/SmartProjectionFactor.h

@@ -499,8 +499,8 @@ public:
       return Base::totalReprojectionError(cameras, *result_);
     else if (params_.degeneracyMode == HANDLE_INFINITY) {
       // Otherwise, manage the exceptions with rotation-only factors
-      const Point2& z0 = this->measured_.at(0);
+      Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
-      Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
+          this->measured_.at(0));
       return Base::totalReprojectionError(cameras, backprojected);
     } else
       // if we don't want to manage the exceptions we discard the factor

gtsam/slam/TriangulationFactor.h

@@ -119,8 +119,7 @@ public:
   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
       boost::none) const {
     try {
-      Measurement error(camera_.project2(point, boost::none, H2) - measured_);
+      return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
-      return error.vector();
     } catch (CheiralityException& e) {
       if (H2)
         *H2 = Matrix::Zero(Measurement::dimension, 3);

gtsam_unstable/geometry/SimWall2D.cpp

@@ -73,7 +73,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
   }
 
   // find lower point by y
-  Point2 low, high;
+  Point2 low(0,0), high(0,0);
   if (Ba.y() > Bb.y()) {
     high = Ba;
     low = Bb;

gtsam_unstable/geometry/tests/testSimPolygon2D.cpp

@@ -16,7 +16,7 @@ const double tol=1e-5;
 TEST(testPolygon, triangle_basic) {
 
   // create a triangle from points, extract landmarks/walls, check occupancy
-  Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0);
+  Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0);
 
   // construct and extract data
   SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC);

gtsam_unstable/geometry/tests/testSimWall2D.cpp

@@ -24,7 +24,7 @@ TEST(testSimWall2D2D, construction ) {
 
 /* ************************************************************************* */
 TEST(testSimWall2D2D, equals ) {
-  Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3;
+  Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0);
   SimWall2D w1(p1, p2), w2(p1, p3);
   EXPECT(assert_equal(w1, w1));
   EXPECT(assert_inequal(w1, w2));
@@ -34,7 +34,7 @@ TEST(testSimWall2D2D, equals ) {
 /* ************************************************************************* */
 TEST(testSimWall2D2D, intersection1 ) {
   SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0);
-  Point2 pt;
+  Point2 pt(0,0);
   EXPECT(w1.intersects(w2));
   EXPECT(w2.intersects(w1));
   w1.intersects(w2, pt);

gtsam_unstable/slam/InvDepthFactor3.h

@@ -24,17 +24,17 @@ namespace gtsam {
  * Ternary factor representing a visual measurement that includes inverse depth
  */
 template<class POSE, class LANDMARK, class INVDEPTH>
-class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
+class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
-  gtsam::Point2 measured_;                ///< 2D measurement
+  Point2 measured_;                ///< 2D measurement
-  boost::shared_ptr<gtsam::Cal3_S2> K_;  ///< shared pointer to calibration object
+  boost::shared_ptr<Cal3_S2> K_;  ///< shared pointer to calibration object
 
 public:
 
   /// shorthand for base class type
-  typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
+  typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
 
   /// shorthand for this class
   typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@@ -43,7 +43,9 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// Default constructor
-  InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {}
+  InvDepthFactor3() :
+      measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
+  }
 
   /**
    * Constructor
@@ -55,8 +57,8 @@ public:
    * @param invDepthKey is the index of inverse depth
    * @param K shared pointer to the constant calibration
    */
-  InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
+  InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
-      const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) :
+      const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
         Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
 
   /** Virtual destructor */
@@ -68,44 +70,43 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactor3",
-      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s, keyFormatter);
     measured_.print(s + ".z");
   }
 
   /// equals
-  virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
+  Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none,
+      boost::optional<Matrix&> H2=boost::none,
-      boost::optional<gtsam::Matrix&> H3=boost::none) const {
+      boost::optional<Matrix&> H3=boost::none) const {
     try {
-      InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
+      InvDepthCamera3<Cal3_S2> camera(pose, K_);
-      gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
+      return camera.project(point, invDepth, H1, H2, H3) - measured_;
-      return reprojectionError.vector();
     } catch( CheiralityException& e) {
       if (H1) *H1 = Matrix::Zero(2,6);
       if (H2) *H2 = Matrix::Zero(2,5);
       if (H3) *H2 = Matrix::Zero(2,1);
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
-      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
-    return (gtsam::Vector(1) << 0.0).finished();
+    return (Vector(1) << 0.0).finished();
   }
 
   /** return the measurement */
-  const gtsam::Point2& imagePoint() const {
+  const Point2& imagePoint() const {
     return measured_;
   }
 
   /** return the calibration object */
-  inline const gtsam::Cal3_S2::shared_ptr calibration() const {
+  inline const Cal3_S2::shared_ptr calibration() const {
     return K_;
   }
 

gtsam_unstable/slam/InvDepthFactorVariant1.h

@@ -41,7 +41,9 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// Default constructor
-  InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
+  InvDepthFactorVariant1() :
+      measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
+  }
 
   /**
    * Constructor
@@ -64,13 +66,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant1",
-      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s, keyFormatter);
     measured_.print(s + ".z");
   }
 
   /// equals
-  virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -86,22 +88,21 @@ public:
       Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
       // Project landmark into Pose2
       PinholeCamera<Cal3_S2> camera(pose, *K_);
-      gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
+      return camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } catch( CheiralityException& e) {
       std::cout << e.what()
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
           << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
           << std::endl;
-      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
-    return (gtsam::Vector(1) << 0.0).finished();
+    return (Vector(1) << 0.0).finished();
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose, const Vector6& landmark,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H2=boost::none) const {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
@@ -118,7 +119,7 @@ public:
   }
 
   /** return the measurement */
-  const gtsam::Point2& imagePoint() const {
+  const Point2& imagePoint() const {
     return measured_;
   }
 

gtsam_unstable/slam/InvDepthFactorVariant2.h

@@ -43,7 +43,9 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// Default constructor
-  InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
+  InvDepthFactorVariant2() :
+      measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
+  }
 
   /**
    * Constructor
@@ -67,13 +69,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant2",
-      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s, keyFormatter);
     measured_.print(s + ".z");
   }
 
   /// equals
-  virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -89,22 +91,21 @@ public:
       Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
       // Project landmark into Pose2
       PinholeCamera<Cal3_S2> camera(pose, *K_);
-      gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
+      return camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } catch( CheiralityException& e) {
       std::cout << e.what()
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
           << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
           << std::endl;
-      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
-    return (gtsam::Vector(1) << 0.0).finished();
+    return (Vector(1) << 0.0).finished();
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose, const Vector3& landmark,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H2=boost::none) const {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
@@ -121,7 +122,7 @@ public:
   }
 
   /** return the measurement */
-  const gtsam::Point2& imagePoint() const {
+  const Point2& imagePoint() const {
     return measured_;
   }
 

gtsam_unstable/slam/InvDepthFactorVariant3.h

@@ -41,7 +41,9 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// Default constructor
-  InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
+  InvDepthFactorVariant3a() :
+      measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
+  }
 
   /**
    * Constructor
@@ -66,13 +68,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant3a",
-      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s, keyFormatter);
     measured_.print(s + ".z");
   }
 
   /// equals
-  virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -89,22 +91,21 @@ public:
       Point3 world_P_landmark = pose.transform_from(pose_P_landmark);
       // Project landmark into Pose2
       PinholeCamera<Cal3_S2> camera(pose, *K_);
-      gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
+      return camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } catch( CheiralityException& e) {
       std::cout << e.what()
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
           << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
           << std::endl;
-      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (Vector(1) << 0.0).finished();
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose, const Vector3& landmark,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H2=boost::none) const {
 
     if(H1) {
       (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
@@ -117,7 +118,7 @@ public:
   }
 
   /** return the measurement */
-  const gtsam::Point2& imagePoint() const {
+  const Point2& imagePoint() const {
     return measured_;
   }
 
@@ -160,7 +161,9 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// Default constructor
-  InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
+  InvDepthFactorVariant3b() :
+      measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
+  }
 
   /**
    * Constructor
@@ -185,13 +188,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant3",
-      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s, keyFormatter);
     measured_.print(s + ".z");
   }
 
   /// equals
-  virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -208,23 +211,22 @@ public:
       Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark);
       // Project landmark into Pose2
       PinholeCamera<Cal3_S2> camera(pose2, *K_);
-      gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
+      return camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } catch( CheiralityException& e) {
       std::cout << e.what()
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
           << " moved behind camera " << DefaultKeyFormatter(this->key2())
           << std::endl;
-      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (Vector(1) << 0.0).finished();
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none,
+      boost::optional<Matrix&> H2=boost::none,
-      boost::optional<gtsam::Matrix&> H3=boost::none) const {
+      boost::optional<Matrix&> H3=boost::none) const {
 
     if(H1)
       (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
@@ -239,7 +241,7 @@ public:
   }
 
   /** return the measurement */
-  const gtsam::Point2& imagePoint() const {
+  const Point2& imagePoint() const {
     return measured_;
   }
 

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -101,9 +101,9 @@ namespace gtsam {
     virtual ~MultiProjectionFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    virtual NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      return boost::static_pointer_cast<NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+          NonlinearFactor::shared_ptr(new This(*this))); }
 
     /**
      * print
@@ -143,20 +143,20 @@ namespace gtsam {
 //
 //        if(body_P_sensor_) {
 //          if(H1) {
-//            gtsam::Matrix H0;
+//            Matrix H0;
 //            PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
 //            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
 //            *H1 = *H1 * H0;
-//            return reprojectionError.vector();
+//            return reprojectionError;
 //          } else {
 //            PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
 //            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
-//            return reprojectionError.vector();
+//            return reprojectionError;
 //          }
 //        } else {
 //          PinholeCamera<CALIBRATION> camera(pose, *K_);
 //          Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
-//          return reprojectionError.vector();
+//          return reprojectionError;
 //        }
 //      }
 
@@ -168,20 +168,20 @@ namespace gtsam {
       try {
         if(body_P_sensor_) {
           if(H1) {
-            gtsam::Matrix H0;
+            Matrix H0;
             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
             Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
             *H1 = *H1 * H0;
-            return reprojectionError.vector();
+            return reprojectionError;
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
             Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
-            return reprojectionError.vector();
+            return reprojectionError;
           }
         } else {
           PinholeCamera<CALIBRATION> camera(pose, *K_);
           Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
-          return reprojectionError.vector();
+          return reprojectionError;
         }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam_unstable/slam/ProjectionFactorPPP.h

@@ -54,7 +54,9 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
 
     /// Default constructor
-    ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {}
+  ProjectionFactorPPP() :
+      measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
+  }
 
     /**
      * Constructor
@@ -94,9 +96,9 @@ namespace gtsam {
     virtual ~ProjectionFactorPPP() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    virtual NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      return boost::static_pointer_cast<NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+          NonlinearFactor::shared_ptr(new This(*this))); }
 
     /**
      * print
@@ -125,16 +127,15 @@ namespace gtsam {
         boost::optional<Matrix&> H3 = boost::none) const {
       try {
           if(H1 || H2 || H3) {
-            gtsam::Matrix H0, H02;
+            Matrix H0, H02;
             PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
             Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
             *H2 = *H1 * H02;
             *H1 = *H1 * H0;
-            return reprojectionError.vector();
+            return reprojectionError;
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
+            return camera.project(point, H1, H3, boost::none) - measured_;
-            return reprojectionError.vector();
           }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam_unstable/slam/ProjectionFactorPPPC.h

@@ -52,7 +52,9 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
 
     /// Default constructor
-    ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {}
+  ProjectionFactorPPPC() :
+      measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
+  }
 
     /**
      * Constructor
@@ -89,9 +91,9 @@ namespace gtsam {
     virtual ~ProjectionFactorPPPC() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    virtual NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      return boost::static_pointer_cast<NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+          NonlinearFactor::shared_ptr(new This(*this))); }
 
     /**
      * print
@@ -120,16 +122,15 @@ namespace gtsam {
         boost::optional<Matrix&> H4 = boost::none) const {
       try {
           if(H1 || H2 || H3 || H4) {
-            gtsam::Matrix H0, H02;
+            Matrix H0, H02;
             PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
             Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
             *H2 = *H1 * H02;
             *H1 = *H1 * H0;
-            return reprojectionError.vector();
+            return reprojectionError;
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
-            Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
+            return camera.project(point, H1, H3, H4) - measured_;
-            return reprojectionError.vector();
           }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam_unstable/slam/TSAMFactors.h

@@ -48,9 +48,7 @@ public:
   Vector evaluateError(const Pose2& pose, const Point2& point,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
           boost::none) const {
-    Point2 d = pose.transform_to(point, H1, H2);
+    return pose.transform_to(point, H1, H2) - measured_;
-    Point2 e = d - measured_;
-    return e.vector();
   }
 };
 

matlab.h

@@ -91,7 +91,7 @@ Matrix extractPoint2(const Values& values) {
   Values::ConstFiltered<Point2> points = values.filter<Point2>();
   Matrix result(points.size(), 2);
   for(const auto& key_value: points)
-    result.row(j++) = key_value.value.vector();
+    result.row(j++) = key_value.value;
   return result;
 }
 

tests/smallExample.h

@@ -180,7 +180,7 @@ inline boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph(
       new NonlinearFactorGraph);
 
   // prior on x1
-  Point2 mu;
+  Point2 mu(0,0);
   shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
   nlfg->push_back(f1);
 
@@ -597,7 +597,7 @@ inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
   VectorValues xtrue;
   for (size_t x = 1; x <= N; x++)
     for (size_t y = 1; y <= N; y++)
-      xtrue.insert(key(x, y), Point2((double)x, (double)y).vector());
+      xtrue.insert(key(x, y), Point2((double)x, (double)y));
 
   // linearize around zero
   boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);

tests/testBoundingConstraint.cpp

@@ -181,7 +181,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, MaxDistance_basics) {
   gtsam::Key key1 = 1, key2 = 2;
-  Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
+  Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
   iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
   EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
   EXPECT(!rangeBound.isGreaterThan());
@@ -220,7 +220,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
 /* ************************************************************************* */
 TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
 
-  Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
+  Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
   Symbol x1('x',1), x2('x',2);
 
   NonlinearFactorGraph graph;
@@ -246,7 +246,7 @@ TEST( testBoundingConstraint, avoid_demo) {
 
   Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
   double radius = 1.0;
-  Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
+  Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
   Point2 odo(2.0, 0.0);
 
   NonlinearFactorGraph graph;

tests/testExtendedKalmanFilter.cpp

@@ -400,7 +400,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
   ExtendedKalmanFilter<Point2> ekf(X(0), x_initial, P_initial);
 
   // Enter Predict-Update Loop
-  Point2 x_predict, x_update;
+  Point2 x_predict(0,0), x_update(0,0);
   for(unsigned int i = 0; i < 10; ++i){
     // Create motion factor
     NonlinearMotionModel motionFactor(X(i), X(i+1));

tests/testManifold.cpp

@@ -166,7 +166,7 @@ template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair>
 
 TEST(Manifold, ProductManifold) {
   BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
-  MyPoint2Pair pair1;
+  MyPoint2Pair pair1(Point2(0,0),Point2(0,0));
   Vector4 d;
   d << 1,2,3,4;
   MyPoint2Pair expected(Point2(1,2),Point2(3,4));