Increased version to 0.20.5

Refactored how features are stored in Signature (significative memory optimization, causing major refactoring in Memory, RegistrationVis, OdometryF2M)
FLANN: optimized memory usage when Kp/IncrementalFlann is false
Added memory usage functions
Added statistics Loop/Visual_inliers_ratio/ and Memory/RAM_estimated/MB
EpipolarGeometry: templated findPairs functions
graph::filterLinks: added inverted option
LocalBundleOnLoopClosure: Force to use only neighbor links
MainWindow: fixed max depth filtering for map's features
Rtabmap::getSignatureCopy() fixed links not returned
Added UPlot::getAllCurveDataAsText() function.
DbViewer: fixed features not rendered in right view when failing ro refine a constraint
report: added --export and --export_prefix options (to export figures data)
This commit is contained in:
matlabbe
2020-10-05 17:34:32 -04:00
parent bedc771fa4
commit bbccbd63e4
41 changed files with 1456 additions and 898 deletions

View File

@@ -398,6 +398,7 @@ public:
bool isFlat() const {return _flat;}
void addItem(UPlotCurve * curve);
bool remove(const UPlotCurve * curve);
QString getAllCurveDataAsText() const;
private Q_SLOTS:
void removeLegendItem(const UPlotCurve * curve);
@@ -510,6 +511,7 @@ public:
QStringList curveNames();
bool contains(const QString & curveName);
void removeCurves();
QString getAllCurveDataAsText() const;
/**
* Add a threshold to the plot.
*/

View File

