RegVis: fixed out-of-range vector error when guess is used and we match to projections. ImageView: Added right-click menu option to set a fixed size for features

corelib/src/RegistrationVis.cpp

@@ -845,7 +845,7 @@ Transform RegistrationVis::computeTransformationImpl(
 										{
 											std::vector<std::vector<cv::DMatch> > matches;
 											cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
-											matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
+											matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
 											UASSERT(matches.size() == 1);
 											UASSERT(matches[0].size() == 2);
 											if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)

corelib/src/util3d_filtering.cpp

@@ -203,7 +203,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 	}
 	else if(cloud->size() && !cloud->is_dense && indices->size() == 0)
 	{
-		UWARN("Cannot voxelize a cloud not dense with empty indices! (input=%d pts)", (int)cloud->size());
+		UWARN("Cannot voxelize a not dense (organized) cloud with empty indices! (input=%d pts). Returning empty cloud!", (int)cloud->size());
 	}
 	return output;
 }

guilib/include/rtabmap/gui/ImageView.h

@@ -61,6 +61,7 @@ public:
 	bool isFeaturesShown() const;
 	bool isLinesShown() const;
 	int getAlpha() const {return _alpha;}
+	int getFeaturesSize() const {return _featuresSize;}
 	bool isGraphicsViewMode() const;
 	bool isGraphicsViewScaled() const;
 	const QColor & getDefaultBackgroundColor() const;
@@ -86,6 +87,7 @@ public:
 	void setFeatureColor(int id, QColor color);
 	void setFeaturesColor(QColor color);
 	void setAlpha(int alpha);
+	void setFeaturesSize(int size);
 	void setSceneRect(const QRectF & rect);
 
 	const QMultiMap<int, rtabmap::KeypointItem *> & getFeatures() const {return _features;}
@@ -114,6 +116,7 @@ private:
 private:
 	QString _savedFileName;
 	int _alpha;
+	int _featuresSize;
 	QColor _defaultBgColor;
 
 	QMenu * _menu;
@@ -123,6 +126,7 @@ private:
 	QAction * _showLines;
 	QAction * _saveImage;
 	QAction * _setAlpha;
+	QAction * _setFeaturesSize;
 	QAction * _graphicsViewMode;
 	QAction * _graphicsViewScaled;
 

guilib/include/rtabmap/gui/KeypointItem.h

@@ -45,6 +45,7 @@ public:
 	virtual ~KeypointItem();
 
 	void setColor(const QColor & color);
+	const cv::KeyPoint & keypoint() const {return _kpt;}
 
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event );

guilib/src/ImageView.cpp

@@ -155,6 +155,7 @@ ImageView::ImageView(QWidget * parent) :
 		QWidget(parent),
 		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
 		_alpha(50),
+		_featuresSize(0.0f),
 		_defaultBgColor(Qt::black),
 		_imageItem(0),
 		_imageDepthItem(0)
@@ -178,6 +179,7 @@ ImageView::ImageView(QWidget * parent) :
 	_showFeatures = _menu->addAction(tr("Show features"));
 	_showFeatures->setCheckable(true);
 	_showFeatures->setChecked(true);
+	_setFeaturesSize = _menu->addAction(tr("Set features size..."));
 	_showLines = _menu->addAction(tr("Show lines"));
 	_showLines->setCheckable(true);
 	_showLines->setChecked(true);
@@ -208,6 +210,7 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
 	settings.setValue("image_shown", this->isImageShown());
 	settings.setValue("depth_shown", this->isImageDepthShown());
 	settings.setValue("features_shown", this->isFeaturesShown());
+	settings.setValue("features_size", this->getFeaturesSize());
 	settings.setValue("lines_shown", this->isLinesShown());
 	settings.setValue("alpha", this->getAlpha());
 	settings.setValue("bg_color", this->getDefaultBackgroundColor());
