Avoid calling default constructors and/or vector

gtsam/geometry/Point3.h

@@ -52,7 +52,9 @@ class GTSAM_EXPORT Point3 : public Vector3 {
 
 #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4
     /// Default constructor now creates *uninitialized* point !!!!
-    Point3() {}
+    Point3() {
+      throw std::runtime_error("Default constructor called!");
+    }
 #endif
 
 
@@ -106,9 +108,14 @@ class GTSAM_EXPORT Point3 : public Vector3 {
     /// @name Standard Interface
     /// @{
 
-    /** return vectorized form (column-wise)*/
+    /// return as Vector3
     const Vector3& vector() const { return *this; }
 
+    /// return as transposed vector
+    Eigen::DenseBase<Vector3>::ConstTransposeReturnType transpose() const {
+      return this->Vector3::transpose();
+    }
+
     /// @}
 
     /// Output stream operator

gtsam/slam/dataset.cpp

@@ -779,7 +779,7 @@ bool writeBAL(const string& filename, SfM_data &data) {
   os << endl;
 
   for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    SfM_Track track = data.tracks[j];
+    const SfM_Track& track = data.tracks[j];
 
     for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
       size_t i = track.measurements[k].first; // camera id
@@ -808,7 +808,9 @@ bool writeBAL(const string& filename, SfM_data &data) {
     Cal3Bundler cameraCalibration = data.cameras[i].calibration();
     Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
     os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
-    os << poseOpenGL.translation() << endl;
+    os << poseOpenGL.translation().x() << endl;
+    os << poseOpenGL.translation().y() << endl;
+    os << poseOpenGL.translation().z() << endl;
     os << cameraCalibration.fx() << endl;
     os << cameraCalibration.k1() << endl;
     os << cameraCalibration.k2() << endl;

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -37,7 +37,7 @@ PoseRTV::PoseRTV(const Vector& rtv) :
 Vector PoseRTV::vector() const {
   Vector rtv(9);
   rtv.head(3) = rotation().xyz();
-  rtv.segment(3,3) = translation().vector();
+  rtv.segment(3,3) = translation();
   rtv.tail(3) = velocity();
   return rtv;
 }
@@ -52,7 +52,7 @@ bool PoseRTV::equals(const PoseRTV& other, double tol) const {
 void PoseRTV::print(const string& s) const {
   cout << s << ":" << endl;
   gtsam::print((Vector)R().xyz(), "  R:rpy");
-  t().print("  T");
+  cout << "  T" << t().transpose() << endl;
   gtsam::print((Vector)velocity(), "  V");
 }
 
@@ -103,7 +103,7 @@ PoseRTV PoseRTV::flyingDynamics(
   Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
   Point3 forward(forward_accel, 0.0, 0.0);
   Vector Acc_n =
-      yaw_correction_bn.rotate(forward).vector()              // applies locally forward force in the global frame
+      yaw_correction_bn.rotate(forward)              // applies locally forward force in the global frame
       - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished()  // drag term dependent on v1
       + delta(3, 2, loss_lift - lift_control);                // falling due to lift lost from pitch
 
@@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other,
   const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
   const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
   Matrix13 D_d_t1, D_d_t2;
-  double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
+  double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
   if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
   if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
   return d;
@@ -188,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
 
   // Note that we rotate the velocity
   Matrix3 D_newvel_R, D_newvel_v;
-  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector();
+  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);
 
   if (Dglobal) {
     Dglobal->setZero();

gtsam_unstable/dynamics/PoseRTV.h

@@ -66,7 +66,7 @@ public:
   // Access to vector for ease of use with Matlab
   // and avoidance of Point3
   Vector vector() const;
-  Vector translationVec() const { return pose().translation().vector(); }
+  Vector translationVec() const { return pose().translation(); }
   const Velocity3& velocityVec() const { return velocity(); }
 
   // testable
@@ -126,7 +126,7 @@ public:
 
   /// @return a vector for Matlab compatibility
   inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const {
-    return translationIntegration(x2, dt).vector();
+    return translationIntegration(x2, dt);
   }
 
   /**

gtsam_unstable/geometry/BearingS2.cpp

@@ -34,7 +34,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) {
       0.,1.,0.,
       -1.,0.,0.).finished();
   //  p_rel_c = Cbc*Cnb*(PosObj - Pos);
-  Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector());
+  Vector p_rel_c = Cbc*Cnb*(B - A.translation());
 
   // FIXME: the matlab code checks for p_rel_c(0) greater than
 
@@ -50,7 +50,7 @@ BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) {
   //  Cnb = DCMnb(Att);
   Matrix Cnb = A.rotation().matrix().transpose();
 
-  Vector p_rel_c = Cnb*(B.vector() - A.translation().vector());
+  Vector p_rel_c = Cnb*(B - A.translation());
 
   // FIXME: the matlab code checks for p_rel_c(0) greater than
 

gtsam_unstable/geometry/Event.h

@@ -60,14 +60,13 @@ public:
 
   /** print with optional string */
   void print(const std::string& s = "") const {
-    std::cout << s << "time = " << time_;
+    std::cout << s << "time = " << time_ << "location = " << location_.transpose();
-    location_.print("; location");
   }
 
   /** equals with an tolerance */
   bool equals(const Event& other, double tol = 1e-9) const {
     return std::abs(time_ - other.time_) < tol
-        && location_.equals(other.location_, tol);
+        && traits<Point3>::Equals(location_, other.location_, tol);
   }
 
   /// Updates a with tangent space delta
@@ -86,7 +85,7 @@ public:
       OptionalJacobian<1, 3> H2 = boost::none) const {
     static const double Speed = 330;
     Matrix13 D1, D2;
-    double distance = location_.distance(microphone, D1, D2);
+    double distance = gtsam::distance(location_, microphone, D1, D2);
     if (H1)
       // derivative of toa with respect to event
       *H1 << 1.0, D1 / Speed;

gtsam_unstable/geometry/Similarity3.cpp

@@ -43,7 +43,7 @@ Similarity3::Similarity3(const Matrix4& T) :
 }
 
 bool Similarity3::equals(const Similarity3& other, double tol) const {
-  return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
+  return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
       && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
 }
 
@@ -55,8 +55,7 @@ void Similarity3::print(const std::string& s) const {
   std::cout << std::endl;
   std::cout << s;
   rotation().print("R:\n");
-  translation().print("t: ");
+  std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl;
-  std::cout << "s: " << scale() << std::endl;
 }
 
 Similarity3 Similarity3::identity() {
@@ -79,7 +78,7 @@ Point3 Similarity3::transform_from(const Point3& p, //
     // For this derivative, see LieGroups.pdf
     const Matrix3 sR = s_ * R_.matrix();
     const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z());
-    *H1 << DR, sR, sR * p.vector();
+    *H1 << DR, sR, sR * p;
   }
   if (H2)
     *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
@@ -103,7 +102,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) {
 Matrix7 Similarity3::AdjointMap() const {
   // http://www.ethaneade.org/latex2html/lie/node30.html
   const Matrix3 R = R_.matrix();
-  const Vector3 t = t_.vector();
+  const Vector3 t = t_;
   const Matrix3 A = s_ * skewSymmetric(t) * R;
   Matrix7 adj;
   adj << R, Z_3x3, Matrix31::Zero(), // 3*7
@@ -153,7 +152,7 @@ Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) {
   const Vector3 w = Rot3::Logmap(T.R_);
   const double lambda = log(T.s_);
   Vector7 result;
-  result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda;
+  result << w, GetV(w, lambda).inverse() * T.t_, lambda;
   if (Hm) {
     throw std::runtime_error("Similarity3::Logmap: derivative not implemented");
   }
@@ -173,13 +172,13 @@ Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
 
 std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
   os << "[" << p.rotation().xyz().transpose() << " "
-      << p.translation().vector().transpose() << " " << p.scale() << "]\';";
+      << p.translation().transpose() << " " << p.scale() << "]\';";
   return os;
 }
 
 const Matrix4 Similarity3::matrix() const {
   Matrix4 T;
-  T.topRows<3>() << R_.matrix(), t_.vector();
+  T.topRows<3>() << R_.matrix(), t_;
   T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
   return T;
 }

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -1,12 +1,12 @@
 /* ----------------------------------------------------------------------------
- 
+
  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
- 
+
  * See LICENSE for the license information
- 
+
  * -------------------------------------------------------------------------- */
 
 /**
@@ -253,7 +253,7 @@ public:
         reprojections.push_back(cameras[i].backproject(measured_[i]));
       }
 
-      Point3 pw_sum;
+      Point3 pw_sum(0,0,0);
       BOOST_FOREACH(const Point3& pw, reprojections) {
         pw_sum = pw_sum + pw;
       }

wrap/Class.cpp

@@ -567,7 +567,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
   //  string serialized = unwrap< string >(in[0]);
   //  istringstream in_archive_stream(serialized);
   //  boost::archive::text_iarchive in_archive(in_archive_stream);
-  //  Shared output(new Point3());
+  //  Shared output(new Point3(0,0,0));
   //  in_archive >> *output;
   //  out[0] = wrap_shared_ptr(output,"Point3", false);
   //}

wrap/tests/expected/geometry_wrapper.cpp

@@ -326,7 +326,7 @@ void gtsamPoint3_string_deserialize_19(int nargout, mxArray *out[], int nargin,
   string serialized = unwrap< string >(in[0]);
   istringstream in_archive_stream(serialized);
   boost::archive::text_iarchive in_archive(in_archive_stream);
-  Shared output(new gtsam::Point3());
+  Shared output(new gtsam::Point3(0,0,0));
   in_archive >> *output;
   out[0] = wrap_shared_ptr(output,"gtsam.Point3", false);
 }