mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 00:57:46 +08:00
Chi-Wei Leeandmatlabbe
20b040a77a
When estimating a missing depth from its 4-connected neighbors, a neighbor is accepted if it is within depthErrorRatio of the mean of the neighbors accepted so far. The tolerance was computed from the running sum instead of the mean, so it grew to 2x and then 3x the ratio for the third and fourth neighbor, letting inconsistent depths into the average. Co-authored-by: matlabbe <[email protected]>
1443 lines
50 KiB
C++
1443 lines
50 KiB
C++
#include "gtest/gtest.h"
|
|
#include "rtabmap/core/util2d.h"
|
|
#include "rtabmap/utilite/UException.h"
|
|
#include "rtabmap/core/Features2d.h"
|
|
#include "rtabmap/core/util3d.h"
|
|
#include "rtabmap/core/util3d_filtering.h"
|
|
#include "pcl/io/pcd_io.h"
|
|
|
|
using namespace rtabmap;
|
|
|
|
TEST(Util2dTest, SsdIdenticalImages)
|
|
{
|
|
cv::Mat img1 = (cv::Mat_<uint8_t>(3,3) << 10, 20, 30, 40, 50, 60, 70, 80, 90);
|
|
cv::Mat img2 = img1.clone();
|
|
float score = util2d::ssd(img1, img2);
|
|
EXPECT_FLOAT_EQ(score, 0.0f);
|
|
}
|
|
|
|
TEST(Util2dTest, SsdDifferentImages)
|
|
{
|
|
cv::Mat img1 = (cv::Mat_<uint8_t>(2,2) << 10, 20, 30, 40);
|
|
cv::Mat img2 = (cv::Mat_<uint8_t>(2,2) << 11, 19, 31, 39);
|
|
float score = util2d::ssd(img1, img2);
|
|
float expected = pow(10 - 11, 2) + pow(20 - 19, 2) + pow(30 - 31, 2) + pow(40 - 39, 2);
|
|
EXPECT_FLOAT_EQ(score, expected);
|
|
}
|
|
|
|
TEST(Util2dTest, SsdStereoLikeInput)
|
|
{
|
|
cv::Mat left = (cv::Mat_<cv::Vec2s>(1,1) << cv::Vec2s(10, 20));
|
|
cv::Mat right = (cv::Mat_<cv::Vec2s>(1,1) << cv::Vec2s(11, 19));
|
|
float avgL = (10 + 20) / 2.0f;
|
|
float avgR = (11 + 19) / 2.0f;
|
|
float expected = (avgL - avgR) * (avgL - avgR);
|
|
EXPECT_FLOAT_EQ(util2d::ssd(left, right), expected);
|
|
}
|
|
|
|
TEST(Util2dTest, SadIdenticalImages)
|
|
{
|
|
cv::Mat img1 = (cv::Mat_<float>(2,2) << 1.0, 2.0, 3.0, 4.0);
|
|
cv::Mat img2 = img1.clone();
|
|
float score = util2d::sad(img1, img2);
|
|
EXPECT_FLOAT_EQ(score, 0.0f);
|
|
}
|
|
|
|
TEST(Util2dTest, SadDifferentImages)
|
|
{
|
|
cv::Mat img1 = (cv::Mat_<float>(2,2) << 1.0, 2.0, 3.0, 4.0);
|
|
cv::Mat img2 = (cv::Mat_<float>(2,2) << 0.5, 2.5, 2.5, 5.0);
|
|
float score = fabs(1.0 - 0.5) + fabs(2.0 - 2.5) + fabs(3.0 - 2.5) + fabs(4.0 - 5.0);
|
|
EXPECT_FLOAT_EQ(util2d::sad(img1, img2), score);
|
|
}
|
|
|
|
TEST(Util2dTest, SadStereoLikeInput)
|
|
{
|
|
cv::Mat left = (cv::Mat_<cv::Vec2s>(1,1) << cv::Vec2s(5, 15));
|
|
cv::Mat right = (cv::Mat_<cv::Vec2s>(1,1) << cv::Vec2s(10, 10));
|
|
float avgL = (5 + 15) / 2.0f;
|
|
float avgR = (10 + 10) / 2.0f;
|
|
float expected = fabs(avgL - avgR);
|
|
EXPECT_FLOAT_EQ(util2d::sad(left, right), expected);
|
|
}
|
|
|
|
TEST(Util2dTest, CalcStereoCorrespondencesCheckInputs) {
|
|
|
|
// Create synthetic images
|
|
cv::Mat left = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
cv::Mat right = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
|
|
std::vector<cv::Point2f> emptyCorners;
|
|
std::vector<unsigned char> status;
|
|
|
|
// Check inputs
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(left, cv::Mat(), emptyCorners, status), UException);
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(cv::Mat(), right, emptyCorners, status), UException);
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(left, cv::Mat(2,2,CV_8UC1), emptyCorners, status), UException);
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(left, cv::Mat(left.size(),CV_8UC3), emptyCorners, status), UException);
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(left, right, emptyCorners, status, cv::Size(1,1)), cv::Exception);
|
|
ASSERT_NO_THROW(util2d::calcStereoCorrespondences(left, right, emptyCorners, status, cv::Size(3,3)));
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(left, cv::Mat(2,2,CV_8UC1), emptyCorners, status, cv::Size(6,3), 3, 5, -1.0f), UException);
|
|
ASSERT_THROW(util2d::calcStereoCorrespondences(left, cv::Mat(2,2,CV_8UC1), emptyCorners, status, cv::Size(6,3), 3, 5, 10, 1), UException);
|
|
|
|
// Check results
|
|
ASSERT_TRUE(util2d::calcStereoCorrespondences(left, right, emptyCorners, status).empty());
|
|
}
|
|
|
|
TEST(Util2dTest, CalcStereoCorrespondencesSSD) {
|
|
|
|
// Create synthetic images
|
|
cv::Mat left = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
cv::Mat right = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
|
|
std::vector<unsigned char> status;
|
|
|
|
// Draw white dots in both images
|
|
cv::Point2f leftPoint(50, 50);
|
|
cv::Point2f subPixelLeftPoint(25, 25);
|
|
cv::Point2f wrongPoint(75, 75);
|
|
|
|
left.at<uchar>((int)leftPoint.y, (int)leftPoint.x) = 255;
|
|
right.at<uchar>((int)leftPoint.y, (int)leftPoint.x-5) = 255;
|
|
|
|
left.at<uchar>((int)subPixelLeftPoint.y, (int)subPixelLeftPoint.x) = 64;
|
|
left.at<uchar>((int)subPixelLeftPoint.y, (int)subPixelLeftPoint.x+1) = 191;
|
|
right.at<uchar>((int)subPixelLeftPoint.y, (int)subPixelLeftPoint.x-6) = 255;
|
|
|
|
left.at<uchar>((int)wrongPoint.y, (int)wrongPoint.x) = 75;
|
|
|
|
std::vector<cv::Point2f> leftCorners = { leftPoint, subPixelLeftPoint, wrongPoint };
|
|
|
|
// SSD
|
|
std::vector<cv::Point2f> rightCorners = util2d::calcStereoCorrespondences(left, right, leftCorners, status, cv::Size(6,3), 3, 5, 0.0f, 64.0f, true);
|
|
ASSERT_EQ(rightCorners.size(), 3);
|
|
ASSERT_EQ(status.size(), 3);
|
|
ASSERT_TRUE(status[0]);
|
|
ASSERT_TRUE(status[1]);
|
|
ASSERT_FALSE(status[2]);
|
|
ASSERT_NEAR(leftCorners[0].x - rightCorners[0].x, 5.0f, 0.1f);
|
|
ASSERT_NEAR(leftCorners[1].x - rightCorners[1].x, 6.75f, 0.1f);
|
|
}
|
|
|
|
TEST(Util2dTest, CalcStereoCorrespondencesSAD) {
|
|
|
|
// Create synthetic images
|
|
cv::Mat left = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
cv::Mat right = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
|
|
std::vector<unsigned char> status;
|
|
|
|
// Draw white dots in both images
|
|
cv::Point2f leftPoint(50, 50);
|
|
cv::Point2f subPixelLeftPoint(25, 25);
|
|
cv::Point2f wrongPoint(75, 75);
|
|
|
|
left.at<uchar>((int)leftPoint.y, (int)leftPoint.x) = 255;
|
|
right.at<uchar>((int)leftPoint.y, (int)leftPoint.x-5) = 255;
|
|
|
|
left.at<uchar>((int)subPixelLeftPoint.y, (int)subPixelLeftPoint.x) = 64;
|
|
left.at<uchar>((int)subPixelLeftPoint.y, (int)subPixelLeftPoint.x+1) = 191;
|
|
right.at<uchar>((int)subPixelLeftPoint.y, (int)subPixelLeftPoint.x-6) = 255;
|
|
|
|
left.at<uchar>((int)wrongPoint.y, (int)wrongPoint.x) = 75;
|
|
|
|
std::vector<cv::Point2f> leftCorners = { leftPoint, subPixelLeftPoint, wrongPoint };
|
|
|
|
// SAD
|
|
std::vector<cv::Point2f> rightCorners = util2d::calcStereoCorrespondences(left, right, leftCorners, status, cv::Size(6,3), 3, 5, 0.0f, 64.0f, false);
|
|
ASSERT_EQ(rightCorners.size(), 3);
|
|
ASSERT_EQ(status.size(), 3);
|
|
ASSERT_TRUE(status[0]);
|
|
ASSERT_TRUE(status[1]);
|
|
ASSERT_FALSE(status[2]);
|
|
ASSERT_NEAR(leftCorners[0].x - rightCorners[0].x, 5.0f, 0.1f);
|
|
ASSERT_NEAR(leftCorners[1].x - rightCorners[1].x, 6.75f, 0.1f);
|
|
}
|
|
|
|
TEST(Util2dTest, CalcOpticalFlowPyrLKStereo)
|
|
{
|
|
// Create synthetic images
|
|
cv::Mat left = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
cv::Mat right = cv::Mat::zeros(100, 100, CV_8UC1);
|
|
|
|
// Draw a white dot in both images, shifted by 5 pixels in x-direction and 0.5 pixel in y-direction
|
|
cv::Point2f leftPoint(50, 50);
|
|
cv::Point2f rightPoint = leftPoint - cv::Point2f(5.0f, 0.5f);
|
|
|
|
left.at<uchar>((int)leftPoint.y, (int)leftPoint.x) = 255;
|
|
right.at<uchar>((int)rightPoint.y, (int)rightPoint.x) = 255;
|
|
|
|
std::vector<cv::Point2f> prevPts = { leftPoint };
|
|
std::vector<cv::Point2f> nextPts;
|
|
|
|
cv::Mat status, err;
|
|
|
|
// Run the stereo optical flow function
|
|
util2d::calcOpticalFlowPyrLKStereo(
|
|
left, right,
|
|
prevPts, nextPts,
|
|
status, err,
|
|
cv::Size(21, 21), // window size
|
|
3, // max level
|
|
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01),
|
|
0, // no flags
|
|
1e-4 // minEigThreshold
|
|
);
|
|
|
|
ASSERT_EQ(status.at<uchar>(0), 1) << "Optical flow failed.";
|
|
ASSERT_NEAR(nextPts[0].y, leftPoint.y, 0.1f) << "Y position should remain constant.";
|
|
ASSERT_NEAR(leftPoint.x - nextPts[0].x, 5.0f, 0.1f) << "X displacement should be close to 5.";
|
|
}
|
|
|
|
TEST(Util2dTest, DisparityFromStereoImages)
|
|
{
|
|
// Check inputs
|
|
ASSERT_THROW(util2d::disparityFromStereoImages(cv::Mat(), cv::Mat(2,2,CV_8UC1)), UException);
|
|
ASSERT_THROW(util2d::disparityFromStereoImages(cv::Mat(2,2,CV_8UC1), cv::Mat()), UException);
|
|
ASSERT_THROW(util2d::disparityFromStereoImages(cv::Mat(2,2,CV_8UC1), cv::Mat(3,3,CV_8UC1)), UException);
|
|
ASSERT_THROW(util2d::disparityFromStereoImages(cv::Mat(2,2,CV_8UC3), cv::Mat(2,2,CV_8UC3)), UException);
|
|
|
|
cv::Mat left = cv::imread(std::string(RTABMAP_TEST_DATA_ROOT)+"/stereo_rect/left/50.jpg");
|
|
cv::Mat right = cv::imread(std::string(RTABMAP_TEST_DATA_ROOT)+"/stereo_rect/right/50.jpg", cv::IMREAD_GRAYSCALE);
|
|
|
|
cv::Mat disparity = util2d::disparityFromStereoImages(left, right);
|
|
|
|
EXPECT_FALSE(disparity.empty());
|
|
EXPECT_EQ(disparity.size(), left.size());
|
|
EXPECT_EQ(disparity.type(), CV_16SC1);
|
|
|
|
cv::Rect rect(300, 400, 40, 40);
|
|
cv::Mat roi = disparity(rect);
|
|
|
|
// Check if all values in ROI are close to 10 (float comparison)
|
|
double minVal, maxVal;
|
|
cv::minMaxLoc(roi, &minVal, &maxVal);
|
|
|
|
EXPECT_NEAR(minVal, 313, 0.5);
|
|
EXPECT_NEAR(maxVal, 388, 0.5);
|
|
}
|
|
|
|
TEST(Util2dTest, DepthFromDisparityFloat32) {
|
|
const int rows = 2;
|
|
const int cols = 3;
|
|
const float fx = 500.0f; // focal length in pixels
|
|
const float baseline = 0.1f; // 10 cm baseline
|
|
const float disparityValue = 25.0f;
|
|
|
|
cv::Mat disparity(rows, cols, CV_32FC1, cv::Scalar(disparityValue));
|
|
cv::Mat depth = util2d::depthFromDisparity(disparity, fx, baseline, CV_32FC1);
|
|
|
|
ASSERT_EQ(depth.rows, rows);
|
|
ASSERT_EQ(depth.cols, cols);
|
|
ASSERT_EQ(depth.type(), CV_32FC1);
|
|
|
|
float expectedDepth = baseline * fx / disparityValue; // = 0.1 * 500 / 25 = 2.0
|
|
for (int i = 0; i < rows; ++i) {
|
|
for (int j = 0; j < cols; ++j) {
|
|
EXPECT_NEAR(depth.at<float>(i, j), expectedDepth, 1e-5);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, DepthFromDisparityUInt16) {
|
|
const int rows = 2;
|
|
const int cols = 3;
|
|
const float fx = 500.0f;
|
|
const float baseline = 0.1f;
|
|
const float disparityValue = 25.0f;
|
|
|
|
cv::Mat disparity(rows, cols, CV_32FC1, cv::Scalar(disparityValue));
|
|
cv::Mat depth = util2d::depthFromDisparity(disparity, fx, baseline, CV_16UC1);
|
|
|
|
ASSERT_EQ(depth.rows, rows);
|
|
ASSERT_EQ(depth.cols, cols);
|
|
ASSERT_EQ(depth.type(), CV_16UC1);
|
|
|
|
unsigned short expectedDepth = static_cast<unsigned short>((baseline * fx / disparityValue) * 1000); // in mm
|
|
for (int i = 0; i < rows; ++i) {
|
|
for (int j = 0; j < cols; ++j) {
|
|
EXPECT_EQ(depth.at<unsigned short>(i, j), expectedDepth);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, DepthFromStereoImages)
|
|
{
|
|
// Parameters
|
|
const int width = 20;
|
|
const int height = 20;
|
|
float fx = 100.0f; // focal length in pixels
|
|
float baseline = 0.1f; // 10 cm
|
|
int flowWinSize = 5;
|
|
int flowMaxLevel = 3;
|
|
int flowIterations = 10;
|
|
double flowEps = 0.01;
|
|
|
|
// Create synthetic stereo images
|
|
cv::Mat leftImage = cv::Mat::zeros(height, width, CV_8UC1);
|
|
cv::Mat rightImage = cv::Mat::zeros(height, width, CV_8UC1);
|
|
|
|
// Place a dot at (10,10) in the left image and simulate a disparity of 5 pixels
|
|
leftImage.at<uchar>(10, 10) = 255;
|
|
rightImage.at<uchar>(10, 5) = 255;
|
|
|
|
// Known point to track
|
|
std::vector<cv::Point2f> leftCorners = {cv::Point2f(10.0f, 10.0f)};
|
|
|
|
// Call the function under test
|
|
cv::Mat depth = util2d::depthFromStereoImages(
|
|
leftImage,
|
|
rightImage,
|
|
leftCorners,
|
|
fx,
|
|
baseline,
|
|
flowWinSize,
|
|
flowMaxLevel,
|
|
flowIterations,
|
|
flowEps
|
|
);
|
|
|
|
// Check result
|
|
float expectedDisparity = 5.0f;
|
|
float expectedDepth = (fx * baseline) / expectedDisparity;
|
|
float actualDepth = depth.at<float>(10, 10);
|
|
|
|
ASSERT_NEAR(actualDepth, expectedDepth, 1e-3);
|
|
}
|
|
|
|
TEST(Util2dTest, DisparityFromStereoCorrespondencesDisparityComputation)
|
|
{
|
|
// Test setup
|
|
cv::Size disparitySize(5, 5);
|
|
std::vector<cv::Point2f> leftCorners = {
|
|
cv::Point2f(1.0f, 1.0f),
|
|
cv::Point2f(2.0f, 1.0f),
|
|
cv::Point2f(3.0f, 1.0f),
|
|
cv::Point2f(4.0f, 1.0f)
|
|
};
|
|
std::vector<cv::Point2f> rightCorners = {
|
|
cv::Point2f(0.5f, 1.0f),
|
|
cv::Point2f(1.5f, 1.0f),
|
|
cv::Point2f(2.5f, 1.0f),
|
|
cv::Point2f(3.5f, 1.0f)
|
|
};
|
|
std::vector<unsigned char> mask = {1, 1, 1, 1};
|
|
|
|
// Call the function to test
|
|
cv::Mat disparity = util2d::disparityFromStereoCorrespondences(disparitySize, leftCorners, rightCorners, mask);
|
|
|
|
// Verify the disparity values
|
|
EXPECT_EQ(disparity.at<float>(1, 1), 0.5f); // 1.0f - 0.5f = 0.5f
|
|
EXPECT_EQ(disparity.at<float>(2, 1), 0.5f); // 2.0f - 1.5f = 0.5f
|
|
EXPECT_EQ(disparity.at<float>(3, 1), 0.5f); // 3.0f - 2.5f = 0.5f
|
|
EXPECT_EQ(disparity.at<float>(4, 1), 0.5f); // 4.0f - 3.5f = 0.5f
|
|
|
|
// Check that locations with no disparity (e.g., the first col) remain 0
|
|
EXPECT_EQ(disparity.at<float>(0, 0), 0.0f);
|
|
EXPECT_EQ(disparity.at<float>(1, 0), 0.0f);
|
|
EXPECT_EQ(disparity.at<float>(2, 0), 0.0f);
|
|
EXPECT_EQ(disparity.at<float>(3, 0), 0.0f);
|
|
EXPECT_EQ(disparity.at<float>(4, 0), 0.0f);
|
|
}
|
|
|
|
TEST(Util2dTest, DisparityFromStereoCorrespondencesEmptyMask)
|
|
{
|
|
// Test with empty mask (all points should be included)
|
|
cv::Size disparitySize(4, 4);
|
|
std::vector<cv::Point2f> leftCorners = {
|
|
cv::Point2f(1.0f, 1.0f),
|
|
cv::Point2f(2.0f, 1.0f),
|
|
cv::Point2f(3.0f, 1.0f)
|
|
};
|
|
std::vector<cv::Point2f> rightCorners = {
|
|
cv::Point2f(0.5f, 1.0f),
|
|
cv::Point2f(1.5f, 1.0f),
|
|
cv::Point2f(2.5f, 1.0f)
|
|
};
|
|
std::vector<unsigned char> mask;
|
|
|
|
// Call the function
|
|
cv::Mat disparity = util2d::disparityFromStereoCorrespondences(disparitySize, leftCorners, rightCorners, mask);
|
|
|
|
// Verify the disparity values for all points
|
|
EXPECT_EQ(disparity.at<float>(1, 1), 0.5f); // 1.0f - 0.5f = 0.5f
|
|
EXPECT_EQ(disparity.at<float>(2, 1), 0.5f); // 2.0f - 1.5f = 0.5f
|
|
EXPECT_EQ(disparity.at<float>(3, 1), 0.5f); // 3.0f - 2.5f = 0.5f
|
|
}
|
|
|
|
TEST(Util2dTest, DisparityFromStereoCorrespondencesInconsistentCornersSize)
|
|
{
|
|
// Test with inconsistent corners size (should fail)
|
|
cv::Size disparitySize(5, 5);
|
|
std::vector<cv::Point2f> leftCorners = {
|
|
cv::Point2f(1.0f, 1.0f),
|
|
cv::Point2f(2.0f, 1.0f)
|
|
};
|
|
std::vector<cv::Point2f> rightCorners = {
|
|
cv::Point2f(0.5f, 1.0f)
|
|
};
|
|
std::vector<unsigned char> mask = {1};
|
|
|
|
ASSERT_THROW(util2d::disparityFromStereoCorrespondences(disparitySize, leftCorners, rightCorners, mask), UException);
|
|
rightCorners.push_back(cv::Point2f(1.5f,1.0f));
|
|
ASSERT_THROW(util2d::disparityFromStereoCorrespondences(disparitySize, leftCorners, rightCorners, mask), UException);
|
|
mask.push_back(1);
|
|
leftCorners.back().x = -2;
|
|
ASSERT_THROW(util2d::disparityFromStereoCorrespondences(disparitySize, leftCorners, rightCorners, mask), UException);
|
|
leftCorners.back().x = 6;
|
|
ASSERT_THROW(util2d::disparityFromStereoCorrespondences(disparitySize, leftCorners, rightCorners, mask), UException);
|
|
}
|
|
|
|
TEST(Util2dTest, DepthFromStereoCorrespondences)
|
|
{
|
|
// Create a dummy left image (only used for size)
|
|
cv::Mat leftImage = cv::Mat::zeros(10, 10, CV_8UC1);
|
|
|
|
// Define synthetic stereo correspondences
|
|
std::vector<cv::Point2f> leftCorners = {
|
|
cv::Point2f(5.0f, 5.0f)
|
|
};
|
|
std::vector<cv::Point2f> rightCorners = {
|
|
cv::Point2f(2.0f, 5.0f) // disparity = 3.0
|
|
};
|
|
std::vector<unsigned char> mask = {1};
|
|
|
|
// Intrinsics
|
|
float fx = 100.0f; // focal length in pixels
|
|
float baseline = 0.1f; // 10 cm
|
|
|
|
// Expected depth = fx * baseline / disparity = 100 * 0.1 / 3 = 3.333...
|
|
float expectedDepth = (fx * baseline) / (leftCorners[0].x - rightCorners[0].x);
|
|
|
|
// Call function
|
|
cv::Mat depth = util2d::depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, mask, fx, baseline);
|
|
|
|
// Check result
|
|
ASSERT_EQ(depth.type(), CV_32FC1);
|
|
float actualDepth = depth.at<float>(5, 5); // rounded position from input
|
|
ASSERT_NEAR(actualDepth, expectedDepth, 1e-4);
|
|
}
|
|
|
|
TEST(Util2dTest, CvtDepthFromFloat) {
|
|
// Create a simple 3x3 depth image in meters
|
|
cv::Mat depth32F = (cv::Mat_<float>(3, 3) <<
|
|
0.5f, 1.0f, 1.5f,
|
|
2.0f, 3.0f, 4.0f,
|
|
5.0f, 6.0f, 7.0f); // in meters
|
|
|
|
// Convert to 16-bit unsigned short (mm)
|
|
cv::Mat depth16U = util2d::cvtDepthFromFloat(depth32F);
|
|
|
|
ASSERT_EQ(depth16U.type(), CV_16UC1);
|
|
ASSERT_EQ(depth16U.rows, 3);
|
|
ASSERT_EQ(depth16U.cols, 3);
|
|
|
|
EXPECT_EQ(depth16U.at<unsigned short>(0, 0), 500); // 0.5m -> 500mm
|
|
EXPECT_EQ(depth16U.at<unsigned short>(0, 1), 1000); // 1.0m -> 1000mm
|
|
EXPECT_EQ(depth16U.at<unsigned short>(2, 2), 7000); // 7.0m -> 7000mm
|
|
}
|
|
|
|
TEST(Util2dTest, CvtDepthToFloat) {
|
|
// Create a simple 3x3 depth image in millimeters
|
|
cv::Mat depth16U = (cv::Mat_<unsigned short>(3, 3) <<
|
|
500, 1000, 1500,
|
|
2000, 3000, 4000,
|
|
5000, 6000, 7000); // in mm
|
|
|
|
// Convert to 32-bit float (meters)
|
|
cv::Mat depth32F = util2d::cvtDepthToFloat(depth16U);
|
|
|
|
ASSERT_EQ(depth32F.type(), CV_32FC1);
|
|
ASSERT_EQ(depth32F.rows, 3);
|
|
ASSERT_EQ(depth32F.cols, 3);
|
|
|
|
EXPECT_FLOAT_EQ(depth32F.at<float>(0, 0), 0.5f); // 500mm -> 0.5m
|
|
EXPECT_FLOAT_EQ(depth32F.at<float>(0, 1), 1.0f); // 1000mm -> 1.0m
|
|
EXPECT_FLOAT_EQ(depth32F.at<float>(2, 2), 7.0f); // 7000mm -> 7.0m
|
|
}
|
|
|
|
TEST(Util2dTest, CvtDepthRoundTripConversion) {
|
|
// Test round-trip conversion
|
|
cv::Mat original = (cv::Mat_<float>(2, 2) << 0.25f, 1.0f, 2.5f, 3.3f);
|
|
|
|
cv::Mat depth16U = util2d::cvtDepthFromFloat(original);
|
|
cv::Mat depth32F = util2d::cvtDepthToFloat(depth16U);
|
|
|
|
for (int i = 0; i < original.rows; ++i) {
|
|
for (int j = 0; j < original.cols; ++j) {
|
|
float expected = std::floor(original.at<float>(i, j) * 1000.0f + 0.5f) / 1000.0f;
|
|
EXPECT_NEAR(depth32F.at<float>(i, j), expected, 0.001);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, GetDepthCenterDepthValue32FNoSmoothingNoEstimation) {
|
|
cv::Mat depth = cv::Mat::zeros(5, 5, CV_32FC1);
|
|
depth.at<float>(2, 2) = 1.5f;
|
|
|
|
float result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.1f, false);
|
|
EXPECT_FLOAT_EQ(result, 1.5f);
|
|
}
|
|
|
|
TEST(Util2dTest, GetDepthCenterDepthValue16UNoSmoothingNoEstimation) {
|
|
cv::Mat depth = cv::Mat::zeros(5, 5, CV_16UC1);
|
|
depth.at<unsigned short>(2, 2) = 1500; // 1.5 meters
|
|
|
|
float result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.1f, false);
|
|
EXPECT_FLOAT_EQ(result, 1.5f);
|
|
}
|
|
|
|
TEST(Util2dTest, GetDepthSmoothing32F) {
|
|
cv::Mat depth = cv::Mat::zeros(5, 5, CV_32FC1);
|
|
depth.at<float>(2, 2) = 1.0f;
|
|
depth.at<float>(2, 1) = 1.0f;
|
|
depth.at<float>(1, 2) = 1.0f;
|
|
depth.at<float>(2, 3) = 1.0f;
|
|
depth.at<float>(3, 2) = 1.0f;
|
|
|
|
float result = util2d::getDepth(depth, 2.0f, 2.0f, true, 0.1f, false);
|
|
EXPECT_NEAR(result, 1.0f, 1e-5f);
|
|
}
|
|
|
|
TEST(Util2dTest, GetDepthEstimationFromNeighbors16U) {
|
|
cv::Mat depth = cv::Mat::zeros(5, 5, CV_16UC1);
|
|
depth.at<unsigned short>(2, 1) = 1500; // 1.5m
|
|
depth.at<unsigned short>(1, 2) = 1500;
|
|
|
|
float result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.1f, true);
|
|
EXPECT_NEAR(result, 1.5f, 1e-3f);
|
|
}
|
|
|
|
TEST(Util2dTest, GetDepthEstimationFromNeighborsRejectsOutlier) {
|
|
// Neighbors are visited as (2,1), (1,2), (3,2), (2,3). The last one is
|
|
// 25% away from the mean of the first three and must be ignored.
|
|
cv::Mat depth = cv::Mat::zeros(5, 5, CV_32FC1);
|
|
depth.at<float>(2, 1) = 1.00f;
|
|
depth.at<float>(1, 2) = 1.02f;
|
|
depth.at<float>(3, 2) = 0.98f;
|
|
depth.at<float>(2, 3) = 1.25f;
|
|
|
|
float result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.1f, true);
|
|
EXPECT_NEAR(result, 1.0f, 1e-5f);
|
|
|
|
cv::Mat depth16U = util2d::cvtDepthFromFloat(depth);
|
|
result = util2d::getDepth(depth16U, 2.0f, 2.0f, false, 0.1f, true);
|
|
EXPECT_NEAR(result, 1.0f, 1e-3f);
|
|
|
|
// Same with the default ratio (0.02): the last neighbor is 5% away.
|
|
depth.at<float>(2, 1) = 1.00f;
|
|
depth.at<float>(1, 2) = 1.01f;
|
|
depth.at<float>(3, 2) = 0.99f;
|
|
depth.at<float>(2, 3) = 1.05f;
|
|
result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.02f, true);
|
|
EXPECT_NEAR(result, 1.0f, 1e-5f);
|
|
}
|
|
|
|
TEST(Util2dTest, GetDepthOutOfBounds) {
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
|
|
|
|
float result = util2d::getDepth(depth, 5.5f, 5.5f, false, 0.1f, false);
|
|
EXPECT_FLOAT_EQ(result, 0.0f);
|
|
}
|
|
|
|
TEST(Util2dTest, ComputeRoiValidStringInput)
|
|
{
|
|
cv::Size imageSize(200, 100);
|
|
cv::Mat image = cv::Mat::zeros(imageSize, CV_8UC1);
|
|
cv::Rect roi = util2d::computeRoi(image, "0.1 0.1 0.2 0.2");
|
|
EXPECT_EQ(roi.x, 20);
|
|
EXPECT_EQ(roi.width, 160);
|
|
EXPECT_EQ(roi.y, 20);
|
|
EXPECT_EQ(roi.height, 60);
|
|
roi = util2d::computeRoi(imageSize, "0.1 0.1 0.2 0.2");
|
|
EXPECT_EQ(roi.x, 20);
|
|
EXPECT_EQ(roi.width, 160);
|
|
EXPECT_EQ(roi.y, 20);
|
|
EXPECT_EQ(roi.height, 60);
|
|
}
|
|
|
|
TEST(Util2dTest, ComputeRoiInvalidStringInput)
|
|
{
|
|
cv::Mat image = cv::Mat::zeros(100, 200, CV_8UC1);
|
|
cv::Rect roi = util2d::computeRoi(image, "0.5 0.6"); // Invalid format
|
|
EXPECT_EQ(roi, cv::Rect()); // Expect empty ROI
|
|
}
|
|
|
|
TEST(Util2dTest, ComputeRoiValidVectorInput)
|
|
{
|
|
cv::Size imageSize(300, 150);
|
|
cv::Mat image = cv::Mat::zeros(imageSize, CV_8UC1);
|
|
std::vector<float> ratios = {0.1f, 0.2f, 0.1f, 0.1f};
|
|
cv::Rect roi = util2d::computeRoi(image, ratios);
|
|
EXPECT_EQ(roi.x, 30);
|
|
EXPECT_EQ(roi.width, 210);
|
|
EXPECT_EQ(roi.y, 15);
|
|
EXPECT_EQ(roi.height, 120);
|
|
roi = util2d::computeRoi(imageSize, ratios);
|
|
EXPECT_EQ(roi.x, 30);
|
|
EXPECT_EQ(roi.width, 210);
|
|
EXPECT_EQ(roi.y, 15);
|
|
EXPECT_EQ(roi.height, 120);
|
|
}
|
|
|
|
TEST(Util2dTest, ComputeRoiInvalidVectorSize)
|
|
{
|
|
cv::Size imageSize(300, 150);
|
|
std::vector<float> invalidRatios = {0.1f, 0.2f}; // Invalid size
|
|
cv::Rect roi = util2d::computeRoi(imageSize, invalidRatios);
|
|
EXPECT_EQ(roi, cv::Rect());
|
|
}
|
|
|
|
TEST(Util2dTest, ComputeRoiZeroImageSize)
|
|
{
|
|
cv::Size imageSize(0, 0);
|
|
cv::Mat image = cv::Mat::zeros(imageSize, CV_8UC1);
|
|
std::vector<float> ratios = {0.1f, 0.1f, 0.1f, 0.1f};
|
|
cv::Rect roi = util2d::computeRoi(image, ratios);
|
|
EXPECT_EQ(roi, cv::Rect());
|
|
roi = util2d::computeRoi(imageSize, ratios);
|
|
EXPECT_EQ(roi, cv::Rect());
|
|
}
|
|
|
|
TEST(Util2dTest, DecimateFloatDepthImage)
|
|
{
|
|
// Create a 4x4 depth image with increasing values
|
|
cv::Mat depth = (cv::Mat_<float>(4, 4) <<
|
|
1, 2, 3, 4,
|
|
5, 6, 7, 8,
|
|
9,10,11,12,
|
|
13,14,15,16);
|
|
|
|
// Decimate by 2
|
|
cv::Mat decimated = util2d::decimate(depth, 2);
|
|
|
|
ASSERT_EQ(decimated.rows, 2);
|
|
ASSERT_EQ(decimated.cols, 2);
|
|
EXPECT_FLOAT_EQ(decimated.at<float>(0,0), 1);
|
|
EXPECT_FLOAT_EQ(decimated.at<float>(0,1), 3);
|
|
EXPECT_FLOAT_EQ(decimated.at<float>(1,0), 9);
|
|
EXPECT_FLOAT_EQ(decimated.at<float>(1,1), 11);
|
|
}
|
|
|
|
TEST(Util2dTest, Decimate16UDepthImage)
|
|
{
|
|
cv::Mat depth = (cv::Mat_<uint16_t>(4, 4) <<
|
|
100, 200, 300, 400,
|
|
500, 600, 700, 800,
|
|
900,1000,1100,1200,
|
|
1300,1400,1500,1600);
|
|
|
|
cv::Mat decimated = util2d::decimate(depth, 2);
|
|
|
|
ASSERT_EQ(decimated.rows, 2);
|
|
ASSERT_EQ(decimated.cols, 2);
|
|
EXPECT_EQ(decimated.at<uint16_t>(0,0), 100);
|
|
EXPECT_EQ(decimated.at<uint16_t>(0,1), 300);
|
|
EXPECT_EQ(decimated.at<uint16_t>(1,0), 900);
|
|
EXPECT_EQ(decimated.at<uint16_t>(1,1), 1100);
|
|
}
|
|
|
|
TEST(Util2dTest, InterpolateFloatDepthImage)
|
|
{
|
|
// Create a simple 2x2 image to interpolate into 4x4
|
|
cv::Mat input = (cv::Mat_<float>(2,2) <<
|
|
1.0f, 1.0f,
|
|
1.0f, 1.0f);
|
|
|
|
int factor = 2;
|
|
float depthErrorRatio = 0.1f;
|
|
|
|
cv::Mat interpolated = util2d::interpolate(input, factor, depthErrorRatio);
|
|
|
|
ASSERT_EQ(interpolated.rows, 4);
|
|
ASSERT_EQ(interpolated.cols, 4);
|
|
|
|
// All interpolated values should still be 1.0f
|
|
for (int r = 0; r < 3; ++r)
|
|
{
|
|
for (int c = 0; c < 3; ++c)
|
|
{
|
|
EXPECT_FLOAT_EQ(interpolated.at<float>(r, c), 1.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, Interpolate16UDepthImage)
|
|
{
|
|
cv::Mat input = (cv::Mat_<uint16_t>(2,2) <<
|
|
1000, 1000,
|
|
1000, 1000);
|
|
|
|
int factor = 2;
|
|
float depthErrorRatio = 0.1f;
|
|
|
|
cv::Mat interpolated = util2d::interpolate(input, factor, depthErrorRatio);
|
|
|
|
ASSERT_EQ(interpolated.rows, 4);
|
|
ASSERT_EQ(interpolated.cols, 4);
|
|
|
|
for (int r = 0; r < 3; ++r)
|
|
{
|
|
for (int c = 0; c < 3; ++c)
|
|
{
|
|
EXPECT_EQ(interpolated.at<uint16_t>(r, c), 1000);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, RegisterDepth)
|
|
{
|
|
// Create a 2x2 synthetic depth image in meters
|
|
cv::Mat depth = (cv::Mat_<float>(2,2) << 1.0f, 1.0f,
|
|
1.0f, 1.0f);
|
|
cv::Mat depthK = (cv::Mat_<double>(3,3) <<
|
|
1.0, 0, 0.5,
|
|
0, 1.0, 0.5,
|
|
0, 0, 1.0);
|
|
|
|
// RGB image size (same size for simplicity)
|
|
cv::Size colorSize = depth.size();
|
|
cv::Mat colorK = depthK.clone();
|
|
|
|
// Identity transform (depth and color cameras are perfectly aligned)
|
|
rtabmap::Transform transform = rtabmap::Transform::getIdentity(); // default is identity
|
|
|
|
// Run registration
|
|
cv::Mat registered = util2d::registerDepth(depth, depthK, colorSize, colorK, transform);
|
|
|
|
// Check that the output is the same as input since transform is identity and intrinsics match
|
|
ASSERT_EQ(registered.type(), depth.type());
|
|
ASSERT_EQ(registered.size(), colorSize);
|
|
|
|
for (int y = 0; y < registered.rows; ++y)
|
|
{
|
|
for (int x = 0; x < registered.cols; ++x)
|
|
{
|
|
EXPECT_FLOAT_EQ(registered.at<float>(y, x), 1.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, RegisterDepthWithOverlap)
|
|
{
|
|
cv::Mat depth = cv::Mat::ones(11,11,CV_32FC1);
|
|
cv::Mat depthK = (cv::Mat_<double>(3,3) <<
|
|
10.0, 0, 5,
|
|
0, 10.0, 5,
|
|
0, 0, 1.0);
|
|
|
|
cv::Size colorSize = depth.size();
|
|
cv::Mat colorK = depthK;
|
|
|
|
// move the color camera right and rotate to left (optical frame)
|
|
rtabmap::Transform transform = rtabmap::Transform(1.5,0.0,1.0,0,-M_PI/2,0);
|
|
|
|
// Run registration
|
|
cv::Mat registered = util2d::registerDepth(depth, depthK, colorSize, colorK, transform.inverse());
|
|
|
|
ASSERT_EQ(registered.type(), depth.type());
|
|
ASSERT_EQ(registered.size(), colorSize);
|
|
|
|
// last column from original should match the middle column of registered
|
|
// all other columns should be 0
|
|
for (int y = 0; y < registered.rows; ++y)
|
|
{
|
|
for (int x = 0; x < registered.cols; ++x)
|
|
{
|
|
EXPECT_FLOAT_EQ(registered.at<float>(y, x), x != registered.cols/2 ? 0.0f : 1.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FillDepthHoles) {
|
|
cv::Mat depth = (cv::Mat_<float>(3,3) <<
|
|
1, 0, 2,
|
|
0, 3, 0,
|
|
2, 0, 4);
|
|
|
|
int maximumHoleSize = 2;
|
|
float errorRatio = 1.0f;
|
|
|
|
cv::Mat filledDepth = util2d::fillDepthHoles(depth, maximumHoleSize, errorRatio);
|
|
|
|
EXPECT_EQ(filledDepth.size(), depth.size());
|
|
EXPECT_EQ(filledDepth.type(), depth.type());
|
|
|
|
cv::Mat expected = (cv::Mat_<float>(3,3) <<
|
|
1, 1.5, 2,
|
|
1.5, 3, 0,
|
|
2, 0, 4);
|
|
|
|
for (int i = 0; i < filledDepth.rows; i++) {
|
|
for (int j = 0; j < filledDepth.cols; j++) {
|
|
EXPECT_EQ(filledDepth.at<float>(i, j), expected.at<float>(i,j));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FillDepthHolesLargeHoleTest) {
|
|
|
|
cv::Mat depth = (cv::Mat_<float>(5,5) <<
|
|
1, 0, 2, 3, 5,
|
|
0, 3, 0, 0, 4,
|
|
2, 0, 4, 2, 3,
|
|
1, 0, 0, 0, 2,
|
|
0, 3, 0, 9, 7);
|
|
cv::Mat expected = (cv::Mat_<float>(5,5) <<
|
|
1, 1.5, 2, 3, 5,
|
|
1.5, 3, 3.16, 3.08, 4,
|
|
2, 3, 4, 2, 3,
|
|
1, 3, 0, 0, 2,
|
|
0, 3, 0, 9, 7);
|
|
|
|
int maximumHoleSize = 2;
|
|
float errorRatio = 1.0f;
|
|
|
|
cv::Mat filledDepth = util2d::fillDepthHoles(depth, maximumHoleSize, errorRatio);
|
|
|
|
EXPECT_EQ(filledDepth.size(), depth.size());
|
|
EXPECT_EQ(filledDepth.type(), depth.type());
|
|
|
|
for (int i = 0; i < filledDepth.rows; i++) {
|
|
for (int j = 0; j < filledDepth.cols; j++) {
|
|
EXPECT_NEAR(filledDepth.at<float>(i, j), expected.at<float>(i,j), 0.1f);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FillDepthHolesInvalidInputTest) {
|
|
cv::Mat depth(5, 5, CV_32FC1);
|
|
|
|
// Test with invalid maximumHoleSize (<= 0)
|
|
int maximumHoleSize = 0;
|
|
float errorRatio = 0.1f;
|
|
EXPECT_THROW(util2d::fillDepthHoles(depth, maximumHoleSize, errorRatio), UException);
|
|
}
|
|
|
|
cv::Mat createTestDepthImage()
|
|
{
|
|
cv::Mat img = (cv::Mat_<unsigned short>(5, 5) <<
|
|
1000, 0, 1010, 0, 1020,
|
|
0, 0, 0, 0, 0,
|
|
1005, 0, 1015, 0, 1025,
|
|
0, 0, 0, 0, 0,
|
|
1010, 0, 1020, 0, 1030);
|
|
return img;
|
|
}
|
|
|
|
TEST(Util2dTest, FillRegisteredDepthHolesVerticalFilling)
|
|
{
|
|
cv::Mat input = createTestDepthImage();
|
|
cv::Mat original = input.clone();
|
|
|
|
util2d::fillRegisteredDepthHoles(input, true, false, false); // Vertical only
|
|
|
|
for (int i = 0; i < input.rows; i++) {
|
|
for (int j = 0; j < input.cols; j++) {
|
|
if((i==1 && j==2) || (i==3 && j==2)) {
|
|
// only middle column should have now all valid values
|
|
EXPECT_GT(input.at<unsigned short>(i, j), 0);
|
|
}
|
|
else {
|
|
EXPECT_EQ(input.at<unsigned short>(i, j), original.at<unsigned short>(i,j));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FillRegisteredDepthHolesHorizontalFilling)
|
|
{
|
|
cv::Mat input = createTestDepthImage();
|
|
cv::Mat original = input.clone();
|
|
|
|
util2d::fillRegisteredDepthHoles(input, false, true, false); // Horizontal only
|
|
|
|
for (int i = 0; i < input.rows; i++) {
|
|
for (int j = 0; j < input.cols; j++) {
|
|
if((i==2 && j==1) || (i==2 && j==3)) {
|
|
// only middle row should have now all valid values
|
|
EXPECT_GT(input.at<unsigned short>(i, j), 0);
|
|
}
|
|
else {
|
|
EXPECT_EQ(input.at<unsigned short>(i, j), original.at<unsigned short>(i,j));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FillRegisteredDepthHolesDoubleHoleFilling)
|
|
{
|
|
cv::Mat input = (cv::Mat_<unsigned short>(5, 5) <<
|
|
1000, 1005, 0, 1020, 1030,
|
|
0, 1010, 0, 0, 1015,
|
|
1010, 0, 0, 0, 1020,
|
|
1015, 0, 0, 1015, 1025,
|
|
1010, 1015, 1020, 1025, 1030);
|
|
|
|
cv::Mat expected = (cv::Mat_<unsigned short>(5, 5) <<
|
|
1000, 1005, 0, 1020, 1030,
|
|
0, 1010, 1011, 1013, 1015,
|
|
1010, 1011, 1013, 0, 1020, // <- Note that when double filling is on, there is a double contour
|
|
1015, 1013, 1017, 1015, 1025,
|
|
1010, 1015, 1020, 1025, 1030);
|
|
|
|
util2d::fillRegisteredDepthHoles(input, true, true, true); // Fill double holes too
|
|
|
|
for (int i = 0; i < input.rows; i++) {
|
|
for (int j = 0; j < input.cols; j++) {
|
|
EXPECT_EQ(input.at<unsigned short>(i, j), expected.at<unsigned short>(i,j));
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FastBilateralFilteringEmptyInput) {
|
|
cv::Mat empty;
|
|
EXPECT_THROW(util2d::fastBilateralFiltering(empty, 2.0f, 0.1f, false), UException);
|
|
}
|
|
|
|
TEST(Util2dTest, FastBilateralFilteringAllZeroInput) {
|
|
cv::Mat depth = cv::Mat::zeros(10, 10, CV_32FC1);
|
|
cv::Mat result = util2d::fastBilateralFiltering(depth, 2.0f, 0.1f, false);
|
|
// All values should still be 0
|
|
for (int i = 0; i < result.rows; ++i) {
|
|
for (int j = 0; j < result.cols; ++j) {
|
|
EXPECT_EQ(result.at<float>(i, j), 0.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FastBilateralFilteringSimpleFloatInput) {
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1) * 1.0f;
|
|
depth.at<float>(2,2) = 1.05f;
|
|
cv::Mat result = util2d::fastBilateralFiltering(depth, 3.0f, 0.05f, false);
|
|
|
|
ASSERT_FALSE(result.empty());
|
|
EXPECT_EQ(result.type(), CV_32FC1);
|
|
EXPECT_EQ(result.rows, depth.rows);
|
|
EXPECT_EQ(result.cols, depth.cols);
|
|
|
|
// All values should still be close to 1.0 (since no variation)
|
|
for (int i = 0; i < result.rows; ++i) {
|
|
for (int j = 0; j < result.cols; ++j) {
|
|
EXPECT_NEAR(result.at<float>(i, j), 1.0f, 1e-2);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(Util2dTest, FastBilateralFilteringSimpleUShortInput) {
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_16UC1) * 1000; // 1 meter
|
|
depth.at<unsigned short>(2,2) = 1050;
|
|
cv::Mat result = util2d::fastBilateralFiltering(depth, 2.0f, 0.05f, true);
|
|
|
|
ASSERT_FALSE(result.empty());
|
|
EXPECT_EQ(result.type(), CV_32FC1);
|
|
EXPECT_EQ(result.rows, depth.rows);
|
|
EXPECT_EQ(result.cols, depth.cols);
|
|
|
|
for (int i = 0; i < result.rows; ++i) {
|
|
for (int j = 0; j < result.cols; ++j) {
|
|
EXPECT_NEAR(result.at<float>(i, j), 1.0f, 1e-2);
|
|
}
|
|
}
|
|
}
|
|
|
|
// High Depth Variation (should preserve edge due to sigmaR)
|
|
TEST(Util2dTest, FastBilateralFilteringPreservesEdgesWithLowSigmaR) {
|
|
// Create a step edge: left side = 1.0f, right side = 3.0f
|
|
cv::Mat depth = cv::Mat::ones(5, 10, CV_32FC1);
|
|
depth.colRange(5, 10).setTo(3.0f);
|
|
depth.at<float>(2,7) = 3.1f;
|
|
depth.at<float>(2,2) = 1.1f;
|
|
|
|
float sigmaS = 2.0f;
|
|
float sigmaR = 0.1f; // Low sigmaR to preserve edge
|
|
|
|
cv::Mat result = util2d::fastBilateralFiltering(depth, sigmaS, sigmaR, false);
|
|
|
|
ASSERT_FALSE(result.empty());
|
|
|
|
for (int y = 0; y < depth.rows; ++y) {
|
|
EXPECT_LT(result.at<float>(y, 4), 2.0f); // Should stay close to 1.0
|
|
EXPECT_GT(result.at<float>(y, 5), 2.0f); // Should stay close to 3.0
|
|
}
|
|
}
|
|
|
|
// High sigmaR (should blur across the edge)
|
|
TEST(Util2dTest, FastBilateralFilteringBlursEdgesWithHighSigmaR) {
|
|
cv::Mat depth = cv::Mat::ones(5, 10, CV_32FC1);
|
|
depth.colRange(5, 10).setTo(3.0f);
|
|
|
|
float sigmaS = 2.0f;
|
|
float sigmaR = 5.0f; // High sigmaR allows more blurring
|
|
|
|
cv::Mat result = util2d::fastBilateralFiltering(depth, sigmaS, sigmaR, false);
|
|
|
|
ASSERT_FALSE(result.empty());
|
|
|
|
// Middle column should be somewhere between 1.0 and 3.0
|
|
for (int y = 0; y < depth.rows; ++y) {
|
|
float val = result.at<float>(y, 5);
|
|
EXPECT_GT(val, 1.1f);
|
|
EXPECT_LT(val, 2.9f);
|
|
}
|
|
}
|
|
|
|
// Random Depth Values with NaNs or Invalids
|
|
TEST(Util2dTest, FastBilateralFilteringHandlesInvalidDepthValues) {
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
|
|
depth.at<float>(2, 2) = std::numeric_limits<float>::quiet_NaN();
|
|
depth.at<float>(1, 3) = -1.0f;
|
|
depth.at<float>(1, 2) = 1.1f;
|
|
|
|
cv::Mat result = util2d::fastBilateralFiltering(depth, 2.0f, 0.1f, false);
|
|
|
|
ASSERT_FALSE(result.empty());
|
|
|
|
// Make sure the invalid pixels are skipped, and output is still valid
|
|
for (int y = 0; y < result.rows; ++y) {
|
|
for (int x = 0; x < result.cols; ++x) {
|
|
float val = result.at<float>(y, x);
|
|
EXPECT_TRUE((val!=0.0f && val-1.0f<0.02f) || val==0.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Early Division Toggle
|
|
TEST(Util2dTest, FastBilateralFilteringEarlyDivisionOptionConsistency) {
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1) * 2.0f;
|
|
depth.at<float>(2,2) = 2.1f;
|
|
cv::Mat result1 = util2d::fastBilateralFiltering(depth, 2.0f, 0.1f, true);
|
|
cv::Mat result2 = util2d::fastBilateralFiltering(depth, 2.0f, 0.1f, false);
|
|
|
|
ASSERT_FALSE(result1.empty());
|
|
ASSERT_FALSE(result2.empty());
|
|
|
|
for (int y = 0; y < depth.rows; ++y) {
|
|
for (int x = 0; x < depth.cols; ++x) {
|
|
EXPECT_NEAR(result1.at<float>(y, x), result2.at<float>(y, x), 1e-3);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Test for empty input
|
|
TEST(Util2dTest, DepthBleedingFilteringHandlesEmptyInput)
|
|
{
|
|
cv::Mat empty;
|
|
EXPECT_NO_THROW(util2d::depthBleedingFiltering(empty, 0.1f));
|
|
}
|
|
|
|
// Test that borders are zeroed out
|
|
TEST(Util2dTest, DepthBleedingFilteringBordersAreZeroed)
|
|
{
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
|
|
util2d::depthBleedingFiltering(depth, 0.1f);
|
|
|
|
for(int i = 0; i < 5; ++i)
|
|
{
|
|
EXPECT_EQ(depth.at<float>(0, i), 0.0f);
|
|
EXPECT_EQ(depth.at<float>(4, i), 0.0f);
|
|
EXPECT_EQ(depth.at<float>(i, 0), 0.0f);
|
|
EXPECT_EQ(depth.at<float>(i, 4), 0.0f);
|
|
}
|
|
}
|
|
|
|
// Test that valid depths are not removed
|
|
TEST(Util2dTest, DepthBleedingFilteringKeepsValidDepths)
|
|
{
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
|
|
depth.at<float>(2,2) = 1.01f; // Within threshold of 0.1
|
|
util2d::depthBleedingFiltering(depth, 0.1f);
|
|
EXPECT_GT(depth.at<float>(2,2), 0.0f);
|
|
}
|
|
|
|
// Test that invalid depth is removed
|
|
TEST(Util2dTest, DepthBleedingFilteringFiltersInvalidDepths)
|
|
{
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
|
|
depth.at<float>(2,2) = 5.0f; // Large depth jump
|
|
util2d::depthBleedingFiltering(depth, 0.1f);
|
|
EXPECT_EQ(depth.at<float>(2,2), 0.0f);
|
|
}
|
|
|
|
// Repeat the above for CV_16UC1
|
|
TEST(Util2dTest, DepthBleedingFilteringFiltersInvalidDepths16U)
|
|
{
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_16UC1) * 1000; // 1.0m in mm
|
|
depth.at<uint16_t>(2,2) = 5000; // 5.0m
|
|
util2d::depthBleedingFiltering(depth, 0.1f);
|
|
EXPECT_EQ(depth.at<uint16_t>(2,2), 0);
|
|
}
|
|
|
|
TEST(Util2dTest, DepthBleedingFilteringKeepsValidDepths16U)
|
|
{
|
|
cv::Mat depth = cv::Mat::ones(5, 5, CV_16UC1) * 1000;
|
|
depth.at<uint16_t>(2,2) = 1090; // 0.09m difference, within 0.1m
|
|
util2d::depthBleedingFiltering(depth, 0.1f);
|
|
EXPECT_GT(depth.at<uint16_t>(2,2), 0);
|
|
}
|
|
|
|
TEST(Util2dTest, HSVtoRGBPureRed) {
|
|
float r, g, b;
|
|
util2d::HSVtoRGB(&r, &g, &b, 0.0f, 1.0f, 1.0f);
|
|
EXPECT_NEAR(r, 1.0f, 1e-4f);
|
|
EXPECT_NEAR(g, 0.0f, 1e-4f);
|
|
EXPECT_NEAR(b, 0.0f, 1e-4f);
|
|
}
|
|
|
|
TEST(Util2dTest, HSVtoRGBPureGreen) {
|
|
float r, g, b;
|
|
util2d::HSVtoRGB(&r, &g, &b, 120.0f, 1.0f, 1.0f);
|
|
EXPECT_NEAR(r, 0.0f, 1e-4f);
|
|
EXPECT_NEAR(g, 1.0f, 1e-4f);
|
|
EXPECT_NEAR(b, 0.0f, 1e-4f);
|
|
}
|
|
|
|
TEST(Util2dTest, HSVtoRGBPureBlue) {
|
|
float r, g, b;
|
|
util2d::HSVtoRGB(&r, &g, &b, 240.0f, 1.0f, 1.0f);
|
|
EXPECT_NEAR(r, 0.0f, 1e-4f);
|
|
EXPECT_NEAR(g, 0.0f, 1e-4f);
|
|
EXPECT_NEAR(b, 1.0f, 1e-4f);
|
|
}
|
|
|
|
TEST(Util2dTest, HSVtoRGBGrayscaleFromZeroSaturation) {
|
|
float r, g, b;
|
|
util2d::HSVtoRGB(&r, &g, &b, 0.0f, 0.0f, 0.5f);
|
|
EXPECT_NEAR(r, 0.5f, 1e-4f);
|
|
EXPECT_NEAR(g, 0.5f, 1e-4f);
|
|
EXPECT_NEAR(b, 0.5f, 1e-4f);
|
|
}
|
|
|
|
TEST(Util2dTest, HSVtoRGBHueWrapAround360) {
|
|
float r, g, b;
|
|
util2d::HSVtoRGB(&r, &g, &b, 360.0f, 1.0f, 1.0f); // Hue 360 = Hue 0
|
|
EXPECT_NEAR(r, 1.0f, 1e-4f);
|
|
EXPECT_NEAR(g, 0.0f, 1e-4f);
|
|
EXPECT_NEAR(b, 0.0f, 1e-4f);
|
|
}
|
|
|
|
TEST(Util2dTest, HSVtoRGBHalfSaturationHalfBrightness) {
|
|
float r, g, b;
|
|
util2d::HSVtoRGB(&r, &g, &b, 60.0f, 0.5f, 0.5f); // Yellowish
|
|
EXPECT_NEAR(r, 0.5f, 1e-4f);
|
|
EXPECT_NEAR(g, 0.5f, 1e-4f);
|
|
EXPECT_NEAR(b, 0.25f, 1e-4f);
|
|
}
|
|
|
|
TEST(Util2dTest, NMSKeepsStrongestKeypointOnly) {
|
|
std::vector<cv::KeyPoint> inputKeypoints = {
|
|
cv::KeyPoint(cv::Point2f(50, 50), 1.f, -1, 0.8f), // weaker
|
|
cv::KeyPoint(cv::Point2f(52, 52), 1.f, -1, 0.9f), // stronger, within dist_thresh
|
|
cv::KeyPoint(cv::Point2f(100, 100), 1.f, -1, 0.7f) // far enough
|
|
};
|
|
|
|
cv::Mat descriptorsIn = cv::Mat::eye(3, 256, CV_32F); // mock descriptors
|
|
std::vector<cv::KeyPoint> outputKeypoints;
|
|
cv::Mat descriptorsOut;
|
|
|
|
int dist_thresh = 5;
|
|
int width = 200;
|
|
int height = 200;
|
|
|
|
util2d::NMS(inputKeypoints, descriptorsIn, outputKeypoints, descriptorsOut, dist_thresh, width, height);
|
|
|
|
ASSERT_EQ(outputKeypoints.size(), 2);
|
|
EXPECT_EQ(outputKeypoints[0].pt, cv::Point2f(52, 52)); // stronger one in the cluster
|
|
EXPECT_EQ(outputKeypoints[1].pt, cv::Point2f(100, 100));
|
|
|
|
ASSERT_EQ(descriptorsOut.rows, 2);
|
|
EXPECT_TRUE(cv::countNonZero(descriptorsOut.row(0) != descriptorsIn.row(1)) == 0);
|
|
}
|
|
|
|
TEST(Util2dTest, NMSNoDescriptorsHandledGracefully) {
|
|
std::vector<cv::KeyPoint> inputKeypoints = {
|
|
cv::KeyPoint(cv::Point2f(10, 10), 1.f, -1, 1.0f)
|
|
};
|
|
std::vector<cv::KeyPoint> outputKeypoints;
|
|
cv::Mat descriptorsOut;
|
|
|
|
util2d::NMS(inputKeypoints, cv::Mat(), outputKeypoints, descriptorsOut, 3, 50, 50);
|
|
|
|
ASSERT_EQ(outputKeypoints.size(), 1);
|
|
EXPECT_TRUE(descriptorsOut.empty());
|
|
}
|
|
|
|
TEST(Util2dTest, NMSAllSuppressedDueToProximity) {
|
|
std::vector<cv::KeyPoint> inputKeypoints = {
|
|
cv::KeyPoint(cv::Point2f(30, 30), 1.f, -1, 0.5f),
|
|
cv::KeyPoint(cv::Point2f(31, 31), 1.f, -1, 0.4f),
|
|
cv::KeyPoint(cv::Point2f(32, 32), 1.f, -1, 0.3f)
|
|
};
|
|
|
|
cv::Mat descriptorsIn = cv::Mat::ones(3, 256, CV_32F);
|
|
std::vector<cv::KeyPoint> outputKeypoints;
|
|
cv::Mat descriptorsOut;
|
|
|
|
util2d::NMS(inputKeypoints, descriptorsIn, outputKeypoints, descriptorsOut, 3, 100, 100);
|
|
|
|
ASSERT_EQ(outputKeypoints.size(), 1); // only strongest survives
|
|
EXPECT_NEAR(outputKeypoints[0].response, 0.5f, 1e-6);
|
|
}
|
|
|
|
TEST(Util2dTest, NMSImageBoundsRespected) {
|
|
std::vector<cv::KeyPoint> inputKeypoints = {
|
|
cv::KeyPoint(cv::Point2f(0, 0), 1.f, -1, 1.0f),
|
|
cv::KeyPoint(cv::Point2f(199, 199), 1.f, -1, 1.0f),
|
|
cv::KeyPoint(cv::Point2f(300, 300), 1.f, -1, 1.0f) // out of bounds
|
|
};
|
|
|
|
cv::Mat descriptorsIn = cv::Mat::eye(3, 256, CV_32F);
|
|
std::vector<cv::KeyPoint> outputKeypoints;
|
|
cv::Mat descriptorsOut;
|
|
|
|
util2d::NMS(inputKeypoints, descriptorsIn, outputKeypoints, descriptorsOut, 2, 200, 200);
|
|
|
|
ASSERT_EQ(outputKeypoints.size(), 2);
|
|
for (const auto& kp : outputKeypoints) {
|
|
EXPECT_LT(kp.pt.x, 200);
|
|
EXPECT_LT(kp.pt.y, 200);
|
|
}
|
|
}
|
|
|
|
std::vector<cv::KeyPoint> generateGridKeypoints(int cols, int rows, int spacing) {
|
|
std::vector<cv::KeyPoint> keypoints;
|
|
for (int y = 0; y < rows; y += spacing) {
|
|
for (int x = 0; x < cols; x += spacing) {
|
|
keypoints.emplace_back(cv::Point2f(x + 0.5f, y + 0.5f), 1.0f, -1, float(rand() % 100) / 100.0f);
|
|
}
|
|
}
|
|
return keypoints;
|
|
}
|
|
|
|
TEST(Util2dTest, SSCSelectsRoughlyCorrectNumber) {
|
|
int imgWidth = 300;
|
|
int imgHeight = 300;
|
|
auto keypoints = generateGridKeypoints(imgWidth, imgHeight, 5); // creates many keypoints
|
|
|
|
int maxKeypoints = 100;
|
|
float tolerance = 0.1f;
|
|
|
|
std::vector<int> selected = util2d::SSC(keypoints, maxKeypoints, tolerance, imgWidth, imgHeight, {});
|
|
|
|
// util2d::SSC() first reduces maxKeypoints by tolerance, then searches within ±tolerance of that.
|
|
const int effectiveMax = maxKeypoints - int(std::round(maxKeypoints * tolerance));
|
|
const int kMin = int(std::round(effectiveMax * (1.f - tolerance)));
|
|
const int kMax = int(std::round(effectiveMax * (1.f + tolerance)));
|
|
EXPECT_GE(selected.size(), kMin);
|
|
EXPECT_LE(selected.size(), kMax);
|
|
EXPECT_LE(selected.size(), maxKeypoints);
|
|
}
|
|
|
|
TEST(Util2dTest, SSCReturnsEmptyOnEmptyInput) {
|
|
std::vector<cv::KeyPoint> keypoints;
|
|
std::vector<int> selected = util2d::SSC(keypoints, 100, 0.1f, 100, 100, {});
|
|
|
|
EXPECT_TRUE(selected.empty());
|
|
}
|
|
|
|
TEST(Util2dTest, SSCRespectsProvidedIndices) {
|
|
int imgWidth = 200;
|
|
int imgHeight = 200;
|
|
auto keypoints = generateGridKeypoints(imgWidth, imgHeight, 10);
|
|
|
|
// Sort keypoints manually (e.g., by response) and use the first N indices
|
|
std::vector<int> topIndices(keypoints.size());
|
|
for(size_t i=0; i<keypoints.size(); ++i)
|
|
{
|
|
topIndices[i] = i;
|
|
}
|
|
int maxKeypoints = 5;
|
|
|
|
std::vector<int> selected = util2d::SSC(keypoints, maxKeypoints, 0.1f, imgWidth, imgHeight, topIndices);
|
|
|
|
for (int idx : selected) {
|
|
EXPECT_TRUE(std::find(topIndices.begin(), topIndices.end(), idx) != topIndices.end());
|
|
}
|
|
|
|
EXPECT_GE(selected.size(), 4);
|
|
EXPECT_LE(selected.size(), 6);
|
|
}
|
|
|
|
TEST(Util2dTest, SSCSpatialDistributionCheck) {
|
|
int imgWidth = 100;
|
|
int imgHeight = 100;
|
|
auto keypoints = generateGridKeypoints(imgWidth, imgHeight, 1); // dense points
|
|
|
|
int maxKeypoints = 10;
|
|
float tolerance = 0.2f;
|
|
|
|
std::vector<int> selected = util2d::SSC(keypoints, maxKeypoints, tolerance, imgWidth, imgHeight, {});
|
|
|
|
// Check that no two selected keypoints are too close
|
|
float minDistance = float(imgWidth + imgHeight) / float(maxKeypoints); // loose estimate
|
|
for (size_t i = 0; i < selected.size(); ++i) {
|
|
for (size_t j = i + 1; j < selected.size(); ++j) {
|
|
auto& pt1 = keypoints[selected[i]].pt;
|
|
auto& pt2 = keypoints[selected[j]].pt;
|
|
float dist = cv::norm(pt1 - pt2);
|
|
EXPECT_GT(dist, minDistance * 0.2f); // 20% of estimated spacing
|
|
}
|
|
}
|
|
}
|
|
|
|
cv::Mat createTestImage(int width, int height, uchar value = 100) {
|
|
return cv::Mat(height, width, CV_8UC3, cv::Scalar(value, value, value));
|
|
}
|
|
|
|
namespace {
|
|
// A 3-channel "marker" pixel that's distinguishable from the uniform base color.
|
|
const cv::Vec3b kRgbMarker(255, 0, 0);
|
|
const cv::Vec3b kDepthMarker(123, 45, 67);
|
|
// Marker pixel position in the input 640x480 (cols x rows) image (top-left quadrant).
|
|
const int kMarkerRow = 100;
|
|
const int kMarkerCol = 200;
|
|
|
|
void stampMarker(cv::Mat & rgb, cv::Mat & depth) {
|
|
rgb.at<cv::Vec3b>(kMarkerRow, kMarkerCol) = kRgbMarker;
|
|
depth.at<cv::Vec3b>(kMarkerRow, kMarkerCol) = kDepthMarker;
|
|
}
|
|
|
|
// Verifies the marker landed at (expectedRow, expectedCol) and that no other pixel
|
|
// in the rotated image carries the marker color (i.e. the rotation is direction-
|
|
// correct, not just dimension-correct).
|
|
void expectMarkerAt(const cv::Mat & img, const cv::Vec3b & marker, int expectedRow, int expectedCol) {
|
|
EXPECT_EQ(img.at<cv::Vec3b>(expectedRow, expectedCol), marker);
|
|
int strays = 0;
|
|
for(int r = 0; r < img.rows; ++r)
|
|
{
|
|
for(int c = 0; c < img.cols; ++c)
|
|
{
|
|
if(r == expectedRow && c == expectedCol) continue;
|
|
if(img.at<cv::Vec3b>(r, c) == marker) ++strays;
|
|
}
|
|
}
|
|
EXPECT_EQ(strays, 0);
|
|
}
|
|
} // namespace
|
|
|
|
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
|
|
CameraModel model(500, 500, 320, 240, CameraModel::opticalRotation(), 0, cv::Size(640, 480));
|
|
cv::Mat rgb = createTestImage(640, 480);
|
|
cv::Mat depth = createTestImage(640, 480);
|
|
stampMarker(rgb, depth);
|
|
|
|
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
|
|
|
|
EXPECT_FALSE(rotated);
|
|
EXPECT_EQ(rgb.cols, 640);
|
|
EXPECT_EQ(rgb.rows, 480);
|
|
EXPECT_EQ(depth.cols, 640);
|
|
EXPECT_EQ(depth.rows, 480);
|
|
// Marker stays in place when no rotation is applied.
|
|
expectMarkerAt(rgb, kRgbMarker, kMarkerRow, kMarkerCol);
|
|
expectMarkerAt(depth, kDepthMarker, kMarkerRow, kMarkerCol);
|
|
float roll,pitch,yaw;
|
|
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
|
|
EXPECT_EQ(roll, 0.0f);
|
|
EXPECT_EQ(pitch, 0.0f);
|
|
EXPECT_EQ(yaw, 0.0f);
|
|
}
|
|
|
|
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation90Degrees) {
|
|
// Simulate +pi/2 roll (camera tilted right) -> upright correction is a 90 CW
|
|
// rotation of the image (transpose then flip(axis=1)). For an input marker at
|
|
// (row=100, col=200) in a 480x640 image:
|
|
// transpose: (100, 200) -> (200, 100) in 640x480
|
|
// flip(1): (200, 100) -> (200, 480-1-100) = (200, 379)
|
|
Transform rot = Transform(0,0,0, M_PI / 2, 0, 0);
|
|
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
|
|
cv::Mat rgb = createTestImage(640, 480, 150);
|
|
cv::Mat depth = createTestImage(640, 480, 200);
|
|
stampMarker(rgb, depth);
|
|
|
|
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
|
|
|
|
EXPECT_TRUE(rotated);
|
|
EXPECT_EQ(rgb.cols, 480); // Transposed
|
|
EXPECT_EQ(rgb.rows, 640);
|
|
EXPECT_EQ(depth.cols, 480);
|
|
EXPECT_EQ(depth.rows, 640);
|
|
expectMarkerAt(rgb, kRgbMarker, /*row=*/200, /*col=*/379);
|
|
expectMarkerAt(depth, kDepthMarker, /*row=*/200, /*col=*/379);
|
|
// After correction the camera is upright (roll=0).
|
|
float roll,pitch,yaw;
|
|
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
|
|
EXPECT_NEAR(roll, 0.0f, 1e-5);
|
|
EXPECT_NEAR(pitch, 0.0f, 1e-5);
|
|
EXPECT_NEAR(yaw, 0.0f, 1e-5);
|
|
}
|
|
|
|
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation180Degrees) {
|
|
// Simulate pi roll (camera upside down) -> 180 rotation:
|
|
// flip(1) + flip(0). For (100, 200) in 480x640:
|
|
// flip(1): (100, 200) -> (100, 640-1-200) = (100, 439)
|
|
// flip(0): (100, 439) -> (480-1-100, 439) = (379, 439)
|
|
Transform rot = Transform(0,0,0, M_PI, 0, 0);
|
|
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
|
|
cv::Mat rgb = createTestImage(640, 480, 123);
|
|
cv::Mat depth = createTestImage(640, 480, 77);
|
|
stampMarker(rgb, depth);
|
|
|
|
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
|
|
|
|
EXPECT_TRUE(rotated);
|
|
EXPECT_EQ(rgb.cols, 640); // Same size
|
|
EXPECT_EQ(rgb.rows, 480);
|
|
EXPECT_EQ(depth.cols, 640); // Same size
|
|
EXPECT_EQ(depth.rows, 480);
|
|
expectMarkerAt(rgb, kRgbMarker, /*row=*/379, /*col=*/439);
|
|
expectMarkerAt(depth, kDepthMarker, /*row=*/379, /*col=*/439);
|
|
float roll,pitch,yaw;
|
|
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
|
|
EXPECT_NEAR(roll, 0.0f, 1e-5);
|
|
EXPECT_NEAR(pitch, 0.0f, 1e-5);
|
|
EXPECT_NEAR(yaw, 0.0f, 1e-5);
|
|
}
|
|
|
|
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation270Degrees) {
|
|
// Simulate 3*pi/2 roll (camera tilted left) -> upright correction is a 90 CCW
|
|
// rotation of the image (flip(axis=1) then transpose). For (100, 200) in 480x640:
|
|
// flip(1): (100, 200) -> (100, 640-1-200) = (100, 439)
|
|
// transpose: (100, 439) -> (439, 100) in 640x480
|
|
Transform rot = Transform(0,0,0, 3*M_PI/2, 0, 0);
|
|
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
|
|
cv::Mat rgb = createTestImage(640, 480, 90);
|
|
cv::Mat depth = createTestImage(640, 480, 60);
|
|
stampMarker(rgb, depth);
|
|
|
|
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
|
|
|
|
EXPECT_TRUE(rotated);
|
|
EXPECT_EQ(rgb.cols, 480);
|
|
EXPECT_EQ(rgb.rows, 640);
|
|
EXPECT_EQ(depth.cols, 480);
|
|
EXPECT_EQ(depth.rows, 640);
|
|
expectMarkerAt(rgb, kRgbMarker, /*row=*/439, /*col=*/100);
|
|
expectMarkerAt(depth, kDepthMarker, /*row=*/439, /*col=*/100);
|
|
// After correction the camera is upright (roll=0).
|
|
float roll,pitch,yaw;
|
|
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
|
|
EXPECT_NEAR(roll, 0.0f, 1e-5);
|
|
EXPECT_NEAR(pitch, 0.0f, 1e-5);
|
|
EXPECT_NEAR(yaw, 0.0f, 1e-5);
|
|
}
|
|
|
|
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryPitchTooHighShouldSkip) {
|
|
// Simulate roll = 90°, but pitch = 90° too (invalid)
|
|
Transform rot = Transform(0,0,0, M_PI/2, M_PI/2, 0);
|
|
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
|
|
Transform orgTransform = model.localTransform();
|
|
cv::Mat rgb = createTestImage(640, 480);
|
|
cv::Mat depth = createTestImage(640, 480);
|
|
|
|
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
|
|
|
|
EXPECT_FALSE(rotated);
|
|
EXPECT_EQ(rgb.cols, 640);
|
|
EXPECT_EQ(rgb.rows, 480);
|
|
float roll,pitch,yaw;
|
|
(model.localTransform().inverse() * orgTransform).getEulerAngles(roll, pitch, yaw);
|
|
EXPECT_NEAR(roll, 0.0f, 1e-5);
|
|
EXPECT_NEAR(pitch, 0.0f, 1e-5);
|
|
EXPECT_NEAR(yaw, 0.0f, 1e-5);
|
|
} |