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

Save intermediate node input features (#1698)

Browse Source 
* Avoid removing nodes from STM when using CreateIntermediateNodes

* Clean up the data of the intermediate node when IntermediateNodeDataKept is false

* Save intermediate node input features

* Don't delete intermediate nodes

* comment typo

* Updated parameter description, bump patch for DBReader API chang

* When ID is not udpated to new one, don't disable words because that signature is still used in the bayes filter

* updated comment

* rtabmap-reprocess: support Rtabmap/DetectionRate when intermediate nodes are created and -skip option is not used

* Fixed smallMovement when rehearsal is not trigdered at the same time

* GUI: visualize rehearsal darkblue when intermediate nodes are used

* Fixed RtabmapThread not transfering input features on intermediate nodes

* Do not override smallMovement and fastMovement when converting to intermediate nodes to keep GUI visualization color working

---------

Co-authored-by: Borong Yuan <yuanborong@hotmail.com>
This commit is contained in:
matlabbe
2026-05-14 09:20:09 -07:00
committed by GitHub
co-authored by Borong Yuan
parent afe8aadff1
commit 8d0692eab0
14 changed files with 704 additions and 505 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 23)
SET(RTABMAP_PATCH_VERSION 5)
SET(RTABMAP_PATCH_VERSION 6)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/DBReader.h
+3
View File
@@ -60,6 +60,7 @@ public:
int stopMapId = -1,
bool priorsIgnored = false,
bool imuIgnored = false,
bool intermediateNodesAreNormalNodes = false,
const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
DBReader(const std::list<std::string> & databasePaths,
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -76,6 +77,7 @@ public:
int stopMapId = -1,
bool priorsIgnored = false,
bool imuIgnored = false,
bool intermediateNodesAreNormalNodes = false,
const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
virtual ~DBReader();
@@ -106,6 +108,7 @@ private:
int _stopId;
std::vector<unsigned int> _cameraIndices;
bool _intermediateNodesIgnored;
bool _intermediateNodesAreNormalNodes;
bool _landmarksIgnored;
bool _featuresIgnored;
bool _priorsIgnored;
corelib/include/rtabmap/core/Memory.h
+1
View File
@@ -140,6 +140,7 @@ public:
float radius,
const std::map<int, Transform> & optimizedPoses,
int maxGraphDepth) const;
void convertToIntermediate(int locationId);
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void saveLocationData(int locationId);
void removeLink(int idA, int idB);
corelib/include/rtabmap/core/Parameters.h
+1 -1
View File
@@ -218,7 +218,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Mem, ReduceGraph, bool, false, uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false, "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, uFormat("On merge, update to new id. When false, no copy. Keep this disable if %s=true.", kRtabmapCreateIntermediateNodes().c_str()));
RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
corelib/include/rtabmap/core/SensorData.h
+8 -4
View File
@@ -212,6 +212,12 @@ public:
_userDataCompressed.empty() &&
_keypoints.size() == 0 &&
_descriptors.empty() &&
_groundCellsRaw.empty() &&
_groundCellsCompressed.empty() &&
_obstacleCellsRaw.empty() &&
_obstacleCellsCompressed.empty() &&
_emptyCellsRaw.empty() &&
_emptyCellsCompressed.empty() &&
imu_.empty());
}
@@ -309,8 +315,6 @@ public:
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewPoint);
// remove raw occupancy grids
void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
@@ -355,12 +359,12 @@ public:
* Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
* Raw data are kept is set.
*/
void clearCompressedData(bool images = true, bool scan = true, bool userData = true);
void clearCompressedData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
/**
* Clear raw rgb/depth (left/right) images, raw laser scan and raw user data.
* Compressed data are kept is set.
*/
void clearRawData(bool images = true, bool scan = true, bool userData = true);
void clearRawData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
corelib/src/DBReader.cpp
+5 -1
View File
@@ -57,6 +57,7 @@ DBReader::DBReader(const std::string & databasePath,
int stopMapId,
bool priorsIgnored,
bool imuIgnored,
bool intermediateNodesAreNormalNodes,
const std::vector<Transform> & cameraLocalTransformOverrides) :
Camera(frameRate),
_paths(uSplit(databasePath, ';')),
@@ -67,6 +68,7 @@ DBReader::DBReader(const std::string & databasePath,
_stopId(stopId),
_cameraIndices(cameraIndices),
_intermediateNodesIgnored(intermediateNodesIgnored),
_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
@@ -99,6 +101,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
int stopMapId,
bool priorsIgnored,
bool imuIgnored,
bool intermediateNodesAreNormalNodes,
const std::vector<Transform> & cameraLocalTransformOverrides) :
Camera(frameRate),
_paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_stopId(stopId),
_cameraIndices(cameraIndices),
_intermediateNodesIgnored(intermediateNodesIgnored),
_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
@@ -749,7 +753,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
data.setStereoCameraModels(combinedStereoModels);
}
}
data.setId(s->getWeight()==-1 ? -1 : seq);
data.setId(!_intermediateNodesAreNormalNodes && s->getWeight()==-1 ? -1 : seq);
data.setStamp(s->getStamp());
data.setGroundTruth(s->getGroundTruthPose());
if(!globalPose.isNull())
corelib/src/Memory.cpp
+362 -273
View File
@@ -2413,7 +2413,9 @@ int Memory::cleanup()
int signatureRemoved = 0;
// bad signature
if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
if(_lastSignature &&
((_lastSignature->isBadSignature() && _badSignaturesIgnored && _lastSignature->getWeight()!=-1) ||
!_incrementalMemory))
{
if(_lastSignature->isBadSignature())
{
@@ -2744,7 +2746,11 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
}
// it is a bad signature (not saved), remove links!
if(keepLinkedToGraph && (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
if(keepLinkedToGraph &&
!s->isSaved() &&
s->isBadSignature() &&
_badSignaturesIgnored &&
s->getWeight()!=-1)
{
keepLinkedToGraph = false;
}
@@ -3042,6 +3048,29 @@ bool Memory::setUserData(int id, const cv::Mat & data)
return false;
}
void Memory::convertToIntermediate(int locationId)
{
UDEBUG("Converting location %d to intermediate node", locationId);
Signature * location = _getSignature(locationId);
if(location)
{
location->setWeight(-1);
location->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
this->disableWordsRef(locationId); // won't be used for loop closure detection anymore
if(!_saveIntermediateNodeData)
{
location->removeAllWords();
location->sensorData().clearGlobalDescriptors();
}
location->sensorData().clearRawData();
if(!_saveIntermediateNodeData || !this->isBinDataKept())
{
location->sensorData().clearCompressedData();
}
}
}
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
{
UDEBUG("Deleting location %d", locationId);
@@ -4317,18 +4346,46 @@ bool Memory::rehearsalMerge(int oldId, int newId)
// just update weight
int w = oldS->getWeight()>=0?oldS->getWeight():0;
newS->setWeight(w + newS->getWeight() + 1);
oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
oldS->setWeight(0);
if(_lastGlobalLoopClosureId == oldS->id())
{
_lastGlobalLoopClosureId = newS->id();
}
if(intermediateMerge)
{
static bool warned = false;
if(!warned)
{
UWARN("A rehearsal was accepted (%d->%d) while not moving but "
"there are intermediate nodes between them in the graph. "
"Because %s=true, the node %d cannot be converted "
"into an intermediate node so it will be kept in the graph "
"even if we are not moving. Set %s=false to handle intermediate "
"nodes with rehearsal enabled so that loop closure hypotheses "
"are propagated correctly. This message is only "
"printed once.",
oldS->id(),
newS->id(),
Parameters::kMemRehearsalIdUpdatedToNewOne().c_str(),
oldS->id(),
Parameters::kMemRehearsalIdUpdatedToNewOne().c_str());
warned = true;
}
}
}
else // !_idUpdatedToNewOneRehearsal
{
int w = newS->getWeight()>=0?newS->getWeight():0;
oldS->setWeight(w + oldS->getWeight() + 1);
newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
if(intermediateMerge)
{
this->convertToIntermediate(newS->id());
}
else
{
newS->setWeight(0);
}
}
}
}
@@ -5584,7 +5641,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("Intermediate node detected, don't extract features!");
}
}
else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
else
{
_receivingOdometryFeatures = true;
UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
@@ -5600,133 +5657,75 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
if((int)keypoints.size() > maxFeatures)
if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
{
if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), ssc);
else
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
if(descriptors.empty())
{
cv::Mat imageMono;
if(data.imageRaw().channels() == 3)
int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
if((int)keypoints.size() > maxFeatures)
{
cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
}
else if(!imagesRectified && !data.cameraModels().empty())
{
std::vector<cv::KeyPoint> keypointsValid;
keypointsValid.reserve(keypoints.size());
cv::Mat descriptorsValid;
descriptorsValid.reserve(descriptors.rows);
std::vector<cv::Point3f> keypoints3DValid;
keypoints3DValid.reserve(keypoints3D.size());
//undistort keypoints before projection (RGB-D)
if(data.cameraModels().size() == 1)
{
std::vector<cv::Point2f> pointsIn, pointsOut;
cv::KeyPoint::convert(keypoints,pointsIn);
if(data.cameraModels()[0].D_raw().cols == 6)
{
#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
data.cameraModels()[0].K_raw(),
D,
data.cameraModels()[0].R(),
data.cameraModels()[0].P());
}
bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
_feature2D->limitKeypoints(keypoints,
keypoints3D,
descriptors,
maxFeatures,
data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(),
data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(),
data.stereoCameraModels()[0].left().imageHeight()),
ssc);
else
#else
UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
}
#endif
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
data.cameraModels()[0].K_raw(),
data.cameraModels()[0].D_raw(),
data.cameraModels()[0].R(),
data.cameraModels()[0].P());
}
UASSERT(pointsOut.size() == keypoints.size());
for(unsigned int i=0; i<pointsOut.size(); ++i)
{
if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut.at(i).x;
keypointsValid.back().pt.y = pointsOut.at(i).y;
descriptorsValid.push_back(descriptors.row(i));
if(!keypoints3D.empty())
{
keypoints3DValid.push_back(keypoints3D.at(i));
}
}
}
_feature2D->limitKeypoints(keypoints,
keypoints3D,
descriptors,
maxFeatures);
}
else
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
if(descriptors.empty())
{
float subImageWidth;
if(!data.imageRaw().empty())
cv::Mat imageMono;
if(data.imageRaw().channels() == 3)
{
UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
subImageWidth = data.imageRaw().cols/data.cameraModels().size();
cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
}
else
{
UASSERT(data.cameraModels()[0].imageWidth()>0);
subImageWidth = data.cameraModels()[0].imageWidth();
imageMono = data.imageRaw();
}
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
}
else if(!imagesRectified && !data.cameraModels().empty())
{
std::vector<cv::KeyPoint> keypointsValid;
keypointsValid.reserve(keypoints.size());
cv::Mat descriptorsValid;
descriptorsValid.reserve(descriptors.rows);
std::vector<cv::Point3f> keypoints3DValid;
keypoints3DValid.reserve(keypoints3D.size());
//undistort keypoints before projection (RGB-D)
if(data.cameraModels().size() == 1)
{
std::vector<cv::Point2f> pointsIn, pointsOut;
pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
cv::KeyPoint::convert(keypoints,pointsIn);
if(data.cameraModels()[0].D_raw().cols == 6)
{
#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
data.cameraModels()[cameraIndex].K_raw(),
data.cameraModels()[0].K_raw(),
D,
data.cameraModels()[cameraIndex].R(),
data.cameraModels()[cameraIndex].P());
data.cameraModels()[0].R(),
data.cameraModels()[0].P());
}
else
#else
@@ -5737,57 +5736,128 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
data.cameraModels()[cameraIndex].K_raw(),
data.cameraModels()[cameraIndex].D_raw(),
data.cameraModels()[cameraIndex].R(),
data.cameraModels()[cameraIndex].P());
data.cameraModels()[0].K_raw(),
data.cameraModels()[0].D_raw(),
data.cameraModels()[0].R(),
data.cameraModels()[0].P());
}
if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
UASSERT(pointsOut.size() == keypoints.size());
for(unsigned int i=0; i<pointsOut.size(); ++i)
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
keypointsValid.back().pt.y = pointsOut[0].y;
descriptorsValid.push_back(descriptors.row(i));
if(!keypoints3D.empty())
if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
{
keypoints3DValid.push_back(keypoints3D.at(i));
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut.at(i).x;
keypointsValid.back().pt.y = pointsOut.at(i).y;
descriptorsValid.push_back(descriptors.row(i));
if(!keypoints3D.empty())
{
keypoints3DValid.push_back(keypoints3D.at(i));
}
}
}
}
else
{
float subImageWidth;
if(!data.imageRaw().empty())
{
UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
subImageWidth = data.imageRaw().cols/data.cameraModels().size();
}
else
{
UASSERT(data.cameraModels()[0].imageWidth()>0);
subImageWidth = data.cameraModels()[0].imageWidth();
}
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
std::vector<cv::Point2f> pointsIn, pointsOut;
pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
{
#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
data.cameraModels()[cameraIndex].K_raw(),
D,
data.cameraModels()[cameraIndex].R(),
data.cameraModels()[cameraIndex].P());
}
else
#else
UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
}
#endif
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
data.cameraModels()[cameraIndex].K_raw(),
data.cameraModels()[cameraIndex].D_raw(),
data.cameraModels()[cameraIndex].R(),
data.cameraModels()[cameraIndex].P());
}
if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
keypointsValid.back().pt.y = pointsOut[0].y;
descriptorsValid.push_back(descriptors.row(i));
if(!keypoints3D.empty())
{
keypoints3DValid.push_back(keypoints3D.at(i));
}
}
}
}
keypoints = keypointsValid;
descriptors = descriptorsValid;
keypoints3D = keypoints3DValid;
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
UDEBUG("time rectification = %fs", t);
}
keypoints = keypointsValid;
descriptors = descriptorsValid;
keypoints3D = keypoints3DValid;
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
UDEBUG("time rectification = %fs", t);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
if(keypoints3D.empty() &&
((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
(!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
}
if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
{
_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
if(keypoints3D.empty() &&
((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
(!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
}
if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
{
_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
}
}
@@ -5810,107 +5880,117 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
std::list<int> wordIds;
if(descriptors.rows)
bool addedToDictionary = false;
if(!keypoints.empty())
{
// In case the number of features we want to do quantization is lower
// than extracted ones (that would be used for transform estimation)
std::vector<bool> inliers;
cv::Mat descriptorsForQuantization = descriptors;
std::vector<int> quantizedToRawIndices;
if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
if(descriptors.rows && !isIntermediateNode)
{
UASSERT((int)keypoints.size() == descriptors.rows);
int inliersCount = 0;
if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
// In case the number of features we want to do quantization is lower
// than extracted ones (that would be used for transform estimation)
std::vector<bool> inliers;
cv::Mat descriptorsForQuantization = descriptors;
std::vector<int> quantizedToRawIndices;
if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
}
else
{
if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
UASSERT((int)keypoints.size() == descriptors.rows);
int inliersCount = 0;
if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
{
UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
}
if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
{
Feature2D::limitKeypoints(
keypoints,
inliers,
_feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
_feature2D->getSSC());
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
}
else
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
}
}
for(size_t i=0; i<inliers.size(); ++i)
{
if(inliers[i])
++inliersCount;
}
descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
quantizedToRawIndices.resize(inliersCount);
unsigned int oi=0;
UASSERT((int)inliers.size() == descriptors.rows);
for(int k=0; k < descriptors.rows; ++k)
{
if(inliers[k])
{
UASSERT(oi < quantizedToRawIndices.size());
if(descriptors.type() == CV_32FC1)
if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
{
memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
}
if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
{
Feature2D::limitKeypoints(
keypoints,
inliers,
_feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
_feature2D->getSSC());
}
else
{
memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
}
quantizedToRawIndices[oi] = k;
++oi;
}
}
UASSERT_MSG((int)oi == inliersCount,
uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
}
// Quantization to vocabulary
wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
// Set ID -1 to features not used for quantization
if(wordIds.size() < keypoints.size())
{
std::vector<int> allWordIds;
allWordIds.resize(keypoints.size(),-1);
int i=0;
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
{
allWordIds[quantizedToRawIndices[i]] = *iter;
++i;
}
int negIndex = -1;
for(i=0; i<(int)allWordIds.size(); ++i)
{
if(allWordIds[i] < 0)
for(size_t i=0; i<inliers.size(); ++i)
{
allWordIds[i] = negIndex--;
if(inliers[i])
++inliersCount;
}
descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
quantizedToRawIndices.resize(inliersCount);
unsigned int oi=0;
UASSERT((int)inliers.size() == descriptors.rows);
for(int k=0; k < descriptors.rows; ++k)
{
if(inliers[k])
{
UASSERT(oi < quantizedToRawIndices.size());
if(descriptors.type() == CV_32FC1)
{
memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
}
quantizedToRawIndices[oi] = k;
++oi;
}
}
UASSERT_MSG((int)oi == inliersCount,
uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
}
wordIds = uVectorToList(allWordIds);
// Quantization to vocabulary
wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
addedToDictionary = true;
// Set ID -1 to features not used for quantization
if(wordIds.size() < keypoints.size())
{
std::vector<int> allWordIds;
allWordIds.resize(keypoints.size(),-1);
int i=0;
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
{
allWordIds[quantizedToRawIndices[i]] = *iter;
++i;
}
int negIndex = -1;
for(i=0; i<(int)allWordIds.size(); ++i)
{
if(allWordIds[i] < 0)
{
allWordIds[i] = negIndex--;
}
}
wordIds = uVectorToList(allWordIds);
}
}
else
{
// Set all words as not used in dictionary
wordIds.resize(keypoints.size(),-1);
}
t = timer.ticks();
@@ -5930,6 +6010,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(wordIds.size() > 0)
{
UASSERT(wordIds.size() == keypoints.size());
UASSERT(descriptors.rows == 0 || descriptors.rows == (int)wordIds.size());
UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
unsigned int i=0;
float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
@@ -5937,7 +6018,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
{
cv::KeyPoint kpt = keypoints[i];
if(preDecimation != _imagePostDecimation)
if(preDecimation != _imagePostDecimation && !isIntermediateNode)
{
// remap keypoints to final image size
kpt.pt.x *= decimationRatio;
@@ -5956,7 +6037,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
++words3DValid;
}
}
if(_rawDescriptorsKept)
if(!descriptors.empty() && _rawDescriptorsKept)
{
wordsDescriptors.push_back(descriptors.row(i));
}
@@ -5964,12 +6045,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
Landmarks landmarks = data.landmarks();
if(!landmarks.empty() && isIntermediateNode)
{
UDEBUG("Landmarks provided (size=%ld) are ignored because this signature is set as intermediate.", landmarks.size());
landmarks.clear();
}
else if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty())
if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty())
{
UDEBUG("Detecting markers...");
if(landmarks.empty())
@@ -6101,7 +6177,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("time post-decimation = %fs", t);
}
if(_stereoFromMotion &&
if(!isIntermediateNode &&
_stereoFromMotion &&
!pose.isNull() &&
cameraModels.size() == 1 &&
words.size() &&
@@ -6514,9 +6591,15 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
compressedUserData));
}
s->setWords(words, wordsKpts,
_reextractLoopClosureFeatures?std::vector<cv::Point3f>():words3D,
_reextractLoopClosureFeatures?cv::Mat():wordsDescriptors);
if(!isIntermediateNode || _saveIntermediateNodeData)
{
s->setWords(words, wordsKpts,
_reextractLoopClosureFeatures?std::vector<cv::Point3f>():words3D,
_reextractLoopClosureFeatures?cv::Mat():wordsDescriptors);
s->sensorData().setLaserScan(laserScan, false);
s->sensorData().setUserData(data.userDataRaw(), false);
}
// set raw data
if(!cameraModels.empty())
@@ -6527,8 +6610,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false);
}
s->sensorData().setLaserScan(laserScan, false);
s->sensorData().setUserData(data.userDataRaw(), false);
UDEBUG("data.groundTruth() =%s", data.groundTruth().prettyPrint().c_str());
UDEBUG("data.gps() =%s", data.gps().stamp()?"true":"false");
@@ -6538,28 +6619,33 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
s->sensorData().setGPS(data.gps());
s->sensorData().setEnvSensors(data.envSensors());
if(!isIntermediateNode)
std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
if(!isIntermediateNode && _globalDescriptorExtractor)
{
std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
if(_globalDescriptorExtractor)
GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
if(!gdescriptor.data().empty())
{
GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
if(!gdescriptor.data().empty())
{
globalDescriptors.push_back(gdescriptor);
}
globalDescriptors.push_back(gdescriptor);
}
s->sensorData().setGlobalDescriptors(globalDescriptors);
}
else if(!data.globalDescriptors().empty())
if(!globalDescriptors.empty())
{
UDEBUG("Global descriptors provided (size=%ld) are ignored because this signature is set as intermediate.", data.globalDescriptors().size());
if(!isIntermediateNode || _saveIntermediateNodeData)
{
s->sensorData().setGlobalDescriptors(globalDescriptors);
}
else
{
UDEBUG("Global descriptors provided (size=%ld) are ignored because this signature is set as intermediate and %s=false.",
globalDescriptors.size(),
Parameters::kMemIntermediateNodeDataKept().c_str());
}
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
UDEBUG("time compressing data (id=%d) %fs", id, t);
if(words.size())
if(words.size() && addedToDictionary)
{
s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords())
}
@@ -6699,16 +6785,19 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
s->addLandmark(landmark);
// Update landmark index
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
if(!isIntermediateNode)
{
nter->second.insert(s->id());
}
else
{
std::set<int> tmp;
tmp.insert(s->id());
_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
{
nter->second.insert(s->id());
}
else
{
std::set<int> tmp;
tmp.insert(s->id());
_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
}
}
}
else
corelib/src/Rtabmap.cpp
+46 -24
View File
@@ -1518,31 +1518,48 @@ bool Rtabmap::process(
}
else
{
bool linkedToIntermediateNode = false;
Transform t;
if(_memory->isIncremental())
{
// Check small motion if current node is not an intermediate node already
if(signature->getWeight() >= 0)
{
// It should contain only the query and its first (non-intermediate) neighbor (smaller id)
std::map<int, int> neighbors = _memory->getNeighborsId(signature->id(), 2, 0, true, true, true, true);
if(neighbors.size() == 2)
{
int nid = neighbors.begin()->first;
const std::multimap<int, Link> & links = signature->getLinks();
if(links.find(nid) != links.end())
{
// direct neighbor
t = links.find(nid)->second.transform();
}
else
{
// Use optimized poses to check how far it is from the latest non-intermediate node
std::map<int, Transform>::iterator niter = _optimizedPoses.find(nid);
if(niter != _optimizedPoses.end())
{
t = niter->second.inverse() * _mapCorrection * signature->getPose();
}
// not direct link, it means there are intermediate nodes
linkedToIntermediateNode = true;
}
}
}
}
else if(!_odomCachePoses.empty())
{
t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
}
if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f)
{
//============================================================
// Minimum displacement required to add to Memory
//============================================================
Transform t;
if(_memory->isIncremental())
{
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// Check small motion if previous node is not an intermediate node (consecutive intermediate nodes are merged later)
if(s->getWeight() >= 0)
{
t = links.begin()->second.transform();
}
}
}
else if(!_odomCachePoses.empty())
{
t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
}
if(!t.isNull())
{
float x,y,z, roll,pitch,yaw;
@@ -1564,7 +1581,7 @@ bool Rtabmap::process(
}
}
}
if(odomVelocity.size() == 6)
if(odomVelocity.size() == 6 && signature->getWeight() != -1)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
@@ -1572,6 +1589,10 @@ bool Rtabmap::process(
(_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) ||
(_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate);
}
if(linkedToIntermediateNode && (smallDisplacement || tooFastMovement))
{
_memory->convertToIntermediate(signature->id());
}
}
// Update optimizedPoses with the newly added node
@@ -2207,7 +2228,7 @@ bool Rtabmap::process(
}
} // if(_memory->getWorkingMemSize())
}// !isBadSignature
else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
else if(!signature->isBadSignature() && signature->getWeight()>=0 && (smallDisplacement || tooFastMovement))
{
_highestHypothesis = lastHighestHypothesis;
UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0);
@@ -3166,7 +3187,7 @@ bool Rtabmap::process(
// Landmark
//============================================================
std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored())
if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored() && signature->getWeight()!=-1)
{
bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
@@ -4446,7 +4467,8 @@ bool Rtabmap::process(
signaturesRemoved.push_back(signature->id());
_memory->deleteLocation(signature->id());
}
else if((smallDisplacement || tooFastMovement) &&
else if((!_memory->isIncremental() || signature->getWeight()>=0) &&
(smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
(rejectedLoopClosure || landmarksDetected.empty()) &&
corelib/src/RtabmapThread.cpp
-1
View File
@@ -556,7 +556,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
// set negative id so rtabmap will detect it as an intermediate node
SensorData tmp = odomEvent.data();
tmp.setId(-1);
tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
}
else
corelib/src/SensorData.cpp
+18 -2
View File
@@ -976,7 +976,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
}
void SensorData::clearCompressedData(bool images, bool scan, bool userData)
void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid)
{
if(images)
{
@@ -992,14 +992,24 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
{
_userDataCompressed=cv::Mat();
}
if(occupancyGrid)
{
_groundCellsCompressed=cv::Mat();
_emptyCellsCompressed=cv::Mat();
_obstacleCellsCompressed=cv::Mat();
}
}
void SensorData::clearRawData(bool images, bool scan, bool userData)
void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid)
{
if(images)
{
_imageRaw=cv::Mat();
_depthOrRightRaw=cv::Mat();
_depthConfidenceRaw=cv::Mat();
#ifdef HAVE_OPENCV_CUDEV
_imageRawGpu = cv::cuda::GpuMat();
_depthOrRightRawGpu = cv::cuda::GpuMat();
#endif
}
if(scan)
{
@@ -1009,6 +1019,12 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
{
_userDataRaw=cv::Mat();
}
if(occupancyGrid)
{
_groundCellsRaw=cv::Mat();
_emptyCellsRaw=cv::Mat();
_obstacleCellsRaw=cv::Mat();
}
}
guilib/src/MainWindow.cpp
+13 -2
View File
@@ -2107,6 +2107,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
// For intermediate empty nodes, keep latest image shown
bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
if(signature.getWeight() >= 0)
{
_ui->imageView_source->clear();
@@ -2131,7 +2132,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->label_matchId->clear();
bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
@@ -2455,6 +2455,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms (update loop closure viewer)", time.restart());
}
}
else if(rehearsedSimilarity)
{
_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
}
else if(smallMovement)
{
_ui->imageView_source->setBackgroundColor(Qt::gray);
}
else if(fastMovement)
{
_ui->imageView_source->setBackgroundColor(Qt::magenta);
}
// PDF AND LIKELIHOOD
if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -2712,7 +2724,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
Signature & s = *_cachedSignatures.find(stat.refImageId());
_cachedMemoryUsage -= s.sensorData().getMemoryUsed();
s.sensorData().clearRawData();
s.sensorData().clearOccupancyGridRaw();
_cachedMemoryUsage += s.sensorData().getMemoryUsed();
}
guilib/src/PreferencesDialog.cpp
+6 -1
View File
@@ -761,6 +761,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_checkBox_ignoreFeatures, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_ignorePriors, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_ignoreIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_intermediateNodesAreNormalNodes, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -2229,6 +2230,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_checkBox_ignoreFeatures->setChecked(true);
_ui->source_checkBox_ignorePriors->setChecked(false);
_ui->source_checkBox_ignoreIMU->setChecked(false);
_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(false);
_ui->source_spinBox_databaseStartId->setValue(0);
_ui->source_spinBox_databaseStopId->setValue(0);
_ui->source_lineEdit_databaseCameraIndex->setText("");
@@ -2991,6 +2993,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->source_checkBox_ignoreFeatures->setChecked(settings.value("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()).toBool());
_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
_ui->source_checkBox_ignoreIMU->setChecked(settings.value("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()).toBool());
_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(settings.value("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked()).toBool());
_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
@@ -3611,6 +3614,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked());
settings.setValue("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked());
settings.setValue("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked());
settings.setValue("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked());
settings.setValue("startId", _ui->source_spinBox_databaseStartId->value());
settings.setValue("stopId", _ui->source_spinBox_databaseStopId->value());
settings.setValue("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text());
@@ -7303,13 +7307,14 @@ Camera * PreferencesDialog::createCamera(
_ui->source_spinBox_databaseStartId->value(),
cameraIndices,
_ui->source_spinBox_databaseStopId->value(),
!_ui->general_checkBox_createIntermediateNodes->isChecked(),
!_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked() && !_ui->general_checkBox_createIntermediateNodes->isChecked(),
_ui->source_checkBox_ignoreLandmarks->isChecked(),
_ui->source_checkBox_ignoreFeatures->isChecked(),
0,
-1,
_ui->source_checkBox_ignorePriors->isChecked(),
_ui->source_checkBox_ignoreIMU->isChecked(),
_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked(),
localTransformOverrides);
}
else
guilib/src/ui/preferencesDialog.ui
+202 -182
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-700</y>
<width>684</width>
<height>5218</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>14</number>
<number>8</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3469,7 +3469,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>0</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -7180,37 +7180,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1,0">
<item row="10" column="0">
<widget class="QSpinBox" name="source_spinBox_databaseStopId">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>99999</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="source_database_lineEdit_path"/>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="source_checkBox_useDbStamps">
<item row="8" column="1">
<widget class="QLabel" name="label_786">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreGoals">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_644">
<property name="text">
<string>Ignore features.</string>
<string>Ignore IMU (i.e., gravity links).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7220,6 +7193,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QToolButton" name="source_database_toolButton_selectSource">
<property name="text">
@@ -7227,123 +7207,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_784">
<item row="10" column="1">
<widget class="QLabel" name="label_58">
<property name="text">
<string>Override camera local transform(s) with local transform(s) set above. For multi-cameras, use a &quot;;&quot; between each transform.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
<string>Start position (node ID).</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLineEdit" name="source_lineEdit_databaseCameraIndex"/>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QLineEdit" name="source_lineEdit_databaseLocalTransformOffset"/>
</item>
<item row="0" column="2">
<widget class="QToolButton" name="toolButton_dbViewer">
<property name="toolTip">
<string>Open database viewer</string>
</property>
<property name="text">
<string/>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QCheckBox" name="source_checkBox_overrideLocalTransforms">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreFeatures">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_72">
<property name="text">
<string>Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_315">
<property name="text">
<string>Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like &quot;0 2&quot; to stream cameras 0 and 2 only.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="source_checkBox_ignorePriors">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_785">
<property name="text">
<string>Add an y-axis offset before optical rotation on the overriden local transform(s). For multi-cameras,explicitly enumerate offsets if they are different (e.g., &quot;0.05 0.075&quot; for two cameras setup), or set single number to apply to all camera transforms.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QSpinBox" name="source_spinBox_databaseStartId">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>99999</number>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_90">
<property name="text">
@@ -7357,25 +7230,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreLandmarks">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_58">
<property name="text">
<string>Start position (node ID).</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
<item row="8" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreIMU">
<property name="text">
<string/>
</property>
@@ -7394,6 +7250,139 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="source_database_lineEdit_path"/>
</item>
<item row="15" column="0">
<widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_644">
<property name="text">
<string>Ignore features.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreFeatures">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_315">
<property name="text">
<string>Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like &quot;0 2&quot; to stream cameras 0 and 2 only.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreLandmarks">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QToolButton" name="toolButton_dbViewer">
<property name="toolTip">
<string>Open database viewer</string>
</property>
<property name="text">
<string/>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_263">
<property name="text">
<string>Ignore goals saved in the database.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_797">
<property name="text">
<string>Publish intermediate nodes as normal nodes.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_557">
<property name="text">
<string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QSpinBox" name="source_spinBox_databaseStopId">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>99999</number>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_529">
<property name="text">
<string>Stop position (node ID) is the last node to process. If 0, all nodes after start position are published.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_546">
<property name="text">
@@ -7407,6 +7396,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QLineEdit" name="source_lineEdit_databaseLocalTransformOffset"/>
</item>
<item row="10" column="0">
<widget class="QSpinBox" name="source_spinBox_databaseStartId">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>99999</number>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_80">
<property name="text">
@@ -7421,9 +7423,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_557">
<widget class="QLabel" name="label_785">
<property name="text">
<string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
<string>Add an y-axis offset before optical rotation on the overriden local transform(s). For multi-cameras,explicitly enumerate offsets if they are different (e.g., &quot;0.05 0.075&quot; for two cameras setup), or set single number to apply to all camera transforms.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7433,10 +7435,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_529">
<item row="13" column="1">
<widget class="QLabel" name="label_784">
<property name="text">
<string>Stop position (node ID) is the last node to process. If 0, all nodes after start position are published.</string>
<string>Override camera local transform(s) with local transform(s) set above. For multi-cameras, use a &quot;;&quot; between each transform.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7446,10 +7448,34 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_263">
<item row="13" column="0">
<widget class="QCheckBox" name="source_checkBox_overrideLocalTransforms">
<property name="text">
<string>Ignore goals saved in the database.</string>
<string/>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="source_checkBox_intermediateNodesAreNormalNodes">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="source_checkBox_ignorePriors">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLineEdit" name="source_lineEdit_databaseCameraIndex"/>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_72">
<property name="text">
<string>Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7459,21 +7485,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_786">
<item row="3" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreGoals">
<property name="text">
<string>Ignore IMU (i.e., gravity links).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<string/>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreIMU">
<item row="2" column="0">
<widget class="QCheckBox" name="source_checkBox_useDbStamps">
<property name="text">
<string/>
</property>
@@ -11102,7 +11122,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="2" column="1">
<widget class="QLabel" name="label_rehearsalIdUpdate">
<property name="text">
<string>On merging, update to new id.</string>
<string>On merging, update to new id. Keep this unchecked if intermediate nodes are created.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
tools/Reprocess/main.cpp
+38 -13
View File
@@ -96,6 +96,7 @@ void showUsage()
" -nopriors Don't republish priors contained in input database.\n"
" -noimu Don't republish IMU contained in input database.\n"
" -pub_loops Republish loop closures contained in input database.\n"
" -pub_inter_as_normal Republish intermediate nodes as normal nodes.\n"
" -loc_null On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
" -gt When reprocessing a single database, load its original optimized graph, then \n"
" set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
@@ -279,6 +280,7 @@ int main(int argc, char * argv[])
bool ignorePriors = false;
bool ignoreImu = false;
bool republishLoopClosures = false;
bool pubInterNodesAsNormalNodes = false;
bool locNull = false;
bool originalGraphAsGT = false;
bool scanFromDepth = false;
@@ -510,6 +512,11 @@ int main(int argc, char * argv[])
republishLoopClosures = true;
printf("Republish loop closures from input database (-pub_loops option).\n");
}
else if(strcmp(argv[i], "-pub_inter_as_normal") == 0 || strcmp(argv[i], "--pub_inter_as_normal") == 0)
{
pubInterNodesAsNormalNodes = true;
printf("Republish intermdiate nodes as normal nodes (-pub_inter_as_normal option).\n");
}
else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
{
locNull = true;
@@ -815,7 +822,7 @@ int main(int argc, char * argv[])
int totalIds = 0;
std::set<int> ids;
dbDriver->getAllNodeIds(ids, false, false, !intermediateNodes);
dbDriver->getAllNodeIds(ids, false, false, !pubInterNodesAsNormalNodes && !intermediateNodes);
if(ids.empty())
{
printf("Input database doesn't have any nodes saved in it.\n");
@@ -844,7 +851,7 @@ int main(int argc, char * argv[])
return 1;
}
ids.clear();
dbDriver->getAllNodeIds(ids, false, false, !intermediateNodes);
dbDriver->getAllNodeIds(ids, false, false, !pubInterNodesAsNormalNodes && !intermediateNodes);
totalIds += ids.size();
dbDriver->closeConnection(false);
}
@@ -920,13 +927,14 @@ int main(int argc, char * argv[])
startId,
cameraIndices,
stopId,
!intermediateNodes,
!pubInterNodesAsNormalNodes && !intermediateNodes,
ignoreLandmarks,
!useOdomFeatures,
startMapId,
stopMapId,
ignorePriors,
ignoreImu,
pubInterNodesAsNormalNodes,
cameraLocalTransformOverrides);
dbReader->init();
@@ -945,6 +953,11 @@ int main(int argc, char * argv[])
Odometry * odometry = 0;
float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
if(rtabmapUpdateRate!=0)
{
rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
}
double lastUpdateStamp = 0;
if(recomputeOdometry)
{
@@ -957,14 +970,17 @@ int main(int argc, char * argv[])
{
printf("Odometry will be recomputed (\"odom\" option is set)%s.\n",
useInputOdometryAsGuess?" with input odometry guess (\"odom_guess_input\" option is set)":"");
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
if(rtabmapUpdateRate!=0)
{
rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
}
odometry = Odometry::create(parameters);
}
}
else if(!intermediateNodes && framesToSkip == 0 &&
(configParameters.find(Parameters::kRtabmapDetectionRate())!=configParameters.end() ||
customParameters.find(Parameters::kRtabmapDetectionRate())!=customParameters.end()))
{
printf("[Warning] Parameter %s is ignored because parameter %s=false.\n",
Parameters::kRtabmapDetectionRate().c_str(),
Parameters::kRtabmapCreateIntermediateNodes().c_str());
}
printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
std::map<std::string, float> globalMapStats;
@@ -1071,11 +1087,16 @@ int main(int argc, char * argv[])
info.odomPose = pose;
info.odomCovariance = odomCovariance;
odomCovariance = cv::Mat();
if(data.id() != -1)
lastUpdateStamp = data.stamp();
uInsert(globalMapStats, odomInfo.statistics(pose));
}
else if(framesToSkip==0 && intermediateNodes && lastUpdateStamp > 0.0 && (data.stamp() < lastUpdateStamp + rtabmapUpdateRate))
{
data.setId(-1); // intermediate node
}
if(data.id() != -1)
lastUpdateStamp = data.stamp();
UTimer iterationTime;
std::string status;
@@ -1266,15 +1287,19 @@ int main(int argc, char * argv[])
{
++loopIntra;
}
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
}
else if(landmarkId != 0)
{
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), landmarkId);
printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), landmarkId);
}
else if(data.id() == -1)
{
printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Intermediate node\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
}
else
{
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
}
// Here we accumulate statistics about distance from last localization