DBViewer: fixed calibration not found on refining without re-extracting features, fixed visualized ids switched on refine in some cases, fixed clouds not shown in constraints view after refine without re-extracting features

corelib/include/rtabmap/core/DBDriver.h

@@ -157,11 +157,13 @@ public:
 
 	// Load objects
 	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
-	void loadLastNodes(std::list<Signature *> & signatures) const;
+	void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
+	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
-	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
+	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage
 
 	// Specific queries...
+	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;

corelib/src/DBDriver.cpp

@@ -534,6 +534,23 @@ void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
 	_dbSafeAccessMutex.unlock();
 }
 
+Signature * DBDriver::loadSignature(int id, bool * loadedFromTrash)
+{
+	std::list<int> ids;
+	ids.push_back(id);
+	std::list<Signature*> signatures;
+	std::set<int> loadedFromTrashSet;
+	loadSignatures(ids, signatures, &loadedFromTrashSet);
+	if(loadedFromTrash && loadedFromTrashSet.size())
+	{
+		*loadedFromTrash = true;
+	}
+	if(!signatures.empty())
+	{
+		return signatures.front();
+	}
+	return 0;
+}
 void DBDriver::loadSignatures(const std::list<int> & signIds,
 		std::list<Signature *> & signatures,
 		std::set<int> * loadedFromTrash)
@@ -628,6 +645,13 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
+void DBDriver::loadNodeData(Signature * signature, bool images, bool scan, bool userData, bool occupancyGrid) const
+{
+	std::list<Signature *> signatures;
+	signatures.push_back(signature);
+	this->loadNodeData(signatures, images, scan, userData, occupancyGrid);
+}
+
 void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	// Don't look in the trash, we assume that if we want to load

corelib/src/DBDriverSqlite3.cpp

@@ -1257,7 +1257,8 @@ std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery(
 
 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
-	UDEBUG("load data for %d signatures", (int)signatures.size());
+	UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
+			(int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
 
 	if(!images && !scan && !userData && !occupancyGrid)
 	{
@@ -3383,7 +3384,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
 		}
-	if(ids.size() != loaded)
+		if(ids.size() != loaded)
 		{
 			UERROR("Some signatures not found in database");
 		}

corelib/src/RegistrationVis.cpp

@@ -221,7 +221,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());
 
-	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
+	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
@@ -230,9 +230,11 @@ Transform RegistrationVis::computeTransformationImpl(
 			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows,
 			fromSignature.sensorData().imageRaw().cols,
-			fromSignature.sensorData().imageRaw().rows);
+			fromSignature.sensorData().imageRaw().rows,
+			(int)fromSignature.sensorData().cameraModels().size(),
+			fromSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
 
-	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
+	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
@@ -241,7 +243,9 @@ Transform RegistrationVis::computeTransformationImpl(
 			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows,
 			toSignature.sensorData().imageRaw().cols,
-			toSignature.sensorData().imageRaw().rows);
+			toSignature.sensorData().imageRaw().rows,
+			(int)toSignature.sensorData().cameraModels().size(),
+			toSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
 
 	std::string msg;
 	info.projectedIDs.clear();

guilib/src/DatabaseViewer.cpp

@@ -5303,12 +5303,20 @@ void DatabaseViewer::updateConstraintView(
 		const rtabmap::Link & linkIn,
 		bool updateImageSliders,
 		const Signature & signatureFrom,
+		const Signature & signatureTo)
 {
 	UDEBUG("%d -> %d", linkIn.from(), linkIn.to());
 	std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
 
-	if(iterLink != linksRefined_.end())
+	if(iterLink != linksRefined_.end())
+	{
+		if(iterLink->second.from() == link.to())
+		{
+			link = iterLink->second.inverse();
+		}
+		else
+		{
 			link = iterLink->second;
 		}
 	}
@@ -5328,6 +5336,7 @@ void DatabaseViewer::updateConstraintView(
 						link.type(),
 						poseFrom.inverse() * poseTo);
 			}
+		}
 	}
 	UDEBUG("%d -> %d", link.from(), link.to());
 	rtabmap::Transform t = link.transform();
@@ -6751,6 +6760,7 @@ void DatabaseViewer::refineConstraint()
 	refineConstraint(from, to, false);
 }
 
+void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 {
 	UDEBUG("%d -> %d", from, to);
 	bool switchedIds = false;
@@ -6802,11 +6812,15 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		}
 	}
 
-	Transform transform;
+	Transform transform;
-	RegistrationInfo info;
+	RegistrationInfo info;
 	Signature * fromS = 0;
-	Signature toS;
+	Signature * toS = 0;
-
+
+	fromS = dbDriver_->loadSignature(currentLink.from());
+	if(fromS == 0)
+	{
+		UERROR("Signature %d not found!", currentLink.from());
 		return;
 	}
 
@@ -6895,7 +6909,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
 		}
 
-		Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
+		Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
+		LaserScan fromScan;
 		dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
 		fromS->sensorData().uncompressData();
 		int maxPoints = fromScan.size();
@@ -6980,7 +6995,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 				fromScan.rangeMax(),
 				fromScan.format(),
 				fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
-
+
 		RegistrationIcp registrationIcp(parameters);
 		transform = registrationIcp.computeTransformation(fromS->sensorData(), assembledData, currentLink.transform(), &info);
 		if(!transform.isNull())
@@ -6989,36 +7004,70 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			info.covariance*=100.0;
 		}
 	}
