mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-06 10:07:47 +08:00
Support Inverted Depth compression (with png or rvl) (#1785)
* Support Inverted Depth compression (with png or rvl) * Updated parameter's ui description * ios: fixing minor version bump * Improved test coverage for this branch * fixing opencv debug assert * SensorData: ignore invalid CameraModel * added lazy decoding check
This commit is contained in:
matlabbe
@@ -162,6 +162,20 @@ IF(BUILD_PERF_TESTS)
|
||||
set_tests_properties(test_graph_perf PROPERTIES
|
||||
TIMEOUT ${_perf_timeout}
|
||||
LABELS "performance")
|
||||
|
||||
# Comparison of the depth image compression approaches (sizes, times, errors) for
|
||||
# 16UC1 and 32FC1 depth images: PNG, RVL, zlib, the legacy 4-channel PNG of 32FC1
|
||||
# images, their conversion to 16UC1 millimeters, and their quantization as 16 bits
|
||||
# inverse depth (Mem/DepthCompressionFormat=".png:max:q" or ".rvl:max:q"):
|
||||
# bin/test_compression_perf
|
||||
# bin/test_compression_perf --gtest_filter=*Synthetic*
|
||||
add_executable(test_compression_perf perf_compression.cpp)
|
||||
target_link_libraries(test_compression_perf gtest_main rtabmap_core)
|
||||
|
||||
add_test(NAME test_compression_perf COMMAND test_compression_perf)
|
||||
set_tests_properties(test_compression_perf PROPERTIES
|
||||
TIMEOUT ${_perf_timeout}
|
||||
LABELS "performance")
|
||||
ENDIF(BUILD_PERF_TESTS)
|
||||
|
||||
# Rtabmap end-to-end replay of sample DBs (test data fetched by
|
||||
|
||||
@@ -0,0 +1,342 @@
|
||||
// Comparison of the depth image compression approaches of Compression.h, for each
|
||||
// depth type rtabmap receives:
|
||||
//
|
||||
// 16UC1 (millimeters):
|
||||
// - ".png" lossless, 16 bits grayscale PNG
|
||||
// - ".rvl" lossless, RVL (Mem/DepthCompressionFormat default)
|
||||
// - zlib lossless, compressData2(), as a reference
|
||||
// 32FC1 (meters):
|
||||
// - ".png" lossless, float bytes as a 4-channel 8 bits PNG (legacy)
|
||||
// - zlib lossless, compressData2(), as a reference
|
||||
// - 16UC1 mm + ".png/.rvl" lossy, util2d::cvtDepthFromFloat() then 16 bits codec,
|
||||
// what Mem/SaveDepth16Format=true does
|
||||
// - ".png:max:q/.rvl:max:q" lossy, 16 bits quantized inverse depth (same
|
||||
// quantization than ROS's compressed_depth_image_transport)
|
||||
//
|
||||
// over the depth images of data/rgbd/depth (a structured light camera, millimeters),
|
||||
// the same images converted to meters in 32FC1 (as many drivers publish them), and a
|
||||
// synthetic 32FC1 image with continuous values, like stereo or lidar projected depth.
|
||||
//
|
||||
// Its own executable, run by ctest under the "performance" label, so that its seconds
|
||||
// of benchmarking stay out of the unit test shards:
|
||||
// ctest -L performance to run them
|
||||
// ctest -LE performance to skip them
|
||||
// bin/test_compression_perf --gtest_filter=*Synthetic*
|
||||
//
|
||||
// The times are reported rather than asserted on, as they depend on the machine. What
|
||||
// is asserted is that the lossless approaches give back the same image, and that the
|
||||
// lossy ones stay within their error bounds for the depth range they keep.
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace {
|
||||
|
||||
static const int ITERATIONS = 15;
|
||||
|
||||
struct Approach
|
||||
{
|
||||
std::string name;
|
||||
std::function<std::vector<unsigned char>(const cv::Mat &)> encode;
|
||||
std::function<cv::Mat(const std::vector<unsigned char> &)> decode;
|
||||
bool lossless;
|
||||
float maxDepth; // meters, lossy approaches only: depth kept under it
|
||||
float minDepth; // meters, lossy approaches only: depth kept over it
|
||||
std::function<float(float)> tolerance; // meters, lossy approaches only, for a depth in meters
|
||||
};
|
||||
|
||||
struct Result
|
||||
{
|
||||
size_t bytes = 0;
|
||||
double encodeMs = 0.0;
|
||||
double decodeMs = 0.0;
|
||||
double maxError = 0.0; // mm, over the depth range kept
|
||||
double rmse = 0.0; // mm, over the depth range kept
|
||||
double lost = 0.0; // % of the valid pixels set to 0
|
||||
int outOfTolerance = 0; // pixels with an error over the tolerance
|
||||
};
|
||||
|
||||
double median(std::vector<double> v)
|
||||
{
|
||||
std::sort(v.begin(), v.end());
|
||||
return v[v.size()/2];
|
||||
}
|
||||
|
||||
float toMeters(const cv::Mat & depth, int r, int c)
|
||||
{
|
||||
return depth.type() == CV_16UC1 ? float(depth.at<uint16_t>(r, c)) * 0.001f : depth.at<float>(r, c);
|
||||
}
|
||||
|
||||
Result run(const cv::Mat & depth, const Approach & approach)
|
||||
{
|
||||
Result result;
|
||||
std::vector<unsigned char> bytes;
|
||||
cv::Mat restored;
|
||||
std::vector<double> encodeTimes, decodeTimes;
|
||||
for(int i=0; i<ITERATIONS; ++i)
|
||||
{
|
||||
UTimer timer;
|
||||
bytes = approach.encode(depth);
|
||||
encodeTimes.push_back(timer.restart() * 1000.0);
|
||||
restored = approach.decode(bytes);
|
||||
decodeTimes.push_back(timer.ticks() * 1000.0);
|
||||
}
|
||||
result.bytes = bytes.size();
|
||||
result.encodeMs = median(encodeTimes);
|
||||
result.decodeMs = median(decodeTimes);
|
||||
|
||||
EXPECT_EQ(restored.size(), depth.size());
|
||||
EXPECT_EQ(restored.type(), approach.lossless ? depth.type() : restored.type());
|
||||
if(restored.size() != depth.size())
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
if(approach.lossless)
|
||||
{
|
||||
EXPECT_EQ(memcmp(restored.data, depth.data, depth.total()*depth.elemSize()), 0);
|
||||
return result;
|
||||
}
|
||||
|
||||
int valid = 0, lost = 0, kept = 0;
|
||||
double sumSq = 0.0;
|
||||
for(int r=0; r<depth.rows; ++r)
|
||||
{
|
||||
for(int c=0; c<depth.cols; ++c)
|
||||
{
|
||||
const float d = toMeters(depth, r, c);
|
||||
if(!(std::isfinite(d) && d > 0.0f))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
++valid;
|
||||
const float out = toMeters(restored, r, c);
|
||||
if(out == 0.0f)
|
||||
{
|
||||
++lost;
|
||||
// Only allowed outside the kept range
|
||||
if(d >= approach.minDepth && d < approach.maxDepth)
|
||||
{
|
||||
++result.outOfTolerance;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
const double err = std::fabs(out - d);
|
||||
result.maxError = std::max(result.maxError, err*1000.0);
|
||||
sumSq += err*err*1e6;
|
||||
++kept;
|
||||
if(err > approach.tolerance(d))
|
||||
{
|
||||
++result.outOfTolerance;
|
||||
}
|
||||
}
|
||||
}
|
||||
result.rmse = kept ? std::sqrt(sumSq / kept) : 0.0;
|
||||
result.lost = valid ? 100.0 * lost / valid : 0.0;
|
||||
EXPECT_EQ(result.outOfTolerance, 0) << approach.name;
|
||||
return result;
|
||||
}
|
||||
|
||||
void report(const std::string & title, const cv::Mat & depth, const std::vector<Approach> & approaches)
|
||||
{
|
||||
const size_t raw = depth.total() * depth.elemSize();
|
||||
std::printf("\n%s: %dx%d %s, %zu bytes raw\n", title.c_str(), depth.cols, depth.rows,
|
||||
depth.type() == CV_16UC1 ? "16UC1" : "32FC1", raw);
|
||||
std::printf(" %-22s %10s %7s %10s %10s %11s %10s %8s\n",
|
||||
"approach", "bytes", "ratio", "encode ms", "decode ms", "max err mm", "rmse mm", "lost %");
|
||||
for(const Approach & approach : approaches)
|
||||
{
|
||||
SCOPED_TRACE(title + " " + approach.name);
|
||||
const Result r = run(depth, approach);
|
||||
if(approach.lossless)
|
||||
{
|
||||
std::printf(" %-22s %10zu %6.1fx %10.2f %10.2f %11s %10s %8s\n",
|
||||
approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
|
||||
"lossless", "-", "-");
|
||||
}
|
||||
else
|
||||
{
|
||||
std::printf(" %-22s %10zu %6.1fx %10.2f %10.2f %11.3f %10.3f %8.2f\n",
|
||||
approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
|
||||
r.maxError, r.rmse, r.lost);
|
||||
}
|
||||
}
|
||||
std::fflush(stdout);
|
||||
}
|
||||
|
||||
std::vector<unsigned char> encode(const cv::Mat & depth, const std::string & format)
|
||||
{
|
||||
return compressImage(depth, format);
|
||||
}
|
||||
|
||||
cv::Mat decode(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
return uncompressImage(bytes);
|
||||
}
|
||||
|
||||
std::vector<unsigned char> encodeZlib(const cv::Mat & depth)
|
||||
{
|
||||
return compressData(depth);
|
||||
}
|
||||
|
||||
cv::Mat decodeZlib(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
return uncompressData(bytes);
|
||||
}
|
||||
|
||||
std::vector<Approach> approaches16U()
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
return {
|
||||
{".png", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
|
||||
{".rvl", std::bind(encode, _1, ".rvl"), decode, true, 0, 0, nullptr},
|
||||
{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr}};
|
||||
}
|
||||
|
||||
Approach invDepth(const std::string & codec, float maxDepth, float quantization)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
const float A = quantization * (quantization + 1.0f);
|
||||
const float B = 1.0f - A / maxDepth;
|
||||
const std::string format = uFormat("%s:%g:%g", codec.c_str(), maxDepth, quantization);
|
||||
return {format, std::bind(encode, _1, format), decode, false,
|
||||
maxDepth,
|
||||
A / (65535.0f - B) * 1.001f,
|
||||
[A](float d) { return 0.51f * d * d / A + 1e-6f; }};
|
||||
}
|
||||
|
||||
Approach depth16(const std::string & codec)
|
||||
{
|
||||
return {"16UC1 mm + " + codec,
|
||||
[codec](const cv::Mat & depth) { return compressImage(util2d::cvtDepthFromFloat(depth), codec); },
|
||||
[](const std::vector<unsigned char> & bytes) { return util2d::cvtDepthToFloat(uncompressImage(bytes)); },
|
||||
false,
|
||||
65.535f,
|
||||
0.0f,
|
||||
[](float) { return 0.001f + 1e-6f; }}; // truncated to millimeters
|
||||
}
|
||||
|
||||
std::vector<Approach> approaches32F()
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
return {
|
||||
{".png (legacy RGBA)", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
|
||||
{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr},
|
||||
depth16(".png"),
|
||||
depth16(".rvl"),
|
||||
invDepth(".png", 10.0f, 100.0f),
|
||||
invDepth(".rvl", 10.0f, 100.0f),
|
||||
invDepth(".png", 40.0f, 100.0f),
|
||||
invDepth(".rvl", 40.0f, 100.0f),
|
||||
invDepth(".rvl", 40.0f, 200.0f)};
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> loadSampleDepths()
|
||||
{
|
||||
std::vector<cv::Mat> depths;
|
||||
for(const std::string & name : {"17.png", "154.png"})
|
||||
{
|
||||
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/rgbd/depth/" + name;
|
||||
cv::Mat depth = cv::imread(path, cv::IMREAD_UNCHANGED);
|
||||
if(depth.type() == CV_16UC1)
|
||||
{
|
||||
depths.push_back(depth);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::printf("Cannot load 16UC1 depth image \"%s\", skipped.\n", path.c_str());
|
||||
}
|
||||
}
|
||||
return depths;
|
||||
}
|
||||
|
||||
// Ground plane, walls and boxes seen by a 640x480 camera, with continuous
|
||||
// values up to ~35 m, noise growing with depth (as stereo) and holes.
|
||||
cv::Mat makeSyntheticDepth(int cols = 640, int rows = 480)
|
||||
{
|
||||
cv::RNG rng(42);
|
||||
const float fx = 0.75f * cols, cx = cols / 2.0f, cy = rows / 2.0f;
|
||||
const float cameraHeight = 1.0f;
|
||||
cv::Mat depth(rows, cols, CV_32FC1);
|
||||
for(int v=0; v<rows; ++v)
|
||||
{
|
||||
for(int u=0; u<cols; ++u)
|
||||
{
|
||||
const float x = (u - cx) / fx; // ray direction, z = 1
|
||||
const float y = (v - cy) / fx;
|
||||
float d = 35.0f; // far wall
|
||||
if(y > 0.0f)
|
||||
{
|
||||
d = std::min(d, cameraHeight / y); // ground
|
||||
}
|
||||
if(x < 0.0f)
|
||||
{
|
||||
d = std::min(d, 3.0f / -x); // left wall, 3 m away
|
||||
}
|
||||
// boxes
|
||||
if(x > 0.05f && x < 0.25f && y > -0.1f && y < cameraHeight / 2.5f)
|
||||
{
|
||||
d = std::min(d, 2.5f - 1.5f * x);
|
||||
}
|
||||
if(x > -0.35f && x < -0.15f && y > -0.2f && y < cameraHeight / 12.0f)
|
||||
{
|
||||
d = std::min(d, 12.0f);
|
||||
}
|
||||
d += (float)rng.gaussian(0.002 * d * d); // stereo-like noise
|
||||
depth.at<float>(v, u) = d;
|
||||
}
|
||||
}
|
||||
// Holes
|
||||
for(int i=0; i<40; ++i)
|
||||
{
|
||||
const int u = rng.uniform(0, cols - 20), v = rng.uniform(0, rows - 20);
|
||||
depth(cv::Rect(u, v, rng.uniform(2, 20), rng.uniform(2, 20))).setTo(0.0f);
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(CompressionPerf, SampleDepth16UC1)
|
||||
{
|
||||
const std::vector<cv::Mat> depths = loadSampleDepths();
|
||||
if(depths.empty())
|
||||
{
|
||||
GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
|
||||
}
|
||||
for(size_t i=0; i<depths.size(); ++i)
|
||||
{
|
||||
report(uFormat("Sample depth %d", (int)i), depths[i], approaches16U());
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionPerf, SampleDepth32FC1)
|
||||
{
|
||||
const std::vector<cv::Mat> depths = loadSampleDepths();
|
||||
if(depths.empty())
|
||||
{
|
||||
GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
|
||||
}
|
||||
for(size_t i=0; i<depths.size(); ++i)
|
||||
{
|
||||
report(uFormat("Sample depth %d in meters", (int)i), util2d::cvtDepthToFloat(depths[i]), approaches32F());
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionPerf, Synthetic32FC1)
|
||||
{
|
||||
report("Synthetic continuous depth", makeSyntheticDepth(), approaches32F());
|
||||
report("Synthetic continuous depth HD", makeSyntheticDepth(1280, 720), approaches32F());
|
||||
}
|
||||
@@ -1,6 +1,9 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
@@ -183,3 +186,228 @@ TEST(CompressionTest, CompressionThreadDataRoundTrip)
|
||||
|
||||
expectMatEqual(uncompressThread.getUncompressedData(), data);
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
// 32FC1 depth image covering [minDepth, maxDepth[ with sub-millimeter values,
|
||||
// and the invalid values of the inverse depth format on the first row.
|
||||
cv::Mat makeFloatDepth(int rows, int cols, float minDepth, float maxDepth)
|
||||
{
|
||||
cv::Mat depth(rows, cols, CV_32FC1);
|
||||
for(int r = 0; r < rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < cols; ++c)
|
||||
{
|
||||
depth.at<float>(r, c) = minDepth + (maxDepth - minDepth) * float(r * cols + c) / float(rows * cols);
|
||||
}
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
// Error bound of the inverse depth format: half a quantization step.
|
||||
float invDepthTolerance(float d, float quantization)
|
||||
{
|
||||
// (with some margin for the float rounding of A/d + B, up to ~66000)
|
||||
return 0.51f * d * d / (quantization * (quantization + 1.0f)) + 1e-6f;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(CompressionTest, ParseImageCompressionFormat)
|
||||
{
|
||||
std::string codec;
|
||||
float maxDepth, quantization;
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat("", codec, maxDepth, quantization));
|
||||
EXPECT_TRUE(codec.empty());
|
||||
EXPECT_EQ(maxDepth, 0.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".jpg", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".jpg");
|
||||
EXPECT_EQ(maxDepth, 0.0f);
|
||||
EXPECT_EQ(quantization, 0.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".rvl", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".rvl");
|
||||
EXPECT_EQ(maxDepth, 0.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".png:20", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".png");
|
||||
EXPECT_FLOAT_EQ(maxDepth, 20.0f);
|
||||
EXPECT_FLOAT_EQ(quantization, 100.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".rvl:10.5:50", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".rvl");
|
||||
EXPECT_FLOAT_EQ(maxDepth, 10.5f);
|
||||
EXPECT_FLOAT_EQ(quantization, 50.0f);
|
||||
|
||||
EXPECT_FALSE(parseImageCompressionFormat("png", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".jpg:10:100", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:abc", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:0:100", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:-10:100", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:10:0", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:10:100:1", codec, maxDepth, quantization));
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InvalidFormatReturnsEmpty)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(4, 4, 1.0f, 2.0f);
|
||||
EXPECT_TRUE(compressImage(depth, ".jpg:10").empty());
|
||||
EXPECT_TRUE(compressImage(depth, ".png:x").empty());
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InverseDepthRoundTrip)
|
||||
{
|
||||
const float maxDepth = 10.0f;
|
||||
const float quantization = 100.0f;
|
||||
const float minDepth = quantization * (quantization + 1.0f) / (65535.0f + quantization * (quantization + 1.0f) / maxDepth);
|
||||
cv::Mat depth = makeFloatDepth(48, 64, minDepth * 1.001f, maxDepth * 0.999f);
|
||||
const float invalid[] = {
|
||||
0.0f, -1.0f, maxDepth, maxDepth * 2.0f, minDepth * 0.9f,
|
||||
std::numeric_limits<float>::quiet_NaN(),
|
||||
std::numeric_limits<float>::infinity(),
|
||||
-std::numeric_limits<float>::infinity()};
|
||||
const int nInvalid = sizeof(invalid) / sizeof(float);
|
||||
for(int i = 0; i < nInvalid; ++i)
|
||||
{
|
||||
depth.at<float>(0, i) = invalid[i];
|
||||
}
|
||||
|
||||
for(const std::string codec : {".png", ".rvl"})
|
||||
{
|
||||
SCOPED_TRACE(codec);
|
||||
const std::string format = codec + ":10:100";
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, format);
|
||||
ASSERT_FALSE(bytes.empty());
|
||||
EXPECT_LT(bytes.size(), depth.total() * depth.elemSize() / 2);
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), format);
|
||||
|
||||
const cv::Mat restored = uncompressImage(bytes);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
ASSERT_EQ(restored.size(), depth.size());
|
||||
for(int r = 0; r < depth.rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < depth.cols; ++c)
|
||||
{
|
||||
const float d = depth.at<float>(r, c);
|
||||
if(r == 0 && c < nInvalid)
|
||||
{
|
||||
EXPECT_EQ(restored.at<float>(r, c), 0.0f) << "input=" << d;
|
||||
}
|
||||
else
|
||||
{
|
||||
ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, quantization)) << "r=" << r << " c=" << c;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Re-compressing with the detected format gives back the same bytes
|
||||
// (e.g., DatabaseViewer saving an edited depth image).
|
||||
EXPECT_EQ(compressImage(restored, compressedDepthFormat(bytes)), compressImage(restored, format));
|
||||
|
||||
// Same through cv::Mat and thread overloads
|
||||
CompressionThread compressThread(depth, format);
|
||||
compressThread.start();
|
||||
compressThread.join();
|
||||
const cv::Mat bytesMat = compressThread.getCompressedData();
|
||||
ASSERT_EQ(bytesMat.total(), bytes.size());
|
||||
EXPECT_EQ(memcmp(bytesMat.data, bytes.data(), bytes.size()), 0);
|
||||
CompressionThread uncompressThread(bytesMat, true);
|
||||
uncompressThread.start();
|
||||
uncompressThread.join();
|
||||
expectMatEqual(uncompressThread.getUncompressedData(), restored);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InverseDepthQuantizationParameters)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(32, 32, 1.0f, 39.0f);
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, ".png:40:50");
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), ".png:40:50");
|
||||
const cv::Mat restored = uncompressImage(bytes);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
for(int r = 0; r < depth.rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < depth.cols; ++c)
|
||||
{
|
||||
const float d = depth.at<float>(r, c);
|
||||
ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 50.0f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InverseDepthNonContinuousImage)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(20, 30, 1.0f, 5.0f);
|
||||
const cv::Mat roi = depth(cv::Rect(3, 2, 10, 8));
|
||||
ASSERT_FALSE(roi.isContinuous());
|
||||
const cv::Mat restored = uncompressImage(compressImage(roi, ".rvl:10:100"));
|
||||
ASSERT_EQ(restored.size(), roi.size());
|
||||
for(int r = 0; r < roi.rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < roi.cols; ++c)
|
||||
{
|
||||
const float d = roi.at<float>(r, c);
|
||||
ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 100.0f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, DepthParametersIgnoredFor16UC1)
|
||||
{
|
||||
cv::Mat depth(24, 32, CV_16UC1);
|
||||
cv::randu(depth, 0, 20000); // includes values over the max depth below
|
||||
for(const std::string codec : {".png", ".rvl"})
|
||||
{
|
||||
SCOPED_TRACE(codec);
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, codec + ":10:100");
|
||||
EXPECT_EQ(bytes, compressImage(depth, codec));
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), codec);
|
||||
expectMatEqual(uncompressImage(bytes), depth);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, LegacyFloatDepthIsLossless)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(16, 16, 0.01f, 100.0f);
|
||||
for(const std::string format : {".png", ".rvl"})
|
||||
{
|
||||
SCOPED_TRACE(format);
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, format);
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), ".png");
|
||||
const cv::Mat restored = uncompressImage(bytes);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
EXPECT_EQ(memcmp(restored.data, depth.data, depth.total() * depth.elemSize()), 0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, MalformedDepthFormatsDecodeToEmpty)
|
||||
{
|
||||
// Signature and header only, no payload
|
||||
std::vector<unsigned char> invDepth = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
|
||||
invDepth.resize(16, 0);
|
||||
EXPECT_TRUE(uncompressImage(invDepth).empty());
|
||||
EXPECT_EQ(compressedDepthFormat(invDepth), ".png") << "too short to be inverse depth";
|
||||
|
||||
// Inverse depth header followed by an 8 bits image instead of a 16 bits one
|
||||
const std::vector<unsigned char> png8 = compressImage(cv::Mat(4, 4, CV_8UC1, cv::Scalar(1)), ".png");
|
||||
invDepth.insert(invDepth.end(), png8.begin(), png8.end());
|
||||
EXPECT_TRUE(uncompressImage(invDepth).empty());
|
||||
|
||||
// RVL signature without its size
|
||||
const std::vector<unsigned char> rvl = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L', 4, 0};
|
||||
EXPECT_TRUE(uncompressImage(rvl).empty());
|
||||
EXPECT_EQ(compressedDepthFormat(rvl), ".rvl");
|
||||
|
||||
EXPECT_TRUE(uncompressImage(nullptr, 0).empty());
|
||||
}
|
||||
|
||||
TEST(CompressionTest, CompressionThreadRejectsInvalidFormat)
|
||||
{
|
||||
// std::string: a string literal would select the (bytes, isImage) constructor
|
||||
const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.0f));
|
||||
EXPECT_THROW(CompressionThread(depth, std::string(".jpg:10")), UException);
|
||||
EXPECT_THROW(CompressionThread(depth, std::string(".bmp")), UException);
|
||||
EXPECT_NO_THROW(CompressionThread(depth, std::string(".rvl:10:100")));
|
||||
}
|
||||
|
||||
@@ -4681,3 +4681,128 @@ TEST(MemoryTest, CreateSignatureRecompressesStereoPairAfterRectification)
|
||||
EXPECT_GT(cv::countNonZero(uncompressImage(stored.depthOrRightCompressed()) != right), 0)
|
||||
<< "stored right image still holds the unrectified pixels";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Mem/DepthCompressionFormat with inverse depth (".rvl:max:q"), which databases
|
||||
// older than 0.24 cannot hold: rtabmap 0.23 would still open them (e.g., created
|
||||
// with Db/TargetVersion=0.23.0) but could not decode their depth images.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
enum DepthInput
|
||||
{
|
||||
kRawDepth,
|
||||
kCompressedDepthWithRaw, // e.g., received from ROS and decoded
|
||||
kCompressedDepthOnly // raw depth not needed (no features extracted here)
|
||||
};
|
||||
|
||||
struct InverseDepthCase
|
||||
{
|
||||
const char * targetVersion;
|
||||
DepthInput input;
|
||||
const char * depthCompressionFormat;
|
||||
bool parallelCompression;
|
||||
const char * expectedFormat;
|
||||
};
|
||||
|
||||
class MemoryInverseDepthTest : public ::testing::TestWithParam<InverseDepthCase> {};
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_P(MemoryInverseDepthTest, StoredDepthFormatFollowsDatabaseVersion)
|
||||
{
|
||||
const InverseDepthCase & cs = GetParam();
|
||||
ParametersMap params = defaultMemoryParams();
|
||||
params[Parameters::kMemBinDataKept()] = "true";
|
||||
params[Parameters::kMemDepthCompressionFormat()] = cs.depthCompressionFormat;
|
||||
params[Parameters::kMemCompressionParallelized()] = cs.parallelCompression ? "true" : "false";
|
||||
params[Parameters::kDbTargetVersion()] = cs.targetVersion;
|
||||
Memory memory(params);
|
||||
const std::string dbPath = uniqueDbPath();
|
||||
ASSERT_TRUE(memory.init(dbPath, true, params));
|
||||
|
||||
const cv::Mat rgb(16, 16, CV_8UC3, cv::Scalar(10, 20, 30));
|
||||
cv::Mat depth(16, 16, CV_32FC1);
|
||||
cv::randu(depth, 0.5f, 8.0f);
|
||||
const CameraModel model(10.0, 10.0, 8.0, 8.0, CameraModel::opticalRotation());
|
||||
SensorData data;
|
||||
if(cs.input == kRawDepth)
|
||||
{
|
||||
data = SensorData(rgb, depth, model);
|
||||
}
|
||||
else
|
||||
{
|
||||
data = SensorData(compressImage2(rgb, ".png"), compressImage2(depth, ".png:10:100"), model);
|
||||
if(cs.input == kCompressedDepthWithRaw)
|
||||
{
|
||||
data.uncompressData();
|
||||
ASSERT_FALSE(data.depthRaw().empty());
|
||||
}
|
||||
}
|
||||
|
||||
ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
|
||||
const Signature * s = memory.getSignature(memory.getLastSignatureId());
|
||||
ASSERT_NE(s, nullptr);
|
||||
const cv::Mat & stored = s->sensorData().depthOrRightCompressed();
|
||||
ASSERT_FALSE(stored.empty());
|
||||
EXPECT_EQ(compressedDepthFormat(stored), cs.expectedFormat);
|
||||
|
||||
const cv::Mat restored = uncompressImage(stored);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
ASSERT_EQ(restored.size(), depth.size());
|
||||
EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.01);
|
||||
|
||||
memory.close(false);
|
||||
UFile::erase(dbPath);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
DatabaseVersions,
|
||||
MemoryInverseDepthTest,
|
||||
::testing::Values(
|
||||
InverseDepthCase{"", kRawDepth, ".rvl:10:100", true, ".rvl:10:100"},
|
||||
InverseDepthCase{"", kRawDepth, ".rvl:10:100", false, ".rvl:10:100"},
|
||||
InverseDepthCase{"", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png:10:100"}, // reused as is
|
||||
InverseDepthCase{"", kCompressedDepthOnly, ".rvl:10:100", true, ".png:10:100"}, // reused as is
|
||||
InverseDepthCase{"0.23.0", kRawDepth, ".rvl:10:100", true, ".png"}, // legacy 32FC1 format
|
||||
InverseDepthCase{"0.23.0", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png"}, // re-compressed
|
||||
InverseDepthCase{"0.23.0", kCompressedDepthOnly, ".rvl:10:100", true, ".png"}, // decompressed, re-compressed
|
||||
InverseDepthCase{"", kRawDepth, ".rvl", true, ".png"}, // RVL is 16UC1 only: legacy
|
||||
InverseDepthCase{"", kRawDepth, ".jpg", true, ".png"})); // invalid: default ".rvl"
|
||||
|
||||
// Compressed images that Memory rectifies (Rtabmap/ImagesAlreadyRectified=false) are
|
||||
// decoded for it, even when nothing else needs them (no feature extraction here): they
|
||||
// are stored rectified, not as received.
|
||||
TEST(MemoryTest, DecodesCompressedImagesToRectifyThem)
|
||||
{
|
||||
for(bool alreadyRectified : {true, false})
|
||||
{
|
||||
SCOPED_TRACE(alreadyRectified ? "already rectified" : "rectified by Memory");
|
||||
ParametersMap params = defaultMemoryParams();
|
||||
params[Parameters::kMemBinDataKept()] = "true";
|
||||
params[Parameters::kRtabmapImagesAlreadyRectified()] = alreadyRectified ? "true" : "false";
|
||||
Memory memory(params);
|
||||
ASSERT_TRUE(memory.init(""));
|
||||
|
||||
cv::Mat rgb(48, 64, CV_8UC3);
|
||||
cv::randu(rgb, 0, 255);
|
||||
const cv::Mat K = (cv::Mat_<double>(3, 3) << 50, 0, 32, 0, 50, 24, 0, 0, 1);
|
||||
const cv::Mat D = (cv::Mat_<double>(1, 5) << -0.3, 0.1, 0, 0, 0);
|
||||
const cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
|
||||
const cv::Mat P = (cv::Mat_<double>(3, 4) << 50, 0, 32, 0, 0, 50, 24, 0, 0, 0, 1, 0);
|
||||
const CameraModel model("cam", cv::Size(64, 48), K, D, R, P, CameraModel::opticalRotation());
|
||||
ASSERT_TRUE(model.isValidForRectification());
|
||||
const cv::Mat compressed = compressImage2(rgb, ".png");
|
||||
SensorData data(compressed, cv::Mat(), model);
|
||||
|
||||
ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
|
||||
const Signature * s = memory.getSignature(memory.getLastSignatureId());
|
||||
ASSERT_NE(s, nullptr);
|
||||
const cv::Mat & stored = s->sensorData().imageCompressed();
|
||||
ASSERT_FALSE(stored.empty());
|
||||
const bool sameBytes = stored.total() == compressed.total() &&
|
||||
memcmp(stored.data, compressed.data, compressed.total()) == 0;
|
||||
EXPECT_EQ(sameBytes, alreadyRectified);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -199,7 +199,7 @@ TEST(SensorDataTest, IsValidWithCameraModel)
|
||||
{
|
||||
SensorData data;
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel());
|
||||
EXPECT_FALSE(data.cameraModels().empty());
|
||||
EXPECT_TRUE(data.cameraModels().empty()); // invalid without image: placeholder not kept
|
||||
EXPECT_FALSE(data.isValid()); // not valid for projection
|
||||
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(525.0, 525.0, 320.0, 240.0));
|
||||
@@ -986,3 +986,32 @@ TEST(SensorDataTest, DifferentDepthTypes)
|
||||
EXPECT_EQ(data.depthOrRightRaw().type(), CV_32FC1);
|
||||
}
|
||||
|
||||
|
||||
// An invalid CameraModel without any image is a placeholder (e.g., lidar odometry
|
||||
// creating scan-only data with CameraModel()): it is not kept. With an image, it is kept,
|
||||
// as images can be used without calibration.
|
||||
TEST(SensorDataTest, InvalidCameraModelIsKeptOnlyWithImages)
|
||||
{
|
||||
const LaserScan scan(cv::Mat(1, 3, CV_32FC2, cv::Scalar(1.0f, 0.0f)), 0, 10.0f, LaserScan::kXY);
|
||||
const SensorData scanOnly(scan, cv::Mat(), cv::Mat(), CameraModel(), 1, 1.0);
|
||||
EXPECT_TRUE(scanOnly.cameraModels().empty());
|
||||
EXPECT_TRUE(scanOnly.isValid());
|
||||
|
||||
const cv::Mat image(4, 6, CV_8UC1, cv::Scalar(1));
|
||||
const SensorData uncalibrated(image, CameraModel(), 1, 1.0);
|
||||
EXPECT_EQ(uncalibrated.cameraModels().size(), 1u);
|
||||
|
||||
const SensorData compressedOnly(compressImage2(image, ".png"), CameraModel(), 1, 1.0);
|
||||
EXPECT_EQ(compressedOnly.cameraModels().size(), 1u);
|
||||
|
||||
const CameraModel valid(10.0, 10.0, 3.0, 2.0);
|
||||
const SensorData calibratedNoImage(scan, cv::Mat(), cv::Mat(), valid, 1, 1.0);
|
||||
EXPECT_EQ(calibratedNoImage.cameraModels().size(), 1u) << "valid models are always kept";
|
||||
|
||||
// Keeping the images already there: they still need their model
|
||||
SensorData data(image, CameraModel(), 1, 1.0);
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), false);
|
||||
EXPECT_EQ(data.cameraModels().size(), 1u);
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), true);
|
||||
EXPECT_TRUE(data.cameraModels().empty()) << "images cleared, nothing left to describe";
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user