42 #include <QPushButton> 44 #include <QDoubleSpinBox> 47 #include <QHBoxLayout> 48 #include <QVBoxLayout> 49 #include <QApplication> 58 int qualityWarningThr,
64 processingData_(
false),
66 odomImageDepthShow_(
true),
68 qualityWarningThr_(qualityWarningThr),
70 validDecimationValue_(1),
71 parameters_(parameters)
74 qRegisterMetaType<rtabmap::OdometryEvent>(
"rtabmap::OdometryEvent");
84 QLabel * maxCloudsLabel =
new QLabel(
"Max clouds",
this);
85 QLabel * voxelLabel =
new QLabel(
"Voxel",
this);
86 QLabel * maxDepthLabel =
new QLabel(
"Max depth",
this);
87 QLabel * decimationLabel =
new QLabel(
"Decimation",
this);
120 QPushButton * resetButton =
new QPushButton(
"reset",
this);
121 QPushButton * clearButton =
new QPushButton(
"clear",
this);
122 QPushButton * closeButton =
new QPushButton(
"close",
this);
123 connect(resetButton, SIGNAL(clicked()),
this, SLOT(
reset()));
124 connect(clearButton, SIGNAL(clicked()),
this, SLOT(
clear()));
125 connect(closeButton, SIGNAL(clicked()),
this, SLOT(reject()));
128 QHBoxLayout * layout =
new QHBoxLayout();
129 layout->setMargin(0);
130 layout->setSpacing(0);
134 QHBoxLayout * hlayout2 =
new QHBoxLayout();
135 hlayout2->setMargin(0);
136 hlayout2->addWidget(maxCloudsLabel);
138 hlayout2->addWidget(voxelLabel);
140 hlayout2->addWidget(maxDepthLabel);
142 hlayout2->addWidget(decimationLabel);
148 hlayout2->addStretch(1);
149 hlayout2->addWidget(resetButton);
150 hlayout2->addWidget(clearButton);
151 hlayout2->addWidget(closeButton);
153 QVBoxLayout * vlayout =
new QVBoxLayout(
this);
154 vlayout->setMargin(0);
155 vlayout->setSpacing(0);
156 vlayout->addLayout(layout, 1);
157 vlayout->addLayout(hlayout2);
159 this->setLayout(vlayout);
186 bool lostStateChanged =
false;
232 UWARN(
"Decimation (%d) must be a denominator of the width and height of " 233 "the image (%d/%d). Using last valid decimation value (%d).",
247 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
248 pcl::IndicesPtr validIndices(
new std::vector<int>);
278 std::string cloudName =
uFormat(
"cloud%d",
id_);
317 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
321 UERROR(
"Adding scanMapOdom to viewer failed!");
334 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
339 UERROR(
"Adding scanOdom to viewer failed!");
359 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
362 for(std::map<int, cv::Point3f>::const_iterator iter=odom.
info().
localMap.begin(); iter!=odom.
info().
localMap.end(); ++iter)
367 (*cloud)[i].x = iter->second.x;
368 (*cloud)[i].y = iter->second.y;
369 (*cloud)[i].z = iter->second.z;
373 (*cloud)[i].r = inlier?0:255;
382 UERROR(
"Adding featuresOdom to viewer failed!");
407 std::vector<cv::KeyPoint> kpts;
492 QApplication::processEvents();
506 QMetaObject::invokeMethod(
this,
"processData",
void addLine(float x1, float y1, float x2, float y2, QColor color, const QString &text=QString())
bool isImageDepthShown() const
void setFrustumShown(bool shown)
std::vector< cv::Point2f > refCorners
bool isLinesShown() const
QDoubleSpinBox * maxDepthSpin_
QCheckBox * featuresShown_
void updateCameraFrustum(const Transform &pose, const StereoCameraModel &model)
void setGridShown(bool shown)
void setCloudVisibility(const std::string &id, bool isVisible)
bool isImageShown() const
void setFeatureColor(int id, QColor color)
std::map< int, cv::Point3f > localMap
bool removeCloud(const std::string &id)
std::map< std::string, std::string > ParametersMap
const QColor & getDefaultBackgroundColor() const
std::vector< int > inliersIDs
Some conversion functions.
void setImage(const QImage &image)
const LaserScan & laserScanRaw() const
const cv::Mat & imageRaw() const
void post(UEvent *event, bool async=true) const
virtual bool handleEvent(UEvent *event)
void processData(const rtabmap::OdometryEvent &odom)
#define UASSERT(condition)
const Transform & pose() const
bool isValidForProjection() const
QSpinBox * maxCloudsSpin_
void setSceneRect(const QRectF &rect)
void setImageShown(bool shown)
void setBackgroundColor(const QColor &color)
std::vector< int > cornerInliers
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack)
virtual std::string getClassName() const =0
void setImageDepth(const cv::Mat &imageDepth)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
void setCloudPointSize(const std::string &id, int size)
void updateCameraTargetPosition(const Transform &pose)
Transform localTransform() const
const cv::Mat & depthOrRightRaw() const
const OdometryInfo & info() const
QDoubleSpinBox * voxelSpin_
const QMap< std::string, Transform > & getAddedClouds() const
void setBackgroundColor(const QColor &color)
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
OdometryViewer(int maxClouds=10, int decimation=2, float voxelSize=0.0f, float maxDepth=0, int qualityWarningThr=0, QWidget *parent=0, const ParametersMap ¶meters=ParametersMap())
const std::vector< CameraModel > & cameraModels() const
const QColor & getBackgroundColor() const
void setCameraTargetLocked(bool enabled=true)
std::multimap< int, cv::KeyPoint > words
virtual ~OdometryViewer()
T uNormSquared(const std::vector< T > &v)
ParametersMap parameters_
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
QSpinBox * decimationSpin_
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
QList< std::string > addedClouds_
std::vector< int > matchesIDs
std::vector< cv::Point2f > newCorners
bool isFeaturesShown() const
void unregisterFromEventsManager()
int validDecimationValue_
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
const Transform & localTransform() const
std::string UTILITE_EXP uFormat(const char *fmt,...)
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
void setCloudOpacity(const std::string &id, double opacity=1.0)
void setImageDepthShown(bool shown)