Skip to content

allow pause on saving clouds #14

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
planthaber wants to merge 4 commits into
base: master
Choose a base branch
from
Open

allow pause on saving clouds #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
83d0ad3
allow pause on saving clouds
planthaber Sep 26, 2025
9b571de
Merge remote-tracking branch 'autobuild/master' into feature/pause
planthaber Nov 27, 2025
59da7a7
fix crash
planthaber Nov 28, 2025
a722b9e
Merge remote-tracking branch 'autobuild/master' into feature/pause
planthaber Dec 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 20 additions & 3 deletions slam3d/core/MeasurementStorage.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#include "MeasurementStorage.hpp"
#include "MeasurementStorage.hpp"

#include <boost/uuid/uuid_io.hpp>
#include <boost/lexical_cast.hpp>
Expand All @@ -7,12 +7,19 @@ using namespace slam3d;

void MeasurementStorage::add(Measurement::Ptr measurement)
{
mMeasurements[measurement->getUniqueId()] = measurement;
if (enabled) {

@skasperski skasperski Sep 29, 2025

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure this pausing function should go into the MeasurementStorage. It feels kinda wrong to call add() and then have this method silently not add the measurement because of some internal state. Wouldn't it be better to do this on a higher level (node/application) and just not call MeasurementStorage::add() when in paused mode?

@planthaber planthaber Sep 30, 2025

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, I also thought about this, but it requires a lot of changes in the other slma3d classes, as there still should run a icp and a vertex should be created, adding it here was the least intrusive way of having this funcionality.

@planthaber planthaber Sep 30, 2025

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We could add a pause function to the graph class, but that info was to be passed down to all the implementations that use add() on the storage.

mMeasurements[measurement->getUniqueId()] = measurement;
}

}

Measurement::Ptr MeasurementStorage::get(const boost::uuids::uuid& uuid)
{
return mMeasurements.at(uuid);

@skasperski skasperski Sep 29, 2025

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't agree with this change. When you use get(), you should catch and handle std::out_of_range, not be forced to check the returned pointer.

@planthaber planthaber Sep 30, 2025

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can change that, imo there is not much difference.

Currently the behavior is similar to of std::map::get().

try {
return mMeasurements.at(uuid);
}catch (const std::out_of_range& e) {
return Measurement::Ptr();
}
}

Measurement::Ptr MeasurementStorage::get(const std::string& key)
Expand All @@ -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;
}
8 changes: 7 additions & 1 deletion slam3d/core/MeasurementStorage.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ namespace slam3d
class MeasurementStorage
{
public:

MeasurementStorage():enabled(true) {}
virtual ~MeasurementStorage() {}

/**
Expand Down Expand Up @@ -48,6 +48,12 @@ namespace slam3d
*/
Measurement::Ptr get(const std::string& key);

void enable();
void disable();

protected:
bool enabled;

private:
std::map<boost::uuids::uuid, Measurement::Ptr> mMeasurements;
};
Expand Down
35 changes: 34 additions & 1 deletion slam3d/core/ScanSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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<SE3Constraint>(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)
Expand All @@ -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)
{
Expand Down
19 changes: 14 additions & 5 deletions slam3d/sensor/pcl/PointCloudSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,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 || filtered_source->size() == 0 || filtered_target->size() == 0) {
return Transform::Identity();
}

if(config.point_cloud_density > 0)
{
filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density);
Expand Down Expand Up @@ -241,10 +246,11 @@ PointCloud::Ptr PointCloudSensor::getAccumulatedCloud(const VertexObjectList& ve
{
Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertices[i].measurementUuid);
PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast<PointCloudMeasurement>(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()));
Expand Down Expand Up @@ -276,10 +282,13 @@ Constraint::Ptr PointCloudSensor::createConstraint(const Measurement::Ptr& sourc
// Cast to this sensors measurement type
PointCloudMeasurement::Ptr sourceCloud = boost::dynamic_pointer_cast<PointCloudMeasurement>(source);
PointCloudMeasurement::Ptr targetCloud = boost::dynamic_pointer_cast<PointCloudMeasurement>(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()));

@skasperski skasperski Sep 29, 2025

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Adding the guess (which is just the current relative position of the two vertices) as a constraint is a bad idea. A PointcloudSensor can only create constraints between pointclouds. So similar as above, when an application wants to do something else, catch the BadMeasurement and do everything needed in the handler.

@planthaber planthaber Sep 30, 2025

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This code is only used, when the previous cloud was not saved ( slam was paused), so there is no cloud to compare to. The hope is that eventually a loop closure will catch this ans optimize correctly, as all the vertices in between are available

}

// For large loops, refine guess by a coarse ICP
Expand Down