mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-12 22:40:19 +08:00
CI coverage: adding g2o, gtsam, ceres and libpointmatcher. (#1743)
* CI coverage: adding g2o, gtsam, ceres and libpointmatcher. Also fetch test data for integration tests. * ld * Increasing ci test timeout for coverage * Fixed tests when vertigo is not there * verbose tests * updating flaky test * faster coverage by extending unit tests and disable integration test (only for coverage report) * improving coverage of unit tests in comparison to integrations tests * fixing not covered new lines
This commit is contained in:
matlabbe
@@ -93,7 +93,18 @@ public:
|
||||
/** @return Target schema version for new databases (from parameters). */
|
||||
const std::string & getTargetVersion() const {return _targetVersion;}
|
||||
|
||||
/** @brief Queue a signature for deferred save; ownership is transferred. */
|
||||
/**
|
||||
* @brief Queue a signature for deferred save; ownership is transferred.
|
||||
*
|
||||
* @note Only the *compressed* sensor buffers are written
|
||||
* (SensorData::imageCompressed(), depthOrRightCompressed(),
|
||||
* laserScanCompressed(), ...). Raw matrices are ignored, so a signature
|
||||
* carrying only raw data is stored with empty payloads. Memory compresses
|
||||
* before saving; a caller driving the driver directly should compress first
|
||||
* with @ref compressImage2() / @ref compressData2(), or pass the compressed
|
||||
* buffers to SensorData::setRGBDImage() / setLaserScan(), which treat a
|
||||
* 1-row CV_8UC1 matrix as already compressed.
|
||||
*/
|
||||
void asyncSave(Signature * s);
|
||||
/** @brief Queue a visual word for deferred save; ownership is transferred. */
|
||||
void asyncSave(VisualWord * vw);
|
||||
|
||||
@@ -4669,6 +4669,14 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
|
||||
|
||||
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
|
||||
{
|
||||
if(((*i)->sensorData().imageCompressed().empty() && !(*i)->sensorData().imageRaw().empty()) ||
|
||||
((*i)->sensorData().depthOrRightCompressed().empty() && !(*i)->sensorData().depthOrRightRaw().empty()) ||
|
||||
((*i)->sensorData().laserScanCompressed().isEmpty() && !(*i)->sensorData().laserScanRaw().isEmpty()))
|
||||
{
|
||||
UWARN("Node %d carries raw sensor data but no compressed data. Only compressed "
|
||||
"buffers are saved, so that payload will be empty in the database. Compress "
|
||||
"it first (see compressImage2() / compressData2()).", (*i)->id());
|
||||
}
|
||||
if(!(*i)->sensorData().imageCompressed().empty() ||
|
||||
!(*i)->sensorData().depthOrRightCompressed().empty() ||
|
||||
!(*i)->sensorData().depthConfidenceCompressed().empty() ||
|
||||
|
||||
+12
-1
@@ -3850,7 +3850,12 @@ Transform Memory::computeIcpTransformMulti(
|
||||
guessNorm > fromScan.rangeMax() + toScan.rangeMax())
|
||||
{
|
||||
// stop right known,it is impossible that scans overlay.
|
||||
UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.rangeMax(), toScan.rangeMax());
|
||||
const std::string rejected = uFormat("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.rangeMax(), toScan.rangeMax());
|
||||
UINFO("%s", rejected.c_str());
|
||||
if(info)
|
||||
{
|
||||
info->rejectedMsg = rejected;
|
||||
}
|
||||
return t;
|
||||
}
|
||||
|
||||
@@ -3989,6 +3994,12 @@ Transform Memory::computeIcpTransformMulti(
|
||||
t = t.inverse();
|
||||
}
|
||||
}
|
||||
else if(info)
|
||||
{
|
||||
info->rejectedMsg = uFormat("Node %d (scan %s) or %d (scan %s) has no laser scan, cannot compute ICP transform.",
|
||||
fromId, fromScan.isEmpty()?"empty":"ok",
|
||||
toId, toScan.isEmpty()?"empty":"ok");
|
||||
}
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
@@ -62,6 +62,7 @@ set(corelib_test_sources
|
||||
test_sensorevent.cpp #SensorEvent.h
|
||||
test_sensordata.cpp #SensorData.h
|
||||
test_sensorcapture.cpp #SensorCapture.h
|
||||
test_cameraimages.cpp #camera/CameraImages.h
|
||||
test_sensorcaptureinfo.cpp #SensorCaptureInfo.h
|
||||
test_sensorcapturethread.cpp #SensorCaptureThread.h
|
||||
)
|
||||
@@ -76,6 +77,14 @@ IF(libpointmatcher_FOUND)
|
||||
target_link_libraries(test_corelib ${libpointmatcher_LIBRARIES})
|
||||
ENDIF(libpointmatcher_FOUND)
|
||||
|
||||
# The timeouts below are sized for an optimized build; at -O0 (Debug, or
|
||||
# coverage, which forces -O0) these tests run 5-40x slower.
|
||||
IF(CMAKE_BUILD_TYPE MATCHES "^(Release|RelWithDebInfo|MinSizeRel)$")
|
||||
SET(_test_timeout_scale 1)
|
||||
ELSE()
|
||||
SET(_test_timeout_scale 6)
|
||||
ENDIF()
|
||||
|
||||
# Split the run across processes so `ctest -j` still overlaps work, paying the
|
||||
# library load N times instead of once per source file. gtest partitions by
|
||||
# test index via these two env vars; the shards are disjoint and together
|
||||
@@ -85,10 +94,11 @@ ENDIF(libpointmatcher_FOUND)
|
||||
set(CORELIB_TEST_SHARDS 4 CACHE STRING
|
||||
"Number of processes the corelib unit tests are split across")
|
||||
math(EXPR _corelib_last_shard "${CORELIB_TEST_SHARDS} - 1")
|
||||
math(EXPR _corelib_shard_timeout "900 * ${_test_timeout_scale}")
|
||||
foreach(shard RANGE 0 ${_corelib_last_shard})
|
||||
add_test(NAME test_corelib_${shard} COMMAND test_corelib)
|
||||
set_tests_properties(test_corelib_${shard} PROPERTIES
|
||||
TIMEOUT 900
|
||||
TIMEOUT ${_corelib_shard_timeout}
|
||||
ENVIRONMENT "GTEST_TOTAL_SHARDS=${CORELIB_TEST_SHARDS};GTEST_SHARD_INDEX=${shard}")
|
||||
endforeach()
|
||||
|
||||
@@ -107,8 +117,9 @@ target_link_libraries(test_rtabmap_integration gtest_main rtabmap_core)
|
||||
# while nightlies run:
|
||||
# `ctest -L long`.
|
||||
add_test(NAME test_rtabmap_integration COMMAND test_rtabmap_integration)
|
||||
math(EXPR _integration_timeout "1800 * ${_test_timeout_scale}")
|
||||
set_tests_properties(test_rtabmap_integration PROPERTIES
|
||||
TIMEOUT 1800
|
||||
TIMEOUT ${_integration_timeout}
|
||||
LABELS "long")
|
||||
|
||||
#PythonInterface / PyDetector / PyDescriptor / PyMatcher (optional Python
|
||||
|
||||
@@ -0,0 +1,93 @@
|
||||
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/camera/CameraImages.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/SensorCaptureInfo.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <string>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// CameraImages over a real image directory. The tests above use a mock capture
|
||||
// to cover the SensorCapture base class (frame-rate throttling, local
|
||||
// transform, info filling); this drives the concrete directory-scanning
|
||||
// implementation over the 84 committed images in data/samples.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
TEST(CameraImagesTest, ScansSampleDirectoryAndCapturesFrames)
|
||||
{
|
||||
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";
|
||||
CameraImages camera(path);
|
||||
ASSERT_TRUE(camera.init()) << "could not scan " << path;
|
||||
// data/samples ships no calibration, so the model stays unnamed and
|
||||
// getSerial() (which returns the model name) is empty -- the two must agree.
|
||||
EXPECT_FALSE(camera.isCalibrated());
|
||||
EXPECT_TRUE(camera.getSerial().empty());
|
||||
|
||||
// Take a handful rather than all 84: enough to exercise the scan/decode
|
||||
// loop and the frame ordering without paying for the whole directory.
|
||||
Transform previousStamp;
|
||||
int frames = 0;
|
||||
double lastStamp = -1.0;
|
||||
for(int i = 0; i < 5; ++i)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
SensorData data = camera.takeData(&info);
|
||||
if(data.imageRaw().empty() && data.imageCompressed().empty())
|
||||
{
|
||||
break;
|
||||
}
|
||||
++frames;
|
||||
EXPECT_GT(data.imageRaw().cols, 0);
|
||||
EXPECT_GT(data.imageRaw().rows, 0);
|
||||
EXPECT_GT(data.stamp(), lastStamp) << "stamps must increase across frames";
|
||||
lastStamp = data.stamp();
|
||||
}
|
||||
EXPECT_EQ(5, frames) << "expected 5 frames from a directory of 84 images";
|
||||
}
|
||||
|
||||
// setStartIndex()/setMaxFrames() are how callers replay a slice of a
|
||||
// directory; without them the reader always starts at the first file.
|
||||
TEST(CameraImagesTest, HonoursStartIndexAndMaxFrames)
|
||||
{
|
||||
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";
|
||||
|
||||
CameraImages fromStart(path);
|
||||
ASSERT_TRUE(fromStart.init());
|
||||
SensorData first = fromStart.takeData();
|
||||
ASSERT_FALSE(first.imageRaw().empty());
|
||||
|
||||
CameraImages skipped(path);
|
||||
skipped.setStartIndex(10);
|
||||
skipped.setMaxFrames(2);
|
||||
ASSERT_TRUE(skipped.init());
|
||||
|
||||
SensorData tenth = skipped.takeData();
|
||||
ASSERT_FALSE(tenth.imageRaw().empty()) << "start index 10 returned nothing";
|
||||
// Different file, so the decoded pixels should differ from frame 0.
|
||||
EXPECT_NE(0.0, cv::norm(first.imageRaw(), tenth.imageRaw(), cv::NORM_L1))
|
||||
<< "start index had no effect: same image as the first frame";
|
||||
|
||||
EXPECT_FALSE(skipped.takeData().imageRaw().empty()) << "second of maxFrames=2 missing";
|
||||
EXPECT_TRUE(skipped.takeData().imageRaw().empty()) << "maxFrames=2 should stop after two frames";
|
||||
}
|
||||
|
||||
// The default constructor takes no path; a caller uses setPath()/setStartIndex()
|
||||
// before init(). Covered separately because the path constructor above runs a
|
||||
// different initialiser list.
|
||||
TEST(CameraImagesTest, DefaultConstructedIsUnconfigured)
|
||||
{
|
||||
CameraImages camera;
|
||||
EXPECT_FALSE(camera.isCalibrated());
|
||||
EXPECT_TRUE(camera.getSerial().empty());
|
||||
// No directory yet, so scanning must fail rather than assert.
|
||||
EXPECT_FALSE(camera.init());
|
||||
|
||||
camera.setPath(std::string(RTABMAP_TEST_DATA_ROOT) + "/samples");
|
||||
ASSERT_TRUE(camera.init()) << "init() failed after setPath()";
|
||||
EXPECT_FALSE(camera.takeData().imageRaw().empty());
|
||||
}
|
||||
@@ -3,6 +3,15 @@
|
||||
#include <rtabmap/core/DBDriverSqlite3.h>
|
||||
#include <rtabmap/core/Signature.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/DBReader.h>
|
||||
#include <rtabmap/core/SensorCaptureInfo.h>
|
||||
#include <rtabmap/core/CameraModel.h>
|
||||
#include <rtabmap/core/LaserScan.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <memory>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include "TestUtils.h"
|
||||
@@ -174,3 +183,261 @@ TEST_F(DBDriverSqlite3Fixture, ExecuteNoResultPragma)
|
||||
EXPECT_NO_THROW(driver_->executeNoResult("PRAGMA cache_size=8000;"));
|
||||
EXPECT_TRUE(driver_->isConnected());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Full write/read round trip: build a small database with rich sensor data,
|
||||
// then replay it with DBReader.
|
||||
//
|
||||
// The tests above save bare Signatures, which exercises the schema but not the
|
||||
// payload paths (compressed image/depth/scan blobs, calibration, links) nor
|
||||
// DBReader at all -- until now those only ran during the end-to-end replay
|
||||
// tests, which need the fetched sample databases.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
// A frame carrying everything the payload paths serialise: RGB, depth,
|
||||
// calibration and a laser scan.
|
||||
SensorData makeRichData(int id, double stamp)
|
||||
{
|
||||
cv::Mat rgb(60, 80, CV_8UC3);
|
||||
cv::randu(rgb, cv::Scalar::all(0), cv::Scalar::all(255));
|
||||
cv::Mat depth(60, 80, CV_16UC1);
|
||||
cv::randu(depth, cv::Scalar::all(500), cv::Scalar::all(4000));
|
||||
const CameraModel model(100.0, 100.0, 40.0, 30.0, Transform::getIdentity(), 0.0, cv::Size(80, 60));
|
||||
|
||||
cv::Mat scanData(1, 50, CV_32FC3);
|
||||
for(int i = 0; i < 50; ++i)
|
||||
{
|
||||
scanData.at<cv::Vec3f>(0, i) = cv::Vec3f(0.01f * i, 0.02f * i, 0.0f);
|
||||
}
|
||||
|
||||
// The driver persists the *compressed* buffers, so compress up front the
|
||||
// way Memory does before saving; raw-only data would be written as empty
|
||||
// blobs. setRGBDImage()/setLaserScan() detect a 1-row CV_8UC1 as compressed.
|
||||
SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setRGBDImage(compressImage2(rgb, ".png"), compressImage2(depth, ".png"), model);
|
||||
data.setLaserScan(LaserScan(compressData2(scanData), /*maxPoints=*/0, /*maxRange=*/0.0f,
|
||||
LaserScan::kXYZ));
|
||||
return data;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_F(DBDriverSqlite3Fixture, SavesAndLoadsRichSensorData)
|
||||
{
|
||||
const Transform pose(1.0f, 2.0f, 0.0f, 0.0f, 0.0f, 0.3f);
|
||||
Signature * s = new Signature(10, 0, 1, 1.5, "node10", pose, Transform(), makeRichData(10, 1.5));
|
||||
saveSignature(s);
|
||||
|
||||
std::list<Signature *> loaded;
|
||||
driver_->loadSignatures(std::list<int>(1, 10), loaded);
|
||||
ASSERT_EQ(1u, loaded.size());
|
||||
Signature * back = loaded.front();
|
||||
EXPECT_EQ(10, back->id());
|
||||
EXPECT_EQ(0, back->mapId());
|
||||
EXPECT_DOUBLE_EQ(1.5, back->getStamp());
|
||||
EXPECT_EQ("node10", back->getLabel());
|
||||
EXPECT_LT(back->getPose().getDistance(pose), 1e-4f);
|
||||
|
||||
// Payloads come back compressed; ask the driver to fill them in.
|
||||
std::list<Signature *> toFill(1, back);
|
||||
driver_->loadNodeData(toFill);
|
||||
back->sensorData().uncompressData();
|
||||
EXPECT_FALSE(back->sensorData().imageRaw().empty()) << "image blob did not round-trip";
|
||||
EXPECT_FALSE(back->sensorData().depthRaw().empty()) << "depth blob did not round-trip";
|
||||
EXPECT_FALSE(back->sensorData().laserScanRaw().isEmpty()) << "scan blob did not round-trip";
|
||||
EXPECT_EQ(1u, back->sensorData().cameraModels().size());
|
||||
|
||||
for(std::list<Signature *>::iterator iter = loaded.begin(); iter != loaded.end(); ++iter)
|
||||
{
|
||||
delete *iter;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DBDriverSqlite3Fixture, DBReaderReplaysWrittenDatabase)
|
||||
{
|
||||
// Three consecutive nodes with odometry poses and neighbour links, i.e.
|
||||
// the minimum a recorded session needs to be replayable.
|
||||
const int kNodes = 3;
|
||||
for(int i = 1; i <= kNodes; ++i)
|
||||
{
|
||||
const Transform pose(0.5f * i, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
Signature * s = new Signature(i, 0, 1, static_cast<double>(i), "", pose, Transform(),
|
||||
makeRichData(i, static_cast<double>(i)));
|
||||
if(i > 1)
|
||||
{
|
||||
const Transform motion(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
s->addLink(Link(i, i - 1, Link::kNeighbor, motion.inverse()));
|
||||
}
|
||||
saveSignature(s);
|
||||
}
|
||||
// Close so the file is complete before DBReader opens it.
|
||||
driver_->closeConnection(true);
|
||||
delete driver_;
|
||||
driver_ = nullptr;
|
||||
|
||||
std::unique_ptr<DBReader> reader(new DBReader(dbPath_, /*frameRate=*/0.0f,
|
||||
/*odometryIgnored=*/false));
|
||||
ASSERT_TRUE(reader->init()) << "DBReader could not open " << dbPath_;
|
||||
|
||||
int frames = 0;
|
||||
Transform previousPose;
|
||||
while(frames < kNodes + 1)
|
||||
{
|
||||
SensorCaptureInfo info;
|
||||
SensorData data = reader->takeData(&info);
|
||||
if(data.id() == 0 && data.imageRaw().empty() && data.imageCompressed().empty())
|
||||
{
|
||||
break; // end of database
|
||||
}
|
||||
++frames;
|
||||
EXPECT_FALSE(info.odomPose.isNull()) << "frame " << data.id() << " has no odometry pose";
|
||||
if(!previousPose.isNull() && !info.odomPose.isNull())
|
||||
{
|
||||
// Poses were written 50 cm apart along x.
|
||||
EXPECT_NEAR(previousPose.getDistance(info.odomPose), 0.5f, 1e-3f);
|
||||
}
|
||||
previousPose = info.odomPose;
|
||||
}
|
||||
EXPECT_EQ(kNodes, frames) << "DBReader returned " << frames << " of " << kNodes << " nodes";
|
||||
}
|
||||
|
||||
// Only compressed buffers are persisted, so raw-only sensor data is stored with
|
||||
// empty payloads (documented on DBDriver::asyncSave()). Pinned here because it
|
||||
// is silent from the caller's point of view -- see makeRichData() above, which
|
||||
// compresses first.
|
||||
TEST_F(DBDriverSqlite3Fixture, RawOnlySensorDataIsNotPersisted)
|
||||
{
|
||||
cv::Mat rgb(20, 20, CV_8UC3, cv::Scalar(10, 20, 30));
|
||||
const CameraModel model(100.0, 100.0, 10.0, 10.0, Transform::getIdentity(), 0.0, cv::Size(20, 20));
|
||||
SensorData raw(rgb, cv::Mat(), model, 42, 1.0); // raw, never compressed
|
||||
ASSERT_FALSE(raw.imageRaw().empty());
|
||||
ASSERT_TRUE(raw.imageCompressed().empty());
|
||||
|
||||
saveSignature(new Signature(42, 0, 1, 1.0, "", Transform::getIdentity(), Transform(), raw));
|
||||
|
||||
std::list<Signature *> loaded;
|
||||
driver_->loadSignatures(std::list<int>(1, 42), loaded);
|
||||
ASSERT_EQ(1u, loaded.size());
|
||||
driver_->loadNodeData(loaded);
|
||||
loaded.front()->sensorData().uncompressData();
|
||||
EXPECT_TRUE(loaded.front()->sensorData().imageRaw().empty())
|
||||
<< "raw-only image unexpectedly survived a save/load round trip";
|
||||
delete loaded.front();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Legacy schema round trips. Db/TargetVersion makes the driver create an older
|
||||
// schema, which is how a database recorded by an earlier rtabmap looks. The
|
||||
// readers for those layouts differ substantially -- e.g. link covariance was
|
||||
// stored as separate rotVariance/transVariance columns before the full
|
||||
// information matrix -- and none of it ran in the tests until now.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
class DBSchemaVersionTest : public ::testing::TestWithParam<const char *>
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
dbPath_ = uniqueDbPath();
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), GetParam()));
|
||||
driver_ = new DBDriverSqlite3(parameters);
|
||||
ASSERT_TRUE(driver_->openConnection(dbPath_, true))
|
||||
<< "could not create a " << GetParam() << " database";
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
if(driver_)
|
||||
{
|
||||
driver_->closeConnection(false);
|
||||
delete driver_;
|
||||
driver_ = nullptr;
|
||||
}
|
||||
UFile::erase(dbPath_.c_str());
|
||||
}
|
||||
|
||||
void saveSignature(Signature * s)
|
||||
{
|
||||
driver_->asyncSave(s);
|
||||
driver_->emptyTrashes(false);
|
||||
}
|
||||
|
||||
std::string dbPath_;
|
||||
DBDriverSqlite3 * driver_ = nullptr;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_P(DBSchemaVersionTest, NodesAndLinksSurviveARoundTrip)
|
||||
{
|
||||
// Two nodes joined by a neighbour link carrying a non-default information
|
||||
// matrix: older schemas store that as rotVariance/transVariance, newer ones
|
||||
// as the full 6x6, so this exercises whichever reader the version needs.
|
||||
cv::Mat info = cv::Mat::eye(6, 6, CV_64FC1);
|
||||
info.at<double>(0,0) = info.at<double>(1,1) = info.at<double>(2,2) = 4.0; // 1/transVariance
|
||||
info.at<double>(3,3) = info.at<double>(4,4) = info.at<double>(5,5) = 100.0; // 1/rotVariance
|
||||
|
||||
const Transform motion(0.5f, 0.1f, 0.0f, 0.0f, 0.0f, 0.2f);
|
||||
for(int id = 1; id <= 2; ++id)
|
||||
{
|
||||
const Transform pose(0.5f * (id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
Signature * s = new Signature(id, 0, 1, static_cast<double>(id), "", pose, Transform(),
|
||||
makeRichData(id, static_cast<double>(id)));
|
||||
if(id == 2)
|
||||
{
|
||||
s->addLink(Link(2, 1, Link::kNeighbor, motion.inverse(), info));
|
||||
}
|
||||
saveSignature(s);
|
||||
}
|
||||
|
||||
EXPECT_FALSE(driver_->getDatabaseVersion().empty());
|
||||
|
||||
std::list<Signature *> loaded;
|
||||
driver_->loadSignatures(std::list<int>{1, 2}, loaded);
|
||||
ASSERT_EQ(2u, loaded.size()) << "nodes did not survive the round trip";
|
||||
|
||||
// Payloads
|
||||
driver_->loadNodeData(loaded);
|
||||
for(Signature * s : loaded)
|
||||
{
|
||||
s->sensorData().uncompressData();
|
||||
EXPECT_FALSE(s->sensorData().imageRaw().empty()) << "node " << s->id() << " lost its image";
|
||||
EXPECT_EQ(1u, s->sensorData().cameraModels().size());
|
||||
}
|
||||
|
||||
// Links: the second node must still point back at the first, with the
|
||||
// variances recovered from whatever columns this schema uses.
|
||||
std::multimap<int, Link> links;
|
||||
driver_->loadLinks(2, links);
|
||||
ASSERT_FALSE(links.empty()) << "link did not survive the round trip";
|
||||
const Link & link = links.begin()->second;
|
||||
EXPECT_EQ(1, link.to());
|
||||
EXPECT_LT(link.transform().getDistance(motion.inverse()), 1e-3f);
|
||||
EXPECT_NEAR(4.0, link.infMatrix().at<double>(0,0), 1e-6) << "translational variance lost";
|
||||
EXPECT_NEAR(100.0, link.infMatrix().at<double>(3,3), 1e-6) << "rotational variance lost";
|
||||
|
||||
for(Signature * s : loaded)
|
||||
{
|
||||
delete s;
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
Schemas,
|
||||
DBSchemaVersionTest,
|
||||
::testing::Values("0.16", "0.17", "0.18", "0.20", "0.22"),
|
||||
[](const ::testing::TestParamInfo<const char *> & info) {
|
||||
std::string name(info.param);
|
||||
for(size_t i = 0; i < name.size(); ++i)
|
||||
{
|
||||
if(!isalnum(name[i])) name[i] = '_';
|
||||
}
|
||||
return "v" + name;
|
||||
});
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/GlobalMap.h>
|
||||
#include <rtabmap/core/global_map/OccupancyGrid.h>
|
||||
#include <rtabmap/core/LocalGrid.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
@@ -218,3 +220,112 @@ TEST(GlobalMapTest, GetMemoryUsed)
|
||||
ASSERT_TRUE(map.update(poses));
|
||||
EXPECT_GT(map.getMemoryUsed(), 0u);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// The tests above drive GlobalMap through MockGlobalMap, which overrides
|
||||
// assemble(), so the real rasterisation never runs. These use the concrete
|
||||
// OccupancyGrid: cached local grids are assembled at known poses and the
|
||||
// resulting map is checked for extent and content.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
// A local grid shaped like a short wall: `cols` obstacle cells one cell apart
|
||||
// along +x, plus a matching strip of empty cells in front of it.
|
||||
void addWallGridToCache(LocalGridCache & cache, int nodeId, int cols, float cellSize)
|
||||
{
|
||||
cv::Mat obstacles(1, cols, CV_32FC2);
|
||||
cv::Mat empty(1, cols, CV_32FC2);
|
||||
for(int i = 0; i < cols; ++i)
|
||||
{
|
||||
obstacles.at<cv::Vec2f>(0, i) = cv::Vec2f(cellSize * i, 0.0f);
|
||||
empty.at<cv::Vec2f>(0, i) = cv::Vec2f(cellSize * i, -cellSize);
|
||||
}
|
||||
cache.add(nodeId, cv::Mat(), obstacles, empty, cellSize);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(OccupancyGridTest, AssemblesCachedGridIntoMap)
|
||||
{
|
||||
const float cellSize = 0.2f;
|
||||
LocalGridCache cache;
|
||||
addWallGridToCache(cache, 1, /*cols=*/5, cellSize);
|
||||
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize)));
|
||||
OccupancyGrid grid(&cache, parameters);
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
poses.insert(std::make_pair(1, Transform::getIdentity()));
|
||||
EXPECT_TRUE(grid.update(poses));
|
||||
|
||||
float xMin = 0.0f, yMin = 0.0f;
|
||||
const cv::Mat map = grid.getMap(xMin, yMin);
|
||||
ASSERT_FALSE(map.empty()) << "assemble() produced no map";
|
||||
EXPECT_EQ(CV_8SC1, map.type()) << "occupancy maps are signed char (-1 unknown, 0 empty, 100 occupied)";
|
||||
EXPECT_GT(map.total(), 5u) << "map smaller than the grid that was assembled";
|
||||
|
||||
// The wall spans 5 cells at 20 cm, so the map must be at least that wide.
|
||||
EXPECT_GE(map.cols * cellSize, 5 * cellSize - 1e-3f);
|
||||
|
||||
int occupied = 0, empty = 0, unknown = 0;
|
||||
for(int y = 0; y < map.rows; ++y)
|
||||
{
|
||||
for(int x = 0; x < map.cols; ++x)
|
||||
{
|
||||
const signed char v = map.at<signed char>(y, x);
|
||||
if(v == 0) ++empty;
|
||||
else if(v > 0) ++occupied;
|
||||
else ++unknown;
|
||||
}
|
||||
}
|
||||
EXPECT_GT(occupied, 0) << "no occupied cells rasterised";
|
||||
EXPECT_GT(empty, 0) << "no empty cells rasterised";
|
||||
EXPECT_EQ(map.total(), static_cast<size_t>(occupied + empty + unknown));
|
||||
}
|
||||
|
||||
// Two nodes offset along x must produce a map wider than either alone: this is
|
||||
// what distinguishes real assembly from returning the last grid.
|
||||
TEST(OccupancyGridTest, SecondPoseExtendsTheMap)
|
||||
{
|
||||
const float cellSize = 0.2f;
|
||||
LocalGridCache cache;
|
||||
addWallGridToCache(cache, 1, /*cols=*/5, cellSize);
|
||||
addWallGridToCache(cache, 2, /*cols=*/5, cellSize);
|
||||
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize)));
|
||||
|
||||
OccupancyGrid single(&cache, parameters);
|
||||
std::map<int, Transform> onePose;
|
||||
onePose.insert(std::make_pair(1, Transform::getIdentity()));
|
||||
single.update(onePose);
|
||||
float xMin1 = 0.0f, yMin1 = 0.0f;
|
||||
const cv::Mat mapOne = single.getMap(xMin1, yMin1);
|
||||
ASSERT_FALSE(mapOne.empty());
|
||||
|
||||
OccupancyGrid both(&cache, parameters);
|
||||
std::map<int, Transform> twoPoses;
|
||||
twoPoses.insert(std::make_pair(1, Transform::getIdentity()));
|
||||
twoPoses.insert(std::make_pair(2, Transform(2.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)));
|
||||
both.update(twoPoses);
|
||||
float xMin2 = 0.0f, yMin2 = 0.0f;
|
||||
const cv::Mat mapTwo = both.getMap(xMin2, yMin2);
|
||||
ASSERT_FALSE(mapTwo.empty());
|
||||
|
||||
EXPECT_GT(mapTwo.cols, mapOne.cols)
|
||||
<< "a node 2 m away did not widen the map (" << mapOne.cols << " -> " << mapTwo.cols << ")";
|
||||
EXPECT_GT(mapTwo.total(), mapOne.total());
|
||||
}
|
||||
|
||||
// An empty cache means nothing to assemble; the map must stay empty rather than
|
||||
// producing a degenerate one.
|
||||
TEST(OccupancyGridTest, EmptyCacheProducesEmptyMap)
|
||||
{
|
||||
LocalGridCache cache;
|
||||
OccupancyGrid grid(&cache, globalMapTestParams());
|
||||
grid.update(std::map<int, Transform>());
|
||||
float xMin = 0.0f, yMin = 0.0f;
|
||||
EXPECT_TRUE(grid.getMap(xMin, yMin).empty());
|
||||
}
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
#include <rtabmap/core/Memory.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/RegistrationInfo.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Signature.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
@@ -4015,3 +4017,146 @@ TEST(MemoryTest, CleanupLocalGridsFiltersObstaclesAgainstMap)
|
||||
}
|
||||
UFile::erase(dbPath.c_str());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Memory::computeIcpTransformMulti(): proximity refinement between two nodes
|
||||
// using their laser scans. Until now this only ran during the end-to-end
|
||||
// replays, which need the fetched sample databases.
|
||||
//
|
||||
// A corner is used so ICP has a unique optimum (a single line or plane would
|
||||
// leave a direction unobservable, making the result depend on the guess).
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
// 2D corner: floor at y=-half, wall at x=-half, in the sensor frame.
|
||||
LaserScan memoryCorner2D(float length = 4.0f, int pointsPerLine = 300, uint64_t seed = 0xBEEF)
|
||||
{
|
||||
cv::RNG rng(seed);
|
||||
cv::Mat data(1, 2 * pointsPerLine, CV_32FC2);
|
||||
const float half = 0.5f * length;
|
||||
int idx = 0;
|
||||
for(int i = 0; i < pointsPerLine; ++i, ++idx)
|
||||
{
|
||||
data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
|
||||
rng.uniform(-half, half), -half + rng.uniform(-0.005f, 0.005f));
|
||||
}
|
||||
for(int i = 0; i < pointsPerLine; ++i, ++idx)
|
||||
{
|
||||
data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
|
||||
-half + rng.uniform(-0.005f, 0.005f), rng.uniform(-half, half));
|
||||
}
|
||||
return LaserScan(data, /*maxPoints=*/0, /*maxRange=*/0.0f, LaserScan::kXY);
|
||||
}
|
||||
|
||||
ParametersMap icpMemoryParams()
|
||||
{
|
||||
ParametersMap p = defaultMemoryParams();
|
||||
p[Parameters::kRegStrategy()] = "1"; // ICP
|
||||
p[Parameters::kRegForce3DoF()] = "true";
|
||||
p[Parameters::kMemBinDataKept()] = "true"; // keep the scans
|
||||
p[Parameters::kIcpPointToPlane()] = "false";
|
||||
p[Parameters::kIcpVoxelSize()] = "0.0";
|
||||
p[Parameters::kIcpCorrespondenceRatio()]= "0.1";
|
||||
p[Parameters::kMemLaserScanVoxelSize()] = "0.0";
|
||||
return p;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_F(MemoryFixture, ComputeIcpTransformMultiAlignsCornerScans)
|
||||
{
|
||||
reinit(icpMemoryParams());
|
||||
|
||||
// Same corner seen from two viewpoints 12 cm / 2.9 deg apart.
|
||||
const Transform motion(0.10f, 0.06f, 0.0f, 0.0f, 0.0f, 0.05f);
|
||||
const LaserScan corner = memoryCorner2D();
|
||||
|
||||
SensorData first(image_);
|
||||
first.setLaserScan(corner);
|
||||
ASSERT_TRUE(memory_->update(first, Transform::getIdentity(), covariance_));
|
||||
const int oldId = memory_->getLastSignatureId();
|
||||
|
||||
SensorData second(image_);
|
||||
second.setLaserScan(util3d::transformLaserScan(corner, motion.inverse()));
|
||||
ASSERT_TRUE(memory_->update(second, motion, covariance_));
|
||||
const int newId = memory_->getLastSignatureId();
|
||||
ASSERT_NE(oldId, newId);
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
poses.insert(std::make_pair(oldId, Transform::getIdentity()));
|
||||
poses.insert(std::make_pair(newId, motion));
|
||||
|
||||
RegistrationInfo info;
|
||||
const Transform t = memory_->computeIcpTransformMulti(newId, oldId, poses, &info);
|
||||
ASSERT_FALSE(t.isNull()) << "ICP refinement failed: " << info.rejectedMsg;
|
||||
|
||||
// computeIcpTransformMulti(newId, oldId, ...) reports the transform from the
|
||||
// new node to the old one, i.e. the inverse of the motion between them.
|
||||
const Transform expected = motion.inverse();
|
||||
EXPECT_LT(t.getDistance(expected), 0.002f)
|
||||
<< "got " << t.prettyPrint() << " expected " << expected.prettyPrint();
|
||||
const float angDeg = t.getAngle(expected) * 180.0f / static_cast<float>(CV_PI);
|
||||
EXPECT_LT(angDeg, 0.2f) << "rotation off by " << angDeg << " deg";
|
||||
EXPECT_GT(info.icpInliersRatio, 0.5f) << "poor overlap for two views of the same corner";
|
||||
}
|
||||
|
||||
// Without scans there is nothing for ICP to align: the refinement must decline
|
||||
// with a reason rather than returning a bogus transform.
|
||||
TEST_F(MemoryFixture, ComputeIcpTransformMultiRejectsNodesWithoutScans)
|
||||
{
|
||||
reinit(icpMemoryParams());
|
||||
ASSERT_TRUE(update());
|
||||
const int oldId = memory_->getLastSignatureId();
|
||||
ASSERT_TRUE(update());
|
||||
const int newId = memory_->getLastSignatureId();
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
poses.insert(std::make_pair(oldId, Transform::getIdentity()));
|
||||
poses.insert(std::make_pair(newId, Transform(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)));
|
||||
|
||||
RegistrationInfo info;
|
||||
const Transform t = memory_->computeIcpTransformMulti(newId, oldId, poses, &info);
|
||||
EXPECT_TRUE(t.isNull()) << "expected refusal, got " << t.prettyPrint();
|
||||
EXPECT_FALSE(info.rejectedMsg.empty())
|
||||
<< "declined without a reason, so a caller cannot tell 'no scans' from "
|
||||
"a genuine registration failure";
|
||||
EXPECT_LE(info.icpInliersRatio, 0.0f);
|
||||
}
|
||||
|
||||
// The registration is also declined when the guess puts the two viewpoints
|
||||
// beyond their combined sensor range, since the scans then cannot overlap. That
|
||||
// early-out reports its own reason, so a caller can tell it apart from a
|
||||
// failed alignment.
|
||||
TEST_F(MemoryFixture, ComputeIcpTransformMultiRejectsScansTooFarApart)
|
||||
{
|
||||
reinit(icpMemoryParams());
|
||||
|
||||
// Same corner, but with a declared 2 m max range.
|
||||
const LaserScan corner = memoryCorner2D();
|
||||
const LaserScan ranged(corner.data(), corner.maxPoints(), /*rangeMax=*/2.0f, corner.format());
|
||||
ASSERT_GT(ranged.rangeMax(), 0.0f);
|
||||
|
||||
SensorData first(image_);
|
||||
first.setLaserScan(ranged);
|
||||
ASSERT_TRUE(memory_->update(first, Transform::getIdentity(), covariance_));
|
||||
const int oldId = memory_->getLastSignatureId();
|
||||
|
||||
SensorData second(image_);
|
||||
second.setLaserScan(ranged);
|
||||
const Transform farAway(20.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
ASSERT_TRUE(memory_->update(second, farAway, covariance_));
|
||||
const int newId = memory_->getLastSignatureId();
|
||||
|
||||
// 20 m apart with 2 m + 2 m of range: no overlap is possible.
|
||||
std::map<int, Transform> poses;
|
||||
poses.insert(std::make_pair(oldId, Transform::getIdentity()));
|
||||
poses.insert(std::make_pair(newId, farAway));
|
||||
|
||||
RegistrationInfo info;
|
||||
const Transform t = memory_->computeIcpTransformMulti(newId, oldId, poses, &info);
|
||||
EXPECT_TRUE(t.isNull()) << "aligned scans that cannot overlap: " << t.prettyPrint();
|
||||
ASSERT_FALSE(info.rejectedMsg.empty()) << "declined without a reason";
|
||||
EXPECT_NE(info.rejectedMsg.find("Too far"), std::string::npos)
|
||||
<< "unexpected reason: " << info.rejectedMsg;
|
||||
}
|
||||
|
||||
@@ -2,8 +2,16 @@
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
#include <rtabmap/core/OdometryInfo.h>
|
||||
#include <rtabmap/core/CameraModel.h>
|
||||
#include <rtabmap/core/StereoCameraModel.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/LaserScan.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
@@ -179,3 +187,359 @@ TEST(OdometryTest, DefaultCapabilityFlags)
|
||||
EXPECT_FALSE(odometry.canProcessRawImages());
|
||||
EXPECT_FALSE(odometry.canProcessAsyncIMU());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Strategies driven through the public API on real frames committed under
|
||||
// data/. The tests above use MockOdometry, which overrides computeTransform(),
|
||||
// so they exercise the base class but never enter a backend implementation.
|
||||
// These run the real strategies end to end -- feature extraction,
|
||||
// correspondence and motion estimation all execute. Parameterized on
|
||||
// Odom/Strategy, so covering another backend is one list entry.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
const char * strategyName(Odometry::Type type)
|
||||
{
|
||||
switch(type)
|
||||
{
|
||||
case Odometry::kTypeF2M: return "F2M";
|
||||
case Odometry::kTypeF2F: return "F2F";
|
||||
default: return "other";
|
||||
}
|
||||
}
|
||||
|
||||
// data/stereo_rect holds two rectified pairs (50 and 60) plus the calibration:
|
||||
// enough for one initialisation frame and one motion estimate.
|
||||
SensorData loadStereoFrame(const std::string & name, int id, double stamp)
|
||||
{
|
||||
const std::string root(RTABMAP_TEST_DATA_ROOT);
|
||||
const cv::Mat left = cv::imread(root + "/stereo_rect/left/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
|
||||
const cv::Mat right = cv::imread(root + "/stereo_rect/right/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
|
||||
StereoCameraModel model;
|
||||
if(left.empty() || right.empty() || !model.load(root + "/stereo_rect", "stereo"))
|
||||
{
|
||||
return SensorData();
|
||||
}
|
||||
return SensorData(left, right, model, id, stamp);
|
||||
}
|
||||
|
||||
// data/rgbd holds two RGB-D frames (17 and 154) with per-frame calibration.
|
||||
SensorData loadRgbdFrame(const std::string & name, int id, double stamp)
|
||||
{
|
||||
const std::string root(RTABMAP_TEST_DATA_ROOT);
|
||||
const cv::Mat rgb = cv::imread(root + "/rgbd/rgb/" + name + ".jpg", cv::IMREAD_COLOR);
|
||||
const cv::Mat depth = cv::imread(root + "/rgbd/depth/" + name + ".png", cv::IMREAD_UNCHANGED);
|
||||
CameraModel model;
|
||||
if(rgb.empty() || depth.empty() || !model.load(root + "/rgbd/calib", name))
|
||||
{
|
||||
return SensorData();
|
||||
}
|
||||
return SensorData(rgb, depth, model, id, stamp);
|
||||
}
|
||||
|
||||
class OdometryStrategyTest : public ::testing::TestWithParam<Odometry::Type>
|
||||
{
|
||||
protected:
|
||||
// Deliberately minimal: exercise each strategy's default path rather than
|
||||
// a tuned configuration.
|
||||
std::unique_ptr<Odometry> createOdometry() const
|
||||
{
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kOdomStrategy(),
|
||||
uNumber2Str(static_cast<int>(GetParam()))));
|
||||
std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
|
||||
if(odometry && odometry->getType() != GetParam())
|
||||
{
|
||||
// create() falls back to F2M for a backend that is not compiled
|
||||
// in, which would otherwise silently test F2M twice.
|
||||
return nullptr;
|
||||
}
|
||||
return odometry;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
// A first frame has nothing to register against: every strategy should accept
|
||||
// it, report the identity pose, and initialise its map / previous frame.
|
||||
TEST_P(OdometryStrategyTest, FirstStereoFrameInitialisesAtOrigin)
|
||||
{
|
||||
std::unique_ptr<Odometry> odometry = createOdometry();
|
||||
if(!odometry)
|
||||
{
|
||||
GTEST_SKIP() << strategyName(GetParam()) << " not built in";
|
||||
}
|
||||
|
||||
SensorData first = loadStereoFrame("50", 1, 0.0);
|
||||
ASSERT_FALSE(first.imageRaw().empty()) << "data/stereo_rect/left/50.jpg missing";
|
||||
|
||||
OdometryInfo info;
|
||||
const Transform pose = odometry->process(first, &info);
|
||||
|
||||
EXPECT_FALSE(pose.isNull());
|
||||
EXPECT_TRUE(pose.isIdentity()) << "first pose should be the origin, got " << pose.prettyPrint();
|
||||
EXPECT_EQ(0, info.reg.inliers) << "nothing to register against on the first frame";
|
||||
}
|
||||
|
||||
// Second frame: the strategy must recover a real motion between the two
|
||||
// rectified pairs. The bounds are loose on purpose -- the claim is "a
|
||||
// plausible, non-degenerate transform from real correspondences", not a
|
||||
// specific value, which differs per backend.
|
||||
TEST_P(OdometryStrategyTest, StereoPairRecoversMotion)
|
||||
{
|
||||
std::unique_ptr<Odometry> odometry = createOdometry();
|
||||
if(!odometry)
|
||||
{
|
||||
GTEST_SKIP() << strategyName(GetParam()) << " not built in";
|
||||
}
|
||||
|
||||
SensorData first = loadStereoFrame("50", 1, 0.0);
|
||||
SensorData second = loadStereoFrame("60", 2, 0.1);
|
||||
ASSERT_FALSE(first.imageRaw().empty());
|
||||
ASSERT_FALSE(second.imageRaw().empty());
|
||||
|
||||
OdometryInfo firstInfo;
|
||||
ASSERT_FALSE(odometry->process(first, &firstInfo).isNull());
|
||||
|
||||
OdometryInfo info;
|
||||
const Transform pose = odometry->process(second, &info);
|
||||
|
||||
ASSERT_FALSE(pose.isNull())
|
||||
<< strategyName(GetParam()) << " lost tracking between frames 50 and 60";
|
||||
EXPECT_GT(info.reg.inliers, 20)
|
||||
<< strategyName(GetParam()) << " too few inliers (matches=" << info.reg.matches << ")";
|
||||
EXPECT_LE(info.reg.inliers, info.reg.matches);
|
||||
|
||||
// The frames are ~15 cm apart; beyond a metre the estimate diverged.
|
||||
const float distance = pose.getNorm();
|
||||
EXPECT_GT(distance, 0.01f) << strategyName(GetParam()) << " reported no motion at all";
|
||||
EXPECT_LT(distance, 1.0f) << strategyName(GetParam()) << " implausible motion "
|
||||
<< pose.prettyPrint();
|
||||
}
|
||||
|
||||
// Same entry point with RGB-D input, so the depth-to-3D path runs instead of
|
||||
// stereo correspondence. Frames 17 and 154 are far apart in the sequence, so
|
||||
// losing tracking is a legitimate outcome; what must hold is that the reported
|
||||
// info agrees with the returned transform.
|
||||
TEST_P(OdometryStrategyTest, HandlesRgbdFrames)
|
||||
{
|
||||
std::unique_ptr<Odometry> odometry = createOdometry();
|
||||
if(!odometry)
|
||||
{
|
||||
GTEST_SKIP() << strategyName(GetParam()) << " not built in";
|
||||
}
|
||||
|
||||
SensorData first = loadRgbdFrame("17", 1, 0.0);
|
||||
SensorData second = loadRgbdFrame("154", 2, 0.1);
|
||||
ASSERT_FALSE(first.imageRaw().empty()) << "data/rgbd/rgb/17.jpg missing";
|
||||
ASSERT_FALSE(second.imageRaw().empty()) << "data/rgbd/rgb/154.jpg missing";
|
||||
|
||||
OdometryInfo firstInfo;
|
||||
EXPECT_FALSE(odometry->process(first, &firstInfo).isNull());
|
||||
|
||||
OdometryInfo info;
|
||||
const Transform pose = odometry->process(second, &info);
|
||||
if(pose.isNull())
|
||||
{
|
||||
EXPECT_LT(info.reg.inliers, 20) << "null transform but plenty of inliers";
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_GT(info.reg.inliers, 0);
|
||||
EXPECT_LT(pose.getNorm(), 20.0f) << "implausible jump " << pose.prettyPrint();
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
Strategies,
|
||||
OdometryStrategyTest,
|
||||
::testing::Values(Odometry::kTypeF2M, Odometry::kTypeF2F),
|
||||
[](const ::testing::TestParamInfo<Odometry::Type> & info) {
|
||||
return strategyName(info.param);
|
||||
});
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Scan-matching odometry (Reg/Strategy=1). The visual tests above depend on
|
||||
// real imagery; these use synthetic geometry instead, which lets the expected
|
||||
// motion be known exactly rather than merely plausible.
|
||||
//
|
||||
// A corner is used on purpose: two perpendicular surfaces constrain every
|
||||
// translational DoF, so ICP has a unique optimum. A single plane or a corridor
|
||||
// would leave a direction unobservable and the test would pass or fail on the
|
||||
// initial guess rather than on the registration.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
// 2D corner: floor at y=-half and wall at x=-half, expressed in the sensor
|
||||
// frame of the first observation. Jitter keeps the per-point normals
|
||||
// well-defined (see the same construction in test_registrationicp.cpp).
|
||||
LaserScan makeCorner2D(float length = 4.0f, int pointsPerLine = 400, uint64_t seed = 0xC0FFEE)
|
||||
{
|
||||
cv::RNG rng(seed);
|
||||
cv::Mat data(1, 2 * pointsPerLine, CV_32FC2);
|
||||
const float half = 0.5f * length;
|
||||
int idx = 0;
|
||||
for(int i = 0; i < pointsPerLine; ++i, ++idx)
|
||||
{
|
||||
data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
|
||||
rng.uniform(-half, half), -half + rng.uniform(-0.005f, 0.005f));
|
||||
}
|
||||
for(int i = 0; i < pointsPerLine; ++i, ++idx)
|
||||
{
|
||||
data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
|
||||
-half + rng.uniform(-0.005f, 0.005f), rng.uniform(-half, half));
|
||||
}
|
||||
return LaserScan(data, /*maxPoints=*/0, /*maxRange=*/0.0f, LaserScan::kXY);
|
||||
}
|
||||
|
||||
// 3D corner: floor plus two walls, so all 6 DoF are constrained.
|
||||
LaserScan makeCorner3D(float length = 4.0f, int pointsPerSurface = 400, uint64_t seed = 0xC0FFEE)
|
||||
{
|
||||
cv::RNG rng(seed);
|
||||
cv::Mat data(1, 3 * pointsPerSurface, CV_32FC3);
|
||||
const float half = 0.5f * length;
|
||||
int idx = 0;
|
||||
for(int i = 0; i < pointsPerSurface; ++i, ++idx) // floor z=-half
|
||||
{
|
||||
data.at<cv::Vec3f>(0, idx) = cv::Vec3f(rng.uniform(-half, half), rng.uniform(-half, half),
|
||||
-half + static_cast<float>(rng.gaussian(0.005)));
|
||||
}
|
||||
for(int i = 0; i < pointsPerSurface; ++i, ++idx) // wall x=-half
|
||||
{
|
||||
data.at<cv::Vec3f>(0, idx) = cv::Vec3f(-half + static_cast<float>(rng.gaussian(0.005)),
|
||||
rng.uniform(-half, half), rng.uniform(-half, half));
|
||||
}
|
||||
for(int i = 0; i < pointsPerSurface; ++i, ++idx) // wall y=-half
|
||||
{
|
||||
data.at<cv::Vec3f>(0, idx) = cv::Vec3f(rng.uniform(-half, half),
|
||||
-half + static_cast<float>(rng.gaussian(0.005)), rng.uniform(-half, half));
|
||||
}
|
||||
return LaserScan(data, /*maxPoints=*/0, /*maxRange=*/0.0f, LaserScan::kXYZ);
|
||||
}
|
||||
|
||||
SensorData makeScanData(const LaserScan & scan, int id, double stamp)
|
||||
{
|
||||
SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setLaserScan(scan);
|
||||
return data;
|
||||
}
|
||||
|
||||
ParametersMap icpOdometryParameters(Odometry::Type type, bool force3DoF, bool guessMotion)
|
||||
{
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kOdomStrategy(), uNumber2Str(static_cast<int>(type))));
|
||||
parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1")); // ICP
|
||||
parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), force3DoF ? "true" : "false"));
|
||||
parameters.insert(ParametersPair(Parameters::kOdomGuessMotion(), guessMotion ? "true" : "false"));
|
||||
parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), "false"));
|
||||
parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0.0"));
|
||||
parameters.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.1"));
|
||||
return parameters;
|
||||
}
|
||||
|
||||
// Distance between two transforms, reported as translation and angle so a
|
||||
// failure says which part diverged.
|
||||
void expectPoseNear(const Transform & actual, const Transform & expected,
|
||||
float transTol, float angTolDeg, const std::string & what)
|
||||
{
|
||||
ASSERT_FALSE(actual.isNull()) << what << ": null transform";
|
||||
EXPECT_LT(actual.getDistance(expected), transTol)
|
||||
<< what << ": translation off -- got " << actual.prettyPrint()
|
||||
<< " expected " << expected.prettyPrint();
|
||||
const float angDeg = actual.getAngle(expected) * 180.0f / static_cast<float>(CV_PI);
|
||||
EXPECT_LT(angDeg, angTolDeg)
|
||||
<< what << ": rotation off by " << angDeg << " deg -- got " << actual.prettyPrint()
|
||||
<< " expected " << expected.prettyPrint();
|
||||
}
|
||||
|
||||
// Observe one corner from two viewpoints and return what odometry reports for
|
||||
// the second frame. `guess` may be null (no external guess).
|
||||
Transform runTwoScanOdometry(
|
||||
const ParametersMap & parameters,
|
||||
const LaserScan & corner,
|
||||
const Transform & motion,
|
||||
const Transform & guess,
|
||||
OdometryInfo * info)
|
||||
{
|
||||
std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
|
||||
if(!odometry)
|
||||
{
|
||||
return Transform();
|
||||
}
|
||||
// The corner is fixed in the world; the second scan is the same points
|
||||
// expressed in the moved sensor frame.
|
||||
SensorData first = makeScanData(corner, 1, 0.0);
|
||||
SensorData second = makeScanData(util3d::transformLaserScan(corner, motion.inverse()), 2, 0.1);
|
||||
|
||||
OdometryInfo firstInfo;
|
||||
const Transform firstPose = odometry->process(first, &firstInfo);
|
||||
if(firstPose.isNull())
|
||||
{
|
||||
return Transform();
|
||||
}
|
||||
return guess.isNull() ? odometry->process(second, info)
|
||||
: odometry->process(second, guess, info);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// 2D corner, no guess: ICP starts from identity and must find the motion from
|
||||
// the geometry alone.
|
||||
TEST_P(OdometryStrategyTest, Icp2DCornerRecoversMotionWithoutGuess)
|
||||
{
|
||||
const Transform motion(0.10f, 0.06f, 0.0f, 0.0f, 0.0f, 0.05f); // 12 cm / ~3 deg
|
||||
OdometryInfo info;
|
||||
const Transform pose = runTwoScanOdometry(
|
||||
icpOdometryParameters(GetParam(), /*force3DoF=*/true, /*guessMotion=*/false),
|
||||
makeCorner2D(), motion, Transform(), &info);
|
||||
if(pose.isNull() && info.reg.icpInliersRatio == 0.0f)
|
||||
{
|
||||
GTEST_SKIP() << "scan-matching odometry unavailable for this strategy";
|
||||
}
|
||||
expectPoseNear(pose, motion, 0.002f, 0.2f, "2D corner, no guess");
|
||||
}
|
||||
|
||||
// 3D corner, no guess: same in 6DoF, so the two walls plus floor have to pin
|
||||
// all three rotations as well.
|
||||
TEST_P(OdometryStrategyTest, Icp3DCornerRecoversMotionWithoutGuess)
|
||||
{
|
||||
const Transform motion(0.10f, 0.06f, 0.04f, 0.02f, 0.03f, 0.05f);
|
||||
OdometryInfo info;
|
||||
const Transform pose = runTwoScanOdometry(
|
||||
icpOdometryParameters(GetParam(), /*force3DoF=*/false, /*guessMotion=*/false),
|
||||
makeCorner3D(), motion, Transform(), &info);
|
||||
if(pose.isNull() && info.reg.icpInliersRatio == 0.0f)
|
||||
{
|
||||
GTEST_SKIP() << "scan-matching odometry unavailable for this strategy";
|
||||
}
|
||||
expectPoseNear(pose, motion, 0.002f, 0.2f, "3D corner, no guess");
|
||||
}
|
||||
|
||||
// With a guess that is deliberately off, the result must be closer to the truth
|
||||
// than the guess was -- i.e. ICP actually converged instead of returning the
|
||||
// guess unchanged, which is the failure mode this asserts against.
|
||||
TEST_P(OdometryStrategyTest, IcpConvergesFromOffsetGuess)
|
||||
{
|
||||
const Transform motion(0.10f, 0.06f, 0.0f, 0.0f, 0.0f, 0.05f);
|
||||
// Guess is in the right neighbourhood but ~4 cm and ~1.7 deg away.
|
||||
const Transform guess(0.14f, 0.03f, 0.0f, 0.0f, 0.0f, 0.02f);
|
||||
const float guessError = guess.getDistance(motion);
|
||||
|
||||
OdometryInfo info;
|
||||
const Transform pose = runTwoScanOdometry(
|
||||
icpOdometryParameters(GetParam(), /*force3DoF=*/true, /*guessMotion=*/false),
|
||||
makeCorner2D(), motion, guess, &info);
|
||||
if(pose.isNull() && info.reg.icpInliersRatio == 0.0f)
|
||||
{
|
||||
GTEST_SKIP() << "scan-matching odometry unavailable for this strategy";
|
||||
}
|
||||
ASSERT_FALSE(pose.isNull()) << "lost tracking despite a close guess";
|
||||
EXPECT_LT(pose.getDistance(motion), guessError)
|
||||
<< "result (" << pose.prettyPrint() << ") is no closer to the truth than the guess ("
|
||||
<< guess.prettyPrint() << "); ICP did not converge";
|
||||
expectPoseNear(pose, motion, 0.002f, 0.2f, "2D corner, offset guess");
|
||||
}
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
// them rather than backend-specific quirks.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Version.h>
|
||||
#include <rtabmap/core/Optimizer.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
@@ -870,6 +871,14 @@ protected:
|
||||
{
|
||||
GTEST_SKIP() << optimizerTypeName(t) << " not built in";
|
||||
}
|
||||
#ifndef RTABMAP_VERTIGO
|
||||
if(std::get<1>(GetParam()))
|
||||
{
|
||||
// Without Vertigo, optimize() warns and silently clears the robust
|
||||
// flag, so the corrupted loop closure is never rejected.
|
||||
GTEST_SKIP() << "robust optimization needs Vertigo (WITH_VERTIGO=ON)";
|
||||
}
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -3972,3 +3972,57 @@ TEST(RtabmapTest, GetGraphWithSignaturePayloadsAttachesRequestedFields)
|
||||
rtabmap.close(false);
|
||||
}
|
||||
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Ground-truth statistics. Rtabmap/ComputeRMSE is on by default, but the block
|
||||
// that computes it only runs when the memory holds ground-truth poses, which
|
||||
// requires SensorData::setGroundTruth() -- something no test did, so the whole
|
||||
// RMSE-vs-ground-truth path only ran during the end-to-end replays.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(RtabmapFixture, ComputesGroundTruthStatisticsWhenGroundTruthProvided)
|
||||
{
|
||||
ParametersMap parameters = defaultRtabmapParams();
|
||||
parameters[Parameters::kRtabmapComputeRMSE()] = "true";
|
||||
reinit(parameters);
|
||||
|
||||
// More than 5 matched poses on purpose: graph::calcRMSE() only aligns the
|
||||
// trajectories with an SVD fit above that count, and falls back to anchoring
|
||||
// on the first pose below it -- so a shorter run would exercise a degenerate
|
||||
// path and report a less meaningful error.
|
||||
//
|
||||
// Odometry drifts 5 cm per frame relative to the ground truth, so the RMSE
|
||||
// must come out non-zero.
|
||||
for(int i = 1; i <= 8; ++i)
|
||||
{
|
||||
SensorData data(image_);
|
||||
data.setId(i);
|
||||
const Transform truth(0.5f * i, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
const Transform odom (0.5f * i + 0.05f * i, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
data.setGroundTruth(truth);
|
||||
ASSERT_TRUE(rtabmap_->process(data, odom, covariance_)) << "frame " << i << " rejected";
|
||||
}
|
||||
|
||||
const std::map<std::string, float> & stats = rtabmap_->getStatistics().data();
|
||||
ASSERT_TRUE(stats.find(Statistics::kGtTranslational_rmse()) != stats.end())
|
||||
<< "no ground-truth RMSE statistic published";
|
||||
const float rmse = stats.at(Statistics::kGtTranslational_rmse());
|
||||
EXPECT_GT(rmse, 0.0f) << "ground truth differs from odometry, so RMSE cannot be zero";
|
||||
EXPECT_LT(rmse, 1.0f) << "implausible RMSE " << rmse << " for a 20 cm drift";
|
||||
|
||||
// The companion statistics come from the same block.
|
||||
EXPECT_TRUE(stats.find(Statistics::kGtTranslational_max()) != stats.end());
|
||||
EXPECT_TRUE(stats.find(Statistics::kGtRotational_rmse()) != stats.end());
|
||||
EXPECT_GE(stats.at(Statistics::kGtTranslational_max()), rmse)
|
||||
<< "max error should be at least the RMSE";
|
||||
}
|
||||
|
||||
// Without ground truth the same statistics must be absent rather than zero, so
|
||||
// a consumer can tell "not measured" from "perfect".
|
||||
TEST_F(RtabmapFixture, NoGroundTruthStatisticsWithoutGroundTruth)
|
||||
{
|
||||
ASSERT_TRUE(process());
|
||||
ASSERT_TRUE(process());
|
||||
const std::map<std::string, float> & stats = rtabmap_->getStatistics().data();
|
||||
EXPECT_TRUE(stats.find(Statistics::kGtTranslational_rmse()) == stats.end());
|
||||
}
|
||||
|
||||
@@ -1234,13 +1234,17 @@ TEST_F(RtabmapIntegrationFixture, PR2_Scan2D_RGBD_IcpReg)
|
||||
EXPECT_EQ(21, result.finalGlobalGraphSize);
|
||||
EXPECT_GE(result.proximityDetections, 1)
|
||||
<< "PR2 2D-scan dataset should produce proximity detections";
|
||||
// Observed: empty 22785-24111, obstacle 1302-1637. Range widened to
|
||||
// absorb run-to-run variance from the RANSAC correspondence rejector
|
||||
// installed in the PCL ICP path (util3d_registration.cpp).
|
||||
EXPECT_GE(result.gridEmptyCells, 22500);
|
||||
EXPECT_LE(result.gridEmptyCells, 24500);
|
||||
EXPECT_GE(result.gridObstacleCells, 1250);
|
||||
EXPECT_LE(result.gridObstacleCells, 1700);
|
||||
// Cell counts vary run to run: the RANSAC correspondence rejector in the
|
||||
// PCL ICP path (util3d_registration.cpp) shifts the registered scans
|
||||
// slightly, which moves how many cells they sweep. Observed empty
|
||||
// 22785-24111, obstacle 1302-1730. The previous caps sat ~4% above the
|
||||
// then-highest sample and were tripped by the next run, so these carry
|
||||
// ~10% headroom instead -- still far tighter than a broken grid (which
|
||||
// collapses toward 0 or changes by multiples), which is what they guard.
|
||||
EXPECT_GE(result.gridEmptyCells, 21500);
|
||||
EXPECT_LE(result.gridEmptyCells, 25500);
|
||||
EXPECT_GE(result.gridObstacleCells, 1150);
|
||||
EXPECT_LE(result.gridObstacleCells, 2000);
|
||||
#ifdef RTABMAP_OCTOMAP
|
||||
// 2D-laser-only signatures: nothing to assemble into a 3D OctoMap.
|
||||
EXPECT_EQ(0, result.octomapEmptyCells);
|
||||
@@ -1684,6 +1688,15 @@ TEST_F(RtabmapIntegrationFixture, RobustGraphOptimizationStereo)
|
||||
std::cerr << "[skip] " << v.label << " (optimizer unavailable)\n";
|
||||
continue;
|
||||
}
|
||||
#ifndef RTABMAP_VERTIGO
|
||||
if(v.robust)
|
||||
{
|
||||
// optimize() would warn and clear the robust flag, so this variant
|
||||
// would silently measure the non-robust path instead of failing.
|
||||
std::cerr << "[skip] " << v.label << " (needs Vertigo)\n";
|
||||
continue;
|
||||
}
|
||||
#endif
|
||||
SCOPED_TRACE(std::string(v.label));
|
||||
|
||||
ParametersMap params = baseRtabmapParams();
|
||||
|
||||
@@ -444,4 +444,3 @@ TEST(SensorCaptureTest, VeryLowFrameRate)
|
||||
EXPECT_GE(elapsed, 0.5);
|
||||
EXPECT_LT(elapsed, 0.75); // allow scheduler jitter on loaded CI runners (e.g. macOS)
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user