Skip to content

Commit

Permalink
Export CLI tool memory optimization (#1391)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@matlabbe
matlabbe authored Dec 15, 2024
1 parent 340248f commit 97c5636
Show file tree
Hide file tree
Showing 10 changed files with 1,017 additions and 411 deletions.
2 changes: 2 additions & 0 deletions corelib/include/rtabmap/core/DBDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,7 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
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;
Expand Down Expand Up @@ -286,6 +287,7 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
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;
Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/DBDriverSqlite3.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
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;
Expand Down
16 changes: 16 additions & 0 deletions corelib/include/rtabmap/core/util3d_surface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
39 changes: 39 additions & 0 deletions corelib/src/DBDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
69 changes: 69 additions & 0 deletions corelib/src/DBDriverSqlite3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
61 changes: 61 additions & 0 deletions corelib/src/util3d_surface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
2 changes: 1 addition & 1 deletion guilib/src/ExportCloudsDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
41 changes: 31 additions & 10 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions guilib/src/ui/exportCloudsDialog.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand Down Expand Up @@ -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>
Expand Down
Loading

0 comments on commit 97c5636

Please sign in to comment.