Merge branch 'develop' into feature/wrap-multiple-interfaces

gtsam/base/Matrix.h

@@ -29,7 +29,7 @@
 #include <gtsam/config.h>
 
 #include <boost/format.hpp>
-#include <boost/function.hpp>
+#include <functional>
 #include <boost/tuple/tuple.hpp>
 #include <boost/math/special_functions/fpclassify.hpp>
 
@@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction {
 
   // The function phi should calculate f(a)*b, with derivatives in a and b.
   // Naturally, the derivative in b is f(a).
-  typedef boost::function<VectorN(
+  typedef std::function<VectorN(
       const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
       Operator;
 

gtsam/base/Testable.h

@@ -34,8 +34,9 @@
 #pragma once
 
 #include <boost/concept_check.hpp>
-#include <boost/shared_ptr.hpp>
+#include <functional>
 #include <iostream>
+#include <memory>
 #include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
@@ -119,10 +120,10 @@ namespace gtsam {
    * Binary predicate on shared pointers
    */
   template<class V>
-  struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
+  struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
-    bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
+    bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
       if (!actual && !expected) return true;
       return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
     }

gtsam/base/lieProxies.h

@@ -24,7 +24,7 @@
  *
  * These should not be used outside of tests, as they are just remappings
  * of the original functions.  We use these to avoid needing to do
- * too much boost::bind magic or writing a bunch of separate proxy functions.
+ * too much std::bind magic or writing a bunch of separate proxy functions.
  *
  * Don't expect all classes to work for all of these functions.
  */

gtsam/discrete/DecisionTree-inl.h

@@ -450,7 +450,7 @@ namespace gtsam {
   template<typename L, typename Y>
   template<typename M, typename X>
   DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
-      const std::map<M, L>& map, boost::function<Y(const X&)> op)  {
+      const std::map<M, L>& map, std::function<Y(const X&)> op)  {
     root_ = convert(other.root_, map, op);
   }
 
@@ -568,7 +568,7 @@ namespace gtsam {
   template<typename M, typename X>
   typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
       const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
-      boost::function<Y(const X&)> op) {
+      std::function<Y(const X&)> op) {
 
     typedef DecisionTree<M, X> MX;
     typedef typename MX::Leaf MXLeaf;

gtsam/discrete/DecisionTree.h

@@ -20,10 +20,12 @@
 #pragma once
 
 #include <gtsam/discrete/Assignment.h>
+
 #include <boost/function.hpp>
+#include <functional>
 #include <iostream>
-#include <vector>
 #include <map>
+#include <vector>
 
 namespace gtsam {
 
@@ -38,8 +40,8 @@ namespace gtsam {
   public:
 
     /** Handy typedefs for unary and binary function types */
-    typedef boost::function<Y(const Y&)> Unary;
-    typedef boost::function<Y(const Y&, const Y&)> Binary;
+    typedef std::function<Y(const Y&)> Unary;
+    typedef std::function<Y(const Y&, const Y&)> Binary;
 
     /** A label annotated with cardinality */
     typedef std::pair<L,size_t> LabelC;
@@ -107,7 +109,7 @@ namespace gtsam {
     /** Convert to a different type */
     template<typename M, typename X> NodePtr
     convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
-        L>& map, boost::function<Y(const X&)> op);
+        L>& map, std::function<Y(const X&)> op);
 
     /** Default constructor */
     DecisionTree();
@@ -143,7 +145,7 @@ namespace gtsam {
     /** Convert from a different type */
     template<typename M, typename X>
     DecisionTree(const DecisionTree<M, X>& other,
-        const std::map<M, L>& map, boost::function<Y(const X&)> op);
+        const std::map<M, L>& map, std::function<Y(const X&)> op);
 
     /// @}
     /// @name Testable

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
 }
 
 /** I can't get this to work !
- class Mul: boost::function<double(const double&, const double&)> {
+ class Mul: std::function<double(const double&, const double&)> {
  inline double operator()(const double& a, const double& b) {
  return a * b;
  }

gtsam/discrete/tests/testDecisionTree.cpp

@@ -196,7 +196,7 @@ TEST(DT, conversion)
   map<string, Label> ordering;
   ordering[A] = X;
   ordering[B] = Y;
-  boost::function<bool(const int&)> op = convert;
+  std::function<bool(const int&)> op = convert;
   BDT f2(f1, ordering, op);
   //  f1.print("f1");
   //  f2.print("f2");

gtsam/geometry/Cal3Fisheye.cpp

@@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 /* ************************************************************************* */
 Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // initial gues just inverts the pinhole model
+  // Apply inverse camera matrix to map the pixel coordinate (u, v) 
+  // of the equidistant fisheye image to angular coordinate space (xd, yd)
+  // with radius theta given in radians.
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
-  Point2 pi(xd, yd);
+  const double theta = sqrt(xd * xd + yd * yd);
+
+  // Provide initial guess for the Gauss-Newton search.
+  // The angular coordinates given by (xd, yd) are mapped back to
+  // the focal plane of the perspective undistorted projection pi.
+  // See Cal3Unified.calibrate() using the same pattern for the 
+  // undistortion of omnidirectional fisheye projection.
+  const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
+  Point2 pi(scale * xd, scale * yd);
 
   // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached

gtsam/geometry/Cyclic.h

@@ -17,7 +17,9 @@
 
 #include <gtsam/base/Group.h>
 #include <gtsam/base/Testable.h>
-#include <iostream> // for cout :-(
+
+#include <cassert>
+#include <iostream>  // for cout :-(
 
 namespace gtsam {
 

gtsam/geometry/SimpleCamera.h

@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -20,12 +20,11 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) {
   EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
 
   // Check derivative
-  boost::function<CalibratedCamera(Pose3)> f =  //
-      boost::bind(CalibratedCamera::Create, _1, boost::none);
+  std::function<CalibratedCamera(Pose3)> f =  //
+      std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
   Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -5,16 +5,15 @@
  * @date December 17, 2013
  */
 
-#include <gtsam/geometry/EssentialMatrix.h>
-#include <gtsam/geometry/CalibratedCamera.h>
-#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/EssentialMatrix.h>
 
-#include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
 #include <sstream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -42,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
       1e-8));
 
   Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
-      boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
-                  boost::none),
+      std::bind(EssentialMatrix::FromRotationAndDirection,
+                std::placeholders::_1, trueDirection, boost::none, boost::none),
       trueRotation);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
 
   Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
-      boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
-                  boost::none),
-                  trueDirection);
+      std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation,
+                std::placeholders::_1, boost::none, boost::none),
+      trueDirection);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
 }
 
