Export CLI tool memory optimization (#1391)

* Stashing changes so far

* Tested and working

* removed unused options

* Added density_radius and density_angle options

* improved description

* backward compatibility fixes, matching texture exported file names with base name

* Setting global full opt by default, updated preferences defaulting to g2o or gtsam if one or the other is unavailable

corelib/include/rtabmap/core/DBDriver.h

@@ -178,6 +178,7 @@ public:
 	void getWeight(int signatureId, int & weight) const;
 	void getLastNodeIds(std::set<int> & ids) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) const;
+	void getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren = false, bool ignoreIntermediateNodes = false) const;
 	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
 	void getLastMapId(int & mapId) const;
@@ -286,6 +287,7 @@ protected:
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -147,6 +147,7 @@ protected:
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;

corelib/include/rtabmap/core/util3d_surface.h

@@ -482,6 +482,22 @@ void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint(
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
 		float groundNormalsUp = 0.0f);
 
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+
 void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,

corelib/src/DBDriver.cpp

@@ -922,6 +922,45 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren, bool igno
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			bool hasNeighbors = !ignoreChildren;
+			if(ignoreChildren)
+			{
+				for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
+						nIter!=sIter->second->getLinks().end();
+						++nIter)
+				{
+					if(nIter->second.type() == Link::kNeighbor ||
+					   nIter->second.type() == Link::kNeighborMerged)
+					{
+						hasNeighbors = true;
+						break;
+					}
+				}
+			}
+			if(hasNeighbors && (!ignoreIntermediateNodes || sIter->second->getWeight() != -1))
+			{
+				poses.insert(std::make_pair(sIter->first, sIter->second->getPose()));
+			}
+		}
+
+		std::vector<int> keys = uKeys(_trashSignatures);
+
+	}
+	_trashesMutex.unlock();
+
+	_dbSafeAccessMutex.lock();
+	this->getAllOdomPosesQuery(poses, ignoreChildren, ignoreIntermediateNodes);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	_dbSafeAccessMutex.lock();

corelib/src/DBDriverSqlite3.cpp

@@ -2483,6 +2483,75 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 	}
 }
 
+void DBDriverSqlite3::getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
+{
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT DISTINCT id, pose "
+			  << "FROM Node ";
+		if(ignoreChildren)
+		{
+			query << "INNER JOIN Link ";
+			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
+			query << "WHERE from_id != to_id "; // ignore self referring links
+			query << "AND weight>-9 "; //ignore invalid nodes
+			if(ignoreIntermediateNodes)
+			{
+				query << "AND weight!=-1 "; //ignore intermediate nodes
+			}
+		}
+		else if(ignoreIntermediateNodes)
+		{
+			query << "WHERE weight!=-1 "; //ignore intermediate nodes
+		}
+
+		query  << "ORDER BY id";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		const void * data = 0;
+		int dataSize = 0;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			int id = sqlite3_column_int(ppStmt, 0); // Signature Id
+			data = sqlite3_column_blob(ppStmt, 1);  // Pose
+			dataSize = sqlite3_column_bytes(ppStmt, 1);
+
+			Transform pose;
+			if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
+			{
+				memcpy(pose.data(), data, dataSize);
+				if(uStrNumCmp(_version, "0.15.2") < 0)
+				{
+					pose.normalizeRotation();
+				}
+			}
+			else if(dataSize)
+			{
+				UERROR("Error while loading pose for node %d! Setting to null...", id);
+			}
+			poses.insert(std::make_pair(id, pose));
+			rc = sqlite3_step(ppStmt);
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)poses.size());
+	}
+}
+
 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	links.clear();

corelib/src/util3d_surface.cpp

@@ -3625,6 +3625,67 @@ void adjustNormalsToViewPoint(
 	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }
 
+template<typename PointT>
+void adjustNormalsToViewPointsImpl(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	if(poses.size() && cloud->size() == cameraIndices.size() && cloud->size())
+	{
+		#pragma omp parallel for
+		for(int i=0; i<(int)cloud->size(); ++i)
+		{
+			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
+			if(pcl::isFinite(normal))
+			{
+				const Transform & p = poses.at(cameraIndices[i]);
+				pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
+				Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
+
+				Eigen::Vector3f n(normal.x, normal.y, normal.z);
+
+				float result = v.dot(n);
+				if(result < 0 ||
+					(groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
+				{
+					//reverse normal
+					cloud->points[i].normal_x *= -1.0f;
+					cloud->points[i].normal_y *= -1.0f;
+					cloud->points[i].normal_z *= -1.0f;
+				}
+			}
+		}
+	}
+}
+
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, cameraIndices, cloud, groundNormalsUp);
+}
+
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, cameraIndices, cloud, groundNormalsUp);
+}
+
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, cameraIndices, cloud, groundNormalsUp);
+}
 
 template<typename PointT>
 void adjustNormalsToViewPointsImpl(

guilib/src/ExportCloudsDialog.cpp

@@ -816,7 +816,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_gp3Mu->setValue(2.5);
 	_ui->doubleSpinBox_meshDecimationFactor->setValue(0.0);
 	_ui->spinBox_meshMaxPolygons->setValue(0);
-	_ui->doubleSpinBox_transferColorRadius->setValue(0.025);
+	_ui->doubleSpinBox_transferColorRadius->setValue(0.05);
 	_ui->checkBox_cleanMesh->setChecked(true);
 	_ui->spinBox_mesh_minClusterSize->setValue(0);
 

guilib/src/PreferencesDialog.cpp

@@ -4871,22 +4871,43 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 				{
 					if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
 					{
-						UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
-							  "with g2o. Keeping default combo value: %s.",
-							  combo->objectName().toStdString().c_str(),
-							  combo->currentText().toStdString().c_str());
-						ok = false;
+						if(Optimizer::isAvailable(Optimizer::kTypeGTSAM)) {
+							UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
+								"with g2o. Falling back to GTSAM.",
+								combo->objectName().toStdString().c_str());
+							valueInt = 2;
+						}
+						else
+						{
+							UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
+								"with g2o. Keeping default combo value: %s.",
+								combo->objectName().toStdString().c_str(),
+								combo->currentText().toStdString().c_str());
+							ok = false;
+						}
 					}
 				}
 				if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 				{
 					if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
 					{
-						UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
-							  "with GTSAM. Keeping default combo value: %s.",
-							  combo->objectName().toStdString().c_str(),
-							  combo->currentText().toStdString().c_str());
-						ok = false;
+#ifndef RTABMAP_ORB_SLAM
+						if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+#endif
+						{
+							UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
+								"with GTSAM. Falling back to g2o.",
+								combo->objectName().toStdString().c_str());
+							valueInt = 1;
+						}
+						else
+						{
+							UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
+								"with GTSAM. Keeping default combo value: %s.",
+								combo->objectName().toStdString().c_str(),
+								combo->currentText().toStdString().c_str());
+							ok = false;
+						}
 					}
 				}
 				if(ok)

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-1328</y>
+        <y>-2995</y>
         <width>885</width>
-        <height>6169</height>
+        <height>6152</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2353,7 +2353,7 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                <string> m</string>
               </property>
               <property name="decimals">
-               <number>2</number>
+               <number>3</number>
               </property>
               <property name="minimum">
                <double>-1.000000000000000</double>