Added Depth information to ImageView's keypoint items

corelib/include/rtabmap/core/util3d.h

@@ -121,6 +121,12 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
+float RTABMAP_EXP getDepth(
+		const cv::Mat & depthImage,
+		float x, float y,
+		bool smoothing,
+		float maxZError = 0.02f);
+
 pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		const cv::Mat & depthImage,
 		float x, float y,

corelib/src/util3d.cpp

@@ -770,19 +770,15 @@ void findCorrespondences(
 	inliers2.resize(oi);
 }
 
-pcl::PointXYZ projectDepthTo3D(
+float getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,
-		float cx, float cy,
-		float fx, float fy,
 		bool smoothing,
 		float maxZError)
 {
+	UASSERT(!depthImage.empty());
 	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
 
-	pcl::PointXYZ pt;
-	float bad_point = std::numeric_limits<float>::quiet_NaN ();
-
 	int u = int(x+0.5f);
 	int v = int(y+0.5f);
 
@@ -790,8 +786,7 @@ pcl::PointXYZ projectDepthTo3D(
 	{
 		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
 				x,u,y,v,depthImage.cols, depthImage.rows);
-		pt.x = pt.y = pt.z = bad_point;
+		return 0;
-		return pt;
 	}
 
 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
@@ -845,7 +840,30 @@ pcl::PointXYZ projectDepthTo3D(
 			// mean
 			depth = (depth+sumDepths)/sumWeights;
 		}
+	}
+	else
+	{
+		depth = 0;
+	}
+	return depth;
+}
 
+pcl::PointXYZ projectDepthTo3D(
+		const cv::Mat & depthImage,
+		float x, float y,
+		float cx, float cy,
+		float fx, float fy,
+		bool smoothing,
+		float maxZError)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+
+	pcl::PointXYZ pt;
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	float depth = getDepth(depthImage, x, y, smoothing, maxZError);
+	if(depth)
+	{
 		// Use correct principal point from calibration
 		cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
 		cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)

guilib/include/rtabmap/gui/ImageView.h

@@ -74,9 +74,9 @@ public:
 	void setGraphicsViewScaled(bool scaled);
 	void setBackgroundColor(const QColor & color);
 
-	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color = Qt::yellow);
+	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
-	void setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color = Qt::yellow);
+	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
-	void addFeature(int id, const cv::KeyPoint & kpt, QColor color);
+	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
 	void addLine(float x1, float y1, float x2, float y2, QColor color);
 	void setImage(const QImage & image);
 	void setImageDepth(const QImage & image);

guilib/include/rtabmap/gui/KeypointItem.h

@@ -41,7 +41,7 @@ namespace rtabmap {
 class RTABMAPGUI_EXP KeypointItem : public QGraphicsEllipseItem
 {
 public:
-	KeypointItem(int id, const cv::KeyPoint & kpt, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
+	KeypointItem(int id, const cv::KeyPoint & kpt, float depth = 0, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
 	virtual ~KeypointItem();
 
 	void setColor(const QColor & color);
@@ -61,6 +61,7 @@ private:
 	cv::KeyPoint _kpt;
 	QGraphicsRectItem * _placeHolder;
 	int _width;
+	float _depth;
 };
 
 }

guilib/src/DatabaseViewer.cpp

@@ -1446,7 +1446,7 @@ void DatabaseViewer::update(int value,
 
 				if(data.getWords().size())
 				{
-					view->setFeatures(data.getWords());
+					view->setFeatures(data.getWords(), data.getDepthRaw().type() == CV_8UC1?cv::Mat():data.getDepthRaw(), Qt::yellow);
 				}
 
 				Transform odomPose;
@@ -2934,8 +2934,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 
 			if(!silent)
 			{
-				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords());
+				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depth());
-				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords());
+				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depth());
 				updateWordsMatching();
 			}
 		}

guilib/src/ImageView.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QVBoxLayout>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
+#include "rtabmap/core/util3d.h"
 
 namespace rtabmap {
 
@@ -543,14 +544,14 @@ void ImageView::updateOpacity()
 	}
 }
 
-void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color)
+void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth, const QColor & color)
 {
 	qDeleteAll(_features);
 	_features.clear();
 
 	for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
 	{
-		addFeature(iter->first, iter->second, color);
+		addFeature(iter->first, iter->second, depth.empty()?0:util3d::getDepth(depth, iter->second.pt.x, iter->second.pt.y, false), color);
 	}
 
 	if(!_graphicsView->isVisible())
@@ -559,14 +560,14 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, c
 	}
 }
 
