mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-10 03:49:50 +08:00
Added multisession_3it integration test (test memory management, multisession and dense/sparse bayes in that settings)
This commit is contained in:
@@ -55,8 +55,13 @@ jobs:
|
|||||||
if: steps.cache-testdata.outputs.cache-hit != 'true'
|
if: steps.cache-testdata.outputs.cache-hit != 'true'
|
||||||
shell: bash
|
shell: bash
|
||||||
run: |
|
run: |
|
||||||
# The ROS images are slim; fetch_test_data.sh needs curl.
|
# The ROS images are slim; fetch_test_data.sh needs curl, and 7-Zip for
|
||||||
|
# the assets that come as an archive. The package holding it is
|
||||||
|
# p7zip-full up to Ubuntu 24.04 and 7zip on the newer ones.
|
||||||
command -v curl >/dev/null || (apt-get update && apt-get install -y --no-install-recommends curl)
|
command -v curl >/dev/null || (apt-get update && apt-get install -y --no-install-recommends curl)
|
||||||
|
command -v 7z >/dev/null || command -v 7zz >/dev/null || (apt-get update && \
|
||||||
|
(apt-get install -y --no-install-recommends p7zip-full || \
|
||||||
|
apt-get install -y --no-install-recommends 7zip))
|
||||||
bash scripts/fetch_test_data.sh
|
bash scripts/fetch_test_data.sh
|
||||||
|
|
||||||
# `${{ github.workspace }}` is expanded by the runner on the *host*
|
# `${{ github.workspace }}` is expanded by the runner on the *host*
|
||||||
|
|||||||
@@ -14,6 +14,7 @@
|
|||||||
// pass.
|
// pass.
|
||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
#include <rtabmap/core/DBDriver.h>
|
||||||
#include <rtabmap/core/DBReader.h>
|
#include <rtabmap/core/DBReader.h>
|
||||||
#include <rtabmap/core/Features2d.h>
|
#include <rtabmap/core/Features2d.h>
|
||||||
#include <rtabmap/core/camera/CameraImages.h>
|
#include <rtabmap/core/camera/CameraImages.h>
|
||||||
@@ -94,6 +95,7 @@ struct ReplayResult
|
|||||||
int finalGlobalGraphSize = 0; // poses returned by Rtabmap::getGraph(global=true)
|
int finalGlobalGraphSize = 0; // poses returned by Rtabmap::getGraph(global=true)
|
||||||
std::map<int, Transform> finalLocalPoses; // optimized, global=false
|
std::map<int, Transform> finalLocalPoses; // optimized, global=false
|
||||||
std::map<int, Transform> finalGlobalPoses; // optimized, global=true
|
std::map<int, Transform> finalGlobalPoses; // optimized, global=true
|
||||||
|
std::multimap<int, Link> finalGlobalLinks; // constraints of the global graph
|
||||||
// Occupancy-grid cell counts after assembling the global grid from per-
|
// Occupancy-grid cell counts after assembling the global grid from per-
|
||||||
// node local maps (only populated when RGBD/CreateOccupancyGrid=true).
|
// node local maps (only populated when RGBD/CreateOccupancyGrid=true).
|
||||||
int gridEmptyCells = 0;
|
int gridEmptyCells = 0;
|
||||||
@@ -110,6 +112,16 @@ struct ReplayResult
|
|||||||
// Wall-clock seconds spent inside Odometry::process across all
|
// Wall-clock seconds spent inside Odometry::process across all
|
||||||
// frames; divide by framesRead to get per-frame average.
|
// frames; divide by framesRead to get per-frame average.
|
||||||
double odomTotalSeconds = 0.0;
|
double odomTotalSeconds = 0.0;
|
||||||
|
// Timing/Posterior_computation (ms) over the frames that reported it,
|
||||||
|
// which is the prediction and the multiplication of the Bayes filter.
|
||||||
|
double posteriorMsSum = 0.0;
|
||||||
|
float posteriorMsMin = -1.0f;
|
||||||
|
float posteriorMsMax = 0.0f;
|
||||||
|
int posteriorSamples = 0;
|
||||||
|
float posteriorMsAvg() const
|
||||||
|
{
|
||||||
|
return posteriorSamples > 0 ? (float)(posteriorMsSum/(double)posteriorSamples) : -1.0f;
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
// Synchronous replay: DBReader -> Odometry::process -> Rtabmap::process.
|
// Synchronous replay: DBReader -> Odometry::process -> Rtabmap::process.
|
||||||
@@ -415,8 +427,7 @@ ReplayResult replayDatabase(
|
|||||||
rtabmap.getGraph(result.finalLocalPoses, constraints,
|
rtabmap.getGraph(result.finalLocalPoses, constraints,
|
||||||
/*optimized=*/true, /*global=*/false);
|
/*optimized=*/true, /*global=*/false);
|
||||||
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
|
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
|
||||||
constraints.clear();
|
rtabmap.getGraph(result.finalGlobalPoses, result.finalGlobalLinks,
|
||||||
rtabmap.getGraph(result.finalGlobalPoses, constraints,
|
|
||||||
/*optimized=*/true, /*global=*/true);
|
/*optimized=*/true, /*global=*/true);
|
||||||
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
|
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
|
||||||
}
|
}
|
||||||
@@ -550,7 +561,15 @@ ReplayResult replayDatabaseWithStoredOdom(
|
|||||||
// When >0, points beyond this range (in meters) are dropped from
|
// When >0, points beyond this range (in meters) are dropped from
|
||||||
// the LaserScan of each SensorData after dbReader.takeData(),
|
// the LaserScan of each SensorData after dbReader.takeData(),
|
||||||
// simulating a lidar with a tighter max range.
|
// simulating a lidar with a tighter max range.
|
||||||
float scanMaxRange = 0.0f)
|
float scanMaxRange = 0.0f,
|
||||||
|
// Starts a new map on every frame whose stored odom covariance is
|
||||||
|
// the 9999 of a session start, which is how rtabmap-reprocess
|
||||||
|
// replays a database holding more than one session. Off by default:
|
||||||
|
// a test that wants its own boundaries uses the frame above.
|
||||||
|
bool triggerNewMapOnSessionStart = false,
|
||||||
|
// Filled with the number of sessions the replay ran, meaning one
|
||||||
|
// plus the boundaries it triggered on.
|
||||||
|
int * sessionsReplayed = 0)
|
||||||
{
|
{
|
||||||
ReplayResult result;
|
ReplayResult result;
|
||||||
|
|
||||||
@@ -656,6 +675,22 @@ ReplayResult replayDatabaseWithStoredOdom(
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Session boundary: the stored odom covariance of the first frame of
|
||||||
|
// a session is the 9999 that says the pose does not continue the
|
||||||
|
// previous one. The new map is started before that frame is
|
||||||
|
// processed, so it is the first of the new session rather than the
|
||||||
|
// last of the one before. Same as rtabmap-reprocess.
|
||||||
|
if(triggerNewMapOnSessionStart
|
||||||
|
&& result.framesProcessed > 0
|
||||||
|
&& info.odomCovariance.at<double>(0, 0) >= 9999.0)
|
||||||
|
{
|
||||||
|
rtabmap.triggerNewMap();
|
||||||
|
if(sessionsReplayed)
|
||||||
|
{
|
||||||
|
++*sessionsReplayed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
SensorData rtabmapData = data;
|
SensorData rtabmapData = data;
|
||||||
if(throttle)
|
if(throttle)
|
||||||
{
|
{
|
||||||
@@ -705,6 +740,17 @@ ReplayResult replayDatabaseWithStoredOdom(
|
|||||||
{
|
{
|
||||||
result.translationalRmseFinal = rmseIt->second;
|
result.translationalRmseFinal = rmseIt->second;
|
||||||
}
|
}
|
||||||
|
const auto postIt = stats.data().find(Statistics::kTimingPosterior_computation());
|
||||||
|
if(postIt != stats.data().end())
|
||||||
|
{
|
||||||
|
result.posteriorMsSum += postIt->second;
|
||||||
|
result.posteriorMsMax = std::max(result.posteriorMsMax, postIt->second);
|
||||||
|
if(result.posteriorMsMin < 0.0f || postIt->second < result.posteriorMsMin)
|
||||||
|
{
|
||||||
|
result.posteriorMsMin = postIt->second;
|
||||||
|
}
|
||||||
|
++result.posteriorSamples;
|
||||||
|
}
|
||||||
data = dbReader.takeData(&info);
|
data = dbReader.takeData(&info);
|
||||||
applyScanRangeFilter(data);
|
applyScanRangeFilter(data);
|
||||||
}
|
}
|
||||||
@@ -714,8 +760,7 @@ ReplayResult replayDatabaseWithStoredOdom(
|
|||||||
rtabmap.getGraph(result.finalLocalPoses, constraints,
|
rtabmap.getGraph(result.finalLocalPoses, constraints,
|
||||||
/*optimized=*/true, /*global=*/false);
|
/*optimized=*/true, /*global=*/false);
|
||||||
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
|
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
|
||||||
constraints.clear();
|
rtabmap.getGraph(result.finalGlobalPoses, result.finalGlobalLinks,
|
||||||
rtabmap.getGraph(result.finalGlobalPoses, constraints,
|
|
||||||
/*optimized=*/true, /*global=*/true);
|
/*optimized=*/true, /*global=*/true);
|
||||||
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
|
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
|
||||||
}
|
}
|
||||||
@@ -733,12 +778,84 @@ ReplayResult replayDatabaseWithStoredOdom(
|
|||||||
<< " localGraph=" << result.finalLocalGraphSize
|
<< " localGraph=" << result.finalLocalGraphSize
|
||||||
<< " globalGraph=" << result.finalGlobalGraphSize
|
<< " globalGraph=" << result.finalGlobalGraphSize
|
||||||
<< " rmse=" << result.translationalRmseFinal << "m"
|
<< " rmse=" << result.translationalRmseFinal << "m"
|
||||||
|
<< " posterior(avg/min/max)=" << result.posteriorMsAvg()
|
||||||
|
<< "/" << result.posteriorMsMin << "/" << result.posteriorMsMax << "ms"
|
||||||
<< " wall=" << result.replayWallSeconds << "s"
|
<< " wall=" << result.replayWallSeconds << "s"
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Where a node sits in the union-find of countConnectedComponents(), the path
|
||||||
|
// to it halved on the way up.
|
||||||
|
int graphComponentRoot(std::map<int, int> & parent, int id)
|
||||||
|
{
|
||||||
|
while(parent.at(id) != id)
|
||||||
|
{
|
||||||
|
const int up = parent.at(id);
|
||||||
|
parent.at(id) = parent.at(up);
|
||||||
|
id = up;
|
||||||
|
}
|
||||||
|
return id;
|
||||||
|
}
|
||||||
|
|
||||||
|
// How many pieces a graph is in: the nodes linked to each other, directly or
|
||||||
|
// through others, are one of them. Two sessions that never closed a loop with
|
||||||
|
// each other are two.
|
||||||
|
int countConnectedComponents(
|
||||||
|
const std::map<int, Transform> & poses,
|
||||||
|
const std::multimap<int, Link> & links)
|
||||||
|
{
|
||||||
|
std::map<int, int> parent;
|
||||||
|
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
|
||||||
|
{
|
||||||
|
parent.insert(std::make_pair(iter->first, iter->first));
|
||||||
|
}
|
||||||
|
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||||
|
{
|
||||||
|
// A link to a landmark, or to a node the graph does not hold, joins
|
||||||
|
// nothing here.
|
||||||
|
if(parent.find(iter->second.from()) == parent.end() ||
|
||||||
|
parent.find(iter->second.to()) == parent.end())
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const int a = graphComponentRoot(parent, iter->second.from());
|
||||||
|
const int b = graphComponentRoot(parent, iter->second.to());
|
||||||
|
if(a != b)
|
||||||
|
{
|
||||||
|
parent.at(a) = b;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::set<int> roots;
|
||||||
|
for(std::map<int, int>::const_iterator iter=parent.begin(); iter!=parent.end(); ++iter)
|
||||||
|
{
|
||||||
|
roots.insert(graphComponentRoot(parent, iter->first));
|
||||||
|
}
|
||||||
|
return (int)roots.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
// The optimized graph a database holds, which is what its own mapping session
|
||||||
|
// converged to and what a replay of it is compared against, together with the
|
||||||
|
// parameters it was recorded with, which the replay is run with.
|
||||||
|
bool loadDatabaseGraphAndParameters(
|
||||||
|
const std::string & path,
|
||||||
|
std::map<int, Transform> & optimizedPoses,
|
||||||
|
ParametersMap & parameters)
|
||||||
|
{
|
||||||
|
DBDriver * driver = DBDriver::create();
|
||||||
|
if(!driver->openConnection(path))
|
||||||
|
{
|
||||||
|
delete driver;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
optimizedPoses = driver->loadOptimizedPoses();
|
||||||
|
parameters = driver->getLastParameters();
|
||||||
|
driver->closeConnection(false);
|
||||||
|
delete driver;
|
||||||
|
return !optimizedPoses.empty();
|
||||||
|
}
|
||||||
|
|
||||||
ParametersMap baseRtabmapParams()
|
ParametersMap baseRtabmapParams()
|
||||||
{
|
{
|
||||||
ParametersMap params;
|
ParametersMap params;
|
||||||
@@ -1990,16 +2107,210 @@ TEST_F(RtabmapIntegrationFixture, Loop3ItGps)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ---------------------------------------------------------------------------
|
// ---------------------------------------------------------------------------
|
||||||
// Appearance-only loop closure on the 84-image `data/samples` set with the
|
// Multi-session 2D lidar + SIFT (3 sessions, 935 nodes, ~270 m).
|
||||||
// shipped `data/samples_GT.bmp` ground truth. Measures recall at 100%
|
//
|
||||||
// precision (the rtabmap "max recall while no false positive has appeared
|
// The optimized graph the database already holds is the reference: it is what
|
||||||
// yet" metric — same definition as the legacy MATLAB getPrecisionRecall.m
|
// the session that recorded it converged to, so replaying the same frames with
|
||||||
// script) for every Features2D detector strategy that is available in this
|
// the same parameters has to land on the same trajectory and find about as many
|
||||||
// build. Detector strategies for which Feature2D::create() silently
|
// loop closures. The database keeps no images, so the replay reuses the
|
||||||
// substitutes a different backend (e.g. SURF -> SIFT without nonfree,
|
// features stored with each node (Mem/UseOdomFeatures) rather than extracting
|
||||||
// SuperPointTorch -> GFTT/ORB without RTABMAP_TORCH) are skipped.
|
// any, and the scans it does keep are what the ICP registration verifies loop
|
||||||
|
// closures with.
|
||||||
|
//
|
||||||
|
// Each run is done with the prediction of the Bayes filter held both as a
|
||||||
|
// matrix and in its sparse form: the two are the same probabilities, so a real
|
||||||
|
// session over a real graph has to come out the same either way.
|
||||||
// ---------------------------------------------------------------------------
|
// ---------------------------------------------------------------------------
|
||||||
|
TEST_F(RtabmapIntegrationFixture, Multisession3It)
|
||||||
|
{
|
||||||
|
const std::string srcPath = testDataPath("multisession_3it.db");
|
||||||
|
SKIP_IF_MISSING(srcPath);
|
||||||
|
|
||||||
|
std::map<int, Transform> goldenPoses;
|
||||||
|
ParametersMap dbParams;
|
||||||
|
ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
|
||||||
|
<< "No optimized graph in " << srcPath;
|
||||||
|
std::cerr << "[ ] Reference graph: " << goldenPoses.size()
|
||||||
|
<< " poses, recorded with " << dbParams.size() << " parameters\n";
|
||||||
|
|
||||||
|
for(const bool sparsePrediction : {false, true})
|
||||||
|
{
|
||||||
|
const std::string label = sparsePrediction ? "sparse" : "dense";
|
||||||
|
SCOPED_TRACE(label);
|
||||||
|
|
||||||
|
// The parameters the database was recorded with, and on top of them
|
||||||
|
// what replaying instead of recording needs.
|
||||||
|
ParametersMap params = dbParams;
|
||||||
|
uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(),
|
||||||
|
sparsePrediction ? "true" : "false"));
|
||||||
|
// The nodes of the database are already the ones its detection rate
|
||||||
|
// kept, so every frame the reader hands over is processed.
|
||||||
|
uInsert(params, ParametersPair(Parameters::kRtabmapDetectionRate(), "0"));
|
||||||
|
uInsert(params, ParametersPair(Parameters::kMemUseOdomFeatures(), "true"));
|
||||||
|
uInsert(params, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false"));
|
||||||
|
|
||||||
|
const std::string workDb = test::tempPath(uFormat(
|
||||||
|
"rtabmap_integration_Multisession3It_%s.db", label.c_str()));
|
||||||
|
std::cerr << "Working DB for " << label << ": " << workDb << "\n";
|
||||||
|
|
||||||
|
int sessions = 1;
|
||||||
|
const ReplayResult result = replayDatabaseWithStoredOdom(
|
||||||
|
srcPath, workDb, params,
|
||||||
|
/*triggerNewMapAfterFrame=*/-1,
|
||||||
|
/*overrideOdomAngularVariance=*/-1.0,
|
||||||
|
/*overrideOdomLinearVariance=*/-1.0,
|
||||||
|
/*scanMaxRange=*/0.0f,
|
||||||
|
/*triggerNewMapOnSessionStart=*/true,
|
||||||
|
&sessions);
|
||||||
|
|
||||||
|
ASSERT_GT(result.framesProcessed, 0) << label << " produced no frames";
|
||||||
|
ASSERT_GT(result.finalGlobalGraphSize, 0) << label << " produced an empty graph";
|
||||||
|
|
||||||
|
float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
|
||||||
|
float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
|
||||||
|
graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
|
||||||
|
tRmse, tMean, tMed, tStd, tMin, tMax,
|
||||||
|
rRmse, rMean, rMed, rStd, rMin, rMax,
|
||||||
|
/*align2D=*/false);
|
||||||
|
std::cerr << "[" << label << "] sessions=" << sessions
|
||||||
|
<< " nodes=" << result.finalGlobalGraphSize
|
||||||
|
<< " loops=" << result.loopClosuresAccepted
|
||||||
|
<< " rejected=" << result.loopClosuresRejected
|
||||||
|
<< " proximity=" << result.proximityDetections
|
||||||
|
<< " trans rmse=" << tRmse << "m max=" << tMax << "m"
|
||||||
|
<< " rot rmse=" << rRmse << "deg max=" << rMax << "deg"
|
||||||
|
<< " posterior avg=" << result.posteriorMsAvg()
|
||||||
|
<< "ms min=" << result.posteriorMsMin
|
||||||
|
<< "ms max=" << result.posteriorMsMax << "ms\n";
|
||||||
|
|
||||||
|
// Every frame of the database is a node it kept, so the replay makes
|
||||||
|
// the same number of them, in the same three sessions.
|
||||||
|
EXPECT_EQ(sessions, 3) << label;
|
||||||
|
EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << label;
|
||||||
|
|
||||||
|
// The same frames, the same parameters and the same features, so the
|
||||||
|
// trajectory comes out on top of the one the database holds: 7-18 cm
|
||||||
|
// and under 1.2 deg over five runs, dense and sparse alike.
|
||||||
|
//
|
||||||
|
// The replay is not the same twice, whichever form the prediction is
|
||||||
|
// held in: the visual word search is over randomized kd-trees, so a
|
||||||
|
// hypothesis sitting on the loop closure threshold falls either side
|
||||||
|
// of it from one run to the next, and one loop closure more or less
|
||||||
|
// pulls the graph. The bands are what that spread asks for, not what
|
||||||
|
// a single run would allow.
|
||||||
|
EXPECT_LT(tRmse, 0.5f) << label << " trajectory drifted from the reference";
|
||||||
|
EXPECT_LT(rRmse, 3.0f) << label << " orientation drifted from the reference";
|
||||||
|
|
||||||
|
// The reference graph holds 285 global loop closures; the replay finds
|
||||||
|
// 284-293 of them over five runs.
|
||||||
|
EXPECT_GT(result.loopClosuresAccepted, 240) << label << " found too few loop closures";
|
||||||
|
EXPECT_LT(result.loopClosuresAccepted, 340) << label << " found more loop closures than the reference";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ---------------------------------------------------------------------------
|
||||||
|
// The same replay under memory management: Rtabmap/MemoryThr caps the working
|
||||||
|
// memory, so the oldest nodes are transferred to long-term memory as the map
|
||||||
|
// grows and only a window of it is ever held, a loop closure hypothesis on a
|
||||||
|
// node that left bringing it back. The global optimized graph still covers
|
||||||
|
// every node, so it is compared against the same reference, and the loop
|
||||||
|
// closures found from a limited working memory are fewer.
|
||||||
|
// ---------------------------------------------------------------------------
|
||||||
|
TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
|
||||||
|
{
|
||||||
|
const std::string srcPath = testDataPath("multisession_3it.db");
|
||||||
|
SKIP_IF_MISSING(srcPath);
|
||||||
|
|
||||||
|
std::map<int, Transform> goldenPoses;
|
||||||
|
ParametersMap dbParams;
|
||||||
|
ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
|
||||||
|
<< "No optimized graph in " << srcPath;
|
||||||
|
|
||||||
|
for(const bool sparsePrediction : {false, true})
|
||||||
|
{
|
||||||
|
const std::string label = sparsePrediction ? "sparse" : "dense";
|
||||||
|
SCOPED_TRACE(label);
|
||||||
|
|
||||||
|
ParametersMap params = dbParams;
|
||||||
|
uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(),
|
||||||
|
sparsePrediction ? "true" : "false"));
|
||||||
|
uInsert(params, ParametersPair(Parameters::kRtabmapDetectionRate(), "0"));
|
||||||
|
uInsert(params, ParametersPair(Parameters::kMemUseOdomFeatures(), "true"));
|
||||||
|
uInsert(params, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false"));
|
||||||
|
uInsert(params, ParametersPair(Parameters::kRtabmapMemoryThr(), "300"));
|
||||||
|
|
||||||
|
const std::string workDb = test::tempPath(uFormat(
|
||||||
|
"rtabmap_integration_Multisession3ItMemoryThr_%s.db", label.c_str()));
|
||||||
|
std::cerr << "Working DB for " << label << ": " << workDb << "\n";
|
||||||
|
|
||||||
|
int sessions = 1;
|
||||||
|
const ReplayResult result = replayDatabaseWithStoredOdom(
|
||||||
|
srcPath, workDb, params,
|
||||||
|
/*triggerNewMapAfterFrame=*/-1,
|
||||||
|
/*overrideOdomAngularVariance=*/-1.0,
|
||||||
|
/*overrideOdomLinearVariance=*/-1.0,
|
||||||
|
/*scanMaxRange=*/0.0f,
|
||||||
|
/*triggerNewMapOnSessionStart=*/true,
|
||||||
|
&sessions);
|
||||||
|
|
||||||
|
ASSERT_GT(result.framesProcessed, 0) << label << " produced no frames";
|
||||||
|
ASSERT_GT(result.finalGlobalGraphSize, 0) << label << " produced an empty graph";
|
||||||
|
|
||||||
|
const int components = countConnectedComponents(
|
||||||
|
result.finalGlobalPoses, result.finalGlobalLinks);
|
||||||
|
|
||||||
|
float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
|
||||||
|
float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
|
||||||
|
graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
|
||||||
|
tRmse, tMean, tMed, tStd, tMin, tMax,
|
||||||
|
rRmse, rMean, rMed, rStd, rMin, rMax,
|
||||||
|
/*align2D=*/false);
|
||||||
|
std::cerr << "[" << label << "] sessions=" << sessions
|
||||||
|
<< " nodes=" << result.finalGlobalGraphSize
|
||||||
|
<< " components=" << components
|
||||||
|
<< " localGraph=" << result.finalLocalGraphSize
|
||||||
|
<< " loops=" << result.loopClosuresAccepted
|
||||||
|
<< " rejected=" << result.loopClosuresRejected
|
||||||
|
<< " proximity=" << result.proximityDetections
|
||||||
|
<< " trans rmse=" << tRmse << "m max=" << tMax << "m"
|
||||||
|
<< " rot rmse=" << rRmse << "deg max=" << rMax << "deg"
|
||||||
|
<< " posterior avg=" << result.posteriorMsAvg()
|
||||||
|
<< "ms min=" << result.posteriorMsMin
|
||||||
|
<< "ms max=" << result.posteriorMsMax << "ms\n";
|
||||||
|
|
||||||
|
// Nothing is lost: the nodes leave the working memory for long-term
|
||||||
|
// memory, and the global graph still covers every one of them, in one
|
||||||
|
// piece.
|
||||||
|
EXPECT_EQ(sessions, 3) << label;
|
||||||
|
EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << label;
|
||||||
|
EXPECT_EQ(components, 1) << label << " graph came out in pieces";
|
||||||
|
|
||||||
|
// The working memory is the part that is capped, and it stays there:
|
||||||
|
// observed at 247-270 of the 935 nodes. Anything near 935 would mean
|
||||||
|
// the cap never took effect and the test is no longer about memory
|
||||||
|
// management.
|
||||||
|
EXPECT_LT(result.finalLocalGraphSize, 400)
|
||||||
|
<< label << " working memory was not capped";
|
||||||
|
|
||||||
|
// Loop closures are only found against what the working memory holds,
|
||||||
|
// retrieval bringing back what a hypothesis points at, so somewhat
|
||||||
|
// fewer than the 284-293 of the uncapped replay: 259-284 over several
|
||||||
|
// runs.
|
||||||
|
EXPECT_GT(result.loopClosuresAccepted, 200) << label << " found too few loop closures";
|
||||||
|
|
||||||
|
// The trajectory holds up on the strength of those, at 0.10-0.52 m and
|
||||||
|
// 1.0-2.4 deg over several runs against the 7-18 cm of the uncapped
|
||||||
|
// replay. A loop closure that is not found leaves a stretch of the map
|
||||||
|
// on odometry alone, and this database drifts about half a meter over
|
||||||
|
// such a stretch, so which loop closures a run happens to find moves
|
||||||
|
// the result far more than it does without the cap.
|
||||||
|
EXPECT_LT(tRmse, 1.5f) << label << " trajectory drifted from the reference";
|
||||||
|
EXPECT_LT(rRmse, 6.0f) << label << " orientation drifted from the reference";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
|
TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
|
||||||
{
|
{
|
||||||
const std::string samplesDir = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";
|
const std::string samplesDir = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";
|
||||||
|
|||||||
@@ -1,6 +1,8 @@
|
|||||||
# Test data fetched by scripts/fetch_test_data.sh -- never tracked.
|
# Test data fetched by scripts/fetch_test_data.sh -- never tracked.
|
||||||
# The manifest (basename / source URL / sha256) is the source of truth.
|
# The manifest (basename / source URL / sha256) is the source of truth.
|
||||||
*.db
|
*.db
|
||||||
|
*.7z
|
||||||
|
*.zip
|
||||||
*.pt
|
*.pt
|
||||||
*.pth
|
*.pth
|
||||||
*.py
|
*.py
|
||||||
|
|||||||
+11
-4
@@ -1,14 +1,20 @@
|
|||||||
# Test data assets fetched by scripts/fetch_test_data.sh.
|
# Test data assets fetched by scripts/fetch_test_data.sh.
|
||||||
# Format: <basename>\t<source>\t<sha256>
|
# Format: <basename>\t<source>\t<sha256>[\t<extracted>]
|
||||||
# - basename is the file's name on disk under data/tests/
|
# - basename is the file's name on disk under data/tests/
|
||||||
# - source is one of:
|
# - source is one of:
|
||||||
# * a Google Drive file ID (from https://drive.google.com/file/d/<ID>/...);
|
# * a Google Drive file ID (from https://drive.google.com/file/d/<ID>/...);
|
||||||
# assembled into a direct-download URL by the fetch script.
|
# assembled into a direct-download URL by the fetch script.
|
||||||
# * a full http(s):// URL; used as-is. Append "&resourcekey=<key>" to a
|
# * a full http(s):// URL; used as-is. Append "&resourcekey=<key>" to a
|
||||||
# GDrive URL for legacy files (uploaded before 2017) that require it.
|
# GDrive URL for legacy files (uploaded before 2017) that require it.
|
||||||
# - sha256 must match the downloaded bytes (compute with `sha256sum file.db`).
|
# - sha256 must match the bytes of the file the tests read (compute with
|
||||||
# Leave as TODO_FILL_SHA256 to bypass the integrity check on first download
|
# `sha256sum file.db`), which for an archive is the file it holds rather than
|
||||||
# then paste the actual hash back in.
|
# the archive itself: a corrupt archive is caught by the extractor. Leave as
|
||||||
|
# TODO_FILL_SHA256 to bypass the integrity check on first download then paste
|
||||||
|
# the actual hash back in.
|
||||||
|
# - extracted is optional and only for archives (.7z, .zip): the file the
|
||||||
|
# archive holds, which the fetch script extracts into data/tests/ and which
|
||||||
|
# the tests open. The archive is removed once extracted, so this file is both
|
||||||
|
# what the sha256 is of and what tells the script the asset is already there.
|
||||||
#
|
#
|
||||||
# Lines starting with '#' and blank lines are ignored.
|
# Lines starting with '#' and blank lines are ignored.
|
||||||
netherdrone_lidar3d_sample_15s.db 1IBE8aMgY1_kmme_7Kb9W2wz6yBMLl63i 3fedb26d76d080e687d3926ab80cadee27d7c0115638562a75c1d9f08cc1c885
|
netherdrone_lidar3d_sample_15s.db 1IBE8aMgY1_kmme_7Kb9W2wz6yBMLl63i 3fedb26d76d080e687d3926ab80cadee27d7c0115638562a75c1d9f08cc1c885
|
||||||
@@ -19,6 +25,7 @@ pr2_scan2d_corridor_50s.db 18ClEPTRM98icorrlDYBzKTWRwVOU8-bh f518cb4d9284b24a150
|
|||||||
robust_graph_optimization_stereo.db https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/robust_graph_optimization_stereo.db 247694b5bdb82168ebe88bb6bb5bc31c7142234f8c64567afd93e464418cf02e
|
robust_graph_optimization_stereo.db https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/robust_graph_optimization_stereo.db 247694b5bdb82168ebe88bb6bb5bc31c7142234f8c64567afd93e464418cf02e
|
||||||
loop_3it_gps.db https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/loop_3it_gps.db 7aefeb573107a27b0a7826cb87ea03db495367aa56868b4d2b2543e6cacad3c8
|
loop_3it_gps.db https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/loop_3it_gps.db 7aefeb573107a27b0a7826cb87ea03db495367aa56868b4d2b2543e6cacad3c8
|
||||||
stereo_20Hz.db https://github.com/introlab/rtabmap/releases/download/0.23.1/stereo_20Hz.db 94e219e1c96e540cbb1490cc23c81c65b9e7b132e8d61eda05e7990bc13393b7
|
stereo_20Hz.db https://github.com/introlab/rtabmap/releases/download/0.23.1/stereo_20Hz.db 94e219e1c96e540cbb1490cc23c81c65b9e7b132e8d61eda05e7990bc13393b7
|
||||||
|
multisession_3it.7z 10ulRDMqQy5V_3_kx_IeNzAJ8iUMdkkBZ 7b0b02108f05516870eea42cbb519aa6c46a0ffd1eb3e525a9920d3b1997b6f4 multisession_3it.db
|
||||||
superpoint_v1.pth https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth 52b6708629640ca883673b5d5c097c4ddad37d8048b33f09c8ca0d69db12c40e
|
superpoint_v1.pth https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth 52b6708629640ca883673b5d5c097c4ddad37d8048b33f09c8ca0d69db12c40e
|
||||||
superpoint_v6_from_tf.pth https://github.com/rpautrat/SuperPoint/raw/refs/heads/master/weights/superpoint_v6_from_tf.pth cd5d19a5061848e248c17728878ea166b66512076d43c77dbcf27f4a88a56084
|
superpoint_v6_from_tf.pth https://github.com/rpautrat/SuperPoint/raw/refs/heads/master/weights/superpoint_v6_from_tf.pth cd5d19a5061848e248c17728878ea166b66512076d43c77dbcf27f4a88a56084
|
||||||
demo_superpoint.py https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py 613706ae7e9ce3fbc2cfe042fc3f37d739838cc2e1dc8ddd08f8ec037765df04
|
demo_superpoint.py https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py 613706ae7e9ce3fbc2cfe042fc3f37d739838cc2e1dc8ddd08f8ec037765df04
|
||||||
|
|||||||
+89
-15
@@ -1,13 +1,16 @@
|
|||||||
#!/usr/bin/env bash
|
#!/usr/bin/env bash
|
||||||
# Fetch test data assets listed in data/tests/manifest.txt. Each entry's
|
# Fetch test data assets listed in data/tests/manifest.txt. Each entry's
|
||||||
# source can be either a bare Google Drive file ID (assembled into the
|
# source can be either a bare Google Drive file ID (assembled into a
|
||||||
# uc?export=download&id=... URL) or a full http(s):// URL (used as-is).
|
# direct-download URL) or a full http(s):// URL (used as-is). An entry with a
|
||||||
# Skips files that are already present and whose SHA-256 matches the manifest.
|
# fourth column is an archive (.7z or .zip), which is extracted into data/tests/
|
||||||
# Intended for CI and local first-time setup.
|
# and then removed, its SHA-256 being the one of the file it holds. Skips files
|
||||||
|
# that are already present and whose SHA-256 matches the manifest. Intended for
|
||||||
|
# CI and local first-time setup.
|
||||||
#
|
#
|
||||||
# All linked files are public and under ~100 MB, so the direct GDrive download
|
# All linked files are public. The GDrive URL carries confirm=t, which is what
|
||||||
# URL streams the bytes directly without the virus-scan interstitial that
|
# answers the interstitial page Drive puts in front of a file it did not scan
|
||||||
# would otherwise need a tool like gdown.
|
# for viruses (it never scans an archive, whatever its size), so the bytes come
|
||||||
|
# down without needing a tool like gdown.
|
||||||
set -euo pipefail
|
set -euo pipefail
|
||||||
|
|
||||||
REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||||
@@ -30,6 +33,51 @@ else
|
|||||||
exit 1
|
exit 1
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
# Extracts an archive into DEST_DIR. Which tool is there varies: every CI runner
|
||||||
|
# image ships 7-Zip, but it is 7z on some and 7zz on the ones carrying Debian's
|
||||||
|
# 7zip package, the slim ROS images have neither until the workflow installs one,
|
||||||
|
# and bsdtar reads 7z through libarchive where no 7-Zip is to be found (macOS tar
|
||||||
|
# is bsdtar). Same for zip: unzip where there is one, python3's zipfile where
|
||||||
|
# there is not (Git-Bash-on-Windows ships without unzip).
|
||||||
|
extract_archive() {
|
||||||
|
local archive="$1"
|
||||||
|
case "$archive" in
|
||||||
|
*.7z)
|
||||||
|
local sevenzip=""
|
||||||
|
for candidate in 7z 7zz 7za 7zr; do
|
||||||
|
if command -v "$candidate" >/dev/null 2>&1; then
|
||||||
|
sevenzip="$candidate"
|
||||||
|
break
|
||||||
|
fi
|
||||||
|
done
|
||||||
|
if [[ -n "$sevenzip" ]]; then
|
||||||
|
"$sevenzip" x -y -o"$DEST_DIR" "$archive" >/dev/null
|
||||||
|
elif command -v bsdtar >/dev/null 2>&1; then
|
||||||
|
bsdtar -x -f "$archive" -C "$DEST_DIR"
|
||||||
|
elif tar --version 2>/dev/null | grep -qi bsdtar; then
|
||||||
|
tar -x -f "$archive" -C "$DEST_DIR"
|
||||||
|
else
|
||||||
|
echo "Error: no 7z, 7zz, 7za, 7zr or bsdtar on PATH to extract $archive" >&2
|
||||||
|
return 1
|
||||||
|
fi
|
||||||
|
;;
|
||||||
|
*.zip)
|
||||||
|
if command -v unzip >/dev/null 2>&1; then
|
||||||
|
unzip -o -q "$archive" -d "$DEST_DIR"
|
||||||
|
elif command -v python3 >/dev/null 2>&1; then
|
||||||
|
python3 -m zipfile -e "$archive" "$DEST_DIR"
|
||||||
|
else
|
||||||
|
echo "Error: neither unzip nor python3 is on PATH to extract $archive" >&2
|
||||||
|
return 1
|
||||||
|
fi
|
||||||
|
;;
|
||||||
|
*)
|
||||||
|
echo "Error: $archive is not an archive this script extracts (.7z, .zip)" >&2
|
||||||
|
return 1
|
||||||
|
;;
|
||||||
|
esac
|
||||||
|
}
|
||||||
|
|
||||||
verify_sha() {
|
verify_sha() {
|
||||||
local file="$1" expected="$2"
|
local file="$1" expected="$2"
|
||||||
if [[ "$expected" == "TODO_FILL_SHA256" || -z "$expected" ]]; then
|
if [[ "$expected" == "TODO_FILL_SHA256" || -z "$expected" ]]; then
|
||||||
@@ -44,7 +92,7 @@ verify_sha() {
|
|||||||
fi
|
fi
|
||||||
}
|
}
|
||||||
|
|
||||||
while IFS=$'\t' read -r name source expected_sha; do
|
while IFS=$'\t' read -r name source expected_sha extracted; do
|
||||||
# Strip trailing CR so the script works when manifest.txt is checked out
|
# Strip trailing CR so the script works when manifest.txt is checked out
|
||||||
# with CRLF line endings (default on Windows Git unless core.autocrlf=input).
|
# with CRLF line endings (default on Windows Git unless core.autocrlf=input).
|
||||||
# Without this, expected_sha keeps a trailing \r and even a byte-for-byte
|
# Without this, expected_sha keeps a trailing \r and even a byte-for-byte
|
||||||
@@ -52,12 +100,17 @@ while IFS=$'\t' read -r name source expected_sha; do
|
|||||||
name="${name%$'\r'}"
|
name="${name%$'\r'}"
|
||||||
source="${source%$'\r'}"
|
source="${source%$'\r'}"
|
||||||
expected_sha="${expected_sha%$'\r'}"
|
expected_sha="${expected_sha%$'\r'}"
|
||||||
|
extracted="${extracted-}"
|
||||||
|
extracted="${extracted%$'\r'}"
|
||||||
# Skip comments and blank lines.
|
# Skip comments and blank lines.
|
||||||
[[ -z "${name// }" || "$name" =~ ^# ]] && continue
|
[[ -z "${name// }" || "$name" =~ ^# ]] && continue
|
||||||
|
|
||||||
target="$DEST_DIR/$name"
|
target="$DEST_DIR/$name"
|
||||||
if [[ -f "$target" ]] && verify_sha "$target" "$expected_sha" 2>/dev/null; then
|
# An archive is not kept once it is extracted, so what has to be there, and
|
||||||
echo "Already up-to-date: $name"
|
# what the sha is of, is the file it held.
|
||||||
|
kept="${extracted:-$name}"
|
||||||
|
if [[ -f "$DEST_DIR/$kept" ]] && verify_sha "$DEST_DIR/$kept" "$expected_sha" 2>/dev/null; then
|
||||||
|
echo "Already up-to-date: $kept"
|
||||||
continue
|
continue
|
||||||
fi
|
fi
|
||||||
|
|
||||||
@@ -66,17 +119,38 @@ while IFS=$'\t' read -r name source expected_sha; do
|
|||||||
if [[ "$source" =~ ^https?:// ]]; then
|
if [[ "$source" =~ ^https?:// ]]; then
|
||||||
url="$source"
|
url="$source"
|
||||||
else
|
else
|
||||||
url="https://drive.google.com/uc?export=download&id=${source}"
|
url="https://drive.usercontent.google.com/download?id=${source}&export=download&confirm=t"
|
||||||
fi
|
fi
|
||||||
echo "Fetching $name <- $url"
|
echo "Fetching $name <- $url"
|
||||||
mkdir -p "$(dirname "$target")"
|
mkdir -p "$(dirname "$target")"
|
||||||
# -L follows the redirect, -f fails on HTTP errors, -S shows errors on stderr.
|
# -L follows the redirect, -f fails on HTTP errors, -S shows errors on stderr.
|
||||||
curl -fsSL "$url" -o "$target.partial"
|
curl -fsSL "$url" -o "$target.partial"
|
||||||
if ! verify_sha "$target.partial" "$expected_sha"; then
|
|
||||||
rm -f "$target.partial"
|
if [[ -z "$extracted" ]]; then
|
||||||
exit 1
|
if ! verify_sha "$target.partial" "$expected_sha"; then
|
||||||
|
rm -f "$target.partial"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
mv -f "$target.partial" "$target"
|
||||||
|
else
|
||||||
|
mv -f "$target.partial" "$target"
|
||||||
|
echo " Extracting $name"
|
||||||
|
if ! extract_archive "$target"; then
|
||||||
|
rm -f "$target"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
# The archive has served its purpose, and these are large files.
|
||||||
|
rm -f "$target"
|
||||||
|
if [[ ! -f "$DEST_DIR/$extracted" ]]; then
|
||||||
|
echo " $name does not hold $extracted" >&2
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
if ! verify_sha "$DEST_DIR/$extracted" "$expected_sha"; then
|
||||||
|
rm -f "$DEST_DIR/$extracted"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
echo " Extracted $extracted ($(du -h "$DEST_DIR/$extracted" | cut -f1))"
|
||||||
fi
|
fi
|
||||||
mv -f "$target.partial" "$target"
|
|
||||||
done < "$MANIFEST"
|
done < "$MANIFEST"
|
||||||
|
|
||||||
echo "Test data ready under $DEST_DIR"
|
echo "Test data ready under $DEST_DIR"
|
||||||
|
|||||||
Reference in New Issue
Block a user