Jet (strasdat#138)

* LieGroup<ceres::jet<double,...>> compiles

* limit to two threads

* smaller test set for jet types

.travis.yml

@@ -56,7 +56,7 @@ script:
   - if ! [[ "$BUILD_TYPE" == "Python" ]]; then mkdir build ; fi
   - if ! [[ "$BUILD_TYPE" == "Python" ]]; then cd build ; fi
   - if ! [[ "$BUILD_TYPE" == "Python" ]]; then cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=$BUILD_TYPE .. ; fi
-  - if ! [[ "$BUILD_TYPE" == "Python" ]]; then make -j ; fi
+  - if ! [[ "$BUILD_TYPE" == "Python" ]]; then make -j2 ; fi
   - if ! [[ "$BUILD_TYPE" == "Python" ]]; then make CTEST_OUTPUT_ON_FAILURE=1 test ; fi
   - if [[ "$BUILD_TYPE" == "Python" ]]; then cd py ; fi
   - if [[ "$BUILD_TYPE" == "Python" ]]; then ./run_tests.sh ; fi

scripts/install_linux_deps.sh

@@ -4,19 +4,25 @@ set -x # echo on
 set -e # exit on error
 
 wget https://cmake.org/files/v3.9/cmake-3.9.0-Linux-x86_64.sh
-chmod +x cmake-3.9.0-Linux-x86_64.sh 
+chmod +x cmake-3.9.0-Linux-x86_64.sh
 set +x # echo off
 sudo ./cmake-3.9.0-Linux-x86_64.sh  --skip-license --prefix=/usr/local
 set -x # echo on
 sudo update-alternatives --install /usr/bin/cmake cmake /usr/local/bin/cmake 1 --force
-cmake --version 
+cmake --version
 
-sudo apt-get -qq update 
-sudo apt-get install libeigen3-dev libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
+sudo apt-get -qq update
+sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
 sudo unlink /usr/bin/gcc && sudo ln -s /usr/bin/gcc-5 /usr/bin/gcc
 sudo unlink /usr/bin/g++ && sudo ln -s /usr/bin/g++-5 /usr/bin/g++
 gcc --version
 g++ --version
+wget http://bitbucket.org/eigen/eigen/get/3.3.4.tar.bz2
+tar xvf 3.3.4.tar.bz2
+mkdir build-eigen
+cd build-eigen
+cmake ../eigen-eigen-5a0156e40feb
+sudo make install
 git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver
 cd ceres-solver
 git reset --hard afe93546b67cee0ad205fe8044325646ed5deea9

sophus/average.hpp

@@ -163,12 +163,12 @@ average(SequenceContainer const& foo_Ts_bar) {
 
   SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
   Scalar scale_sum = Scalar(0);
-  using std::log;
   using std::exp;
+  using std::log;
   for (RxSO3<Scalar> const& foo_T_bar : foo_Ts_bar) {
     scale_sum += log(foo_T_bar.scale());
   }
-  return RxSO3<Scalar>(exp(scale_sum / N),
+  return RxSO3<Scalar>(exp(scale_sum / Scalar(N)),
                        SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar)));
 }
 

sophus/common.hpp

@@ -131,9 +131,10 @@ SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line,
 }  // namespace Sophus
 
 // LCOV_EXCL_STOP
-#define SOPHUS_ENSURE(expr, ...)                                         \
-  ((expr) ? ((void)0) : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, \
-                                              __LINE__, ##__VA_ARGS__))
+#define SOPHUS_ENSURE(expr, ...)                                       \
+  ((expr) ? ((void)0)                                                  \
+          : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \
+                                  ##__VA_ARGS__))
 #endif
 
 namespace Sophus {
@@ -142,7 +143,10 @@ template <class Scalar>
 struct Constants {
   SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
 
-  SOPHUS_FUNC static Scalar epsilonSqrt() { return std::sqrt(epsilon()); }
+  SOPHUS_FUNC static Scalar epsilonSqrt() {
+    using std::sqrt;
+    return sqrt(epsilon());
+  }
 
   SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); }
 };

sophus/rotation_matrix.hpp

@@ -36,7 +36,7 @@ SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
 
   Scalar det = sR.determinant();
 
-  if (det <= 0) {
+  if (det <= Scalar(0)) {
     return false;
   }
 
@@ -53,9 +53,10 @@ SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
 // Takes in arbiray square matrix (2x2 or larger) and returns closest
 // orthogonal matrix with positive determinant.
 template <class D>
-SOPHUS_FUNC
-    Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>
-    makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
+SOPHUS_FUNC enable_if_t<
+    std::is_floating_point<typename D::Scalar>::value,
+    Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
+makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
   using Scalar = typename D::Scalar;
   static int const N = D::RowsAtCompileTime;
   static int const M = D::ColsAtCompileTime;

sophus/rxso3.hpp

@@ -257,7 +257,7 @@ class RxSO3Base {
     quaternion_nonconst() *= other.quaternion();
     Scalar scale = this->scale();
     if (scale < Constants<Scalar>::epsilon()) {
-      SOPHUS_ENSURE(scale > 0, "Scale must be greater zero.");
+      SOPHUS_ENSURE(scale > Scalar(0), "Scale must be greater zero.");
       // Saturation to ensure class invariant.
       quaternion_nonconst().normalize();
       quaternion_nonconst().coeffs() *= sqrt(Constants<Scalar>::epsilon());
@@ -309,7 +309,7 @@ class RxSO3Base {
     using std::sqrt;
     Scalar saved_scale = scale();
     quaternion_nonconst() = R;
-    quaternion_nonconst().coeffs() *= std::sqrt(saved_scale);
+    quaternion_nonconst().coeffs() *= sqrt(saved_scale);
   }
 
   // Sets scale and leaves rotation as is.
@@ -412,7 +412,8 @@ class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
       : quaternion_(R) {
     SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
                   "Scale factor must be greater-equal epsilon.");
-    quaternion_.coeffs() *= std::sqrt(scale);
+    using std::sqrt;
+    quaternion_.coeffs() *= sqrt(scale);
   }
 
   // Constructor from scale factor and SO3
@@ -423,7 +424,8 @@ class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
       : quaternion_(so3.unit_quaternion()) {
     SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
                   "Scale factor must be greater-equal epsilon.");
-    quaternion_.coeffs() *= std::sqrt(scale);
+    using std::sqrt;
+    quaternion_.coeffs() *= sqrt(scale);
   }
 
   // Constructor from quaternion
@@ -671,6 +673,6 @@ class Map<Sophus::RxSO3<Scalar_> const, Options>
  protected:
   Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_;
 };
-}
+}  // namespace Eigen
 
 #endif  // SOPHUS_RXSO3_HPP

