Merged in feature/Point3_cleanup (pull request #219)

Cleanup some Point3/Rot3 uses

examples/SFMExample.cpp

@@ -99,7 +99,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
   initialEstimate.print("Initial Estimates:\n");
 
   /* Optimize the graph and print results */

examples/SFMExampleExpressions.cpp

@@ -88,7 +88,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initial.insert(Symbol('x', i), poses[i].compose(delta));
   for (size_t j = 0; j < points.size(); ++j)
-    initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
   cout << "initial error = " << graph.error(initial) << endl;
 
   /* Optimize the graph and print results */

examples/SelfCalibrationExample.cpp

@@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
 
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();

examples/VisualISAM2Example.cpp

@@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
       // Add initial guesses to all observed landmarks
       // Intentionally initialize the variables off from the ground truth
       for (size_t j = 0; j < points.size(); ++j)
-        initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+        initialEstimate.insert<Point3>(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15));
 
     } else {
       // Update iSAM with the new factors

examples/VisualISAMExample.cpp

@@ -120,7 +120,7 @@ int main(int argc, char* argv[]) {
       Point3 noise(-0.25, 0.20, 0.15);
       for (size_t j = 0; j < points.size(); ++j) {
         // Intentionally initialize the variables off from the ground truth
-        Point3 initial_lj = points[j].compose(noise);
+        Point3 initial_lj = points[j] + noise;
         initialEstimate.insert(Symbol('l', j), initial_lj);
       }
 

gtsam.h

@@ -360,17 +360,6 @@ class Point3 {
 
   // Group
   static gtsam::Point3 identity();
-  gtsam::Point3 inverse() const;
-  gtsam::Point3 compose(const gtsam::Point3& p2) const;
-  gtsam::Point3 between(const gtsam::Point3& p2) const;
-
-  // Manifold
-  gtsam::Point3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Point3& p) const;
-
-  // Lie Group
-  static gtsam::Point3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Point3& p);
 
   // Standard Interface
   Vector vector() const;

gtsam/base/Lie.h

@@ -295,19 +295,18 @@ private:
 };
 
 /**
- *  Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
+ *  Three term approximation of the Baker-Campbell-Hausdorff formula
  *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
  *  it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
  *  formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
- *  http://en.wikipedia.org/wiki/Baker<EFBFBD>Campbell<EFBFBD>Hausdorff_formula
+ *  http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula
  */
 /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
 template<class T>
 T BCH(const T& X, const T& Y) {
   static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
   T X_Y = bracket(X, Y);
-  return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
+  return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
-      bracket(X, X_Y));
 }
 
 /**

gtsam/geometry/Point3.cpp

@@ -75,6 +75,7 @@ double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   if (H1) *H1 = I_3x3;
@@ -82,13 +83,13 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
   return *this + q;
 }
 
-/* ************************************************************************* */
 Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   if (H1) *H1 = I_3x3;
   if (H2) *H2 = I_3x3;
   return *this - q;
 }
+#endif
 
 /* ************************************************************************* */
 Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {

gtsam/geometry/Point3.h

@@ -107,11 +107,6 @@ namespace gtsam {
     double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) const;
 
-    /** @deprecated The following function has been deprecated, use distance above */
-    inline double dist(const Point3& p2) const {
-      return (p2 - *this).norm();
-    }
-
     /** Distance of the point from the origin, with Jacobian */
     double norm(OptionalJacobian<1,3> H = boost::none) const;
 
@@ -145,19 +140,12 @@ namespace gtsam {
     /// get z
     inline double z() const {return z_;}
 
-    /** add two points, add(this,q) is same as this + q */
-    Point3 add (const Point3 &q,
-          OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
-
-    /** subtract two points, sub(this,q) is same as this - q */
-    Point3 sub (const Point3 &q,
-          OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
-
     /// @}
 
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
     /// @name Deprecated
     /// @{
     Point3 inverse() const { return -(*this);}
@@ -167,7 +155,13 @@ namespace gtsam {
     Point3 retract(const Vector3& v) const { return compose(Point3(v));}
     static Vector3 Logmap(const Point3& p) { return p.vector();}
     static Point3 Expmap(const Vector3& v) { return Point3(v);}
-    /// @}
+    inline double dist(const Point3& p2) const { return (p2 - *this).norm(); }
+    Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
+               OptionalJacobian<3, 3> H2 = boost::none) const;
+    Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
+               OptionalJacobian<3, 3> H2 = boost::none) const;
+  /// @}
+#endif
 
   private:
 

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -30,12 +30,8 @@ using namespace gtsam;
 GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
 
 // Camera situated at 0.5 meters high, looking down
-static const Pose3 pose1((Matrix3() <<
+static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
-              1., 0., 0.,
+                         Point3(0, 0, 0.5));
-              0.,-1., 0.,
-              0., 0.,-1.
-              ).finished(),
-            Point3(0,0,0.5));
 
 static const CalibratedCamera camera(pose1);
 

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -34,7 +34,7 @@ typedef PinholeCamera<Cal3_S2> Camera;
 
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
+static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
 static const Camera camera(pose, K);
 
 static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));

