Merge pull request #96 from artivis/fix/normalize

fix typo compilation issue in normalize SE2/3

Author: artivis

include/manif/impl/se2/SE2_base.h

@@ -383,7 +383,7 @@ SE2Base<_Derived>::y() const
 template <typename _Derived>
 void SE2Base<_Derived>::normalize()
 {
-  coeffs_nonconst().template tails<2>().normalize();
+  coeffs_nonconst().template tail<2>().normalize();
 }
 
 namespace internal {

include/manif/impl/se3/SE3_base.h

@@ -361,7 +361,7 @@ SE3Base<_Derived>::z() const
 template <typename _Derived>
 void SE3Base<_Derived>::normalize()
 {
-  coeffs_nonconst().template tails<4>().normalize();
+  coeffs_nonconst().template tail<4>().normalize();
 }
 
 namespace internal {

test/common_tester.h

@@ -9,6 +9,8 @@
   using TEST_##manifold##_TESTER = CommonTester<manifold>;                \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_COPY_CONSTRUCTOR)    \
   { evalCopyConstructor(); }                                              \
+  TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_UNNORMALIZE_DATA)    \
+  { evalConstructorUnnormalizedData(); }                                  \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_ASSIGNMENT)          \
   { evalAssignment(); }                                                   \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_DATA_PTR_VALID)      \
@@ -64,7 +66,9 @@
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_INNER)               \
   { evalInner(); }                                                        \
   TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_NUMERICAL_STABILITY) \
-  { evalNumericalStability(); }
+  { evalNumericalStability(); }                                           \
+  TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_NORMALIZE)           \
+  { evalNormalize(); }
 
 #define MANIF_TEST_JACOBIANS(manifold)                                            \
   using TEST_##manifold##_JACOBIANS_TESTER = JacobianTester<manifold>;            \
@@ -136,6 +140,14 @@ class CommonTester : public ::testing::Test
     EXPECT_MANIF_NEAR(state, state_copy, tol_);
   }
 
+  void evalConstructorUnnormalizedData()
+  {
+    using DataType = typename LieGroup::DataType;
+    EXPECT_THROW(
+      LieGroup(DataType::Random()*10.), manif::invalid_argument
+    );
+  }
+
   void evalAssignment()
   {
     LieGroup state_copy;
@@ -509,6 +521,22 @@ class CommonTester : public ::testing::Test
     ) << "+= failed at iteration " << i ;
   }
 
+  void evalNormalize()
+  {
+    typename LieGroup::DataType data = LieGroup::DataType::Random() * 100.;
+
+    EXPECT_THROW(
+      LieGroup a(data), manif::invalid_argument
+    );
+
+    Eigen::Map<LieGroup> map(data.data());
+    map.normalize();
+
+    EXPECT_NO_THROW(
+      LieGroup b = map
+    );
+  }
+
 protected:
 
   Scalar tol_ = Constants<Scalar>::eps;