From 83d0ad348cf0111e9f1149ed17c5b65ba44e82c6 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Fri, 26 Sep 2025 14:21:55 +0200 Subject: [PATCH 1/2] allow pause on saving clouds --- slam3d/core/MeasurementStorage.cpp | 23 ++++++++++++++++++++--- slam3d/core/MeasurementStorage.hpp | 8 +++++++- slam3d/sensor/pcl/PointCloudSensor.cpp | 19 ++++++++++++++----- 3 files changed, 41 insertions(+), 9 deletions(-) diff --git a/slam3d/core/MeasurementStorage.cpp b/slam3d/core/MeasurementStorage.cpp index 593dc10b..88513e2a 100644 --- a/slam3d/core/MeasurementStorage.cpp +++ b/slam3d/core/MeasurementStorage.cpp @@ -1,4 +1,4 @@ -#include "MeasurementStorage.hpp" +#include "MeasurementStorage.hpp" #include #include @@ -7,12 +7,19 @@ using namespace slam3d; void MeasurementStorage::add(Measurement::Ptr measurement) { - mMeasurements[measurement->getUniqueId()] = measurement; + if (enabled) { + mMeasurements[measurement->getUniqueId()] = measurement; + } + } Measurement::Ptr MeasurementStorage::get(const boost::uuids::uuid& uuid) { - return mMeasurements.at(uuid); + try { + return mMeasurements.at(uuid); + }catch (const std::out_of_range& e) { + return Measurement::Ptr(); + } } Measurement::Ptr MeasurementStorage::get(const std::string& key) @@ -24,3 +31,13 @@ bool MeasurementStorage::contains(const boost::uuids::uuid& key) { return mMeasurements.count(key); } + +void MeasurementStorage::enable() +{ + enabled = true; +} + +void MeasurementStorage::disable() +{ + enabled = false; +} diff --git a/slam3d/core/MeasurementStorage.hpp b/slam3d/core/MeasurementStorage.hpp index 4149277e..3c5429e5 100644 --- a/slam3d/core/MeasurementStorage.hpp +++ b/slam3d/core/MeasurementStorage.hpp @@ -15,7 +15,7 @@ namespace slam3d class MeasurementStorage { public: - + MeasurementStorage():enabled(true) {} virtual ~MeasurementStorage() {} /** @@ -47,6 +47,12 @@ namespace slam3d */ Measurement::Ptr get(const std::string& key); + void enable(); + void disable(); + + protected: + bool enabled; + private: std::map mMeasurements; }; diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp index 4c50573a..e9e63282 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.cpp +++ b/slam3d/sensor/pcl/PointCloudSensor.cpp @@ -122,6 +122,11 @@ Transform align(PointCloudMeasurement::Ptr source, // Downsample the scans PointCloud::Ptr filtered_source = source->getPointCloud(); PointCloud::Ptr filtered_target = target->getPointCloud(); + + if (!filtered_source || !filtered_target) { + return Transform::Identity(); + } + if(config.point_cloud_density > 0) { filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density); @@ -226,10 +231,11 @@ PointCloud::Ptr PointCloudSensor::getAccumulatedCloud(const VertexObjectList& ve { Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertices[i].measurementUuid); PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast(m); - if(!pcl) + if(!pcl || pcl->getPointCloud()->size() == 0) { - mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!"); - throw BadMeasurementType(); + mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not available or not a point cloud!"); + // throw BadMeasurementType(); + continue; } PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), (vertices[i].correctedPose * pcl->getSensorPose())); @@ -261,10 +267,13 @@ Constraint::Ptr PointCloudSensor::createConstraint(const Measurement::Ptr& sourc // Cast to this sensors measurement type PointCloudMeasurement::Ptr sourceCloud = boost::dynamic_pointer_cast(source); PointCloudMeasurement::Ptr targetCloud = boost::dynamic_pointer_cast(target); - if(!sourceCloud || !targetCloud) + if((!sourceCloud || !targetCloud) || sourceCloud->getPointCloud()->size() == 0 || targetCloud->getPointCloud()->size() == 0) { mLogger->message(ERROR, "Measurement given to createConstraint() is not a PointCloud!"); - throw BadMeasurementType(); + // throw BadMeasurementType(); + // is there was an emptyx cloud, use the guess + Covariance<6> covariance = Covariance<6>::Identity() * mCovarianceScale; + return Constraint::Ptr(new SE3Constraint(mName, guess, covariance.inverse())); } // For large loops, refine guess by a coarse ICP From 59da7a7769bbd120e3f76efc9f37f78fa5ac4d39 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Fri, 28 Nov 2025 16:35:56 +0100 Subject: [PATCH 2/2] fix crash --- slam3d/core/ScanSensor.cpp | 35 +++++++++++++++++++++++++- slam3d/sensor/pcl/PointCloudSensor.cpp | 2 +- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/slam3d/core/ScanSensor.cpp b/slam3d/core/ScanSensor.cpp index 1dded9d7..e43f9767 100644 --- a/slam3d/core/ScanSensor.cpp +++ b/slam3d/core/ScanSensor.cpp @@ -55,6 +55,15 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m) } Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex); + + if (!source.get()) { + IdType newVertex = mMapper->addMeasurement(m); + mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose()); + mLastTransform = Transform::Identity(); + mLastVertex = newVertex; + return true; + } + try { Constraint::Ptr c = createConstraint(source, m, mLastTransform, false); @@ -100,10 +109,34 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom return true; } + Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex); + + + // Add measurement if sufficient movement is reported by the odometry mLastTransform = mLastOdometry.inverse() * odom; if(checkMinDistance(mLastTransform)) { + if (!source.get()) { + IdType newVertex = mMapper->addMeasurement(m); + mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose()); + + Constraint::Ptr c(new SE3Constraint("paused", mLastTransform, Covariance<6>::Identity())); + SE3Constraint::Ptr se3 = boost::dynamic_pointer_cast(c); + mMapper->getGraph()->addConstraint(mLastVertex, newVertex, c); + mLastTransform = se3->getRelativePose(); + Transform last_pose = mMapper->getGraph()->getVertex(mLastVertex).correctedPose; + mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose()); + + + mLastOdometry = odom; + mLastVertex = newVertex; + mLastTransform = Transform::Identity(); + return true; + } + + + IdType newVertex = mMapper->addMeasurement(m); Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex); if(mLinkPrevious) @@ -119,7 +152,7 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom { mLastTransform = se3->getRelativePose(); } - Transform last_pose = mMapper->getGraph()->getVertex(mLastVertex).correctedPose; + // Transform last_pose = mMapper->getGraph()->getVertex(mLastVertex).correctedPose; mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose()); }catch(std::exception &e) { diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp index a6225eb4..47c8d54a 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.cpp +++ b/slam3d/sensor/pcl/PointCloudSensor.cpp @@ -123,7 +123,7 @@ Transform align(PointCloudMeasurement::Ptr source, PointCloud::Ptr filtered_source = source->getPointCloud(); PointCloud::Ptr filtered_target = target->getPointCloud(); - if (!filtered_source || !filtered_target) { + if (!filtered_source || !filtered_target || filtered_source->size() == 0 || filtered_target->size() == 0) { return Transform::Identity(); }