i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added multi-camera feature

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:46:48 -04:00
parent e6923daf1c
commit c6d0d47b1c
51 changed files with 2833 additions and 2297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
guilib/include/rtabmap/gui/DataRecorder.h
View File

@@ -57,7 +57,7 @@ public:
const QString & path() const {return path_;}
public slots:
void addData(const rtabmap::SensorData & data);
void addData(const rtabmap::SensorData & data, const Transform & pose = Transform(), const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1));
void showImage(const cv::Mat & image, const cv::Mat & depth);
protected:
virtual void closeEvent(QCloseEvent* event);

4
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -52,7 +52,7 @@ namespace rtabmap
{
class Memory;
class ImageView;
class Signature;
class SensorData;
class CloudViewer;
class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
@@ -124,7 +124,7 @@ private:
QLabel * labelMapId,
QLabel * labelPose,
bool updateConstraintView);
void updateStereo(const Signature * data);
void updateStereo(const SensorData * data);
void updateWordsMatching();
void updateConstraintView(
const rtabmap::Link & link,

19
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtCore/QSet>
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/gui/PreferencesDialog.h"
#include <pcl/point_cloud.h>
@@ -161,7 +161,7 @@ private slots:
void selectScreenCaptureFormat(bool checked);
void takeScreenshot();
void updateElapsedTime();
void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
void processOdometry(const rtabmap::OdometryEvent & odom);
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
void applyPrefSettings(const rtabmap::ParametersMap & parameters);
void processRtabmapEventInit(int status, const QString & info);
@@ -194,7 +194,7 @@ private slots:
signals:
void statsReceived(const rtabmap::Statistics &);
void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &);
void odometryReceived(const rtabmap::OdometryEvent &);
void thresholdsChanged(int, int);
void stateChanged(MainWindow::State);
void rtabmapEventInitReceived(int status, const QString & info);
@@ -227,19 +227,6 @@ private:
int regenerateDecimation,
float regenerateVoxelSize,
float regenerateMaxDepth) const;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
int id,
const cv::Mat & rgb,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
const Transform & pose,
float voxelSize,
int decimation,
float maxDepth) const;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > getClouds(
const std::map<int, Transform> & poses,
bool regenerateClouds,

5
guilib/include/rtabmap/gui/OdometryViewer.h
View File

@@ -30,8 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/OdometryEvent.h"
#include <QDialog>
#include "rtabmap/utilite/UEventsHandler.h"
@@ -59,7 +58,7 @@ protected:
virtual void handleEvent(UEvent * event);
private slots:
void processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
void processData(const rtabmap::OdometryEvent & odom);
private:
ImageView* imageView_;

4
guilib/src/CalibrationDialog.cpp
View File

@@ -239,8 +239,8 @@ void CalibrationDialog::handleEvent(UEvent * event)
{
processingData_ = true;
QMetaObject::invokeMethod(this, "processImages",
Q_ARG(cv::Mat, e->data().image()),
Q_ARG(cv::Mat, e->data().depthOrRightImage()),
Q_ARG(cv::Mat, e->data().imageRaw()),
Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
Q_ARG(QString, QString(e->cameraName().c_str())));
}
}

16
guilib/src/CameraViewer.cpp
View File

@@ -76,19 +76,11 @@ CameraViewer::~CameraViewer()
void CameraViewer::showImage(const rtabmap::SensorData & data)
{
processingImages_ = true;
imageView_->setImage(uCvMat2QImage(data.image()));
imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
if(!data.depth().empty() && data.fx() && data.fy())
imageView_->setImage(uCvMat2QImage(data.imageRaw()));
imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
if(!data.depthOrRightRaw().empty() && (data.stereoCameraModel().isValid() || data.cameraModels().size()))
{
cloudView_->addOrUpdateCloud("cloud",
util3d::cloudFromDepthRGB(data.image(), data.depth(), data.cx(), data.cy(), data.fx(), data.fy()),
data.localTransform());
}
else if(!data.rightImage().empty() && data.fx() && data.baseline())
{
cloudView_->addOrUpdateCloud("cloud",
util3d::cloudFromStereoImages(data.image(), data.rightImage(), data.cx(), data.cy(), data.fx(), data.baseline()),
data.localTransform());
cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data));
}
else
{

1
guilib/src/CloudViewer.cpp
View File

@@ -93,6 +93,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
-1, 0, 0,
0, 0, 0,
0, 0, 1);
_visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0);
//setup menu/actions
createMenu();

12
guilib/src/DataRecorder.cpp
View File

@@ -120,7 +120,7 @@ DataRecorder::~DataRecorder()
this->closeRecorder();
}
void DataRecorder::addData(const rtabmap::SensorData & data)
void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance)
{
memoryMutex_.lock();
if(memory_)
@@ -134,10 +134,10 @@ void DataRecorder::addData(const rtabmap::SensorData & data)
//save to database
UTimer time;
memory_->update(data);
memory_->update(data, pose, covariance);
const Signature * s = memory_->getLastWorkingSignature();
totalSizeKB_ += (int)s->getImageCompressed().total()/1000;
totalSizeKB_ += (int)s->getDepthCompressed().total()/1000;
totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
memory_->cleanup();
if(++count_ % 30)
@@ -183,8 +183,8 @@ void DataRecorder::handleEvent(UEvent * event)
{
processingImages_ = true;
QMetaObject::invokeMethod(this, "showImage",
Q_ARG(cv::Mat, camEvent->data().image()),
Q_ARG(cv::Mat, camEvent->data().depthOrRightImage()));
Q_ARG(cv::Mat, camEvent->data().imageRaw()),
Q_ARG(cv::Mat, camEvent->data().depthOrRightRaw()));
}
}
}

562
guilib/src/DatabaseViewer.cpp
View File

