i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-03 16:47:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Memory and Rtabmap tests

Browse Source
This commit is contained in:
matlabbe
2026-05-24 20:49:49 -07:00
parent 5786a8796d
commit e9c0194546
8 changed files with 7148 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Memory.h
+16 -6
View File
@@ -211,12 +211,22 @@ public:
/**
* @brief Transfers oldest signatures from WM to LTM to respect memory and/or time limits.
*
* The number of signatures removed depends on the visual word dictionary growth
* since the previous iteration (including retrieved signatures). We remove signatures
* until the number of visual words transferred is greater than the number of retrieved
* ones on the last iteration. In the case that signatures don't have visual words (e.g., lidar-only mapping),
* we remove at least one more signature than the total of signatures added/retrieved in the
* previous iteration.
* Two regimes are used depending on the visual word dictionary state:
* - **Word-count regime**: active only when mapping mode is on, the @ref VWDictionary
* is in incremental mode, contains at least one word, and is *not* using incremental
* FLANN. In this regime, signatures are transferred until the number of visual words
* removed from the dictionary catches up with the number of new words indexed since
* the previous iteration.
* - **Signature-count regime**: used in every other case (localization mode, dictionary
* not incremental, dictionary still empty -- e.g. lidar-only mapping or no feature
* extraction -- or incremental FLANN, where the word count is no longer the bottleneck).
* In this regime, at least one more signature than the count added/retrieved in the
* previous iteration is transferred, regardless of words.
*
* In both regimes, candidate selection (see @c getRemovableSignatures()) honors
* @p ignoredIds, skips intermediate nodes, and excludes WM nodes linked to STM (to
* preserve rehearsal). Intermediate (weight==-1) nodes linked to a transferred
* signature are dragged out with it.
*
* @param ignoredIds Signatures that must not be transferred (e.g. STM, retrieved ids, on the planned path).
* @return Ids of signatures moved to LTM, in transfer order.
corelib/src/Memory.cpp
+3 -1
View File
@@ -2588,7 +2588,9 @@ public:
}
return false;
}
int weight, age, id;
int weight;
double age;
int id;
};
std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
{
corelib/src/Rtabmap.cpp
+55 -41
View File
@@ -7004,6 +7004,33 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
if(iter->first > 0)
{
// Skip intermediate nodes (weight==-1). They are not navigable
// waypoints and updateGoalIndex would otherwise abort the
// plan when it sees them. The poses of the remaining real
// nodes already account for cumulative transform through any
// intermediate chain (relative poses from graph::computePath).
int weight = 0;
const Signature * s = _memory->getSignature(iter->first);
if(s)
{
weight = s->getWeight();
}
else
{
// For nodes in LTM, fetch weight from the database.
Transform p, gt;
int mapId = 0;
std::string label;
double stamp = 0.0;
std::vector<float> vel;
GPS gps;
EnvSensors envs;
_memory->getNodeInfo(iter->first, p, mapId, weight, label, stamp, gt, vel, gps, envs, true);
}
if(weight == -1)
{
continue;
}
// just keep nodes in the path
_path[oi].first = iter->first;
_path[oi++].second = t * iter->second;
@@ -7277,18 +7304,14 @@ void Rtabmap::updateGoalIndex()
if( _memory && _path.size())
{
// remove all previous virtual links
bool hasIntermediateNodes = false;
for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
_memory->removeVirtualLinks(s->id());
}
if(s->getWeight() == -1)
{
hasIntermediateNodes = true;
}
}
// for the current index, only keep the newest virtual link
@@ -7314,51 +7337,42 @@ void Rtabmap::updateGoalIndex()
}
}
// Make sure the next signatures on the path are linked together
// Make sure the next signatures on the path are linked together.
// Intermediate nodes have been filtered out of _path by computePath, so
// every entry is a real node here.
float distanceSoFar = 0.0f;
for(unsigned int i=_pathCurrentIndex+1;
i<_path.size() && !hasIntermediateNodes;
++i)
for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
{
if(i>0)
if(_localRadius > 0.0f)
{
if(_localRadius > 0.0f)
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(_path[i].first != _path[i-1].first)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(_path[i].first != _path[i-1].first)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
const Signature * sPrev = _memory->getSignature(_path[i-1].first);
if(sPrev)
{
if(s->getWeight() == -1)
{
hasIntermediateNodes = true;
break;
}
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
UASSERT_MSG(sPrev->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i-1, _path[i-1].first).c_str());
}
if(!s->hasLink(_path[i-1].first) && sPrev != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}
if(distanceSoFar > _localRadius)
{
UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
break;
}
}
}
if(hasIntermediateNodes)
{
UERROR("Cannot follow a path with a map containing intermediate nodes (not supported: don't use intermediate nodes if rtabmap's planner has to be used). Aborting current plan!");
this->clearPath(-1);
return;
if(distanceSoFar > _localRadius)
{
UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
break;
}
}
UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
corelib/src/util2d.cpp
+48 -15
View File
@@ -2404,7 +2404,7 @@ bool rotateImagesUpsideUpIfNecessary(
Transform localTransform = model.localTransform()*CameraModel::opticalRotation().inverse();
localTransform.getEulerAngles(roll, pitch, yaw);
UDEBUG("roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
if(fabs(pitch > M_PI/4))
if(fabs(pitch) > M_PI/4)
{
// Return original because of ambiguity for what would be considered up...
UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
@@ -2416,29 +2416,49 @@ bool rotateImagesUpsideUpIfNecessary(
}
if(roll >= M_PI/4 && roll < 3*M_PI/4)
{
UDEBUG("ROTATION_90 (roll=%f)", roll);
// Body roll near +pi/2 (right side down): the world-up direction projects to
// the image's left, so rotate the image 90 degrees clockwise to bring it
// upright (transpose + horizontal flip). Image dimensions HxW become WxH.
//
// Marker X moves from top-left to top-right quadrant:
// before (3x6): after (6x3):
// . X . . . . . . .
// . . . . . . . . X
// . . . . . . . . .
// . . .
// . . .
// . . .
UDEBUG("Rotating image 90 deg clockwise to correct body roll (roll=%f)", roll);
if(!rgb.empty())
{
cv::flip(rgb,rgb,1);
cv::transpose(rgb,rgb);
cv::flip(rgb,rgb,1);
}
if(!depth.empty())
{
cv::flip(depth,depth,1);
cv::transpose(depth,depth);
cv::flip(depth,depth,1);
}
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.cy()>0?model.imageHeight()-model.cy():0,
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
}
else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4)
{
UDEBUG("ROTATION_180 (roll=%f)", roll);
// Body roll near pi (upside down): rotate the image 180 degrees (horizontal
// flip + vertical flip). Image dimensions unchanged.
//
// Marker X moves from top-left to bottom-right quadrant:
// before (3x6): after (3x6):
// . X . . . . . . . . . .
// . . . . . . . . . . . .
// . . . . . . . . . . X .
UDEBUG("Rotating image 180 deg to correct body roll (roll=%f)", roll);
if(!rgb.empty())
{
cv::flip(rgb,rgb,1);
@@ -2460,29 +2480,42 @@ bool rotateImagesUpsideUpIfNecessary(
}
else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4)
{
UDEBUG("ROTATION_270 (roll=%f)", roll);
// Body roll near -pi/2 / +3*pi/2 (left side down): the world-up direction
// projects to the image's right, so rotate the image 90 degrees counter-
// clockwise to bring it upright (horizontal flip + transpose). Image
// dimensions HxW become WxH.
//
// Marker X moves from top-left to bottom-left quadrant:
// before (3x6): after (6x3):
// . X . . . . . . .
// . . . . . . . . .
// . . . . . . . . .
// . . .
// X . .
// . . .
UDEBUG("Rotating image 90 deg counter-clockwise to correct body roll (roll=%f)", roll);
if(!rgb.empty())
{
cv::transpose(rgb,rgb);
cv::flip(rgb,rgb,1);
cv::transpose(rgb,rgb);
}
if(!depth.empty())
{
cv::transpose(depth,depth);
cv::flip(depth,depth,1);
cv::transpose(depth,depth);
}
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy()>0?model.imageHeight()-model.cy():0,
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.cy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
}
else
{
UDEBUG("ROTATION_0 (roll=%f)", roll);
UDEBUG("Not rotating image, body roll within +/- pi/4 of upright (roll=%f)", roll);
return false;
}
return true;
corelib/test/CMakeLists.txt
+10
View File
@@ -96,6 +96,16 @@ add_executable(test_bayesfilter test_bayesfilter.cpp)
target_link_libraries(test_bayesfilter gtest_main rtabmap_core)
add_test(NAME test_bayesfilter COMMAND test_bayesfilter)
#Memory.h
add_executable(test_memory test_memory.cpp)
target_link_libraries(test_memory gtest_main rtabmap_core)
add_test(NAME test_memory COMMAND test_memory)
#Rtabmap.h
add_executable(test_rtabmap test_rtabmap.cpp)
target_link_libraries(test_rtabmap gtest_main rtabmap_core)
add_test(NAME test_rtabmap COMMAND test_rtabmap)
#Link.h
add_executable(test_link test_link.cpp)
target_link_libraries(test_link gtest_main rtabmap_core)
corelib/test/test_memory.cpp
+3232
View File
File diff suppressed because it is too large Load Diff
corelib/test/test_rtabmap.cpp
+3722
View File
File diff suppressed because it is too large Load Diff
corelib/test/test_util2d.cpp
+62 -10
View File
@@ -1259,10 +1259,42 @@ cv::Mat createTestImage(int width, int height, uchar value = 100) {
return cv::Mat(height, width, CV_8UC3, cv::Scalar(value, value, value));
}
namespace {
// A 3-channel "marker" pixel that's distinguishable from the uniform base color.
const cv::Vec3b kRgbMarker(255, 0, 0);
const cv::Vec3b kDepthMarker(123, 45, 67);
// Marker pixel position in the input 640x480 (cols x rows) image (top-left quadrant).
const int kMarkerRow = 100;
const int kMarkerCol = 200;
void stampMarker(cv::Mat & rgb, cv::Mat & depth) {
rgb.at<cv::Vec3b>(kMarkerRow, kMarkerCol) = kRgbMarker;
depth.at<cv::Vec3b>(kMarkerRow, kMarkerCol) = kDepthMarker;
}
// Verifies the marker landed at (expectedRow, expectedCol) and that no other pixel
// in the rotated image carries the marker color (i.e. the rotation is direction-
// correct, not just dimension-correct).
void expectMarkerAt(const cv::Mat & img, const cv::Vec3b & marker, int expectedRow, int expectedCol) {
EXPECT_EQ(img.at<cv::Vec3b>(expectedRow, expectedCol), marker);
int strays = 0;
for(int r = 0; r < img.rows; ++r)
{
for(int c = 0; c < img.cols; ++c)
{
if(r == expectedRow && c == expectedCol) continue;
if(img.at<cv::Vec3b>(r, c) == marker) ++strays;
}
}
EXPECT_EQ(strays, 0);
}
} // namespace
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
CameraModel model(500, 500, 320, 240, CameraModel::opticalRotation(), 0, cv::Size(640, 480));
cv::Mat rgb = createTestImage(640, 480);
cv::Mat depth = createTestImage(640, 480);
stampMarker(rgb, depth);
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
@@ -1271,6 +1303,9 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
EXPECT_EQ(rgb.rows, 480);
EXPECT_EQ(depth.cols, 640);
EXPECT_EQ(depth.rows, 480);
// Marker stays in place when no rotation is applied.
expectMarkerAt(rgb, kRgbMarker, kMarkerRow, kMarkerCol);
expectMarkerAt(depth, kDepthMarker, kMarkerRow, kMarkerCol);
float roll,pitch,yaw;
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
EXPECT_EQ(roll, 0.0f);
@@ -1279,43 +1314,54 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
}
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation90Degrees) {
// Simulate 90° roll
// Simulate +pi/2 roll (camera tilted right) -> upright correction is a 90 CW
// rotation of the image (transpose then flip(axis=1)). For an input marker at
// (row=100, col=200) in a 480x640 image:
// transpose: (100, 200) -> (200, 100) in 640x480
// flip(1): (200, 100) -> (200, 480-1-100) = (200, 379)
Transform rot = Transform(0,0,0, M_PI / 2, 0, 0);
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
cv::Mat rgb = createTestImage(640, 480, 150);
cv::Mat depth = createTestImage(640, 480, 200);
stampMarker(rgb, depth);
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
EXPECT_TRUE(rotated);
EXPECT_EQ(rgb.cols, 480); // Transposed
EXPECT_EQ(rgb.rows, 640);
EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0)[0], 150); // Same pixel values
EXPECT_EQ(depth.cols, 480); // Transposed
EXPECT_EQ(depth.cols, 480);
EXPECT_EQ(depth.rows, 640);
EXPECT_EQ(depth.at<cv::Vec3b>(0, 0)[0], 200);
expectMarkerAt(rgb, kRgbMarker, /*row=*/200, /*col=*/379);
expectMarkerAt(depth, kDepthMarker, /*row=*/200, /*col=*/379);
// After correction the camera is upright (roll=0).
float roll,pitch,yaw;
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
EXPECT_NEAR(roll, M_PI, 1e-5);
EXPECT_NEAR(roll, 0.0f, 1e-5);
EXPECT_NEAR(pitch, 0.0f, 1e-5);
EXPECT_NEAR(yaw, 0.0f, 1e-5);
}
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation180Degrees) {
// Simulate pi roll (camera upside down) -> 180 rotation:
// flip(1) + flip(0). For (100, 200) in 480x640:
// flip(1): (100, 200) -> (100, 640-1-200) = (100, 439)
// flip(0): (100, 439) -> (480-1-100, 439) = (379, 439)
Transform rot = Transform(0,0,0, M_PI, 0, 0);
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
cv::Mat rgb = createTestImage(640, 480, 123);
cv::Mat depth = createTestImage(640, 480, 77);
stampMarker(rgb, depth);
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
EXPECT_TRUE(rotated);
EXPECT_EQ(rgb.cols, 640); // Same size
EXPECT_EQ(rgb.rows, 480);
EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0)[0], 123);
EXPECT_EQ(depth.cols, 640); // Same size
EXPECT_EQ(depth.rows, 480);
EXPECT_EQ(depth.at<cv::Vec3b>(0, 0)[0], 77);
expectMarkerAt(rgb, kRgbMarker, /*row=*/379, /*col=*/439);
expectMarkerAt(depth, kDepthMarker, /*row=*/379, /*col=*/439);
float roll,pitch,yaw;
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
EXPECT_NEAR(roll, 0.0f, 1e-5);
@@ -1324,23 +1370,29 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation180Degrees) {
}
TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation270Degrees) {
// Simulate 3*pi/2 roll (camera tilted left) -> upright correction is a 90 CCW
// rotation of the image (flip(axis=1) then transpose). For (100, 200) in 480x640:
// flip(1): (100, 200) -> (100, 640-1-200) = (100, 439)
// transpose: (100, 439) -> (439, 100) in 640x480
Transform rot = Transform(0,0,0, 3*M_PI/2, 0, 0);
CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
cv::Mat rgb = createTestImage(640, 480, 90);
cv::Mat depth = createTestImage(640, 480, 60);
stampMarker(rgb, depth);
bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
EXPECT_TRUE(rotated);
EXPECT_EQ(rgb.cols, 480);
EXPECT_EQ(rgb.rows, 640);
EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0)[0], 90);
EXPECT_EQ(depth.cols, 480);
EXPECT_EQ(depth.rows, 640);
EXPECT_EQ(depth.at<cv::Vec3b>(0, 0)[0], 60);
expectMarkerAt(rgb, kRgbMarker, /*row=*/439, /*col=*/100);
expectMarkerAt(depth, kDepthMarker, /*row=*/439, /*col=*/100);
// After correction the camera is upright (roll=0).
float roll,pitch,yaw;
(model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
EXPECT_NEAR(roll, M_PI, 1e-5);
EXPECT_NEAR(roll, 0.0f, 1e-5);
EXPECT_NEAR(pitch, 0.0f, 1e-5);
EXPECT_NEAR(yaw, 0.0f, 1e-5);
}