gtsam/geometry/tests/testPinholePose.cpp

@@ -35,7 +35,7 @@ typedef PinholePose<Cal3_S2> Camera;
 
 static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
 
-static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
+static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
 static const Camera camera(pose, K);
 
 static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));

gtsam/geometry/tests/testPoint3.cpp

@@ -61,12 +61,12 @@ TEST(Point3, Lie) {
 /* ************************************************************************* */
 TEST( Point3, arithmetic) {
   CHECK(P * 3 == 3 * P);
-  CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
+  CHECK(assert_equal<Point3>(Point3(-1, -5, -6), -Point3(1, 5, 6)));
-  CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
+  CHECK(assert_equal<Point3>(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
-  CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
+  CHECK(assert_equal<Point3>(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
-  CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
+  CHECK(assert_equal<Point3>(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
-  CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
+  CHECK(assert_equal<Point3>(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
-  CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
+  CHECK(assert_equal<Point3>(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -97,10 +97,10 @@ TEST( Rot3, equals)
 // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
 Rot3 slow_but_correct_Rodrigues(const Vector& w) {
   double t = norm_2(w);
-  Matrix J = skewSymmetric(w / t);
+  Matrix3 J = skewSymmetric(w / t);
   if (t < 1e-5) return Rot3();
-  Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
+  Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
-  return R;
+  return Rot3(R);
 }
 
 /* ************************************************************************* */
@@ -274,14 +274,13 @@ TEST(Rot3, manifold_expmap)
 }
 
 /* ************************************************************************* */
-class AngularVelocity: public Point3 {
+class AngularVelocity : public Vector3 {
-public:
+ public:
-  AngularVelocity(const Point3& p) :
+  template <typename Derived>
-    Point3(p) {
+  inline AngularVelocity(const Eigen::MatrixBase<Derived>& v)
-  }
+      : Vector3(v) {}
-  AngularVelocity(double wx, double wy, double wz) :
+
-    Point3(wx, wy, wz) {
+  AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {}
-  }
 };
 
 AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
@@ -294,10 +293,10 @@ TEST(Rot3, BCH)
   // Approximate exmap by BCH formula
   AngularVelocity w1(0.2, -0.1, 0.1);
   AngularVelocity w2(0.01, 0.02, -0.03);
-  Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
+  Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2);
   Rot3 R3 = R1 * R2;
   Vector expected = Rot3::Logmap(R3);
-  Vector actual = BCH(w1, w2).vector();
+  Vector actual = BCH(w1, w2);
   CHECK(assert_equal(expected, actual,1e-5));
 }
 

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -28,12 +28,8 @@ using namespace gtsam;
 
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose1((Matrix3() <<
+static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
-              1., 0., 0.,
+                         Point3(0, 0, 0.5));
-              0.,-1., 0.,
-              0., 0.,-1.
-              ).finished(),
-            Point3(0,0,0.5));
 
 static const SimpleCamera camera(pose1, K);
 

gtsam/geometry/tests/testStereoCamera.cpp

@@ -74,11 +74,11 @@ TEST( StereoCamera, project)
 
 /* ************************************************************************* */
 
-static Pose3 camPose((Matrix)(Matrix(3,3) <<
+static Pose3 camPose(Rot3((Matrix3() <<
            1., 0., 0.,
            0.,-1., 0.,
            0., 0.,-1.
-           ).finished(),
+           ).finished()),
         Point3(0,0,6.25));
 
 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));

gtsam/navigation/GPSFactor.cpp

@@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p,
     H->block < 3, 3 > (0, 0) << zeros(3, 3);
     H->block < 3, 3 > (0, 3) << p.rotation().matrix();
   }
-  return (p.translation() -nT_).vector();
+  return p.translation().vector() -nT_.vector();
 }
 
 //***************************************************************************

gtsam/navigation/ScenarioRunner.h

@@ -73,7 +73,7 @@ class ScenarioRunner {
 
   // An accelerometer measures acceleration in body, but not gravity
   Vector3 actualSpecificForce(double t) const {
-    Rot3 bRn = scenario_->rotation(t).transpose();
+    Rot3 bRn(scenario_->rotation(t).transpose());
     return scenario_->acceleration_b(t) - bRn * gravity_n();
   }
 

gtsam/navigation/tests/testNavState.cpp

@@ -81,7 +81,7 @@ TEST( NavState, Velocity) {
 TEST( NavState, BodyVelocity) {
   Matrix39 aH, eH;
   Velocity3 actual = kState1.bodyVelocity(aH);
-  EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity)));
+  EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
   eH = numericalDerivative11<Velocity3, NavState>(
       boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
   EXPECT(assert_equal((Matrix )eH, aH));

gtsam/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
               Cal3Bundler0(1, 0, 0));
 Point3 point(10, 0, -5);  // negative Z-axis convention of Snavely!
 Vector9 P = Camera().localCoordinates(camera);
-Vector3 X = Point3::Logmap(point);
+Vector3 X = point.vector();
 Vector2 expectedMeasurement(1.2431567, 1.2525694);
 Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
 Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);

gtsam/slam/tests/testGeneralSFMFactor.cpp

@@ -105,16 +105,16 @@ static vector<Point3> genPoint3() {
 
 static vector<GeneralCamera> genCameraDefaultCalibration() {
   vector<GeneralCamera> X;
-  X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.))));
+  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
-  X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.))));
+  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
   return X;
 }
 
 static vector<GeneralCamera> genCameraVariableCalibration() {
   const Cal3_S2 K(640, 480, 0.1, 320, 240);
   vector<GeneralCamera> X;
-  X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K));
+  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
-  X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K));
+  X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
   return X;
 }
 