@@ -579,23 +579,21 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
memory_->addLink(
refinedIter->second.to(),
memory_->addLink(Link(
refinedIter->second.from(),
refinedIter->second.transform(),
refinedIter->second.to(),
refinedIter->second.type(),
refinedIter->second.rotVariance(),
refinedIter->second.transVariance());
refinedIter->second.transform(),
refinedIter->second.infMatrix()));
}
else
{
memory_->addLink(
iter->second.to(),
memory_->addLink(Link(
iter->second.from(),
iter->second.transform(),
iter->second.to(),
iter->second.type(),
iter->second.rotVariance(),
iter->second.transVariance());
iter->second.transform(),
iter->second.infMatrix()));
}
}
@@ -608,8 +606,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
iter->second.from(),
iter->second.to(),
iter->second.transform(),
iter->second.rotVariance(),
iter->second.transVariance());
iter->second.infMatrix());
}
}
@@ -708,6 +705,7 @@ void DatabaseViewer::exportDatabase()
double previousStamp = 0;
std::vector<double> delays(ids_.size());
int oi=0;
std::map<int, Transform> poses;
for(int i=0; i<ids_.size(); i+=1+framesIgnored)
{
Transform odomPose;
@@ -732,6 +730,8 @@ void DatabaseViewer::exportDatabase()
delays[oi++] = stamp - previousStamp;
}
previousStamp = stamp;
poses.insert(std::make_pair(ids_[i], odomPose));
}
}
if(sessionExported >= 0 && mapId > sessionExported)
@@ -753,31 +753,47 @@ void DatabaseViewer::exportDatabase()
{
int id = ids.at(i);
Signature data = memory_->getSignatureData(id, true);
float rotVariance = 1.0f;
float transVariance = 1.0f;
SensorData data = memory_->getNodeData(id, true);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(dialog.isOdomExported())
{
data.getPoseVariance(rotVariance, transVariance);
if(memory_->getSignature(id) == 0)
{
UERROR("could not find node %d in memory.", id);
}
else
{
covariance = memory_->getSignature(id)->getPoseCovariance();
}
}
rtabmap::SensorData sensorData(
dialog.isDepth2dExported()?data.getLaserScanRaw():cv::Mat(),
dialog.isDepth2dExported()?data.getLaserScanMaxPts():0,
dialog.isRgbExported()?data.getImageRaw():cv::Mat(),
dialog.isDepthExported()?data.getDepthRaw():cv::Mat(),
dialog.isRgbExported() || dialog.isDepthExported()?data.getFx():0,
dialog.isRgbExported() || dialog.isDepthExported()?data.getFy():0,
dialog.isRgbExported() || dialog.isDepthExported()?data.getCx():0,
dialog.isRgbExported() || dialog.isDepthExported()?data.getCy():0,
dialog.isRgbExported() || dialog.isDepthExported()?data.getLocalTransform():Transform::getIdentity(),
dialog.isOdomExported()?data.getPose():Transform(),
rotVariance,
transVariance,
data.id(),
data.getStamp(),
dialog.isUserDataExported()?data.getUserData():std::vector<unsigned char>());
recorder.addData(sensorData);
rtabmap::SensorData sensorData;
if(data.cameraModels().size())
{
sensorData = rtabmap::SensorData(
dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
dialog.isDepth2dExported()?data.laserScanMaxPts():0,
dialog.isRgbExported()?data.imageRaw():cv::Mat(),
dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
data.cameraModels(),
data.id(),
data.stamp(),
dialog.isUserDataExported()?data.userData():std::vector<unsigned char>());
}
else
{
sensorData = rtabmap::SensorData(
dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
dialog.isDepth2dExported()?data.laserScanMaxPts():0,
dialog.isRgbExported()?data.imageRaw():cv::Mat(),
dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
data.stereoCameraModel(),
data.id(),
data.stamp(),
dialog.isUserDataExported()?data.userData():std::vector<unsigned char>());
}
recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
progressDialog->appendText(tr("Exported node %1").arg(id));
progressDialog->incrementStep();
@@ -1064,7 +1080,7 @@ void DatabaseViewer::view3DMap()
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
float maxDepth = (float)QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
@@ -1102,60 +1118,35 @@ void DatabaseViewer::view3DMap()
rtabmap::Transform pose = iter->second;
if(!pose.isNull())
{
Signature data = memory_->getSignatureData(iter->first, true);
SensorData data = memory_->getNodeData(iter->first, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
if(data.getDepthRaw().type() == CV_8UC1)
UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
if(cloud->size())
{
cv::Mat leftImg;
if(data.getImageRaw().channels() == 3)
QColor color = Qt::red;
int mapId, weight;
Transform odomPose;
std::string label;
double stamp;
std::vector<unsigned char> userData;
if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true))
{
cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
else
{
leftImg = data.getImageRaw();
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
data.getImageRaw(),
util2d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
data.getCx(), data.getCy(),
data.getFx(), data.getFy(),
decimation);
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
data.getImageRaw(),
data.getDepthRaw(),
data.getCx(), data.getCy(),
data.getFx(), data.getFy(),
decimation);
UINFO("Empty cloud %d", iter->first);
progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
}
if(maxDepth)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud(cloud, data.getLocalTransform());
QColor color = Qt::red;
int mapId, weight;
Transform odomPose;
std::string label;
double stamp;
std::vector<unsigned char> userData;
if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true))
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
@@ -1188,7 +1179,7 @@ void DatabaseViewer::generate3DMap()
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
float maxDepth = (float)QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
@@ -1212,48 +1203,24 @@ void DatabaseViewer::generate3DMap()
const rtabmap::Transform & pose = iter->second;
if(!pose.isNull())
{
Signature data = memory_->getSignatureData(iter->first, true);
SensorData data = memory_->getNodeData(iter->first, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
if(data.getDepthRaw().type() == CV_8UC1)
UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
if(cloud->size())
{
cv::Mat leftImg;
if(data.getImageRaw().channels() == 3)
{
cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
}
else
{
leftImg = data.getImageRaw();
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
data.getImageRaw(),
util2d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
data.getCx(), data.getCy(),
data.getFx(), data.getFy(),
decimation);
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
data.getImageRaw(),
data.getDepthRaw(),
data.getCx(), data.getCy(),
data.getFx(), data.getFy(),
decimation);
UINFO("Ignored empty cloud %s", name.c_str());
progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
}
if(maxDepth)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud(cloud, pose*data.getLocalTransform());
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
@@ -1500,19 +1467,21 @@ void DatabaseViewer::update(int value,
QImage imgDepth;
if(memory_)
{
Signature data = memory_->getSignatureData(id, true);
if(!data.getImageRaw().empty())
SensorData data = memory_->getNodeData(id, true);
if(!data.imageRaw().empty())
{
img = uCvMat2QImage(data.getImageRaw());
img = uCvMat2QImage(data.imageRaw());
}
if(!data.getDepthRaw().empty())
if(!data.depthOrRightRaw().empty())
{
imgDepth = uCvMat2QImage(data.getDepthRaw());
imgDepth = uCvMat2QImage(data.depthOrRightRaw());
}
if(data.getWords().size())
const Signature * signature = memory_->getSignature(id);
if(signature && signature->getWords().size())
{
view->setFeatures(data.getWords(), data.getDepthRaw().type() == CV_8UC1?cv::Mat():data.getDepthRaw(), Qt::yellow);
view->setFeatures(signature->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
}
Transform odomPose;
@@ -1522,47 +1491,36 @@ void DatabaseViewer::update(int value,
std::vector<unsigned char> d;
memory_->getNodeInfo(id, odomPose, mapId, w, l, s, d, true);
weight->setNum(data.getWeight());
label->setText(data.getLabel().c_str());
weight->setNum(w);
label->setText(l.c_str());
labelPose->setText(QString("%1%2, %3, %4").arg(odomPose.isIdentity()?"* ":"").arg(odomPose.x()).arg(odomPose.y()).arg(odomPose.z()));
if(data.getStamp()!=0.0)
if(s!=0.0)
{
stamp->setText(QDateTime::fromMSecsSinceEpoch(data.getStamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
}
//stereo
if(!data.getDepthRaw().empty() && data.getDepthRaw().type() == CV_8UC1)
if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
{
this->updateStereo(&data);
}
// 3d view
if(view3D->isVisible() && !data.getDepthRaw().empty())
if(view3D->isVisible() && !data.depthOrRightRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(data.getDepthRaw().type() == CV_8UC1)
cloud = util3d::cloudRGBFromSensorData(data);
if(cloud->size())
{
cloud = util3d::cloudFromStereoImages(
data.getImageRaw(),
data.getDepthRaw(),
data.getCx(), data.getCy(),
data.getFx(), data.getFy(),
1);
view3D->addOrUpdateCloud("0", cloud);
}
else
{
cloud = util3d::cloudFromDepthRGB(
data.getImageRaw(),
data.getDepthRaw(),
data.getCx(), data.getCy(),
data.getFx(), data.getFy(),
1);
}
view3D->addOrUpdateCloud("0", cloud, data.getLocalTransform());
//add scan
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.getLaserScanRaw());
view3D->addOrUpdateCloud("1", scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw());
if(scan->size())
{
view3D->addOrUpdateCloud("1", scan);
}
view3D->update();
}
@@ -1688,18 +1646,23 @@ void DatabaseViewer::update(int value,
}
}
void DatabaseViewer::updateStereo(const Signature * data)
void DatabaseViewer::updateStereo(const SensorData * data)
{
if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1)
if(data &&
ui_->dockWidget_stereoView->isVisible() &&
!data->imageRaw().empty() &&
!data->depthOrRightRaw().empty() &&
data->depthOrRightRaw().type() == CV_8UC1 &&
data->stereoCameraModel().isValid())
{
cv::Mat leftMono;
if(data->getImageRaw().channels() == 3)
if(data->imageRaw().channels() == 3)
{
cv::cvtColor(data->getImageRaw(), leftMono, CV_BGR2GRAY);
cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
}
else
{
leftMono = data->getImageRaw();
leftMono = data->imageRaw();
}
UTimer timer;
@@ -1726,7 +1689,7 @@ void DatabaseViewer::updateStereo(const Signature * data)
std::vector<cv::Point2f> rightCorners;
cv::calcOpticalFlowPyrLK(
leftMono,
data->getDepthRaw(),
data->depthOrRightRaw(),
leftCorners,
rightCorners,
status,
@@ -1754,11 +1717,14 @@ void DatabaseViewer::updateStereo(const Signature * data)
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
data->getCx(), data->getCy(), data->getFx(), data->getFy());
data->stereoCameraModel().left().cx(),
data->stereoCameraModel().left().cy(),
data->stereoCameraModel().left().fx(),
data->stereoCameraModel().baseline());
if(pcl::isFinite(tmpPt))
{
pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2)
{
status[i] = 100; //blue
@@ -1809,8 +1775,8 @@ void DatabaseViewer::updateStereo(const Signature * data)
ui_->graphicsView_stereo->setFeaturesShown(false);
ui_->graphicsView_stereo->setImageDepthShown(true);
ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->getImageRaw()));
ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->getDepthRaw()));
ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->depthOrRightRaw()));
// Draw lines between corresponding features...
for(unsigned int i=0; i<kpts.size(); ++i)
@@ -1975,7 +1941,9 @@ void DatabaseViewer::updateConstraintView(
UASSERT(!t.isNull() && memory_);
ui_->label_type->setNum(link.type());
ui_->label_variance->setText(QString("%1, %2").arg(sqrt(link.rotVariance())).arg(sqrt(link.transVariance())));
ui_->label_variance->setText(QString("%1, %2")
.arg(sqrt(link.rotVariance()))
.arg(sqrt(link.transVariance())));
ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
if(link.type() == Link::kNeighbor &&
graphes_.size() &&
@@ -2044,15 +2012,15 @@ void DatabaseViewer::updateConstraintView(
if(ui_->constraintsViewer->isVisible())
{
Signature dataFrom, dataTo;
SensorData dataFrom, dataTo;
dataFrom = memory_->getSignatureData(link.from(), true);
UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
dataFrom = memory_->getNodeData(link.from(), true);
UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
dataTo = memory_->getSignatureData(link.to(), true);
UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
dataTo = memory_->getNodeData(link.to(), true);
UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
if(cloudFrom->size() == 0 && cloudTo->size() == 0)
@@ -2060,51 +2028,9 @@ void DatabaseViewer::updateConstraintView(
//cloud 3d
if(!ui_->checkBox_show3DWords->isChecked())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
if(dataFrom.getDepthRaw().type() == CV_8UC1)
{
cloudFrom = rtabmap::util3d::cloudFromStereoImages(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getCx(), dataFrom.getCy(),
dataFrom.getFx(), dataFrom.getFy(),
1);
}
else
{
cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getCx(), dataFrom.getCy(),
dataFrom.getFx(), dataFrom.getFy(),
1);
}
cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
cloudFrom = rtabmap::util3d::transformPointCloud(cloudFrom, dataFrom.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
if(dataTo.getDepthRaw().type() == CV_8UC1)
{
cloudTo = rtabmap::util3d::cloudFromStereoImages(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getCx(), dataTo.getCy(),
dataTo.getFx(), dataTo.getFy(),
1);
}
else
{
cloudTo = rtabmap::util3d::cloudFromDepthRGB(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getCx(), dataTo.getCy(),
dataTo.getFx(), dataTo.getFy(),
1);
}
cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t*dataTo.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1);
if(cloudFrom->size())
{
@@ -2112,6 +2038,7 @@ void DatabaseViewer::updateConstraintView(
}
if(cloudTo->size())
{
cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
}
}
@@ -2196,8 +2123,8 @@ void DatabaseViewer::updateConstraintView(
{
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
scanB = rtabmap::util3d::transformPointCloud(scanB, t);
if(scanA->size())
{
@@ -2309,40 +2236,17 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
bool added = false;
if(ui_->groupBox_gridFromProjection->isChecked())
{
Signature data = memory_->getSignatureData(ids_.at(i), true);
if(!data.getDepthRaw().empty())
SensorData data = memory_->getNodeData(ids_.at(i), true);
if(!data.depthOrRightRaw().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
if(data.getDepthRaw().type() == CV_8UC1)
{
cloud = rtabmap::util3d::cloudFromDisparity(
util2d::disparityFromStereoImages(data.getImageRaw(), data.getDepthRaw()),
data.getCx(),
data.getCy(),
data.getFx(),
data.getFy(),
ui_->spinBox_projDecimation->value());
}
else
{
cloud = util3d::cloudFromDepth(
data.getDepthRaw(),
data.getCx(),
data.getCy(),
data.getFx(),
data.getFy(),
ui_->spinBox_projDecimation->value());
}
if(cloud->size())
{
cloud = util3d::passThrough(cloud, "z", 0, ui_->doubleSpinBox_projMaxDepth->value());
}
cloud = util3d::cloudFromSensorData(data,
ui_->spinBox_projDecimation->value(),
ui_->doubleSpinBox_projMaxDepth->value(),
ui_->doubleSpinBox_gridCellSize->value());
if(cloud->size())
{
cloud = util3d::voxelize(cloud, ui_->doubleSpinBox_gridCellSize->value());
cloud = util3d::transformPointCloud(cloud, data.getLocalTransform());
UTimer timer;
float cellSize = ui_->doubleSpinBox_gridCellSize->value();
float groundNormalMaxAngle = M_PI_4;
@@ -2364,8 +2268,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
else
{
Signature data = memory_->getSignatureData(ids_.at(i), false);
if(!data.getLaserScanCompressed().empty())
SensorData data = memory_->getNodeData(ids_.at(i), false);
if(!data.laserScanCompressed().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat laserScan;
@@ -2700,9 +2604,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
int correspondences = 0;
Transform transform;
Signature dataFrom, dataTo;
dataFrom = memory_->getSignatureData(currentLink.from(), false);
dataTo = memory_->getSignatureData(currentLink.to(), false);
SensorData dataFrom, dataTo;
dataFrom = memory_->getNodeData(currentLink.from(), false);
dataTo = memory_->getNodeData(currentLink.to(), false);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2712,8 +2616,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
if(ui_->checkBox_icp_2d->isChecked())
{
//2D
cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed());
cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed());
cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.laserScanCompressed());
cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.laserScanCompressed());
if(!oldLaserScan.empty() && !newLaserScan.empty())
{
@@ -2740,9 +2644,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
if(!transform.isNull())
{
if(dataTo.getLaserScanMaxPts())
if(dataTo.laserScanMaxPts())
{
correspondenceRatio = float(correspondences)/float(dataTo.getLaserScanMaxPts());
correspondenceRatio = float(correspondences)/float(dataTo.laserScanMaxPts());
}
else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
{
@@ -2755,112 +2659,60 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
else
{
//3D
cv::Mat depthA = rtabmap::uncompressImage(dataFrom.getDepthCompressed());
cv::Mat depthB = rtabmap::uncompressImage(dataTo.getDepthCompressed());
if(depthA.type() == CV_8UC1)
cv::Mat im,de;
dataFrom.uncompressData(&im, &de, 0);
dataTo.uncompressData(&im, &de, 0);
cloudA = util3d::cloudFromSensorData(dataFrom,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value());
cloudB = util3d::cloudFromSensorData(dataTo,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value());
if(cloudA->size() && cloudB->size())
{
cv::Mat leftMono;
cv::Mat left = rtabmap::uncompressImage(dataFrom.getImageCompressed());
if(left.channels() > 1)
cloudB = util3d::transformPointCloud(cloudB, t);
if(ui_->checkBox_icp_p2plane->isChecked())
{
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
}
else
{
leftMono = left;
transform = util3d::icp(cloudB,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
}
cloudA = util3d::cloudFromDisparity(util2d::disparityFromStereoImages(leftMono, depthA), dataFrom.getCx(), dataFrom.getCy(), dataFrom.getFx(), dataFrom.getFy(), ui_->spinBox_icp_decimation->value());
if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
{
cloudA = util3d::passThrough(cloudA, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
}
if(ui_->doubleSpinBox_icp_voxel->value() > 0)
{
cloudA = util3d::voxelize(cloudA, ui_->doubleSpinBox_icp_voxel->value());
}
cloudA = util3d::transformPointCloud(cloudA, dataFrom.getLocalTransform());
correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total());
}
else
{
cloudA = util3d::getICPReadyCloud(depthA,
dataFrom.getFx(), dataFrom.getFy(), dataFrom.getCx(), dataFrom.getCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
dataFrom.getLocalTransform());
}
if(depthB.type() == CV_8UC1)
{
cv::Mat leftMono;
cv::Mat left = rtabmap::uncompressImage(dataTo.getImageCompressed());
if(left.channels() > 1)
{
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = left;
}
cloudB = util3d::cloudFromDisparity(util2d::disparityFromStereoImages(leftMono, depthB), dataTo.getCx(), dataTo.getCy(), dataTo.getFx(), dataTo.getFy(), ui_->spinBox_icp_decimation->value());
if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
{
cloudB = util3d::passThrough(cloudB, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
}
if(ui_->doubleSpinBox_icp_voxel->value() > 0)
{
cloudB = util3d::voxelize(cloudB, ui_->doubleSpinBox_icp_voxel->value());
}
cloudB = util3d::transformPointCloud(cloudB, t * dataTo.getLocalTransform());
}
else
{
cloudB = util3d::getICPReadyCloud(depthB,
dataTo.getFx(), dataTo.getFy(), dataTo.getCx(), dataTo.getCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
t * dataTo.getLocalTransform());
}
if(ui_->checkBox_icp_p2plane->isChecked())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
}
else
{
transform = util3d::icp(cloudB,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
correspondenceRatio = float(correspondences)/float(depthB.total());
UWARN("No cloud generated!");
}
}
@@ -2963,8 +2815,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
Memory tmpMemory(parameters);
// Add signatures
SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
SensorData dataFrom = memory_->getNodeData(from, true);
SensorData dataTo = memory_->getNodeData(to, true);
if(from > to)
{
@@ -3081,8 +2933,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
Memory tmpMemory(parameters);
// Add signatures
SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
SensorData dataFrom = memory_->getNodeData(from, true);
SensorData dataTo = memory_->getNodeData(to, true);
if(from > to)
{
@@ -3100,8 +2952,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
if(!silent)
{
ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depth());
ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depth());
ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depthRaw());
ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depthRaw());
updateWordsMatching();
}
}

71
guilib/src/LoopClosureViewer.cpp
View File

@@ -106,74 +106,14 @@ void LoopClosureViewer::updateView(const Transform & transform)
ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
if(!t.isNull())
{
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(sA_.getDepthRaw().type() == CV_8UC1)
{
cloudA = util3d::cloudFromStereoImages(
sA_.getImageRaw(),
sA_.getDepthRaw(),
sA_.getCx(), sA_.getCy(),
sA_.getFx(), sA_.getFy(),
decimation);
}
else
{
cloudA = util3d::cloudFromDepthRGB(
sA_.getImageRaw(),
sA_.getDepthRaw(),
sA_.getCx(), sA_.getCy(),
sA_.getFx(), sA_.getFy(),
decimation);
}
cloudA = util3d::removeNaNFromPointCloud(cloudA);
if(maxDepth>0.0)
{
cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudA->size() > samples)
{
cloudA = util3d::sampling(cloudA, samples);
}
cloudA = util3d::transformPointCloud(cloudA, sA_.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(sB_.getDepthRaw().type() == CV_8UC1)
{
cloudB = util3d::cloudFromStereoImages(
sB_.getImageRaw(),
sB_.getDepthRaw(),
sB_.getCx(), sB_.getCy(),
sB_.getFx(), sB_.getFy(),
decimation);
}
else
{
cloudB = util3d::cloudFromDepthRGB(
sB_.getImageRaw(),
sB_.getDepthRaw(),
sB_.getCx(), sB_.getCy(),
sB_.getFx(), sB_.getFy(),
decimation);
}
cloudB = util3d::removeNaNFromPointCloud(cloudB);
if(maxDepth>0.0)
{
cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudB->size() > samples)
{
cloudB = util3d::sampling(cloudB, samples);
}
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, 0.0f, samples);
cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, 0.0f, samples);
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw());
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw());
scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw());
scanB = util3d::transformPointCloud(scanB, t);
@@ -184,6 +124,7 @@ void LoopClosureViewer::updateView(const Transform & transform)
ui_->cloudViewerTransform->addOrUpdateCloud("cloud0", cloudA);
}
if(cloudB->size())
{
cloudB = util3d::transformPointCloud(cloudB, t);
ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB);
}

492
guilib/src/MainWindow.cpp
View File

@@ -437,9 +437,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent)), this, SLOT(processOdometry(rtabmap::OdometryEvent)));
connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
@@ -671,7 +670,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
if(!_processingOdometry && !_processingStatistics)
{
_processingOdometry = true; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->info());
emit odometryReceived(*odomEvent);
}
}
}
@@ -695,11 +694,11 @@ void MainWindow::handleEvent(UEvent* anEvent)
}
}
void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
{
_processingOdometry = true;
UTimer time;
Transform pose = data.pose();
Transform pose = odom.pose();
bool lost = false;
bool lostStateChanged = false;
@@ -713,11 +712,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
pose = _lastOdomPose;
lost = true;
}
else if(info.inliers>0 &&
else if(odom.info().inliers>0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
info.inliers < _preferencesDialog->getOdomQualityWarnThr())
odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -730,44 +729,44 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->imageView_odometry->setBackgroundColor(Qt::black);
}
if(info.inliers >= 0)
if(odom.info().inliers >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
}
if(info.matches >= 0)
if(odom.info().matches >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches);
_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
}
if(info.variance >= 0)
if(odom.info().variance >= 0)
{
_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance));
_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance));
}
if(info.variance >= 0)
if(odom.info().variance >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
_ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance);
}
if(info.time > 0)
if(odom.info().time > 0)
{
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f);
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)odom.data().id(), (float)odom.info().time*1000.0f);
}
if(info.features >=0)
if(odom.info().features >=0)
{
_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features);
_ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features);
}
if(info.localMapSize >=0)
if(odom.info().localMapSize >=0)
{
_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)odom.data().id(), (float)odom.info().localMapSize);
}
_ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
_ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id());
float x,y,z, roll,pitch,yaw;
pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x);
_ui->statsToolBox->updateStat("Odometry/T_y/m", (float)data.id(), y);
_ui->statsToolBox->updateStat("Odometry/T_z/m", (float)data.id(), z);
_ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)data.id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_x/m", (float)odom.data().id(), x);
_ui->statsToolBox->updateStat("Odometry/T_y/m", (float)odom.data().id(), y);
_ui->statsToolBox->updateStat("Odometry/T_z/m", (float)odom.data().id(), z);
_ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
{
@@ -780,42 +779,42 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
if(!pose.isNull())
{
// 3d cloud
if(data.depthOrRightImage().cols == data.image().cols &&
data.depthOrRightImage().rows == data.image().rows &&
!data.depthOrRightImage().empty() &&
data.fx() > 0.0f &&
data.fyOrBaseline() > 0.0f &&
if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
!odom.data().depthOrRightRaw().empty() &&
(odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValid()) &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(0,
data.image(),
data.depthOrRightImage(),
data.fx(),
data.fyOrBaseline(),
data.cx(),
data.cy(),
data.localTransform(),
pose,
_preferencesDialog->getCloudVoxelSize(1),
cloud = util3d::cloudRGBFromSensorData(odom.data(),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1));
if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
_preferencesDialog->getCloudMaxDepth(1),
_preferencesDialog->getCloudVoxelSize(1));
if(cloud->size())
{
UERROR("Adding cloudOdom to viewer failed!");
cloud = util3d::transformPointCloud(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}
else
{
UWARN("Empty cloudOdom!");
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false);
}
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}
// 2d cloud
if(!data.laserScan().empty() &&
if(!odom.data().laserScanRaw().empty() &&
_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(data.laserScan());
cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
cloud = util3d::transformPointCloud(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
@@ -826,10 +825,10 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
}
if(!data.pose().isNull())
if(!odom.pose().isNull())
{
// update camera position
_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
}
}
_ui->widget_cloudViewer->update();
@@ -837,27 +836,33 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
if(_ui->graphicsView_graphView->isVisible())
{
if(!pose.isNull() && !data.pose().isNull())
if(!pose.isNull() && !odom.pose().isNull())
{
_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
_ui->graphicsView_graphView->update();
}
}
if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty())
!odom.data().imageRaw().empty())
{
if(_ui->imageView_odometry->isFeaturesShown())
{
if(info.type == 0)
if(odom.info().type == 0)
{
_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
_ui->imageView_odometry->setFeatures(
odom.info().words,
odom.data().depthRaw(),
Qt::yellow);
}
else if(info.type == 1)
else if(odom.info().type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(info.refCorners, kpts);
_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
cv::KeyPoint::convert(odom.info().refCorners, kpts);
_ui->imageView_odometry->setFeatures(
kpts,
odom.data().depthRaw(),
Qt::red);
}
}
@@ -870,7 +875,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_odomImageShow = _ui->imageView_odometry->isImageShown();
_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
}
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(true);
}
@@ -883,54 +888,54 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
}
_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
if(_ui->imageView_odometry->isImageDepthShown())
{
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
}
if(info.type == 0)
if(odom.info().type == 0)
{
if(_ui->imageView_odometry->isFeaturesShown())
{
for(unsigned int i=0; i<info.wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<info.wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
}
}
}
}
if(info.type == 1 && info.cornerInliers.size())
if(odom.info().type == 1 && odom.info().cornerInliers.size())
{
if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
{
if(_ui->imageView_odometry->isFeaturesShown())
{
_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
_ui->imageView_odometry->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
}
if(_ui->imageView_odometry->isLinesShown())
{
_ui->imageView_odometry->addLine(
info.refCorners[info.cornerInliers[i]].x,
info.refCorners[info.cornerInliers[i]].y,
info.newCorners[info.cornerInliers[i]].x,
info.newCorners[info.cornerInliers[i]].y,
odom.info().refCorners[odom.info().cornerInliers[i]].x,
odom.info().refCorners[odom.info().cornerInliers[i]].y,
odom.info().newCorners[odom.info().cornerInliers[i]].x,
odom.info().newCorners[odom.info().cornerInliers[i]].y,
Qt::blue);
}
}
}
}
if(!data.image().empty())
if(!odom.data().imageRaw().empty())
{
_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
}
_ui->imageView_odometry->update();
@@ -941,7 +946,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
this->captureScreen();
}
_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)odom.data().id(), time.elapsed()*1000.0);
_processingOdometry = false;
}
@@ -981,7 +986,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// update cache
Signature signature = stat.getSignature();
signature.uncompressData(); // make sure data are uncompressed
signature.sensorData().uncompressData(); // make sure data are uncompressed
_cachedSignatures.insert(stat.getSignature().id(), signature);
int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -1055,7 +1060,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
if(iter != _cachedSignatures.end())
{
iter.value().uncompressData();
iter.value().sensorData().uncompressData();
loopSignature = iter.value();
}
}
@@ -1065,10 +1070,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
//update image views
{
UCvMat2QImageThread qimageThread(signature.getImageRaw());
UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw());
UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw());
UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
qimageThread.start();
qdepthThread.start();
qimageLoopThread.start();
@@ -1170,7 +1175,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// loop closure view
if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) &&
!stat.loopClosureTransform().isNull() &&
!loopSignature.getImageRaw().empty())
!loopSignature.sensorData().imageRaw().empty())
{
// the last loop closure data
Transform loopClosureTransform = stat.loopClosureTransform();
@@ -1247,7 +1252,7 @@ void MainWindow::updateMapCloud(
{
if(!_ui->actionSave_point_cloud->isEnabled() &&
_cachedSignatures.size() &&
(!(--_cachedSignatures.end())->getDepthCompressed().empty() ||
(!(--_cachedSignatures.end())->sensorData().depthOrRightCompressed().empty() ||
!(--_cachedSignatures.end())->getWords3().empty()))
{
//enable save cloud action
@@ -1257,7 +1262,7 @@ void MainWindow::updateMapCloud(
if(!_ui->actionView_scans->isEnabled() &&
_cachedSignatures.size() &&
!(--_cachedSignatures.end())->getLaserScanCompressed().empty())
!(--_cachedSignatures.end())->sensorData().laserScanCompressed().empty())
{
_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1344,7 +1349,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if((!jter->getImageCompressed().empty() && !jter->getDepthCompressed().empty()) || jter->getWords3().size())
if((!jter->sensorData().imageCompressed().empty() && !jter->sensorData().depthOrRightCompressed().empty()) || jter->getWords3().size())
{
this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
}
@@ -1380,7 +1385,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(!jter->getLaserScanCompressed().empty())
if(!jter->sensorData().laserScanCompressed().empty())
{
this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
}
@@ -1558,25 +1563,19 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
return;
}
if(!iter->getImageCompressed().empty() && !iter->getDepthCompressed().empty())
if(!iter->sensorData().imageCompressed().empty() && !iter->sensorData().depthOrRightCompressed().empty())
{
cv::Mat image, depth;
iter->uncompressData(&image, &depth, 0);
SensorData data = iter->sensorData();
data.uncompressData(&image, &depth, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(nodeId,
image,
depth,
iter->getFx(),
iter->getFy(),
iter->getCx(),
iter->getCy(),
iter->getLocalTransform(),
Transform::getIdentity(),
_preferencesDialog->getCloudVoxelSize(0),
UASSERT(nodeId == data.id());
cloud = util3d::cloudRGBFromSensorData(data,
_preferencesDialog->getCloudDecimation(0),
_preferencesDialog->getCloudMaxDepth(0));
_preferencesDialog->getCloudMaxDepth(0),
_preferencesDialog->getCloudVoxelSize(0));
if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
{
@@ -1711,10 +1710,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
return;
}
if(!iter->getLaserScanCompressed().empty())
if(!iter->sensorData().laserScanCompressed().empty())
{
cv::Mat depth2D;
iter->uncompressData(0, 0, &depth2D);
iter->sensorData().uncompressData(0, 0, &depth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(depth2D);
@@ -1930,10 +1929,12 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
QApplication::processEvents();
int addedSignatures = 0;
std::map<int, int> mapIds;
for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
iter!=event.getSignatures().end();
++iter)
{
mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
if(!_cachedSignatures.contains(iter->first))
{
_cachedSignatures.insert(iter->first, iter->second);
@@ -1953,7 +1954,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->appendText("Updating the 3D map cloud...");
_initProgressDialog->incrementStep();
QApplication::processEvents();
this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), event.getMapIds(), true);
this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), mapIds, true);
_initProgressDialog->appendText("Updating the 3D map cloud... done.");
}
else
@@ -3167,15 +3168,15 @@ void MainWindow::postProcessing()
{
odomPoses.insert(*iter); // fill raw poses
}
if(jter->getLocalTransform().isNull())
if(jter->sensorData().cameraModels().size() == 0 && !jter->sensorData().stereoCameraModel().isValid())
{
UWARN("Local transform of %d is null.", iter->first);
UWARN("Calibration of %d is null.", iter->first);
allDataAvailable = false;
}
if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
{
// depth data required
if(jter->getDepthCompressed().empty() || jter->getFx() <= 0.0f || jter->getFy() <= 0.0f)
if(jter->sensorData().depthOrRightCompressed().empty())
{
UWARN("Depth data of %d missing.", iter->first);
allDataAvailable = false;
@@ -3184,7 +3185,7 @@ void MainWindow::postProcessing()
if(reextractFeatures)
{
// rgb required
if(jter->getImageCompressed().empty())
if(jter->sensorData().imageCompressed().empty())
{
UWARN("Rgb of %d missing.", iter->first);
allDataAvailable = false;
@@ -3233,6 +3234,7 @@ void MainWindow::postProcessing()
int loopClosuresAdded = 0;
if(detectMoreLoopClosures)
{
UDEBUG("");
Memory memory(parameters);
if(reextractFeatures)
{
@@ -3305,13 +3307,15 @@ void MainWindow::postProcessing()
memory.init("", true); // clear previously added signatures
// Add signatures
SensorData dataFrom = signatureFrom.toSensorData();
SensorData dataTo = signatureTo.toSensorData();
SensorData dataFrom = signatureFrom.sensorData();
SensorData dataTo = signatureTo.sensorData();
cv::Mat image, depth;
dataFrom.uncompressData(&image, &depth, 0);
dataTo.uncompressData(&image, &depth, 0);
if(dataFrom.isValid() &&
dataFrom.isMetric() &&
dataTo.isValid() &&
dataTo.isMetric() &&
dataFrom.id() != Memory::kIdInvalid &&
signatureFrom.id() != Memory::kIdInvalid)
{
@@ -3381,6 +3385,7 @@ void MainWindow::postProcessing()
if(refineNeighborLinks || refineLoopClosureLinks)
{
UDEBUG("");
if(refineLoopClosureLinks)
{
_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
@@ -3435,83 +3440,96 @@ void MainWindow::postProcessing()
Signature & signatureTo = _cachedSignatures[to];
//3D
UDEBUG("");
cv::Mat depthA, depthB;
signatureFrom.uncompressData(0, &depthA, 0);
signatureTo.uncompressData(0, &depthB, 0);
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
if(signatureFrom.sensorData().stereoCameraModel().isValid())
{
QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
UERROR("ICP 3D cannot be done on stereo images! Aborting refining links with ICP...");
break;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
signatureFrom.getFx(), signatureFrom.getFy(), signatureFrom.getCx(), signatureFrom.getCy(),
decimation,
maxDepth,
voxelSize,
samples,
signatureFrom.getLocalTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
signatureTo.getFx(), signatureTo.getFy(), signatureTo.getCx(), signatureTo.getCy(),
decimation,
maxDepth,
voxelSize,
samples,
iter->second.transform() * signatureTo.getLocalTransform());
bool hasConverged = false;
double variance = -1;
int correspondences = 0;
Transform transform;
if(pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
maxCorrespondences,
icpIterations,
&hasConverged,
&variance,
&correspondences);
cv::Mat leftA, leftB;
signatureFrom.sensorData().uncompressData(&leftA, &depthA, 0);
signatureTo.sensorData().uncompressData(&leftB, &depthB, 0);
}
else
{
transform = util3d::icp(cloudB,
cloudA,
maxCorrespondences,
icpIterations,
&hasConverged,
&variance,
&correspondences);
signatureFrom.sensorData().uncompressData(0, &depthA, 0);
signatureTo.sensorData().uncompressData(0, &depthB, 0);
}
float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= correspondenceRatio)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::cloudFromSensorData(
signatureFrom.sensorData(),
decimation,
maxDepth,
voxelSize,
samples);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::cloudFromSensorData(
signatureTo.sensorData(),
decimation,
maxDepth,
voxelSize,
samples);
if(cloudA->size() && cloudB->size())
{
Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
iter->second = newLink;
cloudB = util3d::transformPointCloud(cloudB, iter->second.transform());
bool hasConverged = false;
double variance = -1;
int correspondences = 0;
Transform transform;
if(pointToPlane)
{
UDEBUG("");
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
maxCorrespondences,
icpIterations,
&hasConverged,
&variance,
&correspondences);
}
else
{
UDEBUG("");
transform = util3d::icp(cloudB,
cloudA,
maxCorrespondences,
icpIterations,
&hasConverged,
&variance,
&correspondences);
}
float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= correspondenceRatio)
{
Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
iter->second = newLink;
}
else
{
QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
_initProgressDialog->appendText(str, Qt::darkYellow);
UWARN("%s", str.toStdString().c_str());
}
}
else
{
QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
QString str = tr("Cannot refine link %1->%2 (clouds empty!)").arg(from).arg(to);
_initProgressDialog->appendText(str, Qt::darkYellow);
UWARN("%s", str.toStdString().c_str());
}
@@ -4824,70 +4842,6 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
int id,
const cv::Mat & rgb,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
const Transform & pose,
float voxelSize,
int decimation,
float maxDepth) const
{
UTimer timer;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
}
else
{
cloud = util3d::cloudFromDepthRGB(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
}
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, pose * localTransform);
}
}
UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks());
return cloud;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
const std::map<int, Transform> & poses,
float assembledVoxelSize,
@@ -4910,23 +4864,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
if(_cachedSignatures.contains(iter->first))
{
const Signature & s = _cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
cv::Mat image, depth;
s.uncompressDataConst(&image, &depth, 0);
d.uncompressData(&image, &depth, 0);
if(!image.empty() && !depth.empty())
{
cloud = createCloud(iter->first,
image,
depth,
s.getFx(),
s.getFy(),
s.getCx(),
s.getCy(),
s.getLocalTransform(),
iter->second,
regenerateVoxelSize,
UASSERT(iter->first == d.id());
cloud = util3d::cloudRGBFromSensorData(
d,
regenerateDecimation,
regenerateMaxDepth);
regenerateMaxDepth,
regenerateVoxelSize);
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, iter->second);
}
}
else if(s.getWords3().size())
{
@@ -5014,22 +4967,17 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
if(_cachedSignatures.contains(iter->first))
{
const Signature & s = _cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
cv::Mat image, depth;
s.uncompressDataConst(&image, &depth, 0);
d.uncompressData(&image, &depth, 0);
if(!image.empty() && !depth.empty())
{
cloud = createCloud(iter->first,
image,
depth,
s.getFx(),
s.getFy(),
s.getCx(),
s.getCy(),
s.getLocalTransform(),
Transform::getIdentity(),
regenerateVoxelSize,
UASSERT(iter->first == d.id());
cloud = util3d::cloudRGBFromSensorData(
d,
regenerateDecimation,
regenerateMaxDepth);
regenerateMaxDepth,
regenerateVoxelSize);
}
else if(s.getWords3().size())
{

130
guilib/src/OdometryViewer.cpp
View File

@@ -61,8 +61,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
validDecimationValue_(1)
{
qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
imageView_->setImageDepthShown(false);
imageView_->setMinimumSize(320, 240);
@@ -136,15 +135,15 @@ void OdometryViewer::clear()
cloudView_->clear();
}
void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
processingData_ = true;
int quality = info.inliers;
int quality = odom.info().inliers;
bool lost = false;
bool lostStateChanged = false;
if(data.pose().isNull())
if(odom.pose().isNull())
{
UDEBUG("odom lost"); // use last pose
lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
@@ -153,11 +152,11 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
lost = true;
}
else if(info.inliers>0 &&
else if(odom.info().inliers>0 &&
qualityWarningThr_ &&
info.inliers < qualityWarningThr_)
odom.info().inliers < qualityWarningThr_)
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, qualityWarningThr_);
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkYellow);
cloudView_->setBackgroundColor(Qt::darkYellow);
@@ -170,14 +169,16 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
cloudView_->setBackgroundColor(Qt::black);
}
timeLabel_->setText(QString("%1 s").arg(info.time));
timeLabel_->setText(QString("%1 s").arg(odom.info().time));
if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f)
if(!odom.data().imageRaw().empty() &&
!odom.data().depthOrRightRaw().empty() &&
(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
{
UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
if(data.image().cols % decimationSpin_->value() == 0 &&
data.image().rows % decimationSpin_->value() == 0)
if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
odom.data().imageRaw().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
@@ -186,8 +187,8 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
data.image().cols,
data.image().rows,
odom.data().imageRaw().cols,
odom.data().imageRaw().rows,
validDecimationValue_);
}
@@ -195,35 +196,15 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(!data.depth().empty())
{
cloud = util3d::cloudFromDepthRGB(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
validDecimationValue_);
}
else if(!data.rightImage().empty())
{
cloud = util3d::cloudFromStereoImages(
data.image(),
data.rightImage(),
data.cx(), data.cy(),
data.fx(), data.baseline(),
validDecimationValue_);
}
if(voxelSpin_->value() > 0.0f && cloud->size())
{
cloud = util3d::voxelize(cloud, voxelSpin_->value());
}
cloud = util3d::cloudRGBFromSensorData(
odom.data(),
validDecimationValue_,
0.0f,
voxelSpin_->value());
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, data.localTransform());
if(!data.pose().isNull())
if(!odom.pose().isNull())
{
if(cloudView_->getAddedClouds().contains("cloudtmp"))
{
@@ -236,10 +217,10 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
addedClouds_.pop_front();
}
data.id()?id_=data.id():++id_;
odom.data().id()?id_=odom.data().id():++id_;
std::string cloudName = uFormat("cloud%d", id_);
addedClouds_.push_back(cloudName);
UASSERT(cloudView_->addCloud(cloudName, cloud, data.pose()));
UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
}
else
{
@@ -248,18 +229,18 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
}
}
if(!data.pose().isNull())
if(!odom.pose().isNull())
{
lastOdomPose_ = data.pose();
cloudView_->updateCameraTargetPosition(data.pose());
lastOdomPose_ = odom.pose();
cloudView_->updateCameraTargetPosition(odom.pose());
}
if(info.localMap.size())
if(odom.info().localMap.size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(info.localMap.size());
cloud->resize(odom.info().localMap.size());
int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter)
for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
{
(*cloud)[i].x = iter->second.x;
(*cloud)[i].y = iter->second.y;
@@ -268,17 +249,17 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
cloudView_->addOrUpdateCloud("localmap", cloud);
}
if(!data.image().empty())
if(!odom.data().imageRaw().empty())
{
if(info.type == 0)
if(odom.info().type == 0)
{
imageView_->setFeatures(info.words, data.depth(), Qt::yellow);
imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
}
else if(info.type == 1)
else if(odom.info().type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(info.refCorners, kpts);
imageView_->setFeatures(kpts, data.depth(), Qt::red);
cv::KeyPoint::convert(odom.info().refCorners, kpts);
imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
}
imageView_->clearLines();
@@ -290,7 +271,7 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
odomImageShow_ = imageView_->isImageShown();
odomImageDepthShow_ = imageView_->isImageDepthShown();
}
imageView_->setImageDepth(uCvMat2QImage(data.image()));
imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
imageView_->setImageShown(true);
imageView_->setImageDepthShown(true);
}
@@ -303,55 +284,55 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
imageView_->setImageDepthShown(odomImageDepthShow_);
}
imageView_->setImage(uCvMat2QImage(data.image()));
imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
if(imageView_->isImageDepthShown())
{
imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
}
if(info.type == 0)
if(odom.info().type == 0)
{
if(imageView_->isFeaturesShown())
{
for(unsigned int i=0; i<info.wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
{
imageView_->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<info.wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
{
imageView_->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
}
}
}
}
if(info.type == 1 && info.cornerInliers.size())
if(odom.info().type == 1 && odom.info().cornerInliers.size())
{
if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
{
if(imageView_->isFeaturesShown())
{
imageView_->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
}
if(imageView_->isLinesShown())
{
imageView_->addLine(
info.refCorners[info.cornerInliers[i]].x,
info.refCorners[info.cornerInliers[i]].y,
info.newCorners[info.cornerInliers[i]].x,
info.newCorners[info.cornerInliers[i]].y,
odom.info().refCorners[odom.info().cornerInliers[i]].x,
odom.info().refCorners[odom.info().cornerInliers[i]].y,
odom.info().newCorners[odom.info().cornerInliers[i]].x,
odom.info().newCorners[odom.info().cornerInliers[i]].y,
Qt::blue);
}
}
}
}
if(!data.image().empty())
if(!odom.data().imageRaw().empty())
{
imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
}
}
@@ -372,8 +353,7 @@ void OdometryViewer::handleEvent(UEvent * event)
{
processingData_ = true;
QMetaObject::invokeMethod(this, "processData",
Q_ARG(rtabmap::SensorData, odomEvent->data()),
Q_ARG(rtabmap::OdometryInfo, odomEvent->info()));
Q_ARG(rtabmap::OdometryEvent, *odomEvent));
}
}
}

4
guilib/src/PdfPlot.cpp
View File

@@ -70,10 +70,10 @@ void PdfPlotItem::showDescription(bool shown)
{
QImage img;
QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
if(iter != _signaturesRef->constEnd() && !iter.value().getImageCompressed().empty())
if(iter != _signaturesRef->constEnd() && !iter.value().sensorData().imageCompressed().empty())
{
cv::Mat image;
iter.value().uncompressDataConst(&image, 0, 0);
iter.value().sensorData().uncompressDataConst(&image, 0, 0);
if(!image.empty())
{
img = uCvMat2QImage(image);