mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 00:57:46 +08:00
Added doc/test for SensorEvent, added doc for SensorCaptureInfo
This commit is contained in:
matlabbe
@@ -33,10 +33,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
/**
|
||||
* @class SensorCaptureInfo
|
||||
* @brief Metadata structure for sensor data capture and processing
|
||||
*
|
||||
* SensorCaptureInfo contains metadata about sensor data capture, including timing
|
||||
* information for various processing steps, camera identification, and odometry
|
||||
* information. This information is typically attached to SensorEvent objects to
|
||||
* provide context about how the sensor data was captured and processed.
|
||||
*
|
||||
* The class tracks timing for various processing stages:
|
||||
* - **Capture**: Time to capture data from the sensor
|
||||
* - **Processing**: Time for various image processing steps (deskewing, disparity,
|
||||
* mirroring, exposure compensation, decimation, histogram equalization, etc.)
|
||||
* - **Depth processing**: Time for depth-related operations (scan from depth,
|
||||
* undistort depth, bilateral filtering)
|
||||
* - **Total**: Total processing time
|
||||
*
|
||||
* Odometry information includes:
|
||||
* - **Pose**: The odometry pose at the time of capture
|
||||
* - **Covariance**: Uncertainty of the odometry twist (6x6 matrix)
|
||||
* - **Velocity**: 6DOF velocity vector [vx, vy, vz, vroll, vpitch, vyaw]
|
||||
*
|
||||
* @note All timing values are in seconds (float).
|
||||
* @note The odometry covariance matrix is initialized to identity by default.
|
||||
*
|
||||
* @see SensorEvent
|
||||
* @see SensorData
|
||||
*/
|
||||
class SensorCaptureInfo
|
||||
{
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Default constructor
|
||||
*
|
||||
* Initializes all fields to default values:
|
||||
* - cameraName: empty string
|
||||
* - id: 0
|
||||
* - stamp: 0.0
|
||||
* - All timing fields: 0.0f
|
||||
* - odomPose: null transform
|
||||
* - odomCovariance: 6x6 identity matrix
|
||||
* - odomVelocity: empty vector
|
||||
*/
|
||||
SensorCaptureInfo() :
|
||||
cameraName(""),
|
||||
id(0),
|
||||
@@ -55,28 +95,41 @@ public:
|
||||
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor
|
||||
*/
|
||||
virtual ~SensorCaptureInfo() {}
|
||||
|
||||
std::string cameraName;
|
||||
int id;
|
||||
double stamp;
|
||||
float timeCapture;
|
||||
float timeDeskewing;
|
||||
float timeDisparity;
|
||||
float timeMirroring;
|
||||
float timeStereoExposureCompensation;
|
||||
float timeImageDecimation;
|
||||
float timeHistogramEqualization;
|
||||
float timeScanFromDepth;
|
||||
float timeUndistortDepth;
|
||||
float timeBilateralFiltering;
|
||||
float timeTotal;
|
||||
Transform odomPose;
|
||||
cv::Mat odomCovariance;
|
||||
std::vector<float> odomVelocity;
|
||||
std::string cameraName; ///< Camera/sensor name identifier
|
||||
int id; ///< Capture ID (typically matches SensorData ID)
|
||||
double stamp; ///< Timestamp in seconds (typically matches SensorData stamp)
|
||||
|
||||
// Timing information (all in seconds)
|
||||
float timeCapture; ///< Time to capture data from the sensor (seconds)
|
||||
float timeDeskewing; ///< Time for laser scan deskewing (seconds)
|
||||
float timeDisparity; ///< Time to compute stereo disparity (seconds)
|
||||
float timeMirroring; ///< Time for image mirroring/flipping (seconds)
|
||||
float timeStereoExposureCompensation; ///< Time for stereo exposure compensation (seconds)
|
||||
float timeImageDecimation; ///< Time for image decimation/downsampling (seconds)
|
||||
float timeHistogramEqualization; ///< Time for histogram equalization (seconds)
|
||||
float timeScanFromDepth; ///< Time to convert depth image to laser scan (seconds)
|
||||
float timeUndistortDepth; ///< Time to undistort depth image (seconds)
|
||||
float timeBilateralFiltering; ///< Time for bilateral filtering of depth (seconds)
|
||||
float timeTotal; ///< Total processing time (seconds)
|
||||
|
||||
// Odometry information
|
||||
Transform odomPose; ///< Odometry pose at the time of capture (in odometry coordinate frame)
|
||||
cv::Mat odomCovariance; ///< Odometry twist covariance matrix (6x6, CV_64FC1). Default: identity matrix
|
||||
std::vector<float> odomVelocity; ///< 6DOF odometry velocity [vx, vy, vz, vroll, vpitch, vyaw] (m/s, rad/s)
|
||||
};
|
||||
|
||||
//backward compatibility
|
||||
/**
|
||||
* @deprecated Use SensorCaptureInfo instead
|
||||
* @brief Backward compatibility typedef
|
||||
*
|
||||
* CameraInfo is deprecated. Use SensorCaptureInfo instead.
|
||||
*/
|
||||
RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -34,16 +34,57 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
/**
|
||||
* @class SensorEvent
|
||||
* @brief Event class for sensor data communication in RTAB-Map's event system
|
||||
*
|
||||
* SensorEvent is an event class that extends UEvent and is used to communicate
|
||||
* sensor data (images, depth, laser scans, etc.) through RTAB-Map's event-driven
|
||||
* architecture. It is typically posted by sensor capture threads and handled by
|
||||
* event handlers (e.g., RtabmapThread, DataRecorder, CalibrationDialog).
|
||||
*
|
||||
* The class supports two event codes:
|
||||
* - **kCodeData**: Indicates that new sensor data is available
|
||||
* - **kCodeNoMoreImages**: Indicates that the sensor stream has ended (end of file, camera disconnected, etc.)
|
||||
*
|
||||
* SensorEvent carries:
|
||||
* - **SensorData**: The actual sensor data (images, depth, scans, camera models, etc.)
|
||||
* - **SensorCaptureInfo**: Metadata about the capture (camera name, timing information, odometry pose, etc.)
|
||||
*
|
||||
* @note This class is part of RTAB-Map's event system. Handlers should check the event code
|
||||
* and class name before processing the event.
|
||||
*
|
||||
* @see UEvent
|
||||
* @see SensorData
|
||||
* @see SensorCaptureInfo
|
||||
* @see UEventsHandler
|
||||
* @see UEventsManager
|
||||
*/
|
||||
class SensorEvent :
|
||||
public UEvent
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @enum Code
|
||||
* @brief Event codes for SensorEvent
|
||||
*/
|
||||
enum Code {
|
||||
kCodeData,
|
||||
kCodeNoMoreImages
|
||||
kCodeData, ///< Event code indicating new sensor data is available
|
||||
kCodeNoMoreImages ///< Event code indicating the sensor stream has ended
|
||||
};
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor from a single image
|
||||
*
|
||||
* Creates a SensorEvent with kCodeData containing a single image.
|
||||
* This is a convenience constructor for simple image-only sensor data.
|
||||
*
|
||||
* @param image The image to include in the sensor data
|
||||
* @param seq Sequence number (default: 0)
|
||||
* @param stamp Timestamp in seconds (default: 0.0)
|
||||
* @param cameraName Optional camera name identifier (default: empty)
|
||||
*/
|
||||
SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
|
||||
UEvent(kCodeData),
|
||||
data_(image, seq, stamp)
|
||||
@@ -51,23 +92,57 @@ public:
|
||||
sensorCaptureInfo_.cameraName = cameraName;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor for end-of-stream event
|
||||
*
|
||||
* Creates a SensorEvent with kCodeNoMoreImages to signal that the sensor
|
||||
* stream has ended (e.g., end of file, camera disconnected).
|
||||
*
|
||||
* @note This constructor does not include any sensor data.
|
||||
*/
|
||||
SensorEvent() :
|
||||
UEvent(kCodeNoMoreImages)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor from SensorData
|
||||
*
|
||||
* Creates a SensorEvent with kCodeData containing the provided SensorData.
|
||||
*
|
||||
* @param data The sensor data to include in the event
|
||||
*/
|
||||
SensorEvent(const SensorData & data) :
|
||||
UEvent(kCodeData),
|
||||
data_(data)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor from SensorData with camera name
|
||||
*
|
||||
* Creates a SensorEvent with kCodeData containing the provided SensorData
|
||||
* and camera name.
|
||||
*
|
||||
* @param data The sensor data to include in the event
|
||||
* @param cameraName Camera name identifier
|
||||
*/
|
||||
SensorEvent(const SensorData & data, const std::string & cameraName) :
|
||||
UEvent(kCodeData),
|
||||
data_(data)
|
||||
{
|
||||
sensorCaptureInfo_.cameraName = cameraName;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor from SensorData with full capture info
|
||||
*
|
||||
* Creates a SensorEvent with kCodeData containing the provided SensorData
|
||||
* and complete sensor capture information (camera name, timing, odometry, etc.).
|
||||
*
|
||||
* @param data The sensor data to include in the event
|
||||
* @param sensorCaptureInfo Complete sensor capture information
|
||||
*/
|
||||
SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) :
|
||||
UEvent(kCodeData),
|
||||
data_(data),
|
||||
@@ -75,20 +150,60 @@ public:
|
||||
{
|
||||
}
|
||||
|
||||
// Image or descriptors
|
||||
/**
|
||||
* @brief Returns the sensor data contained in this event
|
||||
*
|
||||
* Returns the SensorData object containing images, depth, laser scans,
|
||||
* camera models, and other sensor information.
|
||||
*
|
||||
* @return Const reference to the sensor data
|
||||
*/
|
||||
const SensorData & data() const {return data_;}
|
||||
|
||||
/**
|
||||
* @brief Returns the camera name
|
||||
*
|
||||
* Returns the camera name identifier from the sensor capture info.
|
||||
*
|
||||
* @return Const reference to the camera name string
|
||||
*/
|
||||
const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;}
|
||||
|
||||
/**
|
||||
* @brief Returns the sensor capture information
|
||||
*
|
||||
* Returns the complete sensor capture information including camera name,
|
||||
* timing information, odometry pose, covariance, and other metadata.
|
||||
*
|
||||
* @return Const reference to the sensor capture info
|
||||
*/
|
||||
const SensorCaptureInfo & info() const {return sensorCaptureInfo_;}
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor
|
||||
*/
|
||||
virtual ~SensorEvent() {}
|
||||
|
||||
/**
|
||||
* @brief Returns the class name for event identification
|
||||
*
|
||||
* Returns "SensorEvent" to allow event handlers to identify this event type.
|
||||
*
|
||||
* @return Class name string "SensorEvent"
|
||||
*/
|
||||
virtual std::string getClassName() const {return std::string("SensorEvent");}
|
||||
|
||||
private:
|
||||
SensorData data_;
|
||||
SensorCaptureInfo sensorCaptureInfo_;
|
||||
SensorData data_; ///< Sensor data (images, depth, scans, camera models, etc.)
|
||||
SensorCaptureInfo sensorCaptureInfo_; ///< Sensor capture metadata (camera name, timing, odometry, etc.)
|
||||
};
|
||||
|
||||
//backward compatibility
|
||||
/**
|
||||
* @deprecated Use SensorEvent instead
|
||||
* @brief Backward compatibility typedef
|
||||
*
|
||||
* CameraEvent is deprecated. Use SensorEvent instead.
|
||||
*/
|
||||
RTABMAP_DEPRECATED typedef SensorEvent CameraEvent;
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -94,4 +94,9 @@ gtest_discover_tests(test_statistics)
|
||||
#Signature.h
|
||||
add_executable(test_signature test_signature.cpp)
|
||||
target_link_libraries(test_signature gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_signature)
|
||||
gtest_discover_tests(test_signature)
|
||||
|
||||
#SensorEvent.h
|
||||
add_executable(test_sensorevent test_sensorevent.cpp)
|
||||
target_link_libraries(test_sensorevent gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_sensorevent)
|
||||
|
||||
@@ -0,0 +1,323 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/SensorEvent.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/SensorCaptureInfo.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// Constructor Tests
|
||||
|
||||
TEST(SensorEventTest, ConstructorFromImage)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(480, 640, CV_8UC3);
|
||||
int seq = 42;
|
||||
double stamp = 12345.678;
|
||||
std::string cameraName = "camera1";
|
||||
|
||||
SensorEvent event(image, seq, stamp, cameraName);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(event.getClassName(), "SensorEvent");
|
||||
EXPECT_EQ(event.cameraName(), cameraName);
|
||||
EXPECT_EQ(event.data().id(), seq);
|
||||
EXPECT_DOUBLE_EQ(event.data().stamp(), stamp);
|
||||
EXPECT_FALSE(event.data().imageRaw().empty());
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, ConstructorFromImageDefaults)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
|
||||
SensorEvent event(image);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(event.data().id(), 0);
|
||||
EXPECT_DOUBLE_EQ(event.data().stamp(), 0.0);
|
||||
EXPECT_TRUE(event.cameraName().empty());
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, ConstructorEndOfStream)
|
||||
{
|
||||
SensorEvent event;
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeNoMoreImages);
|
||||
EXPECT_EQ(event.getClassName(), "SensorEvent");
|
||||
// End-of-stream event may have empty or default sensor data
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, ConstructorFromSensorData)
|
||||
{
|
||||
SensorData data;
|
||||
data.setId(100);
|
||||
data.setStamp(54321.0);
|
||||
cv::Mat image = cv::Mat::ones(240, 320, CV_8UC3) * 128;
|
||||
data.setRGBDImage(image, cv::Mat(), CameraModel());
|
||||
|
||||
SensorEvent event(data);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(event.data().id(), 100);
|
||||
EXPECT_DOUBLE_EQ(event.data().stamp(), 54321.0);
|
||||
EXPECT_TRUE(event.cameraName().empty());
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, ConstructorFromSensorDataWithCameraName)
|
||||
{
|
||||
SensorData data;
|
||||
data.setId(200);
|
||||
std::string cameraName = "stereo_camera";
|
||||
|
||||
SensorEvent event(data, cameraName);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(event.data().id(), 200);
|
||||
EXPECT_EQ(event.cameraName(), cameraName);
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, ConstructorFromSensorDataWithCaptureInfo)
|
||||
{
|
||||
SensorData data;
|
||||
data.setId(300);
|
||||
data.setStamp(98765.432);
|
||||
|
||||
SensorCaptureInfo captureInfo;
|
||||
captureInfo.cameraName = "rgbd_camera";
|
||||
captureInfo.id = 300;
|
||||
captureInfo.stamp = 98765.432;
|
||||
captureInfo.timeCapture = 0.01f;
|
||||
captureInfo.timeTotal = 0.05f;
|
||||
captureInfo.odomPose = Transform(1.0f, 2.0f, 3.0f, 0, 0, 0);
|
||||
|
||||
SensorEvent event(data, captureInfo);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(event.data().id(), 300);
|
||||
EXPECT_EQ(event.cameraName(), "rgbd_camera");
|
||||
|
||||
const auto& info = event.info();
|
||||
EXPECT_EQ(info.id, 300);
|
||||
EXPECT_DOUBLE_EQ(info.stamp, 98765.432);
|
||||
EXPECT_FLOAT_EQ(info.timeCapture, 0.01f);
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 0.05f);
|
||||
EXPECT_FLOAT_EQ(info.odomPose.x(), 1.0f);
|
||||
}
|
||||
|
||||
// Event Code Tests
|
||||
|
||||
TEST(SensorEventTest, EventCodeData)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
SensorEvent event(image);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, EventCodeNoMoreImages)
|
||||
{
|
||||
SensorEvent event;
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeNoMoreImages);
|
||||
}
|
||||
|
||||
// Class Name Tests
|
||||
|
||||
TEST(SensorEventTest, GetClassName)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
SensorEvent event1(image);
|
||||
SensorEvent event2; // End of stream
|
||||
|
||||
EXPECT_EQ(event1.getClassName(), "SensorEvent");
|
||||
EXPECT_EQ(event2.getClassName(), "SensorEvent");
|
||||
}
|
||||
|
||||
// Data Access Tests
|
||||
|
||||
TEST(SensorEventTest, DataAccess)
|
||||
{
|
||||
SensorData data;
|
||||
data.setId(500);
|
||||
data.setStamp(11111.0);
|
||||
cv::Mat image = cv::Mat::ones(100, 100, CV_8UC3) * 255;
|
||||
data.setRGBDImage(image, cv::Mat(), CameraModel());
|
||||
|
||||
SensorEvent event(data);
|
||||
|
||||
const auto& retrievedData = event.data();
|
||||
EXPECT_EQ(retrievedData.id(), 500);
|
||||
EXPECT_DOUBLE_EQ(retrievedData.stamp(), 11111.0);
|
||||
EXPECT_FALSE(retrievedData.imageRaw().empty());
|
||||
EXPECT_EQ(retrievedData.imageRaw().rows, 100);
|
||||
EXPECT_EQ(retrievedData.imageRaw().cols, 100);
|
||||
}
|
||||
|
||||
// Camera Name Tests
|
||||
|
||||
TEST(SensorEventTest, CameraName)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
|
||||
// Without camera name
|
||||
SensorEvent event1(image);
|
||||
EXPECT_TRUE(event1.cameraName().empty());
|
||||
|
||||
// With camera name
|
||||
SensorEvent event2(image, 0, 0.0, "my_camera");
|
||||
EXPECT_EQ(event2.cameraName(), "my_camera");
|
||||
|
||||
// With SensorData and camera name
|
||||
SensorData data;
|
||||
SensorEvent event3(data, "another_camera");
|
||||
EXPECT_EQ(event3.cameraName(), "another_camera");
|
||||
}
|
||||
|
||||
// Sensor Capture Info Tests
|
||||
|
||||
TEST(SensorEventTest, SensorCaptureInfo)
|
||||
{
|
||||
SensorData data;
|
||||
data.setId(600);
|
||||
|
||||
SensorCaptureInfo captureInfo;
|
||||
captureInfo.cameraName = "test_camera";
|
||||
captureInfo.id = 600;
|
||||
captureInfo.stamp = 22222.0;
|
||||
captureInfo.timeCapture = 0.005f;
|
||||
captureInfo.timeDeskewing = 0.001f;
|
||||
captureInfo.timeDisparity = 0.010f;
|
||||
captureInfo.timeTotal = 0.020f;
|
||||
captureInfo.odomPose = Transform(10.0f, 20.0f, 30.0f, 0, 0, 0);
|
||||
captureInfo.odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.1;
|
||||
|
||||
SensorEvent event(data, captureInfo);
|
||||
|
||||
const auto& info = event.info();
|
||||
EXPECT_EQ(info.cameraName, "test_camera");
|
||||
EXPECT_EQ(info.id, 600);
|
||||
EXPECT_DOUBLE_EQ(info.stamp, 22222.0);
|
||||
EXPECT_FLOAT_EQ(info.timeCapture, 0.005f);
|
||||
EXPECT_FLOAT_EQ(info.timeDeskewing, 0.001f);
|
||||
EXPECT_FLOAT_EQ(info.timeDisparity, 0.010f);
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 0.020f);
|
||||
EXPECT_FLOAT_EQ(info.odomPose.x(), 10.0f);
|
||||
EXPECT_FLOAT_EQ(info.odomPose.y(), 20.0f);
|
||||
EXPECT_FLOAT_EQ(info.odomPose.z(), 30.0f);
|
||||
EXPECT_FALSE(info.odomCovariance.empty());
|
||||
EXPECT_EQ(info.odomCovariance.rows, 6);
|
||||
EXPECT_EQ(info.odomCovariance.cols, 6);
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, SensorCaptureInfoDefaultValues)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
SensorEvent event(image, 0, 0.0, "camera");
|
||||
|
||||
const auto& info = event.info();
|
||||
EXPECT_EQ(info.id, 0);
|
||||
EXPECT_DOUBLE_EQ(info.stamp, 0.0);
|
||||
EXPECT_FLOAT_EQ(info.timeCapture, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 0.0f);
|
||||
EXPECT_TRUE(info.odomPose.isNull());
|
||||
EXPECT_FALSE(info.odomCovariance.empty()); // Should be initialized to identity
|
||||
}
|
||||
|
||||
// Comprehensive Tests
|
||||
|
||||
TEST(SensorEventTest, ComprehensiveUsage)
|
||||
{
|
||||
// Create sensor data with multiple components
|
||||
SensorData data;
|
||||
data.setId(1000);
|
||||
data.setStamp(99999.999);
|
||||
|
||||
cv::Mat rgb = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
cv::Mat depth = cv::Mat::ones(480, 640, CV_16UC1) * 1000;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
data.setRGBDImage(rgb, depth, model);
|
||||
|
||||
// Create capture info with timing and odometry
|
||||
SensorCaptureInfo captureInfo;
|
||||
captureInfo.cameraName = "comprehensive_camera";
|
||||
captureInfo.id = 1000;
|
||||
captureInfo.stamp = 99999.999;
|
||||
captureInfo.timeCapture = 0.010f;
|
||||
captureInfo.timeDisparity = 0.020f;
|
||||
captureInfo.timeTotal = 0.050f;
|
||||
captureInfo.odomPose = Transform(1.5f, 2.5f, 3.5f, 0, 0, 0);
|
||||
|
||||
SensorEvent event(data, captureInfo);
|
||||
|
||||
// Verify event properties
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(event.getClassName(), "SensorEvent");
|
||||
|
||||
// Verify sensor data
|
||||
EXPECT_EQ(event.data().id(), 1000);
|
||||
EXPECT_DOUBLE_EQ(event.data().stamp(), 99999.999);
|
||||
EXPECT_FALSE(event.data().imageRaw().empty());
|
||||
EXPECT_FALSE(event.data().depthOrRightRaw().empty());
|
||||
|
||||
// Verify capture info
|
||||
EXPECT_EQ(event.cameraName(), "comprehensive_camera");
|
||||
const auto& info = event.info();
|
||||
EXPECT_FLOAT_EQ(info.timeCapture, 0.010f);
|
||||
EXPECT_FLOAT_EQ(info.timeDisparity, 0.020f);
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 0.050f);
|
||||
EXPECT_FLOAT_EQ(info.odomPose.x(), 1.5f);
|
||||
}
|
||||
|
||||
// Edge Cases
|
||||
|
||||
TEST(SensorEventTest, EmptyImage)
|
||||
{
|
||||
cv::Mat emptyImage;
|
||||
SensorEvent event(emptyImage);
|
||||
|
||||
EXPECT_EQ(event.getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_TRUE(event.data().imageRaw().empty());
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, DifferentImageTypes)
|
||||
{
|
||||
// Grayscale
|
||||
cv::Mat gray = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
SensorEvent event1(gray);
|
||||
EXPECT_FALSE(event1.data().imageRaw().empty());
|
||||
|
||||
// Color
|
||||
cv::Mat color = cv::Mat::zeros(100, 100, CV_8UC3);
|
||||
SensorEvent event2(color);
|
||||
EXPECT_FALSE(event2.data().imageRaw().empty());
|
||||
|
||||
// Float
|
||||
cv::Mat floatImg = cv::Mat::zeros(100, 100, CV_32FC1);
|
||||
SensorEvent event;
|
||||
EXPECT_THROW(event = SensorEvent(floatImg), UException);
|
||||
}
|
||||
|
||||
// UEvent Inheritance Tests
|
||||
|
||||
TEST(SensorEventTest, UEventInheritance)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
SensorEvent event(image);
|
||||
|
||||
// Should be usable as UEvent
|
||||
UEvent* baseEvent = &event;
|
||||
EXPECT_EQ(baseEvent->getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(baseEvent->getClassName(), "SensorEvent");
|
||||
}
|
||||
|
||||
TEST(SensorEventTest, PolymorphicBehavior)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
SensorEvent* event = new SensorEvent(image);
|
||||
|
||||
UEvent* baseEvent = event;
|
||||
EXPECT_EQ(baseEvent->getCode(), SensorEvent::kCodeData);
|
||||
EXPECT_EQ(baseEvent->getClassName(), "SensorEvent");
|
||||
|
||||
delete event;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user