Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
Implemented comparison operator for poses.
  • Loading branch information
remod committed Aug 16, 2018
1 parent 047ba02 commit 77f9b72
Show file tree
Hide file tree
Showing 5 changed files with 51 additions and 12 deletions.
12 changes: 12 additions & 0 deletions include/kindr/common/common.hpp
Expand Up @@ -129,6 +129,18 @@ class MultiplicationTraits {
// inline static Left_ mult(const Left_& lhs, const Right_& rhs);
};

/*! \brief Comparison traits for comparing different rotations
* \class ComparisonTraits
* (only for advanced users)
*/
template<typename Left_, typename Right_>
class ComparisonTraits {
public:
inline static bool isEqual(const Left_& left, const Right_& right) {
return left.toImplementation() == Left_(right).toImplementation();
}
};


/*! \class get_scalar
* \brief Gets the primitive of the vector.
Expand Down
11 changes: 11 additions & 0 deletions include/kindr/poses/Pose.hpp
Expand Up @@ -84,6 +84,17 @@ class MultiplicationTraits<PoseBase<Left_>, PoseBase<Right_> > {
// }
//};

/*! \brief Compare two poses.
*/
template<typename Left_, typename Right_>
class ComparisonTraits<PoseBase<Left_>, PoseBase<Right_>>{
public:
inline static bool isEqual(const PoseBase<Left_>& lhs, const PoseBase<Right_>& rhs) {
return lhs.derived().getPosition() == rhs.derived().getPosition() &&
lhs.derived().getRotation() == rhs.derived().getRotation();
}
};


} // namespace internal
} // namespace kindr
8 changes: 8 additions & 0 deletions include/kindr/poses/PoseBase.hpp
Expand Up @@ -139,6 +139,14 @@ class PoseBase {
return internal::MultiplicationTraits<PoseBase<Derived_>,PoseBase<OtherDerived_>>::mult(this->derived(), other.derived()); // todo: 1. ok? 2. may be optimized
}

/*! \brief Compares two poses.
* \returns true if the poses are exactly equal
*/
template<typename OtherDerived_>
bool operator ==(const PoseBase<OtherDerived_>& other) const {
return internal::ComparisonTraits<PoseBase<Derived_>,PoseBase<OtherDerived_>>::isEqual(*this, other);
}

/*! \brief Sets the pose to identity
* \returns reference
*/
Expand Down
12 changes: 0 additions & 12 deletions include/kindr/rotations/RotationBase.hpp
Expand Up @@ -68,18 +68,6 @@ class ConversionTraits {
// inline static Dest_ convert(const Source_& );
};

/*! \brief Comparison traits for comparing different rotations
* \class ComparisonTraits
* (only for advanced users)
*/
template<typename Left_, typename Right_>
class ComparisonTraits {
public:
inline static bool isEqual(const Left_& left, const Right_& right) {
return left.toImplementation() == Left_(right).toImplementation();
}
};



/*! \brief Rotation traits for rotating vectors and matrices
Expand Down
20 changes: 20 additions & 0 deletions test/poses/HomogeneousTransformationTest.cpp
Expand Up @@ -90,6 +90,26 @@ TYPED_TEST(HomogeneousTransformationTest, testSetIdentity)
}


TYPED_TEST(HomogeneousTransformationTest, testComparisonEqual)
{
typedef typename TestFixture::Pose Pose;
typedef typename TestFixture::Position Position;
typedef typename TestFixture::Rotation Rotation;
typedef typename TestFixture::Scalar Scalar;
Position positionAToBInA(1.0,2.0,3.0);
Position positionAToCInA(1.0,2.0,4.0);
Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
Rotation rotationCToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
Pose poseBToA(positionAToBInA, rotationBToA);
Pose poseBToA2(positionAToBInA, rotationBToA);
Pose poseCToA(positionAToCInA, rotationCToA);

// Check equality comparison
ASSERT_TRUE(poseBToA == poseBToA2);
ASSERT_FALSE(poseBToA == poseCToA);
}


TYPED_TEST(HomogeneousTransformationTest, testTransform)
{
typedef typename TestFixture::Pose Pose;
Expand Down

0 comments on commit 77f9b72

Please sign in to comment.