fix jet-type related bug

Ralfhund · Nov 24, 2017 · cb4ce2d · cb4ce2d
1 parent 3f84e42
commit cb4ce2d
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 4 deletions.
diff --git a/.travis.yml b/.travis.yml
@@ -47,7 +47,7 @@ script:
   - cd build
   - cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=$BUILD_TYPE ..
   - make -j
-  - make test
+  - make CTEST_OUTPUT_ON_FAILURE=1 test
 
 after_success:
  - if [[ "$BUILD_TYPE" == "Coverage" ]]; then lcov --directory . --capture --output-file coverage.info ; fi

diff --git a/sophus/se3.hpp b/sophus/se3.hpp
@@ -583,8 +583,9 @@ class SE3 : public SE3Base<SE3<Scalar_, Options>> {
   SOPHUS_FUNC explicit SE3(Matrix4<Scalar> const& T)
       : so3_(T.template topLeftCorner<3, 3>()),
         translation_(T.template block<3, 1>(0, 3)) {
-    SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(0, 0, 0, 1)).squaredNorm() <
-                      Constants<Scalar>::epsilon(),
+    SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0),
+                                                   Scalar(0), Scalar(1)))
+                          .squaredNorm() < Constants<Scalar>::epsilon(),
                   "Last row is not (0,0,0,1), but (%).", T.row(3));
   }
 

diff --git a/sophus/so3.hpp b/sophus/so3.hpp
@@ -561,7 +561,7 @@ class SO3 : public SO3Base<SO3<Scalar_, Options>> {
   SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) {
     SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %",
                   R * R.transpose());
-    SOPHUS_ENSURE(R.determinant() > 0, "det(R) is not positive: %",
+    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
                   R.determinant());
   }
 

diff --git a/test/ceres/test_ceres_se3.cpp b/test/ceres/test_ceres_se3.cpp
@@ -79,6 +79,13 @@ bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init) {
   return passed;
 }
 
+template <typename Scalar>
+bool CreateSE3FromMatrix(Eigen::Matrix<Scalar, 4, 4> mat) {
+  auto se3 = Sophus::SE3<Scalar>(mat);
+  se3 = se3;
+  return true;
+}
+
 int main(int, char**) {
   using SE3Type = Sophus::SE3<double>;
   using SO3Type = Sophus::SO3<double>;
@@ -115,5 +122,9 @@ int main(int, char**) {
     }
   }
 
+  Eigen::Matrix<ceres::Jet<double, 28>, 4, 4> mat;
+  mat.setIdentity();
+  std::cout << CreateSE3FromMatrix(mat) << std::endl;
+
   return 0;
 }