apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed wrong stamp republished (double precision issue, #48). Fixed region selection with SuperPoint.

Browse Source
This commit is contained in:
matlabbe 2021-11-20 13:23:29 -05:00
parent fd9270a561
commit b2677fdde0
13 changed files with 137 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
CMakeLists.txt
View File

@ -73,20 +73,32 @@ IF(TORCH_FOUND)
SET(TORCH 1)
ENDIF(TORCH_FOUND)
IF(OpenCV_VERSION_MAJOR EQUAL 4)
IF(NOT MSVC)
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
IF(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
ELSEIF(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
ELSE()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
IF(NOT MSVC)
IF(TORCH_FOUND)
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
IF(COMPILER_SUPPORTS_CXX14)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
set(CMAKE_CXX_STANDARD 14)
ELSE()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use Torch.")
ENDIF()
ENDIF(TORCH_FOUND)
IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND OpenCV_VERSION_MAJOR EQUAL 4)
#Newest versions require std11
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
IF(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
ELSEIF(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
ELSE()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
ENDIF()
ENDIF()
ENDIF(NOT MSVC)
ENDIF(OpenCV_VERSION_MAJOR EQUAL 4)
ENDIF()
# look for Qt5 before Qt4
FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Network QUIET)

4
include/find_object/Camera.h
View File

@ -28,8 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CAMERA_H_
#define CAMERA_H_
#include <find_object/Header.h>
#include "find_object/FindObjectExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <QtCore/QObject>
#include <QtCore/QTimer>
@ -57,7 +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 imageReceived(const cv::Mat & image, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
void finished();
public Q_SLOTS:

6
include/find_object/FindObject.h
View File

@ -28,11 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef FINDOBJECT_H_
#define FINDOBJECT_H_
#include <find_object/Header.h>
#include "find_object/FindObjectExp.h" // DLL export/import defines
#include "find_object/DetectionInfo.h"
#include "find_object/Settings.h"
#include <QtCore/QObject>
#include <QtCore/QString>
#include <QtCore/QMap>
@ -92,10 +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()
void detect(const cv::Mat & image, const find_object::Header & header, const cv::Mat & depth, float depthConstant); // emit objectsFound()
Q_SIGNALS:
void objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float);
void objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float);
private:
void clearVocabulary();

56
include/find_object/Header.h Normal file
View File

@ -0,0 +1,56 @@
/*
Copyright (c) 2011-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef INCLUDE_FIND_OBJECT_HEADER_H_
#define INCLUDE_FIND_OBJECT_HEADER_H_
#include <bits/stdint-uintn.h>
#include <QString>
namespace find_object {
class Header {
public:
Header() :
sec_(0),
nsec_(0)
{
}
Header(const char * frameId, uint64_t sec, uint64_t nsec) :
frameId_(frameId),
sec_(sec),
nsec_(nsec)
{
}
QString frameId_;
uint64_t sec_;
uint64_t nsec_;
};
}
#endif /* INCLUDE_FIND_OBJECT_HEADER_H_ */

5
include/find_object/MainWindow.h
View File

@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "find_object/FindObjectExp.h" // DLL export/import defines
#include "find_object/DetectionInfo.h"
#include "find_object/Header.h"
#include <QMainWindow>
#include <QtCore/QSet>
@ -80,7 +81,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);
void update(const cv::Mat & image, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
private Q_SLOTS:
void loadSession();
@ -114,7 +115,7 @@ private Q_SLOTS:
void rectHovered(int objId);
Q_SIGNALS:
void objectsFound(const find_object::DetectionInfo &, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
void objectsFound(const find_object::DetectionInfo &, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
private:
bool loadSettings(const QString & path);

25
src/AddObjectDialog.cpp
View File

@ -112,7 +112,7 @@ void AddObjectDialog::closeEvent(QCloseEvent* event)
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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
}
QDialog::closeEvent(event);
}
@ -229,7 +229,7 @@ void AddObjectDialog::setState(int state)
else
{
connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)));
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)));
connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
}
}
else if(state == kSelectFeatures)
@ -237,7 +237,7 @@ void AddObjectDialog::setState(int state)
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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
camera_->pause();
}
@ -268,7 +268,7 @@ void AddObjectDialog::setState(int state)
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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
camera_->pause();
}
@ -302,7 +302,7 @@ void AddObjectDialog::setState(int state)
roi_ = computeROI(selectedKeypoints);
}
cv::Mat imgRoi(cameraImage_, roi_);
cv::Mat imgRoi = cv::Mat(cameraImage_, roi_).clone();
if(ui_->comboBox_selection->currentIndex() == 1)
{
@ -321,7 +321,7 @@ void AddObjectDialog::setState(int state)
selectedKeypoints.clear();
detector_->detect(imgRoi, selectedKeypoints);
}
ui_->objectView->updateImage(cvtCvMat2QImage(imgRoi.clone()));
ui_->objectView->updateImage(cvtCvMat2QImage(imgRoi));
ui_->objectView->updateData(selectedKeypoints, QMultiMap<int,int>());
ui_->objectView->setMinimumSize(roi_.width, roi_.height);
ui_->objectView->update();
@ -345,7 +345,14 @@ void AddObjectDialog::setState(int state)
if(keypoints.size())
{
// Extract descriptors
extractor_->compute(imgRoi, keypoints, descriptors);
if(Settings::currentDetectorType() == Settings::currentDescriptorType())
{
detector_->compute(imgRoi, keypoints, descriptors);
}
else
{
extractor_->compute(imgRoi, keypoints, descriptors);
}
if(keypoints.size() != (unsigned int)descriptors.rows)
{
@ -374,10 +381,10 @@ void AddObjectDialog::setState(int state)
void AddObjectDialog::update(const cv::Mat & image)
{
update(image, "", 0.0, cv::Mat(), 0.0);
update(image, Header(), cv::Mat(), 0.0);
}
void AddObjectDialog::update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
void AddObjectDialog::update(const cv::Mat & image, const Header & header, const cv::Mat & depth, float depthConstant)
{
cameraImage_ = cv::Mat();
if(!image.empty())

3
src/AddObjectDialog.h
View File

@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtCore/QTimer>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/core.hpp>
#include "find_object/Header.h"
class Ui_addObjectDialog;
@ -56,7 +57,7 @@ public:
private Q_SLOTS:
void update(const cv::Mat &);
void update(const cv::Mat &, const QString &, double, const cv::Mat &, float);
void update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float);
void next();
void back();
void cancel();

7
src/FindObject.cpp
View File

@ -57,6 +57,7 @@ FindObject::FindObject(bool keepImagesInRAM, QObject * parent) :
keepImagesInRAM_(keepImagesInRAM)
{
qRegisterMetaType<find_object::DetectionInfo>("find_object::DetectionInfo");
qRegisterMetaType<find_object::Header>("find_object::Header");
UASSERT(detector_ != 0 && extractor_ != 0);
if(Settings::getGeneral_debug())
@ -1383,10 +1384,10 @@ private:
void FindObject::detect(const cv::Mat & image)
{
detect(image, "", 0.0, cv::Mat(), 0.0);
detect(image, Header(), cv::Mat(), 0.0);
}
void FindObject::detect(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
void FindObject::detect(const cv::Mat & image, const Header & header, const cv::Mat & depth, float depthConstant)
{
QTime time;
time.start();
@ -1416,7 +1417,7 @@ void FindObject::detect(const cv::Mat & image, const QString & frameId, double s
if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents())
{
Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant);
Q_EMIT objectsFound(info, header, depth, depthConstant);
}
}

18
src/MainWindow.cpp
View File

@ -287,7 +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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
camera_->stop();
qDeleteAll(objWidgets_);
@ -351,7 +351,7 @@ void MainWindow::setupTCPServer()
delete tcpServer_;
}
tcpServer_ = new TcpServer(Settings::getGeneral_port(), this);
connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)), tcpServer_, SLOT(publishDetectionInfo(find_object::DetectionInfo)));
connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, 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());
@ -786,7 +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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
AddObjectDialog * dialog;
bool resumeCamera = camera_->isRunning();
@ -822,7 +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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), Qt::UniqueConnection);
connect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()), Qt::UniqueConnection);
if(!sceneImage_.empty())
{
@ -1167,7 +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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, 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);
@ -1225,7 +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(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(update(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
disconnect(camera_, SIGNAL(finished()), this, SLOT(stopProcessing()));
camera_->stop();
}
@ -1290,10 +1290,10 @@ void MainWindow::rectHovered(int objId)
void MainWindow::update(const cv::Mat & image)
{
update(image, "", 0.0, cv::Mat(), 0.0);
update(image, Header(), cv::Mat(), 0.0);
}
void MainWindow::update(const cv::Mat & image, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
void MainWindow::update(const cv::Mat & image, const Header & header, const cv::Mat & depth, float depthConstant)
{
if(image.empty())
{
@ -1559,7 +1559,7 @@ void MainWindow::update(const cv::Mat & image, const QString & frameId, double s
if(info.objDetected_.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents())
{
Q_EMIT objectsFound(info, frameId, stamp, depth, depthConstant);
Q_EMIT objectsFound(info, header, depth, depthConstant);
}
ui_->label_objectsDetected->setNum(info.objDetected_.size());
}

4
src/ros/CameraROS.cpp
View File

@ -139,7 +139,7 @@ void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
}
Q_EMIT imageReceived(image, QString(msg->header.frame_id.c_str()), msg->header.stamp.toSec(), cv::Mat(), 0.0f);
Q_EMIT imageReceived(image, Header(msg->header.frame_id.c_str(), msg->header.stamp.sec, msg->header.stamp.nsec), cv::Mat(), 0.0f);
}
catch(const cv_bridge::Exception & e)
{
@ -180,7 +180,7 @@ void CameraROS::imgDepthReceivedCallback(
image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
}
Q_EMIT imageReceived(image, QString(rgbMsg->header.frame_id.c_str()), rgbMsg->header.stamp.toSec(), ptrDepth->image, depthConstant);
Q_EMIT imageReceived(image, Header(rgbMsg->header.frame_id.c_str(), rgbMsg->header.stamp.sec, rgbMsg->header.stamp.nsec), ptrDepth->image, depthConstant);
}
catch(const cv_bridge::Exception & e)
{

19
src/ros/FindObjectROS.cpp
View File

@ -52,10 +52,10 @@ 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(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)));
this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
}
void FindObjectROS::publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant)
void FindObjectROS::publish(const find_object::DetectionInfo & info, const Header & header, const cv::Mat & depth, float depthConstant)
{
// send tf before the message
if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f)
@ -116,8 +116,9 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
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.toStdString();
transform.stamp_.fromSec(stamp);
transform.frame_id_ = header.frameId_.toStdString();
transform.stamp_.sec = header.sec_;
transform.stamp_.nsec = header.nsec_;
tf::Quaternion q;
if(usePnP_)
@ -274,15 +275,17 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
if(pubStamped_.getNumSubscribers())
{
// use same header as the input image (for synchronization and frame reference)
msgStamped.header.frame_id = frameId.toStdString();
msgStamped.header.stamp.fromSec(stamp);
msgStamped.header.frame_id = header.frameId_.toStdString();
msgStamped.header.stamp.sec = header.sec_;
msgStamped.header.stamp.nsec = header.nsec_;
pubStamped_.publish(msgStamped);
}
if(pubInfo_.getNumSubscribers())
{
// use same header as the input image (for synchronization and frame reference)
infoMsg.header.frame_id = frameId.toStdString();
infoMsg.header.stamp.fromSec(stamp);
infoMsg.header.frame_id = header.frameId_.toStdString();
infoMsg.header.stamp.sec = header.sec_;
infoMsg.header.stamp.nsec = header.nsec_;
pubInfo_.publish(infoMsg);
}
}

2
src/ros/FindObjectROS.h
View File

@ -48,7 +48,7 @@ public:
virtual ~FindObjectROS() {}
public Q_SLOTS:
void publish(const find_object::DetectionInfo & info, const QString & frameId, double stamp, const cv::Mat & depth, float depthConstant);
void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
private:
cv::Vec3f getDepth(const cv::Mat & depthImage,

6
src/ros/find_object_2d_node.cpp
View File

@ -148,9 +148,9 @@ int main(int argc, char** argv)
QObject::connect(
&mainWindow,
SIGNAL(objectsFound(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)),
SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)),
findObjectROS,
SLOT(publish(const find_object::DetectionInfo &, const QString &, double, const cv::Mat &, float)));
SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
QStringList topics = camera->subscribedTopics();
if(topics.size() == 1)
@ -179,7 +179,7 @@ int main(int argc, char** argv)
QCoreApplication app(argc, argv);
// connect stuff:
QObject::connect(camera, SIGNAL(imageReceived(const cv::Mat &, const QString &, double, const cv::Mat &, float)), findObjectROS, SLOT(detect(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
QObject::connect(camera, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), findObjectROS, SLOT(detect(const cv::Mat &, const QString &, double, const cv::Mat &, float)));
//loop
camera->start();