-	else
+	else
-	{
+	{
-		SensorData dataTo;
+		toS = dbDriver_->loadSignature(currentLink.to());
-		dbDriver_->getNodeData(currentLink.to(), dataTo);
+		if(toS == 0)
+		{
+			UERROR("Signature %d not found!", currentLink.to());
+			delete fromS;
+			return;
+		}
+
+		bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
+		Registration * reg = Registration::create(parameters);
+		if( reg->isScanRequired() ||
+			reg->isUserDataRequired() ||
+			reextractVisualFeatures ||
+			!silent)
+		{
+			dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
+			dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
+		
+			if(!silent)
+			{
+				fromS->sensorData().uncompressData();
+				toS->sensorData().uncompressData();
+			}
+		}
+
+		if(reextractVisualFeatures)
+		{
+			fromS->setWords(std::multimap<int, cv::KeyPoint>());
+			fromS->setWords3(std::multimap<int, cv::Point3f>());
+			fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
+			fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+			toS->setWords(std::multimap<int, cv::KeyPoint>());
+			toS->setWords3(std::multimap<int, cv::Point3f>());
+			toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
+			toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+		}
 
 		if(reg->isScanRequired())
 		{
 			if(ui_->checkBox_icp_from_depth->isChecked())
 			{
-				// generate laser scans from depth image
+				// generate laser scans from depth image
-				cv::Mat tmpA, tmpB, tmpC, tmpD;
+				cv::Mat tmpA, tmpB, tmpC, tmpD;
 				fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
-				dataTo.uncompressData(&tmpC, &tmpD, 0);
+				toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
 						fromS->sensorData(),
 						ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
 						ui_->doubleSpinBox_icp_maxDepth->value(),
 						ui_->doubleSpinBox_icp_minDepth->value(),
 						0,
-						ui_->parameters_toolbox->getParameters());
+						ui_->parameters_toolbox->getParameters());
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
 						toS->sensorData(),
 						ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
 						ui_->doubleSpinBox_icp_maxDepth->value(),
 						ui_->doubleSpinBox_icp_minDepth->value(),
 						0,
-						ui_->parameters_toolbox->getParameters());
+						ui_->parameters_toolbox->getParameters());
-				int maxLaserScans = cloudFrom->size();
+				int maxLaserScans = cloudFrom->size();
 				fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
-				dataTo.setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
+				toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
 
 				if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
 				{
@@ -7027,34 +7076,31 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 				}
 			}
 			else
-			{
+			{
-				LaserScan tmpA, tmpB;
+				LaserScan tmpA, tmpB;
 				fromS->sensorData().uncompressData(0, 0, &tmpA);
 				toS->sensorData().uncompressData(0, 0, &tmpB);
 			}
-		}
+		}
 
 		if(reg->isImageRequired() && reextractVisualFeatures)
-		{
+		{
-			cv::Mat tmpA, tmpB, tmpC, tmpD;
+			cv::Mat tmpA, tmpB, tmpC, tmpD;
 			fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
 			toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
 		}
 
-		UINFO("Uncompress time: %f s", timer.ticks());
+		UINFO("Uncompress time: %f s", timer.ticks());
-
-		fromS = Signature(dataFrom);
-		toS = Signature(dataTo);
 
-		if(fromS.id() < toS.id())
+		if(fromS->id() < toS->id())
 		{
 			transform = reg->computeTransformationMod(*fromS, *toS, t, &info);
 		}
-		else
+		else
 		{
 			transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
 			switchedIds = true;
-		}
+		}
 
 		delete reg;
 	}
@@ -7100,11 +7146,21 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			this->updateGraphView();
 		}
 
-		if(!silent && ui_->dockWidget_constraints->isVisible())
+		if(!silent && ui_->dockWidget_constraints->isVisible())
 		{
-			if(fromS.id() > 0 && toS.id() > 0)
+			if(fromS->id() > 0 && toS->id() > 0)
-			{
+			{
-				this->updateConstraintView(newLink, true, fromS, toS);
+				updateLoopClosuresSlider(fromS->id(), toS->id());
+				if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
+				{
+					this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
+					ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+					ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+				}
+				else
+				{
+					this->updateConstraintView(newLink, true, *fromS, *toS);
+					ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
 					ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
 				}
 
@@ -7121,6 +7177,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 	{
 		QMessageBox::warning(this,
 				tr("Refine link"),
+				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
+	}
 	delete fromS;
 	delete toS;
 }
@@ -7179,8 +7237,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		if(signatures.size() != 2)
 		{
 			for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
-			{
 			{
+				delete *iter;
 			}
 			return false;
 		}
@@ -7188,12 +7246,13 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		toS = *signatures.rbegin();
 
 		bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
-		if(reg->isScanRequired() ||
+		if(reg->isScanRequired() ||
+			reg->isUserDataRequired() ||
 			reextractVisualFeatures ||
 			!silent)
-		{
+		{
 			// Add sensor data to generate features
-			dbDriver_->getNodeData(from, fromS->sensorData(), reextractVisualFeatures, reg->isScanRequired(), reg->isUserDataRequired(), false);
+			dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
 			fromS->sensorData().uncompressData();
 			dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
 			toS->sensorData().uncompressData();
@@ -7622,6 +7681,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 	return links;
 }
 
+void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 {
 	UDEBUG("%d %d", from, to);
 	int size = loopLinks_.size();