Refactored how depth is used to compute 3D transform (making sure the same depth image is used with same stamp and frame_id). Fixed CameraTcpServer ingoring incoming connections with Qt5. Added approx_sync parameter to ros.

include/find_object/Camera.h

@@ -57,6 +57,7 @@ public:
 
 Q_SIGNALS:
 	void imageReceived(const cv::Mat & image);
+	void imageReceived(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
 	void finished();
 
 public Q_SLOTS:

include/find_object/FindObject.h

@@ -92,9 +92,10 @@ public Q_SLOTS:
 	void addObjectAndUpdate(const cv::Mat & image, int id=0, const QString & filePath = QString());
 	void removeObjectAndUpdate(int id);
 	void detect(const cv::Mat & image); // emit objectsFound()
+	void detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant); // emit objectsFound()
 
 Q_SIGNALS:
-	void objectsFound(const find_object::DetectionInfo &);
+	void objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float);
 
 private:
 	void clearVocabulary();

include/find_object/MainWindow.h

@@ -80,6 +80,7 @@ public Q_SLOTS:
 	void stopProcessing();
 	void pauseProcessing();
 	void update(const cv::Mat & image);
+	void update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
 
 private Q_SLOTS:
 	void loadSession();
@@ -113,7 +114,7 @@ private Q_SLOTS:
 	void rectHovered(int objId);
 
 Q_SIGNALS:
-	void objectsFound(const find_object::DetectionInfo &);
+	void objectsFound(const find_object::DetectionInfo &, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
 
 private:
 	bool loadSettings(const QString & path);

launch/find_object_3d.launch

@@ -8,6 +8,7 @@
 		<param name="subscribe_depth" value="true" type="bool"/>
 		<param name="objects_path" value="" type="str"/>
 		<param name="object_prefix" value="object" type="str"/>
+		<param name="approx_sync" value="true" type="bool"/>
 		
 		<remap from="rgb/image_rect_color" to="camera/rgb/image_rect_color"/>
 		<remap from="depth_registered/image_raw" to="camera/depth_registered/image_raw"/>

src/CameraTcpServer.cpp

@@ -100,8 +100,11 @@ quint16 CameraTcpServer::getPort() const
 {
 	return this->serverPort();
 }
-
+#if QT_VERSION >= 0x050000
+void CameraTcpServer::incomingConnection(qintptr socketDescriptor)
+#else
 void CameraTcpServer::incomingConnection(int socketDescriptor)
+#endif
 {
 	QList<QTcpSocket*> clients = this->findChildren<QTcpSocket*>();
 	if(clients.size() >= 1)

src/CameraTcpServer.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAMERATCPCLIENT_H_
 #define CAMERATCPCLIENT_H_
 
+#include <QtGlobal>
 #include <QtNetwork/QTcpServer>
 #include <opencv2/opencv.hpp>
 
@@ -46,7 +47,11 @@ public:
 	quint16 getPort() const;
 
 protected:
+#if QT_VERSION >= 0x050000
+	virtual void incomingConnection ( qintptr socketDescriptor );
+#else
 	virtual void incomingConnection ( int socketDescriptor );
+#endif
 
 private Q_SLOTS:
 	void readReceivedData();

src/FindObject.cpp

@@ -1382,6 +1382,11 @@ private:
 };
 
 void FindObject::detect(const cv::Mat & image)
+{
+	detect(image, "", 0.0, cv::Mat(), 0.0);
+}
+
+void FindObject::detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
 {
 	QTime time;
 	time.start();
@@ -1411,7 +1416,7 @@ void FindObject::detect(const cv::Mat & image)
 
 	if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents())
 	{
-		Q_EMIT objectsFound(info);
+		Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant);
 	}
 }
 

src/MainWindow.cpp