sophus/se2.hpp

@@ -102,18 +102,19 @@ class SE2Base {
       const {
     Matrix<Scalar, DoF, num_parameters> J;
     Sophus::Vector2<Scalar> const c = unit_complex();
-    J(0, 0) = 0;
-    J(0, 1) = 0;
+    Scalar o(0);
+    J(0, 0) = o;
+    J(0, 1) = o;
     J(0, 2) = c[0];
     J(0, 3) = c[1];
-    J(1, 0) = 0;
-    J(1, 1) = 0;
+    J(1, 0) = o;
+    J(1, 1) = o;
     J(1, 2) = -c[1];
     J(1, 3) = c[0];
     J(2, 0) = -c[1];
     J(2, 1) = c[0];
-    J(2, 2) = 0;
-    J(2, 3) = 0;
+    J(2, 2) = o;
+    J(2, 3) = o;
     return J;
   }
 
@@ -135,6 +136,8 @@ class SE2Base {
   // of SE(2).
   //
   SOPHUS_FUNC Tangent log() const {
+    using std::abs;
+
     Tangent upsilon_theta;
     Scalar theta = so2().log();
     upsilon_theta[2] = theta;
@@ -143,7 +146,7 @@ class SE2Base {
 
     Vector2<Scalar> z = so2().unit_complex();
     Scalar real_minus_one = z.x() - Scalar(1.);
-    if (std::abs(real_minus_one) < Constants<Scalar>::epsilon()) {
+    if (abs(real_minus_one) < Constants<Scalar>::epsilon()) {
       halftheta_by_tan_of_halftheta =
           Scalar(1.) - Scalar(1. / 12) * theta * theta;
     } else {
@@ -431,10 +434,10 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
   //
   SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, num_parameters> Dx_exp_x(
       Tangent const& upsilon_theta) {
+    using std::abs;
+    using std::cos;
     using std::pow;
     using std::sin;
-    using std::cos;
-    using std::abs;
     Sophus::Matrix<Scalar, DoF, num_parameters> J;
     Sophus::Vector<Scalar, 2> upsilon = upsilon_theta.template head<2>();
     Scalar theta = upsilon_theta[2];
@@ -452,21 +455,23 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
     }
 
     Scalar const c0 = sin(theta);
-    Scalar const c1 = 1.0 / theta;
+    Scalar const c1 = Scalar(1.0) / theta;
     Scalar const c2 = c0 * c1;
     Scalar const c3 = cos(theta);
-    Scalar const c4 = -c3 + 1;
+    Scalar const c4 = -c3 + Scalar(1);
     Scalar const c5 = c1 * c4;
     Scalar const c6 = c1 * c3;
     Scalar const c7 = pow(theta, -2);
     Scalar const c8 = c0 * c7;
     Scalar const c9 = c4 * c7;
-    J(0, 0) = 0;
-    J(0, 1) = 0;
+
+    Scalar const o = Scalar(0);
+    J(0, 0) = o;
+    J(0, 1) = o;
     J(0, 2) = c2;
     J(0, 3) = c5;
-    J(1, 0) = 0;
-    J(1, 1) = 0;
+    J(1, 0) = o;
+    J(1, 1) = o;
     J(1, 2) = -c5;
     J(1, 3) = c2;
     J(2, 0) = -c0;
@@ -517,8 +522,9 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
     SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
     Scalar sin_theta_by_theta;
     Scalar one_minus_cos_theta_by_theta;
+    using std::abs;
 
-    if (std::abs(theta) < Constants<Scalar>::epsilon()) {
+    if (abs(theta) < Constants<Scalar>::epsilon()) {
       Scalar theta_sq = theta * theta;
       sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq;
       one_minus_cos_theta_by_theta =
@@ -536,7 +542,9 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
 
   // Returns closest SE3 given arbirary 4x4 matrix.
   //
-  SOPHUS_FUNC static SE2 fitToSE2(Matrix3<Scalar> const& T) {
+  template <class S = Scalar>
+  static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
+  fitToSE2(Matrix3<Scalar> const& T) {
     return SE2(SO2<Scalar>::fitToSO2(T.template block<2, 2>(0, 0)),
                T.template block<2, 1>(0, 2));
   }
@@ -668,7 +676,7 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
   TranslationMember translation_;
 };
 
-}  // end namespace
+}  // namespace Sophus
 
 namespace Eigen {
 
@@ -765,6 +773,6 @@ class Map<Sophus::SE2<Scalar_> const, Options>
   Map<Sophus::SO2<Scalar> const, Options> const so2_;
   Map<Sophus::Vector2<Scalar> const, Options> const translation_;
 };
-}
+}  // namespace Eigen
 
 #endif