gtsam/slam/tests/testStereoFactor.cpp

@@ -31,11 +31,7 @@
 using namespace std;
 using namespace gtsam;
 
-static Pose3 camera1((Matrix) (Matrix(3, 3) <<
+static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()),
-           1., 0., 0.,
-           0.,-1., 0.,
-           0., 0.,-1.
-           ).finished(),
         Point3(0,0,6.25));
 
 static boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -161,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
 Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
   // predict point for constraint
   // NOTE: uses simple Euler approach for prediction
-  Point3 pred_t2 = t().retract(v2 * dt);
+  Point3 pred_t2 = t() + Point3(v2 * dt);
   return pred_t2;
 }
 

gtsam_unstable/dynamics/VelocityConstraint.h

@@ -110,12 +110,12 @@ private:
     const Point3& p1 = x1.t(), p2 = x2.t();
     Point3 hx;
     switch(mode) {
-    case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break;
+    case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break;
-    case dynamics::EULER_START: hx = p1.retract(v1 * dt); break;
+    case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break;
-    case dynamics::EULER_END  : hx = p1.retract(v2 * dt); break;
+    case dynamics::EULER_END  : hx = p1 + Point3(v2 * dt); break;
     default: assert(false); break;
     }
-    return (p2 - hx).vector();
+    return p2.vector() - hx.vector();
   }
 };
 

gtsam_unstable/geometry/Event.h

@@ -72,7 +72,7 @@ public:
 
   /// Updates a with tangent space delta
   inline Event retract(const Vector4& v) const {
-    return Event(time_ + v[0], location_.retract(v.tail(3)));
+    return Event(time_ + v[0], location_ + Point3(v.tail<3>()));
   }
 
   /// Returns inverse retraction

gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp

@@ -75,7 +75,7 @@ int main() {
   Rot3 R2(0.473618898,   0.119523052,  0.872582019,
        0.609241153,   0.67099888, -0.422594037,
       -0.636011287,  0.731761397,  0.244979388);
-  Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
+  Point3 t2 = t1 + Point3(Vel1*measurement_dt);
   Pose3 Pose2(R2, t2);
   Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
   Vector3 Vel2 = Vel1 + dv;

gtsampy.h

@@ -360,17 +360,6 @@ class Point3 {
 
   // Group
   static gtsam::Point3 identity();
-  gtsam::Point3 inverse() const;
-  gtsam::Point3 compose(const gtsam::Point3& p2) const;
-  gtsam::Point3 between(const gtsam::Point3& p2) const;
-
-  // Manifold
-  gtsam::Point3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Point3& p) const;
-
-  // Lie Group
-  static gtsam::Point3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Point3& p);
 
   // Standard Interface
   Vector vector() const;

python/handwritten/geometry/Point3.cpp

@@ -36,16 +36,13 @@ class_<Point3>("Point3")
   .def(init<const Vector3 &>())
   .def("identity", &Point3::identity)
   .staticmethod("identity")
-  .def("add", &Point3::add)
   .def("cross", &Point3::cross)
-  .def("dist", &Point3::dist)
   .def("distance", &Point3::distance)
   .def("dot", &Point3::dot)
   .def("equals", &Point3::equals, equals_overloads(args("q","tol")))
   .def("norm", &Point3::norm)
   .def("normalize", &Point3::normalize)
   .def("print", &Point3::print, print_overloads(args("s")))
-  .def("sub", &Point3::sub)
   .def("vector", &Point3::vector)
   .def("x", &Point3::x)
   .def("y", &Point3::y)

tests/simulated3D.h

@@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
    */
   Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
       boost::none) const {
-    return (prior(x, H) - measured_).vector();
+    return prior(x, H).vector() - measured_.vector();
   }
 };
 
@@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
    */
   Vector evaluateError(const Point3& x1, const Point3& x2,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
-    return (mea(x1, x2, H1, H2) - measured_).vector();
+    return mea(x1, x2, H1, H2).vector() - measured_.vector();
   }
 };
 

timing/timeSFMBAL.h

@@ -49,8 +49,12 @@ SfM_data preamble(int argc, char* argv[]) {
 
   // Load BAL file
   SfM_data db;
-  string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre");
+  string filename;
-  bool success = readBAL(argc > 1 ? argv[argc - 1] : defaultFilename, db);
+  if (argc > 1)
+    filename = argv[argc - 1];
+  else
+    filename = findExampleDataFile("dubrovnik-16-22106-pre");
+  bool success = readBAL(argv[argc - 1], db);
   if (!success) throw runtime_error("Could not access file!");
   return db;
 }