@@ -4163,9 +4163,14 @@ void DatabaseViewer::update(int value,
std::list<int> ids;
ids.push_back(id);
std::list<Signature*> signatures;
dbDriver_->loadSignatures(ids, signatures);
dbDriver_->loadSignatures(ids, signatures);
if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
{
std::multimap<int, cv::KeyPoint> keypoints;
for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
{
keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
}
view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
}
@@ -4522,16 +4527,19 @@ void DatabaseViewer::update(int value,
cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
}
}
//words
if(ui_->checkBox_showWords->isChecked() &&
!signatures.empty() &&
!(*signatures.begin())->getWords3().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize((*signatures.begin())->getWords3().size());
int i=0;
cloud->resize((*signatures.begin())->getWords3().size());
int i=0;
for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
iter!=(*signatures.begin())->getWords().end();
++iter)
++iter)
{
const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
@@ -5593,20 +5601,25 @@ void DatabaseViewer::updateConstraintView(
if(sTo)
{
cloudTo->resize(sTo->getWords3().size());
}
int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
}
int i=0;
++iter)
{
cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
}
i=0;
if(sTo)
{
if(!sFrom->getWords3().empty())
{
for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
iter!=sFrom->getWords().end();
++iter)
++iter)
{
const cv::Point3f & pt = sFrom->getWords3()[iter->second];
cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
}
i=0;
if(sTo && !sTo->getWords3().empty())
{
for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
iter!=sTo->getWords().end();
++iter)
{
const cv::Point3f & pt = sTo->getWords3()[iter->second];
cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
@@ -7074,13 +7087,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
}
if(reextractVisualFeatures)
{
fromS->setWords(std::multimap<int, cv::KeyPoint>());
if(reextractVisualFeatures)
{
fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
toS->setWords(std::multimap<int, cv::KeyPoint>());
fromS->removeAllWords();
fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
toS->removeAllWords();
toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
@@ -7192,17 +7201,33 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
if(!silent && ui_->dockWidget_constraints->isVisible())
{
if(toS && fromS->id() > 0 && toS->id() > 0)
{
updateLoopClosuresSlider(fromS->id(), toS->id());
std::multimap<int, cv::KeyPoint> keypointsFrom;
std::multimap<int, cv::KeyPoint> keypointsTo;
if(!fromS->getWordsKpts().empty())
{
for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
{
keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
}
}
if(!toS->getWordsKpts().empty())
{
for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
{
keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
}
}
if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
{
this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
{
this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
}
else
{
this->updateConstraintView(newLink, true, *fromS, *toS);
{
this->updateConstraintView(newLink, true, *fromS, *toS);
ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
}
@@ -7218,8 +7243,32 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
else if(!silent)
{
if(toS && fromS->id() > 0 && toS->id() > 0)
{
// just update matches in the views
{
// just update matches in the views
std::multimap<int, cv::KeyPoint> keypointsFrom;
std::multimap<int, cv::KeyPoint> keypointsTo;
if(!fromS->getWordsKpts().empty())
{
for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
{
keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
}
}
if(!toS->getWordsKpts().empty())
{
for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
{
keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
}
}
if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
{
ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
}
else
{
ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
}
updateWordsMatching(info.inliersIDs);
@@ -7306,13 +7355,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
fromS->sensorData().uncompressData();
dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
toS->sensorData().uncompressData();
if(reextractVisualFeatures)
{
fromS->setWords(std::multimap<int, cv::KeyPoint>());
if(reextractVisualFeatures)
{
fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
toS->setWords(std::multimap<int, cv::KeyPoint>());
fromS->removeAllWords();
fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
toS->removeAllWords();
toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
@@ -7565,8 +7610,24 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
updateLoopClosuresSlider(fromS->id(), toS->id());
this->updateGraphView();
this->updateConstraintView(newLink, false, *fromS, *toS);
}
}
std::multimap<int, cv::KeyPoint> keypointsFrom;
std::multimap<int, cv::KeyPoint> keypointsTo;
if(!fromS->getWordsKpts().empty())
{
for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
{
keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
}
}
if(!toS->getWordsKpts().empty())
{
for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
{
keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
}
}
ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
updateWordsMatching(info.inliersIDs);

View File

@@ -1715,11 +1715,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_cachedSignatures.insert(signature.id(), signature);
_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
unsigned int count = 0;
for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().upper_bound(-1); jter!=signature.getWords3().end(); ++jter)
if(!signature.getWords3().empty())
{
if(util3d::isFinite(jter->second))
for(std::multimap<int, int>::const_iterator jter=signature.getWords().upper_bound(-1); jter!=signature.getWords().end(); ++jter)
{
++count;
if(util3d::isFinite(signature.getWords3()[jter->second]))
{
++count;
}
}
}
_cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
@@ -1952,7 +1955,17 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
// do it after scaling
this->drawKeypoints(signature.getWords(), loopSignature.getWords());
std::multimap<int, cv::KeyPoint> wordsA;
std::multimap<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
{
wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
}
for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
{
wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
}
this->drawKeypoints(wordsA, wordsB);
UDEBUG("time= %d ms", time.restart());
@@ -3706,40 +3719,57 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
cloud->resize(iter->getWords3().size());
int oi=0;
UASSERT(iter->getWords().size() == iter->getWords3().size());
std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
UDEBUG("rgb.channels()=%d");
for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
jter!=iter->getWords3().end(); ++jter, ++kter)
if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
{
if(util3d::isFinite(jter->second) && (maxDepth == 0.0f || jter->second.z < maxDepth))
Transform invLocalTransform = Transform::getIdentity();
if(iter.value().sensorData().cameraModels().size() == 1 && iter.value().sensorData().cameraModels().at(0).isValidForProjection())
{
(*cloud)[oi].x = jter->second.x;
(*cloud)[oi].y = jter->second.y;
(*cloud)[oi].z = jter->second.z;
int u = kter->second.pt.x+0.5;
int v = kter->second.pt.y+0.5;
if(!rgb.empty() &&
uIsInBounds(u, 0, rgb.cols-1) &&
uIsInBounds(v, 0, rgb.rows-1))
invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().inverse();
}
else if(iter.value().sensorData().stereoCameraModel().isValidForProjection())
{
invLocalTransform = iter.value().sensorData().stereoCameraModel().left().localTransform().inverse();
}
for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
{
const cv::Point3f & pt = iter->getWords3()[jter->second];
if(util3d::isFinite(pt) &&
(maxDepth == 0.0f ||
//move back point in camera frame (to get depth along z), ignore for multi-camera
(iter.value().sensorData().cameraModels().size()<=1 &&
util3d::transformPoint(pt, invLocalTransform).z < maxDepth)))
{
if(rgb.channels() == 1)
(*cloud)[oi].x = pt.x;
(*cloud)[oi].y = pt.y;
(*cloud)[oi].z = pt.z;
const cv::KeyPoint & kpt = iter->getWordsKpts()[jter->second];
int u = kpt.pt.x+0.5;
int v = kpt.pt.y+0.5;
if(!rgb.empty() &&
uIsInBounds(u, 0, rgb.cols-1) &&
uIsInBounds(v, 0, rgb.rows-1))
{
(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
if(rgb.channels() == 1)
{
(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
}
else
{
cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
(*cloud)[oi].b = bgr.val[0];
(*cloud)[oi].g = bgr.val[1];
(*cloud)[oi].r = bgr.val[2];
}
}
else
{
cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
(*cloud)[oi].b = bgr.val[0];
(*cloud)[oi].g = bgr.val[1];
(*cloud)[oi].r = bgr.val[2];
(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
}
++oi;
}
else
{
(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
}
++oi;
}
}
cloud->resize(oi);
@@ -4062,11 +4092,14 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_cachedSignatures.insert(iter->first, iter->second);
_cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
unsigned int count = 0;
for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->second.getWords3().upper_bound(-1); jter!=iter->second.getWords3().end(); ++jter)
if(!iter->second.getWords3().empty())
{
if(util3d::isFinite(jter->second))
for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
{
++count;
if(util3d::isFinite(iter->second.getWords3()[jter->second]))
{
++count;
}
}
}
_cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
@@ -5874,13 +5907,9 @@ void MainWindow::postProcessing()
}
else
{
signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
signatureFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
signatureFrom.removeAllWords();
signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
signatureTo.setWords3(std::multimap<int, cv::Point3f>());
signatureTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
signatureTo.removeAllWords();
signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
}
}

View File

@@ -1678,6 +1678,86 @@ void UPlotLegend::moveDown(UPlotLegendItem * item)
}
}
QString UPlotLegend::getAllCurveDataAsText() const
{
QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
if(items.size())
{
// create common x-axis
QMap<qreal, qreal> xAxisMap;
for(int i=0; i<items.size(); ++i)
{
QMap<qreal, qreal> data;
items.at(i)->curve()->getData(data);
for(QMap<qreal, qreal>::iterator iter=data.begin(); iter!=data.end(); ++iter)
{
xAxisMap.insert(iter.key(), iter.value());
}
}
QList<qreal> xAxis = xAxisMap.uniqueKeys();
QVector<QVector<qreal> > axes;
for(int i=0; i<items.size(); ++i)
{
QMap<qreal, qreal> data;
items.at(i)->curve()->getData(data);
QVector<qreal> y(xAxis.size(), std::numeric_limits<qreal>::quiet_NaN());
// just to make sure that we have the same number of data on each curve, set NAN for unknowns
int j=0;
for(QList<qreal>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
{
if(data.contains(*iter))
{
y[j] = data.value(*iter);
}
++j;
}
axes.push_back(y);
}
if(!xAxis.empty())
{
axes.push_front(xAxis.toVector());
QString text;
text.append('x');
text.append('\t');
for(int i=0; i<items.size(); ++i)
{
text.append(items.at(i)->curve()->name());
if(i+1<axes.size())
{
text.append('\t');
}
}
text.append('\n');
for(int i=0; i<axes[0].size(); ++i)
{
for(int j=0; j<axes.size(); ++j)
{
if(uIsNan(axes[j][i]))
{
text.append("NaN"); // NaN is interpreted by default as NaN in MatLab/Octave
}
else
{
text.append(QString::number(axes[j][i]));
}
if(j+1<axes.size())
{
text.append('\t');
}
}
if(i+1<axes[0].size())
{
text.append("\n");
}
}
return text;
}
}
return "";
}
void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
{
QAction * action = _menu->exec(event->globalPos());
@@ -1687,81 +1767,11 @@ void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
}
else if(action == _aCopyAllCurvesToClipboard)
{
QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
if(items.size())
QString data = getAllCurveDataAsText();
if(!data.isEmpty())
{
// create common x-axis
QMap<qreal, qreal> xAxisMap;
for(int i=0; i<items.size(); ++i)
{
QMap<qreal, qreal> data;
items.at(i)->curve()->getData(data);
for(QMap<qreal, qreal>::iterator iter=data.begin(); iter!=data.end(); ++iter)
{
xAxisMap.insert(iter.key(), iter.value());
}
}
QList<qreal> xAxis = xAxisMap.uniqueKeys();
QVector<QVector<qreal> > axes;
for(int i=0; i<items.size(); ++i)
{
QMap<qreal, qreal> data;
items.at(i)->curve()->getData(data);
QVector<qreal> y(xAxis.size(), std::numeric_limits<qreal>::quiet_NaN());
// just to make sure that we have the same number of data on each curve, set NAN for unknowns
int j=0;
for(QList<qreal>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
{
if(data.contains(*iter))
{
y[j] = data.value(*iter);
}
++j;
}
axes.push_back(y);
}
if(!xAxis.empty())
{
axes.push_front(xAxis.toVector());
QString text;
text.append('x');
text.append('\t');
for(int i=0; i<items.size(); ++i)
{
text.append(items.at(i)->curve()->name());
if(i+1<axes.size())
{
text.append('\t');
}
}
text.append('\n');
for(int i=0; i<axes[0].size(); ++i)
{
for(int j=0; j<axes.size(); ++j)
{
if(uIsNan(axes[j][i]))
{
text.append("NaN"); // NaN is interpreted by default as NaN in MatLab/Octave
}
else
{
text.append(QString::number(axes[j][i]));
}
if(j+1<axes.size())
{
text.append('\t');
}
}
if(i+1<axes[0].size())
{
text.append("\n");
}
}
QClipboard * clipboard = QApplication::clipboard();
clipboard->setText(text);
}
QClipboard * clipboard = QApplication::clipboard();
clipboard->setText(data);
}
}
else if(action == _aShowAllStdDevMeanMax)
@@ -3262,3 +3272,12 @@ void UPlot::moveCurve(const UPlotCurve * curve, int index)
this->update();
}
}
QString UPlot::getAllCurveDataAsText() const
{
if(_legend)
{
return _legend->getAllCurveDataAsText();
}
return "";
}