mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 09:07: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
|
||||
|
||||
Reference in New Issue
Block a user