@@ -228,6 +231,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	this->setImageShown(settings.value("image_shown", this->isImageShown()).toBool());
 	this->setImageDepthShown(settings.value("depth_shown", this->isImageDepthShown()).toBool());
 	this->setFeaturesShown(settings.value("features_shown", this->isFeaturesShown()).toBool());
+	this->setFeaturesSize(settings.value("features_size", this->getFeaturesSize()).toInt());
 	this->setLinesShown(settings.value("lines_shown", this->isLinesShown()).toBool());
 	this->setAlpha(settings.value("alpha", this->getAlpha()).toInt());
 	this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
@@ -641,6 +645,16 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 			emit configChanged();
 		}
 	}
+	else if(action == _setFeaturesSize)
+	{
+		bool ok = false;
+		int value = QInputDialog::getInt(this, tr("Set features size"), tr("Size (0 means actual keypoint size)"), _featuresSize, 0, 999, 1, &ok);
+		if(ok)
+		{
+			this->setFeaturesSize(value);
+			emit configChanged();
+		}
+	}
 
 	if(action == _showImage || action ==_showImageDepth)
 	{
@@ -738,6 +752,10 @@ void ImageView::addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor
 {
 	color.setAlpha(this->getAlpha());
 	rtabmap::KeypointItem * item = new rtabmap::KeypointItem(id, kpt, depth, color);
+	if(_featuresSize>0.0f)
+	{
+		item->setRect(kpt.pt.x-_featuresSize/2.0f, kpt.pt.y-_featuresSize/2.0f, _featuresSize, _featuresSize);
+	}
 	_features.insert(id, item);
 	item->setVisible(isFeaturesShown());
 	item->setZValue(1);
@@ -890,6 +908,26 @@ void ImageView::setAlpha(int alpha)
 	}
 }
 
+void ImageView::setFeaturesSize(int size)
+{
+	_featuresSize = size;
+	for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
+	{
+		const cv::KeyPoint & kpt = iter.value()->keypoint();
+		if(size <= 0.0f)
+		{
+			size = kpt.size==0?3:kpt.size;
+		}
+		float sizef = size;
+		iter.value()->setRect(kpt.pt.x-sizef/2.0f, kpt.pt.y-sizef/2.0f, sizef, sizef);
+	}
+
+	if(!_graphicsView->isVisible())
+	{
+		this->update();
+	}
+}
+
 void ImageView::setSceneRect(const QRectF & rect)
 {
 	_graphicsView->scene()->setSceneRect(rect);

tools/Report/main.cpp

@@ -84,6 +84,7 @@ int main(int argc, char * argv[])
 					std::vector<float> slamTime;
 					slamTime.reserve(ids.size());
 					float rmse = -1;
+					float maxRMSE = -1;
 					for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 					{
 						Transform p, gt;
@@ -100,6 +101,10 @@ int main(int argc, char * argv[])
 							if(uContains(stat, Statistics::kGtTranslational_rmse()))
 							{
 								rmse = stat.at(Statistics::kGtTranslational_rmse());
+								if(maxRMSE==-1 || maxRMSE < rmse)
+								{
+									maxRMSE = rmse;
+								}
 							}
 							if(uContains(stat, std::string("Camera/TotalTime/ms")))
 							{
@@ -115,10 +120,11 @@ int main(int argc, char * argv[])
 							}
 						}
 					}
-					printf("   %s (%d): %fm, slam: avg=%dms max=%dms, odom: avg=%dms max=%dms, camera: avg=%dms max=%dms\n",
+					printf("   %s (%d): %fm (max=%fm), slam: avg=%dms max=%dms, odom: avg=%dms max=%dms, camera: avg=%dms max=%dms\n",
 							fileName.c_str(),
 							(int)ids.size(),
 							rmse,
+							maxRMSE,
 							(int)uMean(slamTime), (int)uMax(slamTime),
 							(int)uMean(odomTime), (int)uMax(odomTime),
 							(int)uMean(cameraTime), (int)uMax(cameraTime));