-void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color)
+void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth, const QColor & color)
 {
 	qDeleteAll(_features);
 	_features.clear();
 
 	for(unsigned int i = 0; i< features.size(); ++i )
 	{
-		addFeature(i, features[i], color);
+		addFeature(i, features[i], depth.empty()?0:util3d::getDepth(depth, features[i].pt.x, features[i].pt.y, false), color);
 	}
 
 	if(!_graphicsView->isVisible())
@@ -575,10 +576,10 @@ void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QC
 	}
 }
 
-void ImageView::addFeature(int id, const cv::KeyPoint & kpt, QColor color)
+void ImageView::addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color)
 {
 	color.setAlpha(this->getAlpha());
-	rtabmap::KeypointItem * item = new rtabmap::KeypointItem(id, kpt, color);
+	rtabmap::KeypointItem * item = new rtabmap::KeypointItem(id, kpt, depth, color);
 	_features.insert(id, item);
 	item->setVisible(isFeaturesShown());
 	item->setZValue(1);

guilib/src/KeypointItem.cpp

@@ -34,11 +34,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, const QColor & color, QGraphicsItem * parent) :
+KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, float depth, const QColor & color, QGraphicsItem * parent) :
 	QGraphicsEllipseItem(kpt.pt.x-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.pt.y-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.size==0?3.0f:kpt.size, kpt.size==0?3.0f:kpt.size, parent),
 	_id(id),
 	_kpt(kpt),
-	_placeHolder(0)
+	_placeHolder(0),
+	_depth(depth)
 {
 	this->setColor(color);
 	this->setAcceptHoverEvents(true);
@@ -69,12 +70,25 @@ void KeypointItem::showDescription()
 		_placeHolder->setBrush(QBrush(QColor ( 0, 0, 0, 170 ))); // Black transparent background
 		QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
 		text->setDefaultTextColor(this->pen().color().rgb());
-		text->setPlainText(QString( "Id = %1\n"
+		if(_depth <= 0)
-				"Dir = %3\n"
+		{
-				"Hessian = %4\n"
+			text->setPlainText(QString( "Id = %1\n"
-				"X = %5\n"
+					"Dir = %3\n"
-				"Y = %6\n"
+					"Hessian = %4\n"
-				"Size = %7").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size));
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size));
+		}
+		else
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7\n"
+					"Depth = %8 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_depth));
+		}
 		_placeHolder->setRect(text->boundingRect());
 	}
 

guilib/src/MainWindow.cpp

@@ -845,13 +845,13 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 		{
 			if(info.type == 0)
 			{
-				_ui->imageView_odometry->setFeatures(info.words, Qt::yellow);
+				_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
 			}
 			else if(info.type == 1)
 			{
 				std::vector<cv::KeyPoint> kpts;
 				cv::KeyPoint::convert(info.refCorners, kpts);
-				_ui->imageView_odometry->setFeatures(kpts, Qt::red);
+				_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
 			}
 		}
 
@@ -2187,7 +2187,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 			// GREEN = NEW
 			color = Qt::green;
 		}
-		_ui->imageView_source->addFeature(iter->first, iter->second, color);
+		_ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
 	}
 	ULOGGER_DEBUG("source time = %f s", timer.ticks());
 
@@ -2225,7 +2225,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 			// GREEN = NEW
 			color = Qt::green;
 		}
-		_ui->imageView_loopClosure->addFeature(iter->first, iter->second, color);
+		_ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
 	}
 
 	ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());

guilib/src/OdometryViewer.cpp

@@ -270,13 +270,13 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 	{
 		if(info.type == 0)
 		{
-			imageView_->setFeatures(info.words, Qt::yellow);
+			imageView_->setFeatures(info.words, data.depth(), Qt::yellow);
 		}
 		else if(info.type == 1)
 		{
 			std::vector<cv::KeyPoint> kpts;
 			cv::KeyPoint::convert(info.refCorners, kpts);
-			imageView_->setFeatures(kpts, Qt::red);
+			imageView_->setFeatures(kpts, data.depth(), Qt::red);
 		}
 
 		imageView_->clearLines();

tools/EpipolarGeometry/main.cpp

@@ -126,7 +126,7 @@ private:
 				// GREEN = NEW
 				color = Qt::green;
 			}
-			view1_->addFeature(id, i->second, color);
+			view1_->addFeature(id, i->second, 0, color);
 		}
 		ULOGGER_DEBUG("source time = %f s", timer.ticks());