@@ -287,6 +287,7 @@ MainWindow::MainWindow(FindObject * findObject, Camera * camera, QWidget * paren
 MainWindow::~MainWindow()
 {
 	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
+	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
 	disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
 	camera_->stop();
 	qDeleteAll(objWidgets_);
@@ -350,7 +351,7 @@ void MainWindow::setupTCPServer()
 		delete tcpServer_;
 	}
 	tcpServer_ = new TcpServer(Settings::getGeneral_port(), this);
-	connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo)));
+	connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo)));
 	ui_->label_ipAddress->setText(tcpServer_->getHostAddress().toString());
 	ui_->label_port->setNum(tcpServer_->getPort());
 	UINFO("Detection sent on port: %d (IP=%s)", tcpServer_->getPort(), tcpServer_->getHostAddress().toString().toStdString().c_str());
@@ -785,6 +786,7 @@ void MainWindow::hideObjectsFeatures()
 void MainWindow::addObjectFromScene()
 {
 	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
+	disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
 	disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
 	AddObjectDialog * dialog;
 	bool resumeCamera = camera_->isRunning();
@@ -820,6 +822,7 @@ void MainWindow::addObjectFromScene()
 	else
 	{
 		connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection);
+		connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)), Qt::UniqueConnection);
 		connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection);
 		if(!sceneImage_.empty())
 		{
@@ -1164,6 +1167,7 @@ void MainWindow::startProcessing()
 	if(camera_->start())
 	{
 		connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection);
+		connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)), Qt::UniqueConnection);
 		connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection);
 		ui_->actionStop_camera->setEnabled(true);
 		ui_->actionPause_camera->setEnabled(true);
@@ -1221,6 +1225,7 @@ void MainWindow::stopProcessing()
 	if(camera_)
 	{
 		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
+		disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
 		disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
 		camera_->stop();
 	}
@@ -1284,6 +1289,12 @@ void MainWindow::rectHovered(int objId)
 }
 
 void MainWindow::update(const cv::Mat & image)
+{
+	UERROR("");
+	update(image, "", 0.0, cv::Mat(), 0.0);
+}
+
+void MainWindow::update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
 {
 	if(image.empty())
 	{
@@ -1549,7 +1560,7 @@ void MainWindow::update(const cv::Mat & image)
 
 		if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents())
 		{
-			Q_EMIT objectsFound(info);
+			Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant);
 		}
 		ui_->label_objectsDetected->setNum(info.objDetected_.size());
 	}

src/ros/CameraROS.cpp

@@ -35,7 +35,9 @@ using namespace find_object;
 
 CameraROS::CameraROS(bool subscribeDepth, QObject * parent) :
 	Camera(parent),
-	subscribeDepth_(subscribeDepth)
+	subscribeDepth_(subscribeDepth),
+	approxSync_(0),
+	exactSync_(0)
 {
 	ros::NodeHandle nh; // public
 	ros::NodeHandle pnh("~"); // private
@@ -51,8 +53,11 @@ CameraROS::CameraROS(bool subscribeDepth, QObject * parent) :
 	else
 	{
 		int queueSize = 10;
+		bool approxSync = true;
 		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("approx_sync", approxSync, approxSync);
 		ROS_INFO("find_object_ros: queue_size = %d", queueSize);
+		ROS_INFO("find_object_ros: approx_sync = %s", approxSync?"true":"false");
 
 		ros::NodeHandle rgb_nh(nh, "rgb");
 		ros::NodeHandle rgb_pnh(pnh, "rgb");
@@ -66,10 +71,22 @@ CameraROS::CameraROS(bool subscribeDepth, QObject * parent) :
 		rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb);
 		depthSub_.subscribe(depth_it, depth_nh.resolveName("image_raw"), 1, hintsDepth);
 		cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
-		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
+		if(approxSync)
-		sync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
+		{
-
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
+			approxSync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
 		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
+			exactSync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
+		}
+	}
+}
+CameraROS::~CameraROS()
+{
+	delete approxSync_;
+	delete exactSync_;
 }
 
 QStringList CameraROS::subscribedTopics() const