@@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) {
   Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
   EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
-      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+      std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }
 
@@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) {
   Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
   EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
-      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+      std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -21,10 +21,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/vector.hpp>
-#include <boost/bind/bind.hpp>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 using boost::none;
@@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) {
                       Vector2(actual[0], actual[1])));
   EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
 
-  boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
-      boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
+  std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
+      std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   expectedH1 = numericalDerivative21(f, plane1, plane2);
   expectedH2 = numericalDerivative22(f, plane1, plane2);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) {
 TEST(OrientedPlane3, jacobian_retract) {
   OrientedPlane3 plane(-1, 0.1, 0.2, 5);
   Matrix33 H_actual;
-  boost::function<OrientedPlane3(const Vector3&)> f =
-      boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
+  std::function<OrientedPlane3(const Vector3&)> f = std::bind(
+      &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
   {
       Vector3 v(-0.1, 0.2, 0.3);
       plane.retract(v, H_actual);

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -22,13 +22,12 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <cmath>
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) {
   EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
 
   // Check derivative
-  boost::function<Camera(Pose3,Cal3_S2)> f = //
-      boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
+  std::function<Camera(Pose3, Cal3_S2)> f =  //
+      std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
   EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
   Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) {
   EXPECT(assert_equal(pose, camera.getPose(actualH)));
 
   // Check derivative
-  boost::function<Pose3(Camera)> f = //
-      boost::bind(&Camera::getPose,_1,boost::none);
+  std::function<Pose3(Camera)> f =  //
+      std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }

gtsam/geometry/tests/testPinholePose.cpp

@@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
   EXPECT(assert_equal(pose, camera.getPose(actualH)));
 
   // Check derivative
-  boost::function<Pose3(Camera)> f = //
-      boost::bind(&Camera::getPose,_1,boost::none);
+  std::function<Pose3(Camera)> f = //
+      std::bind(&Camera::getPose,_1,boost::none);
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }