mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-03 16:47:47 +08:00
Added SensorData tests
This commit is contained in:
matlabbe
@@ -936,7 +936,7 @@ public:
|
||||
/**
|
||||
* @brief Sets landmarks
|
||||
* @param landmarks Map of landmark IDs to landmark data
|
||||
* @note Landmark IDs are typically negative.
|
||||
* @note Landmark IDs should be positive and non-zero.
|
||||
*/
|
||||
void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
|
||||
|
||||
@@ -973,9 +973,9 @@ public:
|
||||
* within the image bounds.
|
||||
*
|
||||
* @param pt 3D point in robot frame (base_link)
|
||||
* @return True if the point is visible from at least one camera, false otherwise
|
||||
* @return Camera index (>=0) of the first camera found with the point visible, -1 if the point is not visible by any camera.
|
||||
*/
|
||||
bool isPointVisibleFromCameras(const cv::Point3f & pt) const;
|
||||
int isPointVisibleFromCameras(const cv::Point3f & pt) const;
|
||||
|
||||
#ifdef HAVE_OPENCV_CUDEV
|
||||
/**
|
||||
|
||||
+38
-10
@@ -89,6 +89,23 @@ SensorData::SensorData(
|
||||
setUserData(userData);
|
||||
}
|
||||
|
||||
// RGB-D constructor + Depth confidence
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
const cv::Mat & depth_confidence,
|
||||
const CameraModel & cameraModel,
|
||||
int id,
|
||||
double stamp,
|
||||
const cv::Mat & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_cellSize(0.0f)
|
||||
{
|
||||
setRGBDImage(rgb, depth, depth_confidence, cameraModel);
|
||||
setUserData(userData);
|
||||
}
|
||||
|
||||
// RGB-D constructor + laser scan
|
||||
SensorData::SensorData(
|
||||
const LaserScan & laserScan,
|
||||
@@ -576,10 +593,14 @@ void SensorData::setOccupancyGrid(
|
||||
_groundCellsRaw = ground;
|
||||
ctGround.start();
|
||||
}
|
||||
else if(ground.type() == CV_8UC1)
|
||||
else if(ground.type() == CV_8UC1 && ground.rows == 1)
|
||||
{
|
||||
_groundCellsCompressed = ground;
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Unsupported local occupancy grid format for ground cells: OpenCV type=%d size=%dx%d", ground.type(), ground.cols, ground.rows);
|
||||
}
|
||||
}
|
||||
if(!obstacles.empty())
|
||||
{
|
||||
@@ -588,10 +609,14 @@ void SensorData::setOccupancyGrid(
|
||||
_obstacleCellsRaw = obstacles;
|
||||
ctObstacles.start();
|
||||
}
|
||||
else if(obstacles.type() == CV_8UC1)
|
||||
else if(obstacles.type() == CV_8UC1 && obstacles.rows == 1)
|
||||
{
|
||||
_obstacleCellsCompressed = obstacles;
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Unsupported local occupancy grid format for obstacle cells: OpenCV type=%d size=%dx%d", obstacles.type(), obstacles.cols, obstacles.rows);
|
||||
}
|
||||
}
|
||||
if(!empty.empty())
|
||||
{
|
||||
@@ -600,10 +625,14 @@ void SensorData::setOccupancyGrid(
|
||||
_emptyCellsRaw = empty;
|
||||
ctEmpty.start();
|
||||
}
|
||||
else if(empty.type() == CV_8UC1)
|
||||
else if(empty.type() == CV_8UC1 && empty.rows == 1)
|
||||
{
|
||||
_emptyCellsCompressed = empty;
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Unsupported local occupancy grid format for empty cells: OpenCV type=%d size=%dx%d", empty.type(), empty.cols, empty.rows);
|
||||
}
|
||||
}
|
||||
ctGround.join();
|
||||
ctObstacles.join();
|
||||
@@ -1012,7 +1041,7 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
|
||||
}
|
||||
|
||||
|
||||
bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
|
||||
int SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
|
||||
{
|
||||
if(_cameraModels.size() >= 1)
|
||||
{
|
||||
@@ -1023,13 +1052,12 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
|
||||
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
|
||||
if(ptInCameraFrame.z > 0.0f)
|
||||
{
|
||||
int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
|
||||
int u, v;
|
||||
_cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
|
||||
if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
|
||||
uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
|
||||
if(uIsInBounds(u, 0, _cameraModels[i].imageWidth()) &&
|
||||
uIsInBounds(v, 0, _cameraModels[i].imageHeight()))
|
||||
{
|
||||
return true;
|
||||
return i;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1049,7 +1077,7 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
|
||||
if(uIsInBounds(u, 0, _stereoCameraModels[i].left().imageWidth()) &&
|
||||
uIsInBounds(v, 0, _stereoCameraModels[i].left().imageHeight()))
|
||||
{
|
||||
return true;
|
||||
return i;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1059,7 +1087,7 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
|
||||
{
|
||||
UERROR("no valid camera model!");
|
||||
}
|
||||
return false;
|
||||
return -1;
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -100,3 +100,8 @@ gtest_discover_tests(test_signature)
|
||||
add_executable(test_sensorevent test_sensorevent.cpp)
|
||||
target_link_libraries(test_sensorevent gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_sensorevent)
|
||||
|
||||
#SensorData.h
|
||||
add_executable(test_sensordata test_sensordata.cpp)
|
||||
target_link_libraries(test_sensordata gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_sensordata)
|
||||
|
||||
@@ -0,0 +1,231 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/SensorCaptureInfo.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// Constructor Tests
|
||||
|
||||
TEST(SensorCaptureInfoTest, DefaultConstructor)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
// Basic fields
|
||||
EXPECT_TRUE(info.cameraName.empty());
|
||||
EXPECT_EQ(info.id, 0);
|
||||
EXPECT_DOUBLE_EQ(info.stamp, 0.0);
|
||||
|
||||
// Timing fields (all should be 0.0f)
|
||||
EXPECT_FLOAT_EQ(info.timeCapture, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeDeskewing, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeDisparity, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeMirroring, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeStereoExposureCompensation, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeImageDecimation, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeHistogramEqualization, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeScanFromDepth, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeUndistortDepth, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeBilateralFiltering, 0.0f);
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 0.0f);
|
||||
|
||||
// Odometry fields
|
||||
EXPECT_TRUE(info.odomPose.isNull());
|
||||
EXPECT_FALSE(info.odomCovariance.empty());
|
||||
EXPECT_EQ(info.odomCovariance.rows, 6);
|
||||
EXPECT_EQ(info.odomCovariance.cols, 6);
|
||||
EXPECT_EQ(info.odomCovariance.type(), CV_64FC1);
|
||||
// Should be identity matrix
|
||||
cv::Mat identity = cv::Mat::eye(6, 6, CV_64FC1);
|
||||
EXPECT_TRUE(cv::countNonZero(info.odomCovariance != identity) == 0);
|
||||
EXPECT_TRUE(info.odomVelocity.empty());
|
||||
}
|
||||
|
||||
// Comprehensive Tests
|
||||
|
||||
TEST(SensorCaptureInfoTest, ComprehensiveUsage)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
// Set all fields
|
||||
info.cameraName = "comprehensive_camera";
|
||||
info.id = 1000;
|
||||
info.stamp = 123456.789;
|
||||
|
||||
info.timeCapture = 0.010f;
|
||||
info.timeDeskewing = 0.005f;
|
||||
info.timeDisparity = 0.020f;
|
||||
info.timeMirroring = 0.001f;
|
||||
info.timeStereoExposureCompensation = 0.003f;
|
||||
info.timeImageDecimation = 0.002f;
|
||||
info.timeHistogramEqualization = 0.004f;
|
||||
info.timeScanFromDepth = 0.015f;
|
||||
info.timeUndistortDepth = 0.008f;
|
||||
info.timeBilateralFiltering = 0.012f;
|
||||
info.timeTotal = 0.080f;
|
||||
|
||||
Transform pose(10.0f, 20.0f, 30.0f, 0.1f, 0.2f, 0.3f);
|
||||
info.odomPose = pose;
|
||||
|
||||
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.1;
|
||||
info.odomCovariance = covariance;
|
||||
|
||||
info.odomVelocity = {1.5f, 2.5f, 3.5f, 0.15f, 0.25f, 0.35f};
|
||||
|
||||
// Verify all fields
|
||||
EXPECT_EQ(info.cameraName, "comprehensive_camera");
|
||||
EXPECT_EQ(info.id, 1000);
|
||||
EXPECT_DOUBLE_EQ(info.stamp, 123456.789);
|
||||
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 0.080f);
|
||||
|
||||
EXPECT_FLOAT_EQ(info.odomPose.x(), 10.0f);
|
||||
EXPECT_FALSE(info.odomCovariance.empty());
|
||||
EXPECT_EQ(info.odomVelocity.size(), 6u);
|
||||
EXPECT_FLOAT_EQ(info.odomVelocity[0], 1.5f);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, CopyAssignment)
|
||||
{
|
||||
SensorCaptureInfo info1;
|
||||
info1.cameraName = "camera1";
|
||||
info1.id = 100;
|
||||
info1.stamp = 123.456;
|
||||
info1.timeCapture = 0.01f;
|
||||
info1.timeTotal = 0.05f;
|
||||
info1.odomPose = Transform(1.0f, 2.0f, 3.0f, 0, 0, 0);
|
||||
info1.odomVelocity = {1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f};
|
||||
|
||||
SensorCaptureInfo info2;
|
||||
info2 = info1;
|
||||
|
||||
EXPECT_EQ(info2.cameraName, info1.cameraName);
|
||||
EXPECT_EQ(info2.id, info1.id);
|
||||
EXPECT_DOUBLE_EQ(info2.stamp, info1.stamp);
|
||||
EXPECT_FLOAT_EQ(info2.timeCapture, info1.timeCapture);
|
||||
EXPECT_FLOAT_EQ(info2.timeTotal, info1.timeTotal);
|
||||
EXPECT_FLOAT_EQ(info2.odomPose.x(), info1.odomPose.x());
|
||||
EXPECT_EQ(info2.odomVelocity.size(), info1.odomVelocity.size());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, CopyConstructor)
|
||||
{
|
||||
SensorCaptureInfo info1;
|
||||
info1.cameraName = "camera1";
|
||||
info1.id = 200;
|
||||
info1.stamp = 456.789;
|
||||
info1.timeDisparity = 0.02f;
|
||||
info1.odomPose = Transform(5.0f, 6.0f, 7.0f, 0, 0, 0);
|
||||
|
||||
SensorCaptureInfo info2(info1);
|
||||
|
||||
EXPECT_EQ(info2.cameraName, info1.cameraName);
|
||||
EXPECT_EQ(info2.id, info1.id);
|
||||
EXPECT_DOUBLE_EQ(info2.stamp, info1.stamp);
|
||||
EXPECT_FLOAT_EQ(info2.timeDisparity, info1.timeDisparity);
|
||||
EXPECT_FLOAT_EQ(info2.odomPose.x(), info1.odomPose.x());
|
||||
}
|
||||
|
||||
// Edge Cases
|
||||
|
||||
TEST(SensorCaptureInfoTest, NegativeTiming)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
// Timing can technically be negative (though unusual)
|
||||
info.timeCapture = -0.001f;
|
||||
EXPECT_FLOAT_EQ(info.timeCapture, -0.001f);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, VeryLargeTiming)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
info.timeTotal = 10.0f;
|
||||
EXPECT_FLOAT_EQ(info.timeTotal, 10.0f);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, EmptyCameraName)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
info.cameraName = "";
|
||||
EXPECT_TRUE(info.cameraName.empty());
|
||||
|
||||
info.cameraName = "camera";
|
||||
EXPECT_FALSE(info.cameraName.empty());
|
||||
|
||||
info.cameraName = "";
|
||||
EXPECT_TRUE(info.cameraName.empty());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, ZeroOdometryCovariance)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
cv::Mat zeroCov = cv::Mat::zeros(6, 6, CV_64FC1);
|
||||
info.odomCovariance = zeroCov;
|
||||
|
||||
EXPECT_EQ(info.odomCovariance.rows, 6);
|
||||
EXPECT_EQ(info.odomCovariance.cols, 6);
|
||||
EXPECT_DOUBLE_EQ(info.odomCovariance.at<double>(0, 0), 0.0);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, EmptyOdometryVelocity)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
EXPECT_TRUE(info.odomVelocity.empty());
|
||||
|
||||
info.odomVelocity = {1.0f};
|
||||
EXPECT_EQ(info.odomVelocity.size(), 1u);
|
||||
|
||||
info.odomVelocity.clear();
|
||||
EXPECT_TRUE(info.odomVelocity.empty());
|
||||
}
|
||||
|
||||
// Real-world Usage Scenarios
|
||||
|
||||
TEST(SensorCaptureInfoTest, StereoCaptureScenario)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
info.cameraName = "stereo_camera";
|
||||
info.id = 500;
|
||||
info.stamp = 1000.0;
|
||||
|
||||
// Typical stereo processing times
|
||||
info.timeCapture = 0.010f;
|
||||
info.timeDisparity = 0.030f;
|
||||
info.timeBilateralFiltering = 0.015f;
|
||||
info.timeTotal = 0.055f;
|
||||
|
||||
info.odomPose = Transform(0.5f, 0.0f, 0.0f, 0, 0, 0);
|
||||
info.odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.01;
|
||||
|
||||
EXPECT_EQ(info.cameraName, "stereo_camera");
|
||||
EXPECT_GE(info.timeTotal, info.timeCapture + info.timeDisparity);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureInfoTest, RGBDCaptureScenario)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
|
||||
info.cameraName = "rgbd_camera";
|
||||
info.id = 600;
|
||||
info.stamp = 2000.0;
|
||||
|
||||
// Typical RGB-D processing times
|
||||
info.timeCapture = 0.008f;
|
||||
info.timeUndistortDepth = 0.005f;
|
||||
info.timeScanFromDepth = 0.010f;
|
||||
info.timeTotal = 0.025f;
|
||||
|
||||
info.odomPose = Transform(1.0f, 1.0f, 1.0f, 0, 0, 0.1f);
|
||||
info.odomVelocity = {0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.05f};
|
||||
|
||||
EXPECT_EQ(info.cameraName, "rgbd_camera");
|
||||
EXPECT_FALSE(info.odomPose.isNull());
|
||||
EXPECT_EQ(info.odomVelocity.size(), 6u);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,917 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/CameraModel.h>
|
||||
#include <rtabmap/core/StereoCameraModel.h>
|
||||
#include <rtabmap/core/LaserScan.h>
|
||||
#include <rtabmap/core/IMU.h>
|
||||
#include <rtabmap/core/GPS.h>
|
||||
#include <rtabmap/core/EnvSensor.h>
|
||||
#include <rtabmap/core/Landmark.h>
|
||||
#include <rtabmap/core/GlobalDescriptor.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// Constructor Tests
|
||||
|
||||
TEST(SensorDataTest, DefaultConstructor)
|
||||
{
|
||||
SensorData data;
|
||||
|
||||
EXPECT_FALSE(data.isValid());
|
||||
EXPECT_EQ(data.id(), 0);
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), 0.0);
|
||||
EXPECT_TRUE(data.imageRaw().empty());
|
||||
EXPECT_TRUE(data.imageCompressed().empty());
|
||||
EXPECT_TRUE(data.depthOrRightRaw().empty());
|
||||
EXPECT_TRUE(data.depthOrRightCompressed().empty());
|
||||
EXPECT_TRUE(data.cameraModels().empty());
|
||||
EXPECT_TRUE(data.stereoCameraModels().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorAppearanceOnly)
|
||||
{
|
||||
cv::Mat image = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
int id = 100;
|
||||
double stamp = 12345.678;
|
||||
|
||||
SensorData data(image, id, stamp);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(data.id(), id);
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), stamp);
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_EQ(data.imageRaw().rows, 480);
|
||||
EXPECT_EQ(data.imageRaw().cols, 640);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorMono)
|
||||
{
|
||||
cv::Mat image = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
int id = 200;
|
||||
|
||||
SensorData data(image, model, id);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(data.id(), id);
|
||||
EXPECT_FALSE(data.cameraModels().empty());
|
||||
EXPECT_EQ(data.cameraModels().size(), 1u);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorRGBD)
|
||||
{
|
||||
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);
|
||||
int id = 300;
|
||||
|
||||
SensorData data(rgb, depth, model, id);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(data.id(), id);
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthOrRightRaw().empty());
|
||||
EXPECT_FALSE(data.cameraModels().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorRGBDWithConfidence)
|
||||
{
|
||||
cv::Mat rgb = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
cv::Mat depth = cv::Mat::ones(480, 640, CV_32FC1) * 1.5f;
|
||||
cv::Mat confidence = cv::Mat::ones(480, 640, CV_8UC1) * 80;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
|
||||
SensorData data(rgb, depth, confidence, model);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_FALSE(data.depthConfidenceRaw().empty());
|
||||
EXPECT_EQ(data.depthConfidenceRaw().type(), CV_8UC1);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorRGBDWithLaserScan)
|
||||
{
|
||||
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);
|
||||
LaserScan scan = LaserScan::backwardCompatibility(cv::Mat(1,100,CV_32FC2));
|
||||
|
||||
SensorData data(scan, rgb, depth, model);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_FALSE(data.laserScanRaw().isEmpty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorMultiCameraRGBD)
|
||||
{
|
||||
cv::Mat rgb = cv::Mat::ones(480, 1280, CV_8UC3) * 128;
|
||||
cv::Mat depth = cv::Mat::ones(480, 1280, CV_16UC1) * 1000;
|
||||
std::vector<CameraModel> models;
|
||||
models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0));
|
||||
models.push_back(CameraModel(525.0, 525.0, 960.0, 240.0));
|
||||
|
||||
SensorData data(rgb, depth, models);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(data.cameraModels().size(), models.size());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorStereo)
|
||||
{
|
||||
cv::Mat left = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
cv::Mat right = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
StereoCameraModel model(525.0, 525.0, 320.0, 240.0, 0.12);
|
||||
|
||||
SensorData data(left, right, model);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthOrRightRaw().empty());
|
||||
EXPECT_FALSE(data.stereoCameraModels().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorMultiStereo)
|
||||
{
|
||||
cv::Mat left = cv::Mat::zeros(480, 1280, CV_8UC1);
|
||||
cv::Mat right = cv::Mat::zeros(480, 1280, CV_8UC1);
|
||||
std::vector<StereoCameraModel> models;
|
||||
models.push_back(StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.12));
|
||||
models.push_back(StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.12));
|
||||
SensorData data(left, right, models);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthOrRightRaw().empty());
|
||||
EXPECT_EQ(data.stereoCameraModels().size(), models.size());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorStereoWithLaserScan)
|
||||
{
|
||||
cv::Mat left = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
cv::Mat right = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
StereoCameraModel model(525.0, 525.0, 320.0, 240.0, 0.12);
|
||||
LaserScan scan = LaserScan::backwardCompatibility(cv::Mat(1,100,CV_32FC2));
|
||||
|
||||
SensorData data(scan, left, right, model);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_FALSE(data.laserScanRaw().isEmpty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ConstructorIMUOnly)
|
||||
{
|
||||
IMU imu(cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat(), Transform::getIdentity());
|
||||
int id = 500;
|
||||
double stamp = 99999.0;
|
||||
|
||||
SensorData data(imu, id, stamp);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(data.id(), id);
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), stamp);
|
||||
EXPECT_FALSE(data.imu().empty());
|
||||
}
|
||||
|
||||
// isValid() Tests
|
||||
|
||||
TEST(SensorDataTest, IsValidWithId)
|
||||
{
|
||||
SensorData data;
|
||||
EXPECT_FALSE(data.isValid());
|
||||
|
||||
data.setId(100);
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, IsValidWithStamp)
|
||||
{
|
||||
SensorData data;
|
||||
EXPECT_FALSE(data.isValid());
|
||||
|
||||
data.setStamp(12345.0);
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, IsValidWithImage)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat image = cv::Mat::ones(100, 100, CV_8UC3);
|
||||
data.setRGBDImage(image, cv::Mat(), CameraModel());
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, IsValidWithLaserScan)
|
||||
{
|
||||
SensorData data;
|
||||
LaserScan scan = LaserScan::backwardCompatibility(cv::Mat(1,100,CV_32FC2));
|
||||
data.setLaserScan(scan);
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, IsValidWithIMU)
|
||||
{
|
||||
SensorData data;
|
||||
IMU imu(cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat(), Transform::getIdentity());
|
||||
data.setIMU(imu);
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
// ID and Stamp Tests
|
||||
|
||||
TEST(SensorDataTest, IdGetterSetter)
|
||||
{
|
||||
SensorData data;
|
||||
EXPECT_EQ(data.id(), 0);
|
||||
|
||||
data.setId(123);
|
||||
EXPECT_EQ(data.id(), 123);
|
||||
|
||||
data.setId(0);
|
||||
EXPECT_EQ(data.id(), 0);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, StampGetterSetter)
|
||||
{
|
||||
SensorData data;
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), 0.0);
|
||||
|
||||
data.setStamp(12345.678);
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), 12345.678);
|
||||
|
||||
data.setStamp(0.0);
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), 0.0);
|
||||
}
|
||||
|
||||
// setRGBDImage Tests
|
||||
|
||||
TEST(SensorDataTest, SetRGBDImage)
|
||||
{
|
||||
SensorData data;
|
||||
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);
|
||||
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthOrRightRaw().empty());
|
||||
EXPECT_EQ(data.imageRaw().rows, 480);
|
||||
EXPECT_EQ(data.imageRaw().cols, 640);
|
||||
EXPECT_FALSE(data.cameraModels().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetRGBDImageWithConfidence)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat rgb = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
cv::Mat depth = cv::Mat::ones(480, 640, CV_32FC1) * 1.5f;
|
||||
cv::Mat confidence = cv::Mat::ones(480, 640, CV_8UC1) * 90;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
|
||||
data.setRGBDImage(rgb, depth, confidence, model);
|
||||
|
||||
EXPECT_FALSE(data.depthConfidenceRaw().empty());
|
||||
EXPECT_EQ(data.depthConfidenceRaw().type(), CV_8UC1);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetRGBDImageMultiCamera)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat rgb = cv::Mat::ones(480, 1280, CV_8UC3) * 128;
|
||||
cv::Mat depth = cv::Mat::ones(480, 1280, CV_16UC1) * 1000;
|
||||
std::vector<CameraModel> models;
|
||||
models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0));
|
||||
models.push_back(CameraModel(525.0, 525.0, 960.0, 240.0));
|
||||
|
||||
data.setRGBDImage(rgb, depth, models);
|
||||
|
||||
EXPECT_EQ(data.cameraModels().size(), 2u);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetRGBDImageClearPreviousData)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat rgb1 = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
cv::Mat depth1 = cv::Mat::ones(480, 640, CV_16UC1) * 1000;
|
||||
CameraModel model1(525.0, 525.0, 320.0, 240.0);
|
||||
data.setRGBDImage(rgb1, depth1, model1);
|
||||
|
||||
cv::Mat rgb2 = cv::Mat::ones(240, 320, CV_8UC3) * 255;
|
||||
cv::Mat depth2 = cv::Mat::ones(240, 320, CV_16UC1) * 2000;
|
||||
CameraModel model2(525.0, 525.0, 160.0, 120.0);
|
||||
data.setRGBDImage(rgb2, depth2, model2, true);
|
||||
|
||||
EXPECT_EQ(data.imageRaw().rows, 240);
|
||||
EXPECT_EQ(data.imageRaw().cols, 320);
|
||||
EXPECT_EQ(data.cameraModels().size(), 1u);
|
||||
}
|
||||
|
||||
// setStereoImage Tests
|
||||
|
||||
TEST(SensorDataTest, SetStereoImage)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat left = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
cv::Mat right = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
StereoCameraModel model(525.0, 525.0, 320.0, 240.0, 0.12);
|
||||
|
||||
data.setStereoImage(left, right, model);
|
||||
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthOrRightRaw().empty());
|
||||
EXPECT_FALSE(data.stereoCameraModels().empty());
|
||||
EXPECT_TRUE(data.cameraModels().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetStereoImageMultiCamera)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat left = cv::Mat::zeros(480, 1280, CV_8UC1);
|
||||
cv::Mat right = cv::Mat::zeros(480, 1280, CV_8UC1);
|
||||
std::vector<StereoCameraModel> models;
|
||||
models.push_back(StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.12));
|
||||
models.push_back(StereoCameraModel(525.0, 525.0, 960.0, 240.0, 0.12));
|
||||
|
||||
data.setStereoImage(left, right, models);
|
||||
|
||||
EXPECT_EQ(data.stereoCameraModels().size(), 2u);
|
||||
}
|
||||
|
||||
// setLaserScan Tests
|
||||
|
||||
TEST(SensorDataTest, SetLaserScan)
|
||||
{
|
||||
SensorData data;
|
||||
LaserScan scan = LaserScan::backwardCompatibility(cv::Mat(1,100,CV_32FC2));
|
||||
|
||||
data.setLaserScan(scan);
|
||||
|
||||
EXPECT_FALSE(data.laserScanRaw().isEmpty());
|
||||
EXPECT_EQ(data.laserScanRaw().size(), 100u);
|
||||
}
|
||||
|
||||
// Camera Model Tests
|
||||
|
||||
TEST(SensorDataTest, SetCameraModel)
|
||||
{
|
||||
SensorData data;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
|
||||
data.setCameraModel(model);
|
||||
|
||||
EXPECT_EQ(data.cameraModels().size(), 1u);
|
||||
EXPECT_DOUBLE_EQ(data.cameraModels()[0].fx(), 525.0);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetCameraModels)
|
||||
{
|
||||
SensorData data;
|
||||
std::vector<CameraModel> models;
|
||||
models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0));
|
||||
models.push_back(CameraModel(600.0, 600.0, 400.0, 300.0));
|
||||
|
||||
data.setCameraModels(models);
|
||||
|
||||
EXPECT_EQ(data.cameraModels().size(), 2u);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetStereoCameraModel)
|
||||
{
|
||||
SensorData data;
|
||||
StereoCameraModel model(525.0, 525.0, 320.0, 240.0, 0.12);
|
||||
|
||||
data.setStereoCameraModel(model);
|
||||
|
||||
EXPECT_EQ(data.stereoCameraModels().size(), 1u);
|
||||
EXPECT_DOUBLE_EQ(data.stereoCameraModels()[0].baseline(), 0.12);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetStereoCameraModels)
|
||||
{
|
||||
SensorData data;
|
||||
std::vector<StereoCameraModel> models;
|
||||
models.push_back(StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.12));
|
||||
models.push_back(StereoCameraModel(600.0, 600.0, 400.0, 300.0, 0.15));
|
||||
|
||||
data.setStereoCameraModels(models);
|
||||
|
||||
EXPECT_EQ(data.stereoCameraModels().size(), 2u);
|
||||
}
|
||||
|
||||
// Convenience Methods Tests
|
||||
|
||||
TEST(SensorDataTest, DepthRaw)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat depth = cv::Mat::ones(480, 640, CV_16UC1) * 1000;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
data.setRGBDImage(cv::Mat(), depth, model);
|
||||
|
||||
EXPECT_TRUE(data.rightRaw().empty());
|
||||
cv::Mat extractedDepth = data.depthRaw();
|
||||
EXPECT_FALSE(extractedDepth.empty());
|
||||
EXPECT_EQ(extractedDepth.type(), CV_16UC1);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, RightRaw)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat left = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
cv::Mat right = cv::Mat::ones(480, 640, CV_8UC1) * 128;
|
||||
StereoCameraModel model(525.0, 525.0, 320.0, 240.0, 0.12);
|
||||
data.setStereoImage(left, right, model);
|
||||
|
||||
EXPECT_TRUE(data.depthRaw().empty());
|
||||
cv::Mat extractedRight = data.rightRaw();
|
||||
EXPECT_FALSE(extractedRight.empty());
|
||||
EXPECT_EQ(extractedRight.type(), CV_8UC1);
|
||||
}
|
||||
|
||||
// Features Tests
|
||||
|
||||
TEST(SensorDataTest, SetFeatures)
|
||||
{
|
||||
SensorData data;
|
||||
std::vector<cv::KeyPoint> keypoints;
|
||||
keypoints.push_back(cv::KeyPoint(100, 200, 5.0f));
|
||||
keypoints.push_back(cv::KeyPoint(300, 400, 5.0f));
|
||||
|
||||
std::vector<cv::Point3f> keypoints3D;
|
||||
keypoints3D.push_back(cv::Point3f(1.0f, 2.0f, 3.0f));
|
||||
keypoints3D.push_back(cv::Point3f(4.0f, 5.0f, 6.0f));
|
||||
|
||||
cv::Mat descriptors = cv::Mat::ones(2, 128, CV_32F);
|
||||
|
||||
data.setFeatures(keypoints, keypoints3D, descriptors);
|
||||
|
||||
EXPECT_EQ(data.keypoints().size(), 2u);
|
||||
EXPECT_EQ(data.keypoints3D().size(), 2u);
|
||||
EXPECT_FALSE(data.descriptors().empty());
|
||||
EXPECT_EQ(data.descriptors().rows, 2);
|
||||
}
|
||||
|
||||
// Global Descriptors Tests
|
||||
|
||||
TEST(SensorDataTest, AddGlobalDescriptor)
|
||||
{
|
||||
SensorData data;
|
||||
GlobalDescriptor desc(0, cv::Mat::ones(1, 100, CV_32F));
|
||||
|
||||
data.addGlobalDescriptor(desc);
|
||||
|
||||
EXPECT_EQ(data.globalDescriptors().size(), 1u);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetGlobalDescriptors)
|
||||
{
|
||||
SensorData data;
|
||||
std::vector<GlobalDescriptor> descriptors;
|
||||
descriptors.push_back(GlobalDescriptor(0, cv::Mat::ones(1, 100, CV_32F)));
|
||||
descriptors.push_back(GlobalDescriptor(0, cv::Mat::ones(1, 200, CV_32F)));
|
||||
|
||||
data.setGlobalDescriptors(descriptors);
|
||||
|
||||
EXPECT_EQ(data.globalDescriptors().size(), 2u);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ClearGlobalDescriptors)
|
||||
{
|
||||
SensorData data;
|
||||
GlobalDescriptor desc(0, cv::Mat::ones(1, 100, CV_32F));
|
||||
data.addGlobalDescriptor(desc);
|
||||
EXPECT_EQ(data.globalDescriptors().size(), 1u);
|
||||
|
||||
data.clearGlobalDescriptors();
|
||||
EXPECT_TRUE(data.globalDescriptors().empty());
|
||||
}
|
||||
|
||||
// Pose Tests
|
||||
|
||||
TEST(SensorDataTest, SetGroundTruth)
|
||||
{
|
||||
SensorData data;
|
||||
Transform pose(1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
|
||||
|
||||
data.setGroundTruth(pose);
|
||||
|
||||
EXPECT_FALSE(data.groundTruth().isNull());
|
||||
EXPECT_FLOAT_EQ(data.groundTruth().x(), 1.0f);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetGlobalPose)
|
||||
{
|
||||
SensorData data;
|
||||
Transform pose(10.0f, 20.0f, 30.0f, 0.1f, 0.2f, 0.3f);
|
||||
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.1;
|
||||
|
||||
data.setGlobalPose(pose, covariance);
|
||||
|
||||
EXPECT_FALSE(data.globalPose().isNull());
|
||||
EXPECT_FALSE(data.globalPoseCovariance().empty());
|
||||
EXPECT_EQ(data.globalPoseCovariance().rows, 6);
|
||||
EXPECT_EQ(data.globalPoseCovariance().cols, 6);
|
||||
}
|
||||
|
||||
// GPS Tests
|
||||
|
||||
TEST(SensorDataTest, SetGPS)
|
||||
{
|
||||
double stamp = 1111.11;
|
||||
double latitude = -73.0;
|
||||
GPS gps(stamp, 45.0, latitude, 100.0, 5.0, 0.0);
|
||||
|
||||
SensorData data;
|
||||
data.setGPS(gps);
|
||||
|
||||
EXPECT_DOUBLE_EQ(data.gps().stamp(), stamp);
|
||||
EXPECT_DOUBLE_EQ(data.gps().latitude(), latitude);
|
||||
}
|
||||
|
||||
// IMU Tests
|
||||
|
||||
TEST(SensorDataTest, SetIMU)
|
||||
{
|
||||
SensorData data;
|
||||
IMU imu;
|
||||
|
||||
data.setIMU(imu);
|
||||
EXPECT_TRUE(data.imu().empty());
|
||||
|
||||
imu = IMU(cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat(), Transform::getIdentity());
|
||||
data.setIMU(imu);
|
||||
EXPECT_FALSE(data.imu().empty());
|
||||
}
|
||||
|
||||
// Environmental Sensors Tests
|
||||
|
||||
TEST(SensorDataTest, SetEnvSensors)
|
||||
{
|
||||
SensorData data;
|
||||
EnvSensors sensors;
|
||||
sensors.insert(std::make_pair(EnvSensor::kAmbientTemperature, EnvSensor(EnvSensor::kAmbientTemperature, 25.0f, 0.0)));
|
||||
|
||||
data.setEnvSensors(sensors);
|
||||
|
||||
EXPECT_FALSE(data.envSensors().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, AddEnvSensor)
|
||||
{
|
||||
SensorData data;
|
||||
EnvSensor sensor(EnvSensor::kAmbientRelativeHumidity, 60.0f, 0.0);
|
||||
|
||||
data.addEnvSensor(sensor);
|
||||
|
||||
EXPECT_FALSE(data.envSensors().empty());
|
||||
EXPECT_EQ(data.envSensors().count(EnvSensor::kAmbientRelativeHumidity), 1u);
|
||||
}
|
||||
|
||||
// Landmarks Tests
|
||||
|
||||
TEST(SensorDataTest, SetLandmarks)
|
||||
{
|
||||
SensorData data;
|
||||
Landmarks landmarks;
|
||||
landmarks.insert(std::make_pair(1, Landmark(1, 0, Transform(1.0f, 2.0f, 3.0f, 0, 0, 0), cv::Mat::eye(6,6,CV_64FC1))));
|
||||
|
||||
data.setLandmarks(landmarks);
|
||||
|
||||
EXPECT_FALSE(data.landmarks().empty());
|
||||
EXPECT_EQ(data.landmarks().count(1), 1u);
|
||||
}
|
||||
|
||||
// User Data Tests
|
||||
|
||||
TEST(SensorDataTest, SetUserData)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat userData = cv::Mat::ones(100, 100, CV_8UC1) * 128;
|
||||
|
||||
data.setUserData(userData);
|
||||
|
||||
EXPECT_FALSE(data.userDataRaw().empty());
|
||||
EXPECT_EQ(data.userDataRaw().rows, 100);
|
||||
EXPECT_EQ(data.userDataRaw().cols, 100);
|
||||
}
|
||||
|
||||
// Occupancy Grid Tests
|
||||
|
||||
TEST(SensorDataTest, SetOccupancyGrid)
|
||||
{
|
||||
SensorData data;
|
||||
// Use supported format: CV_32FC2 (2 channels of float)
|
||||
cv::Mat ground = cv::Mat::ones(1, 100, CV_32FC2);
|
||||
cv::Mat obstacles = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
cv::Mat empty = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
float cellSize = 0.05f;
|
||||
cv::Point3f viewPoint(0.0f, 0.0f, 0.0f);
|
||||
|
||||
data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
|
||||
|
||||
EXPECT_FALSE(data.gridGroundCellsRaw().empty());
|
||||
EXPECT_FALSE(data.gridObstacleCellsRaw().empty());
|
||||
EXPECT_FALSE(data.gridEmptyCellsRaw().empty());
|
||||
EXPECT_FLOAT_EQ(data.gridCellSize(), cellSize);
|
||||
EXPECT_FLOAT_EQ(data.gridViewPoint().x, viewPoint.x);
|
||||
// It should auto compress
|
||||
EXPECT_FALSE(data.gridGroundCellsCompressed().empty());
|
||||
EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
|
||||
EXPECT_FALSE(data.gridEmptyCellsCompressed().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetOccupancyGridCompressed)
|
||||
{
|
||||
SensorData data;
|
||||
// Use supported compressed format: CV_8UC1 with 1 row
|
||||
cv::Mat ground = cv::Mat::ones(1, 100, CV_8UC1);
|
||||
cv::Mat obstacles = cv::Mat::ones(1, 100, CV_8UC1);
|
||||
cv::Mat empty = cv::Mat::ones(1, 100, CV_8UC1);
|
||||
float cellSize = 0.05f;
|
||||
cv::Point3f viewPoint(0.0f, 0.0f, 0.0f);
|
||||
|
||||
data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
|
||||
|
||||
EXPECT_FALSE(data.gridGroundCellsCompressed().empty());
|
||||
EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
|
||||
EXPECT_FALSE(data.gridEmptyCellsCompressed().empty());
|
||||
EXPECT_FLOAT_EQ(data.gridCellSize(), cellSize);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetOccupancyGridUnsupportedGroundFormat)
|
||||
{
|
||||
SensorData data;
|
||||
// Unsupported format: CV_8UC1 with multiple rows (not compressed format)
|
||||
cv::Mat ground = cv::Mat::ones(100, 100, CV_8UC1);
|
||||
cv::Mat obstacles = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
cv::Mat empty = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
float cellSize = 0.05f;
|
||||
cv::Point3f viewPoint(0.0f, 0.0f, 0.0f);
|
||||
|
||||
EXPECT_THROW(data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint), UException);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetOccupancyGridUnsupportedObstacleFormat)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat ground = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
// Unsupported format: CV_16UC1
|
||||
cv::Mat obstacles = cv::Mat::ones(1, 100, CV_16UC1);
|
||||
cv::Mat empty = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
float cellSize = 0.05f;
|
||||
cv::Point3f viewPoint(0.0f, 0.0f, 0.0f);
|
||||
|
||||
EXPECT_THROW(data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint), UException);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetOccupancyGridUnsupportedEmptyFormat)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat ground = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
cv::Mat obstacles = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
// Unsupported format: CV_32FC1 (only CV_32FC2 through CV_32FC7 are supported)
|
||||
cv::Mat empty = cv::Mat::ones(1, 100, CV_32FC1);
|
||||
float cellSize = 0.05f;
|
||||
cv::Point3f viewPoint(0.0f, 0.0f, 0.0f);
|
||||
|
||||
EXPECT_THROW(data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint), UException);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, SetOccupancyGridSupportedFormats)
|
||||
{
|
||||
float cellSize = 0.05f;
|
||||
cv::Point3f viewPoint(0.0f, 0.0f, 0.0f);
|
||||
|
||||
// Test all supported formats: CV_32FC2 through CV_32FC7
|
||||
for (int channels = 2; channels <= 7; ++channels)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat ground = cv::Mat(1, 100, CV_32FC(channels));
|
||||
cv::Mat obstacles = cv::Mat(1, 100, CV_32FC(channels));
|
||||
cv::Mat empty = cv::Mat(1, 100, CV_32FC(channels));
|
||||
|
||||
EXPECT_NO_THROW(data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint))
|
||||
<< "Failed for CV_32FC(" << channels << ")";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ClearOccupancyGridRaw)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat ground = cv::Mat::ones(1, 100, CV_32FC2);
|
||||
cv::Mat obstacles = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
cv::Mat empty = cv::Mat::zeros(1, 100, CV_32FC2);
|
||||
data.setOccupancyGrid(ground, obstacles, empty, 0.05f, cv::Point3f(0, 0, 0));
|
||||
|
||||
EXPECT_FALSE(data.gridGroundCellsRaw().empty());
|
||||
|
||||
data.clearOccupancyGridRaw();
|
||||
|
||||
EXPECT_TRUE(data.gridGroundCellsRaw().empty());
|
||||
EXPECT_TRUE(data.gridObstacleCellsRaw().empty());
|
||||
// Should not remove compressed
|
||||
EXPECT_FALSE(data.gridGroundCellsCompressed().empty());
|
||||
EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
|
||||
}
|
||||
|
||||
// Data Clearing Tests
|
||||
|
||||
TEST(SensorDataTest, ClearCompressedData)
|
||||
{
|
||||
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);
|
||||
// Construct with compressed data (simulated compressed data)
|
||||
SensorData data(cv::Mat::ones(1, 100, CV_8UC1), cv::Mat::ones(1, 100, CV_8UC1), model);
|
||||
// Add raw data
|
||||
data.setRGBDImage(rgb, depth, model, false);
|
||||
|
||||
data.clearCompressedData(true, false, false);
|
||||
|
||||
// Raw data should still be present
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthRaw().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, ClearRawData)
|
||||
{
|
||||
SensorData data;
|
||||
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);
|
||||
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
|
||||
data.clearRawData(true, false, false);
|
||||
|
||||
EXPECT_TRUE(data.imageRaw().empty());
|
||||
}
|
||||
|
||||
// Visibility Tests
|
||||
|
||||
TEST(SensorDataTest, IsPointVisibleFromCameras)
|
||||
{
|
||||
SensorData data;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0, CameraModel::opticalRotation(), 0.0, cv::Size(640, 480));
|
||||
data.setCameraModel(model);
|
||||
cv::Mat rgb = cv::Mat::ones(480, 640, CV_8UC3);
|
||||
data.setRGBDImage(rgb, cv::Mat(), model);
|
||||
|
||||
EXPECT_EQ(data.cameraModels().size(), 1);
|
||||
EXPECT_TRUE(data.cameraModels()[0].isValidForProjection());
|
||||
|
||||
// Point in front of camera (should be visible, returns camera index 0)
|
||||
cv::Point3f pt(1.0f, 0.0f, 0.0f);
|
||||
int cameraIndex = data.isPointVisibleFromCameras(pt);
|
||||
EXPECT_EQ(cameraIndex, 0);
|
||||
|
||||
// Point behind camera (should not be visible, returns -1)
|
||||
cv::Point3f ptBehind(-1.0f, 0.0f, 0.0f);
|
||||
cameraIndex = data.isPointVisibleFromCameras(ptBehind);
|
||||
EXPECT_EQ(cameraIndex, -1);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, IsPointVisibleFromCamerasMultiCamera)
|
||||
{
|
||||
SensorData data;
|
||||
std::vector<CameraModel> models;
|
||||
models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0, CameraModel::opticalRotation(), 0.0, cv::Size(640, 480)));
|
||||
models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0, CameraModel::opticalRotation(), 0.0, cv::Size(640, 480)));
|
||||
data.setCameraModels(models);
|
||||
cv::Mat rgb = cv::Mat::ones(480, 1280, CV_8UC3);
|
||||
data.setRGBDImage(rgb, cv::Mat(), models);
|
||||
|
||||
EXPECT_EQ(data.cameraModels().size(), 2);
|
||||
|
||||
// Point visible from first camera (should return index 0)
|
||||
cv::Point3f pt(1.0f, 0.0f, 0.0f);
|
||||
int cameraIndex = data.isPointVisibleFromCameras(pt);
|
||||
EXPECT_GE(cameraIndex, 0);
|
||||
EXPECT_LE(cameraIndex, 1);
|
||||
|
||||
// Point not visible from any camera (should return -1)
|
||||
cv::Point3f ptBehind(-1.0f, 0.0f, 0.0f);
|
||||
cameraIndex = data.isPointVisibleFromCameras(ptBehind);
|
||||
EXPECT_EQ(cameraIndex, -1);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, IsPointVisibleFromCamerasNoCameras)
|
||||
{
|
||||
SensorData data;
|
||||
// No camera models set
|
||||
|
||||
cv::Point3f pt(1.0f, 0.0f, 0.0f);
|
||||
int cameraIndex = data.isPointVisibleFromCameras(pt);
|
||||
EXPECT_EQ(cameraIndex, -1);
|
||||
}
|
||||
|
||||
// Memory Usage Tests
|
||||
|
||||
TEST(SensorDataTest, GetMemoryUsed)
|
||||
{
|
||||
SensorData data;
|
||||
unsigned long memEmpty = data.getMemoryUsed();
|
||||
|
||||
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);
|
||||
|
||||
unsigned long memWithData = data.getMemoryUsed();
|
||||
|
||||
EXPECT_GT(memWithData, memEmpty);
|
||||
}
|
||||
|
||||
// Comprehensive Tests
|
||||
|
||||
TEST(SensorDataTest, ComprehensiveUsage)
|
||||
{
|
||||
SensorData data;
|
||||
|
||||
// Set ID and stamp
|
||||
data.setId(1000);
|
||||
data.setStamp(123456.789);
|
||||
|
||||
// Set RGB-D images
|
||||
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);
|
||||
|
||||
// Set laser scan
|
||||
LaserScan scan = LaserScan::backwardCompatibility(cv::Mat::ones(1, 640, CV_32FC3));
|
||||
data.setLaserScan(scan);
|
||||
|
||||
// Set features
|
||||
std::vector<cv::KeyPoint> keypoints;
|
||||
keypoints.push_back(cv::KeyPoint(100, 200, 5.0f));
|
||||
std::vector<cv::Point3f> keypoints3D;
|
||||
keypoints3D.push_back(cv::Point3f(1.0f, 2.0f, 3.0f));
|
||||
cv::Mat descriptors = cv::Mat::ones(1, 128, CV_32F);
|
||||
data.setFeatures(keypoints, keypoints3D, descriptors);
|
||||
|
||||
// Set global pose
|
||||
Transform pose(10.0f, 20.0f, 30.0f, 0, 0, 0);
|
||||
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.1;
|
||||
data.setGlobalPose(pose, covariance);
|
||||
|
||||
// Verify all data
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(data.id(), 1000);
|
||||
EXPECT_DOUBLE_EQ(data.stamp(), 123456.789);
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_FALSE(data.depthOrRightRaw().empty());
|
||||
EXPECT_FALSE(data.laserScanRaw().isEmpty());
|
||||
EXPECT_EQ(data.keypoints().size(), 1u);
|
||||
EXPECT_FALSE(data.globalPose().isNull());
|
||||
}
|
||||
|
||||
// Edge Cases
|
||||
|
||||
TEST(SensorDataTest, EmptyImages)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat emptyRgb;
|
||||
cv::Mat emptyDepth;
|
||||
CameraModel model(525.0, 525.0, 320.0, 240.0);
|
||||
|
||||
data.setRGBDImage(emptyRgb, emptyDepth, model);
|
||||
|
||||
EXPECT_TRUE(data.imageRaw().empty());
|
||||
EXPECT_TRUE(data.depthOrRightRaw().empty());
|
||||
EXPECT_FALSE(data.cameraModels().empty());
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, DifferentImageTypes)
|
||||
{
|
||||
SensorData data;
|
||||
|
||||
// Grayscale
|
||||
cv::Mat gray = cv::Mat::zeros(100, 100, CV_8UC1);
|
||||
data.setRGBDImage(gray, cv::Mat(), CameraModel());
|
||||
EXPECT_EQ(data.imageRaw().type(), CV_8UC1);
|
||||
|
||||
// RGB
|
||||
cv::Mat color = cv::Mat::zeros(100, 100, CV_8UC3);
|
||||
data.setRGBDImage(color, cv::Mat(), CameraModel());
|
||||
EXPECT_EQ(data.imageRaw().type(), CV_8UC3);
|
||||
}
|
||||
|
||||
TEST(SensorDataTest, DifferentDepthTypes)
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat rgb = cv::Mat::ones(100, 100, CV_8UC3);
|
||||
CameraModel model(525.0, 525.0, 50.0, 50.0);
|
||||
|
||||
// 16-bit depth (millimeters)
|
||||
cv::Mat depth16 = cv::Mat::ones(100, 100, CV_16UC1) * 1000;
|
||||
data.setRGBDImage(rgb, depth16, model);
|
||||
EXPECT_EQ(data.depthOrRightRaw().type(), CV_16UC1);
|
||||
|
||||
// 32-bit depth (meters)
|
||||
cv::Mat depth32 = cv::Mat::ones(100, 100, CV_32FC1) * 1.0f;
|
||||
data.setRGBDImage(rgb, depth32, model);
|
||||
EXPECT_EQ(data.depthOrRightRaw().type(), CV_32FC1);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user