i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27: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/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;