fixed OdometryViewer where clouds were not all removed or shown

corelib/src/Odometry.cpp

@@ -562,14 +562,13 @@ Transform OdometryBOW::computeTransform(
 		info->localMapSize = (int)localMap_.size();
 	}
 
-	UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d variance=%f local_map=%d[%d] dict=%d nodes=%d",
+	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
 			timer.elapsed(),
-			output.prettyPrint().c_str(),
+			output.isNull()?"true":"false",
 			nFeatures,
 			inliers,
 			correspondences,
 			variance,
-			(int)uUniqueKeys(localMap_).size(),
 			(int)localMap_.size(),
 			(int)_memory->getVWDictionary()->getVisualWords().size(),
 			(int)_memory->getStMem().size());
@@ -1064,8 +1063,9 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 		info->matches = correspondences;
 	}
 
-	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
+	UINFO("Odom update time = %fs lost=%s inliers=%d/%d, new corners=%d, transform accepted=%s",
 			timer.elapsed(),
+			output.isNull()?"true":"false",
 			inliers,
 			correspondences,
 			(int)newCorners.size(),
@@ -1454,8 +1454,9 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 		info->matches = correspondences;
 	}
 
-	UINFO("Odom update time = %fs inliers=%d/%d, variance=%f, new corners=%d",
+	UINFO("Odom update time = %fs lost=%s inliers=%d/%d, variance=%f, new corners=%d",
 			timer.elapsed(),
+			output.isNull()?"true":"false",
 			inliers,
 			correspondences,
 			variance,

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -70,6 +70,7 @@ private:
 	Transform lastOdomPose_;
 	int qualityWarningThr_;
 	int id_;
+	QList<std::string> addedClouds_;
 
 	QSpinBox * maxCloudsSpin_;
 	QDoubleSpinBox * voxelSpin_;

guilib/src/CloudViewer.cpp

@@ -625,8 +625,9 @@ void CloudViewer::removeAllClouds()
 
 bool CloudViewer::removeCloud(const std::string & id)
 {
-	_addedClouds.remove(id);
+	bool success = _visualizer->removePointCloud(id);
-	return _visualizer->removePointCloud(id);
+	_addedClouds.remove(id); // remove after visualizer
+	return success;
 }
 
 bool CloudViewer::getPose(const std::string & id, Transform & pose)

guilib/src/OdometryViewer.cpp

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QLabel>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
+#include <QApplication>
 
 namespace rtabmap {
 
@@ -61,7 +62,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
 
-	imageView_->setImageDepthShown(true);
+	imageView_->setImageDepthShown(false);
 	imageView_->setMinimumSize(320, 240);
 
 	cloudView_->setCameraFree();
@@ -126,6 +127,7 @@ OdometryViewer::~OdometryViewer()
 
 void OdometryViewer::clear()
 {
+	addedClouds_.clear();
 	cloudView_->clear();
 }
 
@@ -214,30 +216,32 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 		if(cloud->size())
 		{
 			cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
-		}
 
-		if(!data.pose().isNull())
+			if(!data.pose().isNull())
-		{
-			lastOdomPose_ = data.pose();
-			if(cloudView_->getAddedClouds().contains("cloudtmp"))
 			{
-				cloudView_->removeCloud("cloudtmp");
+				lastOdomPose_ = data.pose();
+				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();
+				}
+
+				data.id()?id_=data.id():++id_;
+				std::string cloudName = uFormat("cloud%d", id_);
+				addedClouds_.push_back(cloudName);
+				UASSERT(cloudView_->addCloud(cloudName, cloud, data.pose()));
+
+				cloudView_->updateCameraTargetPosition(data.pose());
 			}
-
+			else
-			data.id()?id_=data.id():++id_;
-
-			while(maxCloudsSpin_->value()>0 && (int)cloudView_->getAddedClouds().size() > maxCloudsSpin_->value())
 			{
-				cloudView_->removeCloud(cloudView_->getAddedClouds().begin().key());
+				cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
 			}
-
-			cloudView_->addOrUpdateCloud(uFormat("cloud%d", id_), cloud, data.pose());
-
-			cloudView_->updateCameraTargetPosition(data.pose());
-		}
-		else
-		{
-			cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
 		}
 	}
 
@@ -316,7 +320,6 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 									Qt::blue);
 						}
 					}
-					imageView_->update();
 				}
 			}
 
@@ -327,7 +330,9 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 		}
 	}
 
+	imageView_->update();
 	cloudView_->update();
+	QApplication::processEvents();
 	processingData_ = false;
 }
 

utilite/src/ULogger.cpp

@@ -361,7 +361,7 @@ void ULogger::write(ULogger::Level level,
 		}
 
 		std::string time = "";
-		if(printTime_)
+		if(printTime_ || level == kFatal)
 		{
 			time.append("(");
 			getTime(time);
@@ -369,7 +369,7 @@ void ULogger::write(ULogger::Level level,
 		}
 
 		std::string levelStr = "";
-		if(printLevel_)
+		if(printLevel_ || level == kFatal)
 		{
 			const int bufSize = 30;
 			char buf[bufSize] = {0};
@@ -384,7 +384,7 @@ void ULogger::write(ULogger::Level level,
 		}
 
 		std::string whereStr = "";
-		if(printWhere_)
+		if(printWhere_ || level == kFatal)
 		{
 			whereStr.append("");
 			//File
@@ -530,9 +530,6 @@ void ULogger::write(ULogger::Level level,
 
 int ULogger::getTime(std::string &timeStr)
 {
-    if(!printTime_) {
-        return 0;
-    }
     struct tm timeinfo;
     const int bufSize = 30;
     char buf[bufSize] = {0};