mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-03 16:47:47 +08:00
Added Transform and VisualWord tests
This commit is contained in:
matlabbe
@@ -38,28 +38,62 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
/**
|
||||
* @class Transform
|
||||
* @brief Represents a 3D rigid body transformation (rotation + translation).
|
||||
*
|
||||
* This class provides an abstraction over 3D transformations using a 3x4 matrix representation,
|
||||
* with utilities for conversion to/from Eigen and OpenCV formats, interpolation, inversion,
|
||||
* DoF reduction, and distance calculations. It is fundamental to pose estimation and motion handling
|
||||
* within the RTAB-Map framework.
|
||||
*
|
||||
* The underlying data is stored in a `cv::Mat` (3x4, CV_32FC1), representing a rotation matrix and translation vector.
|
||||
*/
|
||||
class RTABMAP_CORE_EXPORT Transform
|
||||
{
|
||||
public:
|
||||
|
||||
// Zero by default
|
||||
/**
|
||||
* @brief Default constructor. Initializes to a null (all zeros) transform.
|
||||
*/
|
||||
Transform();
|
||||
// rotation matrix r## and origin o##
|
||||
/**
|
||||
* @brief Constructor from rotation matrix elements and translation components.
|
||||
* @param r11...r33 Rotation matrix components.
|
||||
* @param o14,o24,o34 Translation vector components.
|
||||
*/
|
||||
Transform(float r11, float r12, float r13, float o14,
|
||||
float r21, float r22, float r23, float o24,
|
||||
float r31, float r32, float r33, float o34);
|
||||
// should have 3 rows, 4 cols and type CV_32FC1
|
||||
/**
|
||||
* @brief Constructs a Transform from a 3x4 OpenCV matrix.
|
||||
* @param transformationMatrix A 3x4 CV_32FC1 matrix.
|
||||
*/
|
||||
Transform(const cv::Mat & transformationMatrix);
|
||||
// x,y,z, roll,pitch,yaw
|
||||
/**
|
||||
* @brief Constructs a Transform from position and Euler angles (in radians).
|
||||
* @param x, y, z Translation components.
|
||||
* @param roll, pitch, yaw Euler rotation angles (ZYX-convention).
|
||||
*/
|
||||
Transform(float x, float y, float z, float roll, float pitch, float yaw);
|
||||
// x,y,z, qx,qy,qz,qw
|
||||
/**
|
||||
* @brief Constructs a Transform from position and quaternion.
|
||||
* @param x, y, z Translation components.
|
||||
* @param qx, qy, qz, qw Quaternion rotation components.
|
||||
*/
|
||||
Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
|
||||
// x,y, theta
|
||||
/**
|
||||
* @brief Constructs a 2D Transform (x, y, theta).
|
||||
*/
|
||||
Transform(float x, float y, float theta);
|
||||
|
||||
/**
|
||||
* @brief Returns a deep copy of the transform.
|
||||
*/
|
||||
Transform clone() const;
|
||||
|
||||
float r11() const {return data()[0];}
|
||||
// --- Accessors (rotation matrix elements, translation) ---
|
||||
float r11() const {return data()[0];} //!< Rotation matrix element at row 1, col 1.
|
||||
float r12() const {return data()[1];}
|
||||
float r13() const {return data()[2];}
|
||||
float r21() const {return data()[4];}
|
||||
@@ -69,64 +103,157 @@ public:
|
||||
float r32() const {return data()[9];}
|
||||
float r33() const {return data()[10];}
|
||||
|
||||
float o14() const {return data()[3];}
|
||||
float o24() const {return data()[7];}
|
||||
float o34() const {return data()[11];}
|
||||
float o14() const {return data()[3];} //!< Translation x
|
||||
float o24() const {return data()[7];} //!< Translation y
|
||||
float o34() const {return data()[11];} //!< Translation z
|
||||
|
||||
float & operator[](int index) {return data()[index];}
|
||||
const float & operator[](int index) const {return data()[index];}
|
||||
float & operator()(int row, int col) {return data()[row*4 + col];}
|
||||
const float & operator()(int row, int col) const {return data()[row*4 + col];}
|
||||
|
||||
/**
|
||||
* @brief Checks whether the transform is null (all zeros).
|
||||
*/
|
||||
bool isNull() const;
|
||||
/**
|
||||
* @brief Checks whether the transform is identity.
|
||||
*/
|
||||
bool isIdentity() const;
|
||||
|
||||
/**
|
||||
* @brief Sets the transform to null (zero matrix).
|
||||
*/
|
||||
void setNull();
|
||||
/**
|
||||
* @brief Sets the transform to the identity transform.
|
||||
*/
|
||||
void setIdentity();
|
||||
|
||||
/**
|
||||
* @brief Returns the internal OpenCV matrix (3x4).
|
||||
*/
|
||||
const cv::Mat & dataMatrix() const {return data_;}
|
||||
/**
|
||||
* @brief Returns a pointer to the raw data (12 floats).
|
||||
*/
|
||||
const float * data() const {return (const float *)data_.data;}
|
||||
float * data() {return (float *)data_.data;}
|
||||
|
||||
/**
|
||||
* @brief Returns the number of float elements (always 12).
|
||||
*/
|
||||
int size() const {return 12;}
|
||||
|
||||
float & x() {return data()[3];}
|
||||
float & y() {return data()[7];}
|
||||
float & z() {return data()[11];}
|
||||
/// Translation getters/setters
|
||||
float & x() {return data()[3];} //!< Translation x
|
||||
float & y() {return data()[7];} //!< Translation y
|
||||
float & z() {return data()[11];} //!< Translation z
|
||||
const float & x() const {return data()[3];}
|
||||
const float & y() const {return data()[7];}
|
||||
const float & z() const {return data()[11];}
|
||||
|
||||
/**
|
||||
* @brief Returns 2D orientation (theta) in radians.
|
||||
*/
|
||||
float theta() const;
|
||||
|
||||
/**
|
||||
* @brief Returns whether the transform is invertible.
|
||||
*/
|
||||
bool isInvertible() const;
|
||||
/**
|
||||
* @brief Returns the inverse of the transform.
|
||||
*/
|
||||
Transform inverse() const;
|
||||
/**
|
||||
* @brief Returns only the rotation component.
|
||||
*/
|
||||
Transform rotation() const;
|
||||
/**
|
||||
* @brief Returns only the translation component.
|
||||
*/
|
||||
Transform translation() const;
|
||||
/**
|
||||
* @brief Converts to 3 DoF (x, y, theta).
|
||||
*/
|
||||
Transform to3DoF() const;
|
||||
/**
|
||||
* @brief Converts to 4 DoF (x, y, z, yaw).
|
||||
*/
|
||||
Transform to4DoF() const;
|
||||
/**
|
||||
* @brief Checks if the transform is 3 DoF (no pitch/roll).
|
||||
*/
|
||||
bool is3DoF() const;
|
||||
/**
|
||||
* @brief Checks if the transform is 4 DoF (no pitch).
|
||||
*/
|
||||
bool is4DoF() const;
|
||||
|
||||
/**
|
||||
* @brief Returns the 3x3 rotation matrix (cv::Mat).
|
||||
*/
|
||||
cv::Mat rotationMatrix() const;
|
||||
/**
|
||||
* @brief Returns the 3x1 translation matrix (cv::Mat).
|
||||
*/
|
||||
cv::Mat translationMatrix() const;
|
||||
|
||||
/**
|
||||
* @brief Extracts translation and Euler angles (radians).
|
||||
*/
|
||||
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
|
||||
/**
|
||||
* @brief Extracts Euler angles (roll, pitch, yaw).
|
||||
*/
|
||||
void getEulerAngles(float & roll, float & pitch, float & yaw) const;
|
||||
/**
|
||||
* @brief Extracts translation only.
|
||||
*/
|
||||
void getTranslation(float & x, float & y, float & z) const;
|
||||
/**
|
||||
* @brief Returns angular difference (in radians) with another transform.
|
||||
*/
|
||||
float getAngle(const Transform & t) const;
|
||||
/**
|
||||
* @brief Returns the Euclidean norm of the translation vector.
|
||||
*/
|
||||
float getNorm() const;
|
||||
/**
|
||||
* @brief Returns the squared norm of the translation vector.
|
||||
*/
|
||||
float getNormSquared() const;
|
||||
/**
|
||||
* @brief Returns the Euclidean distance to another transform.
|
||||
*/
|
||||
float getDistance(const Transform & t) const;
|
||||
/**
|
||||
* @brief Returns the squared distance to another transform.
|
||||
*/
|
||||
float getDistanceSquared(const Transform & t) const;
|
||||
/**
|
||||
* @brief Interpolates between this and another transform.
|
||||
* @param t Interpolation factor [0, 1].
|
||||
* @param other Target transform.
|
||||
*/
|
||||
Transform interpolate(float t, const Transform & other) const;
|
||||
/**
|
||||
* @brief Normalizes the rotation matrix.
|
||||
*/
|
||||
void normalizeRotation();
|
||||
/**
|
||||
* @brief Returns a string representation of the transform.
|
||||
*/
|
||||
std::string prettyPrint() const;
|
||||
|
||||
/// Operator overloads
|
||||
Transform operator*(const Transform & t) const;
|
||||
Transform & operator*=(const Transform & t);
|
||||
bool operator==(const Transform & t) const;
|
||||
bool operator!=(const Transform & t) const;
|
||||
|
||||
// --- Eigen conversions ---
|
||||
Eigen::Matrix4f toEigen4f() const;
|
||||
Eigen::Matrix4d toEigen4d() const;
|
||||
Eigen::Affine3f toEigen3f() const;
|
||||
@@ -136,7 +263,14 @@ public:
|
||||
Eigen::Quaterniond getQuaterniond() const;
|
||||
|
||||
public:
|
||||
// --- Static helpers ---
|
||||
|
||||
/**
|
||||
* @brief Returns identity transform.
|
||||
*/
|
||||
static Transform getIdentity();
|
||||
|
||||
/// Converts from Eigen representations
|
||||
static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
|
||||
static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
|
||||
static Transform fromEigen3f(const Eigen::Affine3f & matrix);
|
||||
@@ -146,40 +280,75 @@ public:
|
||||
static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
|
||||
static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);
|
||||
|
||||
/**
|
||||
* @brief Returns the transform from RTAB-Map to OpenGL coordinate system.
|
||||
* @note Coordinate systems:
|
||||
* - OpenGL: x → right, y → up, z → out of the screen (toward viewer).
|
||||
* - RTAB-Map: x → forward, y → left, z → up.
|
||||
*/
|
||||
static Transform opengl_T_rtabmap() {return Transform(
|
||||
0.0f, -1.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 1.0f, 0.0f,
|
||||
-1.0f, 0.0f, 0.0f, 0.0f);}
|
||||
/**
|
||||
* @brief Returns the transform from OpenGL to RTAB-Map coordinate system.
|
||||
* @note Coordinate systems:
|
||||
* - OpenGL: x → right, y → up, z → out of the screen (toward viewer).
|
||||
* - RTAB-Map: x → forward, y → left, z → up.
|
||||
*/
|
||||
static Transform rtabmap_T_opengl() {return Transform(
|
||||
0.0f, 0.0f,-1.0f, 0.0f,
|
||||
-1.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 1.0f, 0.0f, 0.0f);}
|
||||
|
||||
/**
|
||||
* Format (3 values): x y z
|
||||
* Format (6 values): x y z roll pitch yaw
|
||||
* Format (7 values): x y z qx qy qz qw
|
||||
* Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
|
||||
* Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
|
||||
*/
|
||||
* @brief Parses a transform from a string representation.
|
||||
* Supported formats:
|
||||
* - (3 values) "x y z"
|
||||
* - (6 values) "x y z roll pitch yaw" (ZYX-Euler convention)
|
||||
* - (7 values) "x y z qx qy qz qw"
|
||||
* - (9 values, 3x3 rotation) "r11 r12 r13 r21 r22 r23 r31 r32 r33"
|
||||
* - (12 values, 3x4 rotation) "r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz"
|
||||
*/
|
||||
static Transform fromString(const std::string & string);
|
||||
/**
|
||||
* @brief Checks if a string can be parsed into a transform.
|
||||
*/
|
||||
static bool canParseString(const std::string & string);
|
||||
|
||||
/**
|
||||
* @brief Retrieves the transform to a given timestamp.
|
||||
* @param tfBuffer Buffer of timestamped transforms.
|
||||
* @param stamp Requested timestamp.
|
||||
* @return transform at the requested timestamp, interpolated if it doesn't fall on a transform
|
||||
* with the exact same timestamp in the buffer. If the timestamp is older
|
||||
* than the oldest timestamp or newer than the latest timestamp in the buffer, a
|
||||
* null transform is returned and a warning message is generated.
|
||||
*/
|
||||
static Transform getTransform(
|
||||
const std::map<double, Transform> & tfBuffer,
|
||||
const double & stamp);
|
||||
// Use Transform::getTransform() instead to get always accurate transforms.
|
||||
/**
|
||||
* @deprecated Use getTransform() instead.
|
||||
*/
|
||||
RTABMAP_DEPRECATED static Transform getClosestTransform(
|
||||
const std::map<double, Transform> & tfBuffer,
|
||||
const double & stamp,
|
||||
double * stampDiff);
|
||||
|
||||
private:
|
||||
cv::Mat data_;
|
||||
cv::Mat data_; ///< 3x4 float matrix (rotation + translation)
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Output stream operator for Transform.
|
||||
*/
|
||||
RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const Transform& s);
|
||||
|
||||
/**
|
||||
* @class TransformStamped
|
||||
* @brief Associates a transform with a timestamp.
|
||||
*/
|
||||
class TransformStamped
|
||||
{
|
||||
public:
|
||||
|
||||
@@ -35,32 +35,95 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
/**
|
||||
* @class VisualWord
|
||||
* @brief Represents a visual word (feature descriptor) used in the bag-of-words (BoW) model.
|
||||
*
|
||||
* A VisualWord holds a unique identifier, its descriptor (usually a feature vector from an image),
|
||||
* and a record of where (in which signatures/images) it was observed. This is a key component in
|
||||
* loop closure detection and visual place recognition in RTAB-Map.
|
||||
*
|
||||
* The word keeps track of references (signature IDs) where it appears, and how many times it was observed
|
||||
* in each signature. It also tracks whether the word has been saved to a database.
|
||||
*/
|
||||
class RTABMAP_CORE_EXPORT VisualWord
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor.
|
||||
* @param id Unique ID of the visual word.
|
||||
* @param descriptor The descriptor associated with the word (e.g., ORB, SIFT).
|
||||
* @param signatureId Optional signature ID where the word is first observed.
|
||||
*/
|
||||
VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);
|
||||
|
||||
/**
|
||||
* @brief Destructor.
|
||||
*/
|
||||
~VisualWord();
|
||||
|
||||
/**
|
||||
* @brief Adds a reference to this word for the given signature ID.
|
||||
* @param signatureId ID of the signature in which this word is observed.
|
||||
*/
|
||||
void addRef(int signatureId);
|
||||
|
||||
/**
|
||||
* @brief Removes all references to this word for the given signature ID.
|
||||
* @param signatureId ID of the signature from which references will be removed.
|
||||
* @return Number of references removed.
|
||||
*/
|
||||
int removeAllRef(int signatureId);
|
||||
|
||||
/**
|
||||
* @brief Estimates the memory used by this visual word in bytes.
|
||||
* @return Size in bytes.
|
||||
*/
|
||||
unsigned long getMemoryUsed() const;
|
||||
|
||||
/**
|
||||
* @brief Returns the total number of references (across all signatures).
|
||||
* @return Number of total references.
|
||||
*/
|
||||
int getTotalReferences() const {return _totalReferences;}
|
||||
int id() const {return _id;}
|
||||
const cv::Mat & getDescriptor() const {return _descriptor;}
|
||||
const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
|
||||
|
||||
/**
|
||||
* @brief Returns the ID of this visual word.
|
||||
* @return Word ID.
|
||||
*/
|
||||
int id() const {return _id;}
|
||||
|
||||
/**
|
||||
* @brief Returns the feature descriptor of this word.
|
||||
* @return The OpenCV matrix descriptor.
|
||||
*/
|
||||
const cv::Mat & getDescriptor() const {return _descriptor;}
|
||||
|
||||
/**
|
||||
* @brief Returns the references of this word.
|
||||
* @return A map of (signature ID, occurrence count).
|
||||
*/
|
||||
const std::map<int, int> & getReferences() const {return _references;}
|
||||
|
||||
/**
|
||||
* @brief Checks if the word has been saved to the database.
|
||||
* @return True if saved, false otherwise.
|
||||
*/
|
||||
bool isSaved() const {return _saved;}
|
||||
|
||||
/**
|
||||
* @brief Sets the saved flag for the word.
|
||||
* @param saved True if the word has been saved to the database.
|
||||
*/
|
||||
void setSaved(bool saved) {_saved = saved;}
|
||||
|
||||
private:
|
||||
int _id;
|
||||
cv::Mat _descriptor;
|
||||
bool _saved; // If it's saved to db
|
||||
int _id; ///< Unique ID of the visual word.
|
||||
cv::Mat _descriptor; ///< Feature descriptor (e.g., ORB, SIFT).
|
||||
bool _saved; ///< Whether the word is saved to the database.
|
||||
|
||||
int _totalReferences;
|
||||
std::map<int, int> _references; // (signature id , occurrence in the signature)
|
||||
std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
|
||||
int _totalReferences; ///< Total reference count across all signatures.
|
||||
std::map<int, int> _references; ///< Active references: map of (signature ID, occurrence count).
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
+15
-15
@@ -530,35 +530,35 @@ Transform Transform::getTransform(
|
||||
const double & stamp)
|
||||
{
|
||||
UASSERT(!tfBuffer.empty());
|
||||
std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
|
||||
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
|
||||
if(imuIterA != tfBuffer.begin())
|
||||
std::map<double, Transform>::const_iterator iterB = tfBuffer.lower_bound(stamp);
|
||||
std::map<double, Transform>::const_iterator iterA = iterB;
|
||||
if(iterA != tfBuffer.begin())
|
||||
{
|
||||
imuIterA = --imuIterA;
|
||||
iterA = --iterA;
|
||||
}
|
||||
if(imuIterB == tfBuffer.end())
|
||||
if(iterB == tfBuffer.end())
|
||||
{
|
||||
imuIterB = --imuIterB;
|
||||
iterB = --iterB;
|
||||
}
|
||||
Transform imuT;
|
||||
if(imuIterB->first == stamp)
|
||||
Transform t;
|
||||
if(iterB->first == stamp)
|
||||
{
|
||||
imuT = imuIterB->second;
|
||||
t = iterB->second;
|
||||
}
|
||||
else if(imuIterA != imuIterB)
|
||||
else if(iterA != iterB)
|
||||
{
|
||||
//interpolate:
|
||||
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
|
||||
t = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
|
||||
}
|
||||
else if(stamp > imuIterB->first)
|
||||
else if(stamp > iterB->first)
|
||||
{
|
||||
UWARN("No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
|
||||
UWARN("No transform found for stamp %f! Latest is %f", stamp, iterB->first);
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
|
||||
UWARN("No transform found for stamp %f! Earliest is %f", stamp, iterA->first);
|
||||
}
|
||||
return imuT;
|
||||
return t;
|
||||
}
|
||||
|
||||
Transform Transform::getClosestTransform(
|
||||
|
||||
@@ -73,7 +73,6 @@ unsigned long VisualWord::getMemoryUsed() const
|
||||
{
|
||||
unsigned long memoryUsage = sizeof(VisualWord);
|
||||
memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
|
||||
memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
|
||||
memoryUsage += _descriptor.total() * _descriptor.elemSize();
|
||||
return memoryUsage;
|
||||
}
|
||||
|
||||
@@ -50,3 +50,13 @@ gtest_discover_tests(test_util3d_motion_estimation)
|
||||
add_executable(test_util3d_surface test_util3d_surface.cpp)
|
||||
target_link_libraries(test_util3d_surface gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_util3d_surface)
|
||||
|
||||
#VisualWord.h
|
||||
add_executable(VisualWordTests VisualWordTests.cpp)
|
||||
target_link_libraries(VisualWordTests gtest_main rtabmap_core)
|
||||
gtest_discover_tests(VisualWordTests)
|
||||
|
||||
#Transform.h
|
||||
add_executable(TransformTests TransformTests.cpp)
|
||||
target_link_libraries(TransformTests gtest_main rtabmap_core)
|
||||
gtest_discover_tests(TransformTests)
|
||||
@@ -0,0 +1,220 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <cmath>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// Helper for floating point comparison
|
||||
static bool approxEqual(float a, float b, float epsilon = 1e-5f)
|
||||
{
|
||||
return std::fabs(a - b) < epsilon;
|
||||
}
|
||||
|
||||
// ---------- TEST CASES ----------
|
||||
|
||||
TEST(TransformTests, DefaultConstructor)
|
||||
{
|
||||
Transform t;
|
||||
EXPECT_TRUE(t.isNull());
|
||||
EXPECT_FALSE(t.isIdentity());
|
||||
}
|
||||
|
||||
TEST(TransformTests, Identity)
|
||||
{
|
||||
Transform t = Transform::getIdentity();
|
||||
EXPECT_TRUE(t.isIdentity());
|
||||
EXPECT_FALSE(t.isNull());
|
||||
|
||||
// Identity * Identity = Identity
|
||||
Transform result = t * Transform::getIdentity();
|
||||
EXPECT_TRUE(result.isIdentity());
|
||||
}
|
||||
|
||||
TEST(TransformTests, EqualityOperators)
|
||||
{
|
||||
Transform t1 = Transform::getIdentity();
|
||||
Transform t2 = Transform::getIdentity();
|
||||
|
||||
EXPECT_TRUE(t1 == t2);
|
||||
EXPECT_FALSE(t1 != t2);
|
||||
|
||||
t2.x() = 1.0f;
|
||||
EXPECT_FALSE(t1 == t2);
|
||||
EXPECT_TRUE(t1 != t2);
|
||||
}
|
||||
|
||||
TEST(TransformTests, TranslationConstructor)
|
||||
{
|
||||
Transform t(1.0f, 2.0f, 3.0f, 0, 0, 0);
|
||||
EXPECT_TRUE(approxEqual(t.x(), 1.0f));
|
||||
EXPECT_TRUE(approxEqual(t.y(), 2.0f));
|
||||
EXPECT_TRUE(approxEqual(t.z(), 3.0f));
|
||||
}
|
||||
|
||||
TEST(TransformTests, MatrixConstructor)
|
||||
{
|
||||
cv::Mat mat = (cv::Mat_<float>(3, 4) <<
|
||||
1, 0, 0, 1,
|
||||
0, 1, 0, 2,
|
||||
0, 0, 1, 3);
|
||||
Transform t(mat);
|
||||
EXPECT_TRUE(approxEqual(t.x(), 1.0f));
|
||||
EXPECT_TRUE(approxEqual(t.y(), 2.0f));
|
||||
EXPECT_TRUE(approxEqual(t.z(), 3.0f));
|
||||
}
|
||||
|
||||
TEST(TransformTests, Inverse)
|
||||
{
|
||||
Transform t(1, 0, 0, 1,
|
||||
0, 1, 0, 2,
|
||||
0, 0, 1, 3);
|
||||
Transform inv = t.inverse();
|
||||
Transform id = t * inv;
|
||||
EXPECT_TRUE(id.isIdentity());
|
||||
}
|
||||
|
||||
TEST(TransformTests, Interpolation)
|
||||
{
|
||||
Transform t1(0, 0, 0, 0, 0, 0);
|
||||
Transform t2(0, 0, 1, 0, 0, 0);
|
||||
Transform mid = t1.interpolate(0.5f, t2);
|
||||
EXPECT_TRUE(approxEqual(mid.z(), 0.5f));
|
||||
}
|
||||
|
||||
TEST(TransformTests, NormAndDistance)
|
||||
{
|
||||
Transform t1(0, 0, 0, 0, 0, 0);
|
||||
Transform t2(1, 2, 2, 0, 0, 0);
|
||||
|
||||
EXPECT_TRUE(approxEqual(t2.getNorm(), 3.0f));
|
||||
EXPECT_TRUE(approxEqual(t1.getDistance(t2), 3.0f));
|
||||
EXPECT_TRUE(approxEqual(t1.getDistanceSquared(t2), 9.0f));
|
||||
}
|
||||
|
||||
TEST(TransformTests, EulerAndQuaternionConversion)
|
||||
{
|
||||
Transform t(1, 2, 3, 0.1f, 0.2f, 0.3f);
|
||||
float roll, pitch, yaw;
|
||||
t.getEulerAngles(roll, pitch, yaw);
|
||||
EXPECT_TRUE(std::isfinite(roll));
|
||||
EXPECT_TRUE(std::isfinite(pitch));
|
||||
EXPECT_TRUE(std::isfinite(yaw));
|
||||
|
||||
Eigen::Quaternionf q = t.getQuaternionf();
|
||||
EXPECT_TRUE(std::isfinite(q.x()));
|
||||
EXPECT_TRUE(std::isfinite(q.y()));
|
||||
EXPECT_TRUE(std::isfinite(q.z()));
|
||||
EXPECT_TRUE(std::isfinite(q.w()));
|
||||
}
|
||||
|
||||
TEST(TransformTests, StringConversion)
|
||||
{
|
||||
Transform original(1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
|
||||
Transform parsed = Transform::fromString("1.0 2.0 3.0 0.1 0.2 0.3");
|
||||
EXPECT_TRUE(original.getDistance(parsed) < 1e-4f);
|
||||
}
|
||||
|
||||
TEST(TransformTests, DoFConversion)
|
||||
{
|
||||
// 6DoF transform
|
||||
Transform t6(1, 2, 3, 0.1f, 0.2f, 0.3f);
|
||||
|
||||
Transform t3dof = t6.to3DoF();
|
||||
EXPECT_TRUE(t3dof.is3DoF());
|
||||
EXPECT_TRUE(t3dof.is4DoF());
|
||||
EXPECT_TRUE(approxEqual(t3dof.z(), 0.0f)); // Z is removed
|
||||
EXPECT_TRUE(approxEqual(t3dof.r31(), 0.0f));
|
||||
EXPECT_TRUE(approxEqual(t3dof.r32(), 0.0f));
|
||||
EXPECT_TRUE(approxEqual(t3dof.r33(), 1.0f));
|
||||
|
||||
Transform t4dof = t6.to4DoF();
|
||||
EXPECT_TRUE(t4dof.is4DoF());
|
||||
EXPECT_FALSE(t4dof.is3DoF());
|
||||
EXPECT_TRUE(approxEqual(t4dof.r31(), 0.0f));
|
||||
EXPECT_TRUE(approxEqual(t4dof.r32(), 0.0f));
|
||||
EXPECT_TRUE(approxEqual(t4dof.r33(), 1.0f));
|
||||
}
|
||||
|
||||
TEST(TransformTests, OpenGLToRTABMap)
|
||||
{
|
||||
// Multiplying should return identity
|
||||
Transform result = Transform::opengl_T_rtabmap() * Transform::rtabmap_T_opengl();
|
||||
EXPECT_TRUE(result.isIdentity());
|
||||
|
||||
// Test conversion from opengl world to rtabmap world
|
||||
EXPECT_NEAR((Transform::rtabmap_T_opengl() * Transform(1,0,0,0,0,0)).y(), -1, 1e-5);
|
||||
EXPECT_NEAR((Transform::rtabmap_T_opengl() * Transform(0,1,0,0,0,0)).z(), 1, 1e-5);
|
||||
EXPECT_NEAR((Transform::rtabmap_T_opengl() * Transform(0,0,1,0,0,0)).x(), -1, 1e-5);
|
||||
|
||||
// Test conversion from rtabmap world to opengl world
|
||||
EXPECT_NEAR((Transform::opengl_T_rtabmap() * Transform(1,0,0,0,0,0)).z(), -1, 1e-5);
|
||||
EXPECT_NEAR((Transform::opengl_T_rtabmap() * Transform(0,1,0,0,0,0)).x(), -1, 1e-5);
|
||||
EXPECT_NEAR((Transform::opengl_T_rtabmap() * Transform(0,0,1,0,0,0)).y(), 1, 1e-5);
|
||||
}
|
||||
|
||||
TEST(TransformTests, EigenConversions)
|
||||
{
|
||||
Transform t(1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
|
||||
|
||||
// Eigen 4x4 float
|
||||
Eigen::Matrix4f e4f = t.toEigen4f();
|
||||
Transform fromE4f = Transform::fromEigen4f(e4f);
|
||||
EXPECT_LT(t.getDistance(fromE4f), 1e-4f);
|
||||
|
||||
// Eigen 4x4 double
|
||||
Eigen::Matrix4d e4d = t.toEigen4d();
|
||||
Transform fromE4d = Transform::fromEigen4d(e4d);
|
||||
EXPECT_LT(t.getDistance(fromE4d), 1e-4f);
|
||||
|
||||
// Eigen Affine3f
|
||||
Eigen::Affine3f aff3f = t.toEigen3f();
|
||||
Transform fromAff3f = Transform::fromEigen3f(aff3f);
|
||||
EXPECT_LT(t.getDistance(fromAff3f), 1e-4f);
|
||||
|
||||
// Eigen Affine3d
|
||||
Eigen::Affine3d aff3d = t.toEigen3d();
|
||||
Transform fromAff3d = Transform::fromEigen3d(aff3d);
|
||||
EXPECT_LT(t.getDistance(fromAff3d), 1e-4f);
|
||||
|
||||
// Quaternion check
|
||||
Eigen::Quaternionf qf = t.getQuaternionf();
|
||||
EXPECT_NEAR(qf.norm(), 1.0f, 1e-5f);
|
||||
|
||||
Eigen::Quaterniond qd = t.getQuaterniond();
|
||||
EXPECT_NEAR(qd.norm(), 1.0, 1e-5);
|
||||
}
|
||||
|
||||
TEST(TransformTests, GetTransformFromBuffer)
|
||||
{
|
||||
std::map<double, Transform> tfBuffer;
|
||||
|
||||
tfBuffer[1.0] = Transform(0, 0, 0, 0, 0, 0); // t=1.0
|
||||
tfBuffer[2.0] = Transform(1, 0, 0, 0, 0, M_PI/2.0); // t=2.0
|
||||
tfBuffer[3.0] = Transform(2, 0, 0, 0, 0, M_PI); // t=3.0
|
||||
|
||||
// Test exact timestamp
|
||||
Transform tExact = Transform::getTransform(tfBuffer, 2.0);
|
||||
EXPECT_NEAR(tExact.x(), 1.0f, 1e-5);
|
||||
|
||||
// Test below
|
||||
Transform tBelow = Transform::getTransform(tfBuffer, 1.5);
|
||||
EXPECT_NEAR(tBelow.x(), 0.5f, 1e-5);
|
||||
EXPECT_NEAR(tBelow.theta(), M_PI/4.0, 1e-5);
|
||||
|
||||
// Test above
|
||||
Transform tAbove = Transform::getTransform(tfBuffer, 2.5);
|
||||
EXPECT_NEAR(tAbove.x(), 1.5f, 1e-5);
|
||||
EXPECT_NEAR(tAbove.theta(), 3.0*M_PI/4.0, 1e-5);
|
||||
|
||||
// Test very close to a border
|
||||
Transform tNear = Transform::getTransform(tfBuffer, 2.0001);
|
||||
EXPECT_NEAR(tNear.x(), 1.0001f, 1e-4);
|
||||
|
||||
// Requesting transform in the past
|
||||
Transform tPast = Transform::getTransform(tfBuffer, 0.5);
|
||||
EXPECT_TRUE(tPast.isNull());
|
||||
|
||||
// Requesting transform in the future
|
||||
Transform tFuture = Transform::getTransform(tfBuffer, 3.5);
|
||||
EXPECT_TRUE(tFuture.isNull());
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include "rtabmap/core/VisualWord.h"
|
||||
#include <map>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
TEST(VisualWordTest, ConstructorAndAccessors)
|
||||
{
|
||||
cv::Mat descriptor = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
|
||||
int wordId = 123;
|
||||
int signatureId = 10;
|
||||
|
||||
VisualWord word(wordId, descriptor, signatureId);
|
||||
|
||||
EXPECT_EQ(word.id(), wordId);
|
||||
EXPECT_EQ(word.getTotalReferences(), 1);
|
||||
EXPECT_EQ(word.getDescriptor().cols, descriptor.cols);
|
||||
EXPECT_EQ(word.getDescriptor().rows, descriptor.rows);
|
||||
|
||||
auto refs = word.getReferences();
|
||||
ASSERT_EQ(refs.size(), 1);
|
||||
EXPECT_EQ(refs.at(signatureId), 1);
|
||||
}
|
||||
|
||||
TEST(VisualWordTest, AddMultipleReferences)
|
||||
{
|
||||
VisualWord word(1, cv::Mat::ones(1, 5, CV_32F));
|
||||
|
||||
word.addRef(100);
|
||||
word.addRef(100); // same signature
|
||||
word.addRef(200);
|
||||
|
||||
EXPECT_EQ(word.getTotalReferences(), 3);
|
||||
|
||||
auto refs = word.getReferences();
|
||||
EXPECT_EQ(refs.size(), 2);
|
||||
EXPECT_EQ(refs.at(100), 2);
|
||||
EXPECT_EQ(refs.at(200), 1);
|
||||
}
|
||||
|
||||
TEST(VisualWordTest, RemoveReferences)
|
||||
{
|
||||
VisualWord word(1, cv::Mat::zeros(1, 3, CV_8U));
|
||||
word.addRef(10);
|
||||
word.addRef(10);
|
||||
word.addRef(20);
|
||||
|
||||
EXPECT_EQ(word.getTotalReferences(), 3);
|
||||
|
||||
int removed = word.removeAllRef(10);
|
||||
EXPECT_EQ(removed, 2);
|
||||
EXPECT_EQ(word.getTotalReferences(), 1);
|
||||
|
||||
auto refs = word.getReferences();
|
||||
EXPECT_EQ(refs.size(), 1);
|
||||
EXPECT_TRUE(refs.find(10) == refs.end());
|
||||
EXPECT_EQ(refs.at(20), 1);
|
||||
}
|
||||
|
||||
TEST(VisualWordTest, SavedFlagBehavior)
|
||||
{
|
||||
VisualWord word(1, cv::Mat());
|
||||
EXPECT_FALSE(word.isSaved());
|
||||
|
||||
word.setSaved(true);
|
||||
EXPECT_TRUE(word.isSaved());
|
||||
|
||||
word.setSaved(false);
|
||||
EXPECT_FALSE(word.isSaved());
|
||||
}
|
||||
|
||||
TEST(VisualWordTest, MemoryUsedEstimation)
|
||||
{
|
||||
cv::Mat descriptor = cv::Mat::ones(1, 128, CV_32FC1); // simulate SIFT descriptor
|
||||
VisualWord word(42, descriptor);
|
||||
|
||||
unsigned long mem = word.getMemoryUsed();
|
||||
|
||||
// Should be at least size of descriptor
|
||||
EXPECT_GE(mem, 128ul*sizeof(float));
|
||||
}
|
||||
Reference in New Issue
Block a user