[C++][Unit test] Add isIdentity method to SE3

src/spatial/se3.hpp

@@ -46,7 +46,7 @@ namespace se3
    * aMb (x) =  aRb*x + aAB
    * where aAB is the vector from origin A to origin B expressed in coordinates A.
    */
-  template< class Derived>
+  template<class Derived>
   class SE3Base
   {
   protected:
@@ -120,6 +120,14 @@ namespace se3
         X.disp(os);
         return os;
       }
+    
+    ///
+    /// \returns true if *this is approximately equal to the identity placement, within the precision given by prec.
+    ///
+    bool isIdentity(const typename traits<Derived>::Scalar & prec = Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
+    {
+      return derived().isIdentity(prec);
+    }
 
   }; // class SE3Base
 
@@ -287,6 +295,11 @@ namespace se3
     {
       return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
     }
+    
+    bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      return rot.isIdentity(prec) && trans.isZero(prec);
+    }
 
     ConstAngular_t & rotation_impl() const { return rot; }
     Angular_t & rotation_impl() { return rot; }

unittest/tspatial.cpp

@@ -33,7 +33,7 @@ BOOST_AUTO_TEST_SUITE ( tspatialTest)
 
 BOOST_AUTO_TEST_CASE ( test_SE3 )
 {
-using namespace se3;
+  using namespace se3;
   typedef Eigen::Matrix<double,4,4> Matrix4;
   typedef SE3::Matrix6 Matrix6;
   typedef SE3::Vector3 Vector3;
@@ -62,7 +62,6 @@ using namespace se3;
   Vector3 Mip = (aMb.inverse()*p4).head(3);
   BOOST_CHECK(amb.actInv(p).isApprox(Mip, 1e-12));
 
-
   // Test action matrix
   Matrix6 aXb = amb;
   Matrix6 bXc = bmc;
@@ -71,6 +70,13 @@ using namespace se3;
 
   Matrix6 bXa = amb.inverse();
   BOOST_CHECK(bXa.isApprox(aXb.inverse(), 1e-12));
+  
+  // Test isIdentity
+  SE3 identity = SE3::Identity();
+  BOOST_CHECK(identity.isIdentity());
+  
+  // Test isApprox
+  BOOST_CHECK(identity.isApprox(identity));
 }
 
 BOOST_AUTO_TEST_CASE ( test_Motion )