@@ -122,8 +139,7 @@ void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
 				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
 			}
 
-			Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f);
+			Q_EMIT imageReceived(image, QString(msg->header.frame_id.c_str()), msg->header.stamp.toSec(), cv::Mat(), 0.0f);
-			Q_EMIT imageReceived(image);
 		}
 		catch(const cv_bridge::Exception & e)
 		{
@@ -164,8 +180,7 @@ void CameraROS::imgDepthReceivedCallback(
 				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
 			}
 
-			Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
+			Q_EMIT imageReceived(image, QString(rgbMsg->header.frame_id.c_str()), rgbMsg->header.stamp.toSec(), ptrDepth->image, depthConstant);
-			Q_EMIT imageReceived(image);
 		}
 		catch(const cv_bridge::Exception & e)
 		{

src/ros/CameraROS.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
 
 #include <image_transport/image_transport.h>
 #include <image_transport/subscriber_filter.h>
@@ -49,19 +50,13 @@ class CameraROS : public find_object::Camera {
 	Q_OBJECT
 public:
 	CameraROS(bool subscribeDepth, QObject * parent = 0);
-	virtual ~CameraROS() {}
+	virtual ~CameraROS();
 
 	virtual bool start();
 	virtual void stop();
 
 	QStringList subscribedTopics() const;
 
-Q_SIGNALS:
-	void rosDataReceived(const std::string & frameId,
-			const ros::Time & stamp,
-			const cv::Mat & depth,
-			float depthConstant);
-
 private Q_SLOTS:
 	virtual void takeImage();
 
@@ -83,8 +78,14 @@ private:
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
 			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MySyncPolicy;
+			sensor_msgs::CameraInfo> MyApproxSyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 };
 
 #endif /* CAMERAROS_H_ */

src/ros/FindObjectROS.cpp

@@ -37,8 +37,7 @@ using namespace find_object;
 
 FindObjectROS::FindObjectROS(QObject * parent) :
 	FindObject(true, parent),
-	objFramePrefix_("object"),
+	objFramePrefix_("object")
-	depthConstant_(0.0f)
 {
 	ros::NodeHandle pnh("~"); // public
 	pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
@@ -50,13 +49,13 @@ FindObjectROS::FindObjectROS(QObject * parent) :
 	pubStamped_ = nh.advertise<find_object_2d::ObjectsStamped>("objectsStamped", 1);
 	pubInfo_ = nh.advertise<find_object_2d::DetectionInfo>("info", 1);
 
-	this->connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), this, SLOT(publish(find_object::DetectionInfo)));
+	this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)));
 }
 
-void FindObjectROS::publish(const find_object::DetectionInfo & info)
+void FindObjectROS::publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
 {
 	// send tf before the message
-	if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f)
+	if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f)
 	{
 		std::vector<tf::StampedTransform> transforms;
 		char multiSubId = 'b';
@@ -87,20 +86,20 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
 			QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
 			QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
 
-			cv::Vec3f center3D = this->getDepth(depth_,
+			cv::Vec3f center3D = this->getDepth(depth,
 					center.x()+0.5f, center.y()+0.5f,
-					float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
+					float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-					1.0f/depthConstant_, 1.0f/depthConstant_);
+					1.0f/depthConstant, 1.0f/depthConstant);
 
-			cv::Vec3f axisEndX = this->getDepth(depth_,
+			cv::Vec3f axisEndX = this->getDepth(depth,
 					xAxis.x()+0.5f, xAxis.y()+0.5f,
-					float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
+					float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-					1.0f/depthConstant_, 1.0f/depthConstant_);
+					1.0f/depthConstant, 1.0f/depthConstant);
 
-			cv::Vec3f axisEndY = this->getDepth(depth_,
+			cv::Vec3f axisEndY = this->getDepth(depth,
 					yAxis.x()+0.5f, yAxis.y()+0.5f,
-					float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
+					float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
-					1.0f/depthConstant_, 1.0f/depthConstant_);
+					1.0f/depthConstant, 1.0f/depthConstant);
 
 			if(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
 				std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
@@ -109,8 +108,8 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
 				tf::StampedTransform transform;
 				transform.setIdentity();
 				transform.child_frame_id_ = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
-				transform.frame_id_ = frameId_;
+				transform.frame_id_ = frameId.toStdString();
-				transform.stamp_ = stamp_;
+				transform.stamp_.fromSec(stamp);
 				transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
 
 				//set rotation
@@ -130,6 +129,9 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
 				q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0);
 				transform.setRotation(q.normalized());
 
+				ROS_WARN("%f %f %f (%f %f %f %f)", center3D.val[0], center3D.val[1], center3D.val[2],
+						transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w());
+
 				transforms.push_back(transform);
 			}
 			else
