void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
processingData_ = true;
int quality = odom.info().inliers;
bool lost = false;
bool lostStateChanged = false;
if(odom.pose().isNull())
{
UDEBUG("odom lost"); // use last pose
lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkRed);
cloudView_->setBackgroundColor(Qt::darkRed);
lost = true;
}
else if(odom.info().inliers>0 &&
qualityWarningThr_ &&
odom.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);
}
else
{
UDEBUG("odom ok");
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
cloudView_->setBackgroundColor(Qt::black);
}
timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
if(!odom.data().imageRaw().empty() &&
!odom.data().depthOrRightRaw().empty() &&
(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
{
UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
if(!odom.data().depthRaw().empty())
{
if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
odom.data().imageRaw().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
else
{
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(),
odom.data().imageRaw().cols,
odom.data().imageRaw().rows,
validDecimationValue_);
}
}
else
{
validDecimationValue_ = decimationSpin_->value();
}
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudRGBFromSensorData(
odom.data(),
validDecimationValue_,
0.0f,
voxelSpin_->value());
if(cloud->size())
{
if(!odom.pose().isNull())
{
if(cloudView_->getAddedClouds().contains("cloudtmp"))
{
cloudView_->removeCloud("cloudtmp");
}
while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
{
UASSERT(cloudView_->removeCloud(addedClouds_.first()));
addedClouds_.pop_front();
}
odom.data().id()?id_=odom.data().id():++id_;
std::string cloudName = uFormat("cloud%d", id_);
addedClouds_.push_back(cloudName);
UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
}
else
{
cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
}
}
}
if(!odom.pose().isNull())
{
lastOdomPose_ = odom.pose();
cloudView_->updateCameraTargetPosition(odom.pose());
}
if(odom.info().localMap.size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(odom.info().localMap.size());
int i=0;
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;
(*cloud)[i++].z = iter->second.z;
}
cloudView_->addOrUpdateCloud("localmap", cloud);
}
if(!odom.data().imageRaw().empty())
{
if(odom.info().type == 0)
{
imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
}
else if(odom.info().type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(odom.info().refCorners, kpts);
imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
}
imageView_->clearLines();
if(lost)
{
if(lostStateChanged)
{
// save state
odomImageShow_ = imageView_->isImageShown();
odomImageDepthShow_ = imageView_->isImageDepthShown();
}
imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
imageView_->setImageShown(true);
imageView_->setImageDepthShown(true);
}
else
{
if(lostStateChanged)
{
// restore state
imageView_->setImageShown(odomImageShow_);
imageView_->setImageDepthShown(odomImageDepthShow_);
}
imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
if(imageView_->isImageDepthShown())
{
imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
}
if(odom.info().type == 0)
{
if(imageView_->isFeaturesShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
}
}
}
}
if(odom.info().type == 1 && odom.info().cornerInliers.size())
{
if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
{
//draw lines
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(odom.info().cornerInliers[i], Qt::green); // inliers
}
if(imageView_->isLinesShown())
{
imageView_->addLine(
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(!odom.data().imageRaw().empty())
{
imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
}
}
imageView_->update();
cloudView_->update();
QApplication::processEvents();
processingData_ = false;
}