@@ -223,31 +225,20 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
 		if(pubStamped_.getNumSubscribers())
 		{
 			// use same header as the input image (for synchronization and frame reference)
-			msgStamped.header.frame_id = frameId_;
+			msgStamped.header.frame_id = frameId.toStdString();
-			msgStamped.header.stamp = stamp_;
+			msgStamped.header.stamp.fromSec(stamp);
 			pubStamped_.publish(msgStamped);
 		}
 		if(pubInfo_.getNumSubscribers())
 		{
 			// use same header as the input image (for synchronization and frame reference)
-			infoMsg.header.frame_id = frameId_;
+			infoMsg.header.frame_id = frameId.toStdString();
-			infoMsg.header.stamp = stamp_;
+			infoMsg.header.stamp.fromSec(stamp);
 			pubInfo_.publish(infoMsg);
 		}
 	}
 }
 
-void FindObjectROS::setDepthData(const std::string & frameId,
-		const ros::Time & stamp,
-		const cv::Mat & depth,
-		float depthConstant)
-{
-	frameId_ = frameId;
-	stamp_ = stamp;
-	depth_ = depth;
-	depthConstant_ = depthConstant;
-}
-
 cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
 				   int x, int y,
 				   float cx, float cy,

src/ros/FindObjectROS.h

@@ -48,12 +48,7 @@ public:
 	virtual ~FindObjectROS() {}
 
 public Q_SLOTS:
-	void publish(const find_object::DetectionInfo & info);
+	void publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
-
-	void setDepthData(const std::string & frameId,
-			const ros::Time & stamp,
-			const cv::Mat & depth,
-			float depthConstant);
 
 private:
 	cv::Vec3f getDepth(const cv::Mat & depthImage,
@@ -67,11 +62,6 @@ private:
 	ros::Publisher pubStamped_;
 	ros::Publisher pubInfo_;
 
-	std::string frameId_;
-	ros::Time stamp_;
-	cv::Mat depth_;
-	float depthConstant_;
-
 	std::string objFramePrefix_;
 	tf::TransformBroadcaster tfBroadcaster_;
 

src/ros/find_object_2d_node.cpp

@@ -138,12 +138,6 @@ int main(int argc, char** argv)
 
 	CameraROS * camera = new CameraROS(subscribeDepth);
 
-	QObject::connect(
-			camera,
-			SIGNAL(rosDataReceived(const std::string &, const ros::Time &, const cv::Mat &, float)),
-			findObjectROS,
-			SLOT(setDepthData(const std::string &, const ros::Time &, const cv::Mat &, float)));
-
 	// Catch ctrl-c to close the gui
 	setupQuitSignal(gui);
 
@@ -154,9 +148,9 @@ int main(int argc, char** argv)
 
 		QObject::connect(
 				&mainWindow,
-				SIGNAL(objectsFound(const find_object::DetectionInfo &)),
+				SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)),
 				findObjectROS,
-				SLOT(publish(const find_object::DetectionInfo &)));
+				SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)));
 
 		QStringList topics = camera->subscribedTopics();
 		if(topics.size() == 1)