From c4afd64074e5154f598070b7bbacf89c20a3b849 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Tue, 9 Jun 2026 17:37:19 +0200 Subject: [PATCH 01/14] WiP --- slam3d/core/Graph.cpp | 24 +- slam3d/sensor/pcl/CMakeLists.txt | 2 + slam3d/sensor/pcl/MultiScanSensor.cpp | 288 ++++++++++++++++++++ slam3d/sensor/pcl/MultiScanSensor.hpp | 194 +++++++++++++ slam3d/sensor/pcl/PointCloudSensor.hpp | 2 +- slam3d/serialization/GraphSerialization.cpp | 2 +- 6 files changed, 504 insertions(+), 8 deletions(-) create mode 100644 slam3d/sensor/pcl/MultiScanSensor.cpp create mode 100644 slam3d/sensor/pcl/MultiScanSensor.hpp diff --git a/slam3d/core/Graph.cpp b/slam3d/core/Graph.cpp index 72586da..da686a2 100644 --- a/slam3d/core/Graph.cpp +++ b/slam3d/core/Graph.cpp @@ -84,16 +84,28 @@ void Graph::reloadToSolver() } // add all edges after vertices are defined - for (const auto& vertex : vertices) + for (const auto& edge : getEdges()) { - for (const auto& edge : getOutEdges(vertex.index)) + printf("%s:%i %li->%li (%s : %s)\n", __PRETTY_FUNCTION__, __LINE__, edge.source, edge.target, edge.constraint->getTypeName(), edge.constraint->getSensorName().c_str()); + if (edge.constraint->getType() != TENTATIVE) { - if (edge.constraint->getType() != TENTATIVE) - { - mSolver->addEdge(edge.source, edge.target, edge.constraint); - } + mSolver->addEdge(edge.source, edge.target, edge.constraint); } } + + // add all edges after vertices are defined + // for (const auto& vertex : vertices) + // { + // for (const auto& edge : getOutEdges(vertex.index)) + // { + // if (edge.constraint->getType() != TENTATIVE && edge.source == vertex.index) + // { + // printf("%s:%i %li->%li (%s : %s)\n", __PRETTY_FUNCTION__, __LINE__, edge.source, edge.target, edge.constraint->getTypeName(), edge.constraint->getSensorName().c_str()); + // mSolver->addEdge(edge.source, edge.target, edge.constraint); + // } + // } + // } + } void Graph::writeGraphToFile(const std::string &name) diff --git a/slam3d/sensor/pcl/CMakeLists.txt b/slam3d/sensor/pcl/CMakeLists.txt index e94e0f5..c17a096 100644 --- a/slam3d/sensor/pcl/CMakeLists.txt +++ b/slam3d/sensor/pcl/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(sensor-pcl SHARED PointCloudSensor.cpp + MultiScanSensor.cpp ) target_include_directories(sensor-pcl @@ -32,6 +33,7 @@ target_link_libraries(sensor-pcl install( FILES PointCloudSensor.hpp + MultiScanSensor.hpp RegistrationParameters.hpp DESTINATION include/slam3d/sensor/pcl ) diff --git a/slam3d/sensor/pcl/MultiScanSensor.cpp b/slam3d/sensor/pcl/MultiScanSensor.cpp new file mode 100644 index 0000000..b94e5d9 --- /dev/null +++ b/slam3d/sensor/pcl/MultiScanSensor.cpp @@ -0,0 +1,288 @@ +#include "MultiScanSensor.hpp" + +#include + +#include + + +#ifdef USE_PCLOMP + #include + #include +#endif + +#include +#include + + +namespace slam3d { + + +template +Transform doICP(PointCloud::Ptr source, + PointCloud::Ptr target, + const Transform& guess, + const RegistrationParameters& config) +{ + ICP_TYPE icp; + icp.setMaxCorrespondenceDistance(config.max_correspondence_distance); + icp.setMaximumIterations(config.maximum_iterations); + icp.setTransformationEpsilon(config.transformation_epsilon); + icp.setEuclideanFitnessEpsilon(config.euclidean_fitness_epsilon); + icp.setCorrespondenceRandomness(config.correspondence_randomness); + icp.setMaximumOptimizerIterations(config.maximum_optimizer_iterations); + icp.setRotationEpsilon(config.rotation_epsilon); + + PointCloud result; + icp.setInputSource(target); + icp.setInputTarget(source); + icp.align(result, guess.matrix().cast()); + + // Check if ICP was successful (kind of...) + double score = icp.getFitnessScore(config.max_correspondence_distance); + if(!icp.hasConverged() || score > config.max_fitness_score) + { + throw NoMatch((boost::format("ICP failed with Fitness-Score %1% > %2%") % score % config.max_fitness_score).str()); + } + + // Get estimated transform + Transform icp_result(Eigen::Isometry3f(icp.getFinalTransformation())); + return icp_result; +} + +template +Transform doNDT(PointCloud::Ptr source, + PointCloud::Ptr target, + const Transform& guess, + const RegistrationParameters& config) +{ + NDT_TYPE ndt; + ndt.setOulierRatio(config.outlier_ratio); + ndt.setMaxCorrespondenceDistance(config.max_correspondence_distance); + ndt.setMaximumIterations(config.maximum_iterations); + ndt.setTransformationEpsilon(config.transformation_epsilon); + ndt.setEuclideanFitnessEpsilon(config.euclidean_fitness_epsilon); + ndt.setStepSize(config.step_size); + ndt.setResolution(config.resolution); + + // Source and target are switched at this point! + // In the pose graph, our edge (with transform) goes from source to target, + // but ICP calculates the transformation from target to source. + ndt.setInputSource(target); + ndt.setInputTarget(source); + PointCloud result; + ndt.align(result, guess.matrix().cast()); + + // Check if NDT was successful (kind of...) + double score = ndt.getFitnessScore(config.max_correspondence_distance); + if(!ndt.hasConverged() || score > config.max_fitness_score) + { + throw NoMatch((boost::format("NDT failed with Fitness-Score %1% > %2%") % score % config.max_fitness_score).str()); + } + + // Get estimated transform + Eigen::Isometry3f tf_matrix(ndt.getFinalTransformation()); + return Transform(tf_matrix); +} + +Transform align(MultiScanMeasurement::Ptr source, + MultiScanMeasurement::Ptr target, + const Transform& guess, + const RegistrationParameters& config) +{ + // Downsample the scans + PointCloud::Ptr filtered_source = source->getPointCloud(); + PointCloud::Ptr filtered_target = target->getPointCloud(); + if(config.point_cloud_density > 0) + { + PointCloud::Ptr filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density); + PointCloud::Ptr filtered_target = PointCloudSensor::downsample(target->getPointCloud(), config.point_cloud_density); + } + + // Make sure that there are enough points left (ICP will crash if not) + if(filtered_target->size() < 100 || filtered_source->size() < 100) + throw NoMatch("Too few points after filtering, you may have to decrease 'point_cloud_density'."); + + // Configure Generalized-ICP + switch(config.registration_algorithm) + { + case GICP: + return doICP< pcl::GeneralizedIterativeClosestPoint > + (filtered_source, filtered_target, guess, config); + case NDT: + return doNDT< pcl::NormalDistributionsTransform > + (filtered_source, filtered_target, guess, config); +#ifdef USE_PCLOMP + case GICP_OMP: + return doICP< pclomp::GeneralizedIterativeClosestPoint > + (filtered_source, filtered_target, guess, config); + case NDT_OMP: + return doNDT< pclomp::NormalDistributionsTransform > + (filtered_source, filtered_target, guess, config); +#else + case GICP_OMP: + case NDT_OMP: + throw std::runtime_error("OMP is not available, you need to rebuild SLAM3D with OMP or use another matching algorithm."); +#endif + default: + throw std::runtime_error("Unknown registration algorithm specified."); + } +} + + +MultiScanMeasurement::MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, const boost::uuids::uuid id) : PointCloudMeasurement(std::make_shared(),r, s, slam3d::Transform::Identity(), id), clouds(clouds) { + + + // todo make this transform if frames differ + for (const auto& subcloud : clouds) { + PointCloud::Ptr tempCloud; + if (!subcloud->getSensorPose().isApprox(slam3d::Transform::Identity())) { + tempCloud = PointCloud::Ptr(new PointCloud); + pcl::transformPointCloud(*subcloud->getPointCloud(), *tempCloud, subcloud->getSensorPose().matrix()); + } else { + tempCloud = subcloud->getPointCloud(); + } + *mPointCloud.get() += *tempCloud.get(); + } + + mPointCloud->header = clouds[0]->getPointCloud()->header; + mStamp.tv_sec = clouds[0]->getPointCloud()->header.stamp / 1000000; + mStamp.tv_usec = clouds[0]->getPointCloud()->header.stamp % 1000000; +} + + +const PointCloud::Ptr MultiScanMeasurement::getCombinedPointCloud(const std::string& annotation) { + PointCloud::Ptr combined(new PointCloud); + for (unsigned int i = 0; i < clouds.size(); ++i) { + if (annotation == "" || annotation == annotations[i]) { + *combined += *(clouds[i]->getPointCloud()); + } + } + return combined; +} + +const std::vector MultiScanMeasurement::getCloudsByAnnotation(const std::string &annotation) { + std::vector result; + for (unsigned int i = 0; i < clouds.size(); ++i) { + if (annotation == annotations[i]) { + result.push_back(clouds[i]->getPointCloud()); + } + } + return result; +} + +PointCloud::Ptr MultiScanSensor::transform(PointCloud::ConstPtr source, const Transform tf) const { + PointCloud::Ptr transformedCloud(new PointCloud); + pcl::transformPointCloud(*source, *transformedCloud, tf.matrix()); + return transformedCloud; +} + + +PointCloud::Ptr MultiScanSensor::getAccumulatedCloud(const VertexObjectList& vertices) const { + PointCloud::Ptr accu(new PointCloud); + + for (VertexObjectList::const_reverse_iterator it = vertices.rbegin(); it != vertices.rend(); it++) { + Measurement::Ptr m = mMapper->getGraph()->getMeasurement(it->measurementUuid); + MultiScanMeasurement::Ptr pcl = boost::dynamic_pointer_cast(m); + if (!pcl) { + mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a MultiScanMeasurement cloud!"); + throw slam3d::BadMeasurementType(); + } + PointCloud::Ptr tempCloud = this->transform(pcl->getCombinedPointCloud(), (it->correctedPose * pcl->getSensorPose())); + *accu += *tempCloud; + } + return accu; +} + +Measurement::Ptr MultiScanSensor::createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const { + printf("%s:%i\n", __PRETTY_FUNCTION__, __LINE__); + PointCloud::Ptr cloud = getAccumulatedCloud(vertices); + PointCloud::Ptr shifted(new PointCloud); + pcl::transformPointCloud(*cloud, *shifted, pose.inverse().matrix()); + mLogger->message(DEBUG, (boost::format("Patch MultiScanMeasurement has %1% points.") % cloud->size()).str()); + std::vector clouds; + PointCloudMeasurement::Ptr pcm (new PointCloudMeasurement(shifted, "AccumulatedPointcloud", mName, Transform::Identity())); + clouds.push_back(pcm); + Measurement::Ptr m(new MultiScanMeasurement(clouds, "AccumulatedPointcloud", mName)); + return m; +} + + +Constraint::Ptr MultiScanSensor::createConstraint(const Measurement::Ptr& source, + const Measurement::Ptr& target, + const Transform& odometry, + bool loop) +{ + + // Transform guess in sensor frame + Transform guess = source->getInverseSensorPose() * odometry * target->getSensorPose(); + + // Cast to this sensors measurement type + MultiScanMeasurement::Ptr sourceCloud = boost::dynamic_pointer_cast(source); + MultiScanMeasurement::Ptr targetCloud = boost::dynamic_pointer_cast(target); + if (!sourceCloud || !targetCloud) { + mLogger->message(ERROR, "Measurement given to createConstraint() is not a MultiScanMeasurement!"); + throw BadMeasurementType(); + } + + // For large loops, refine guess by a coarse ICP + if (loop) { + guess = align(sourceCloud, targetCloud, guess, mCoarseConfiguration); + } + + // Calculate precise alignement with fine ICP + Transform icp_result = align(sourceCloud, targetCloud, guess, mFineConfiguration); + + // Transform back to robot frame + Transform transform = source->getSensorPose() * icp_result * target->getInverseSensorPose(); + Covariance<6> covariance = Covariance<6>::Identity() * mCovarianceScale; + + return Constraint::Ptr(new SE3Constraint(mName, transform, covariance.inverse())); + + +} + + + +void MultiScanSensor::setRegistrationParameters(const RegistrationParameters& conf, bool coarse) +{ + if(coarse) + { + mLogger->message(INFO, " = RegistrationParameters (Coarse) ="); + mCoarseConfiguration = conf; + }else + { + mLogger->message(INFO, " = RegistrationParameters (Fine) ="); + mFineConfiguration = conf; + } + mLogger->message(INFO, (boost::format("correspondence_randomness: %1%") % conf.correspondence_randomness).str()); + mLogger->message(INFO, (boost::format("euclidean_fitness_epsilon: %1%") % conf.euclidean_fitness_epsilon).str()); + mLogger->message(INFO, (boost::format("max_correspondence_distance: %1%") % conf.max_correspondence_distance).str()); + mLogger->message(INFO, (boost::format("max_fitness_score: %1%") % conf.max_fitness_score).str()); + mLogger->message(INFO, (boost::format("maximum_iterations: %1%") % conf.maximum_iterations).str()); + mLogger->message(INFO, (boost::format("maximum_optimizer_iterations: %1%") % conf.maximum_optimizer_iterations).str()); + mLogger->message(INFO, (boost::format("point_cloud_density: %1%") % conf.point_cloud_density).str()); + mLogger->message(INFO, (boost::format("rotation_epsilon: %1%") % conf.rotation_epsilon).str()); + mLogger->message(INFO, (boost::format("transformation_epsilon: %1%") % conf.transformation_epsilon).str()); +} + +void MultiScanSensor::setMapResolution(double r) +{ + mLogger->message(INFO, (boost::format("map_resolution: %1%") % r).str()); + mMapResolution = r; +} + +void MultiScanSensor::setMapOutlierRemoval(double r, unsigned n) +{ + mLogger->message(INFO, (boost::format("map_outlier_radius: %1%") % r).str()); + mLogger->message(INFO, (boost::format("map_outlier_neighbors: %1%") % n).str()); + mMapOutlierRadius = r; + mMapOutlierNeighbors = n; +} + + +} // namespace slam3d + + + + + diff --git a/slam3d/sensor/pcl/MultiScanSensor.hpp b/slam3d/sensor/pcl/MultiScanSensor.hpp new file mode 100644 index 0000000..5418558 --- /dev/null +++ b/slam3d/sensor/pcl/MultiScanSensor.hpp @@ -0,0 +1,194 @@ +#pragma once + +#include +#include + +#include + +#include +#include + +#include + + +namespace slam3d { + +class MultiScanMeasurement : public slam3d::PointCloudMeasurement { + public: + typedef boost::shared_ptr Ptr; + + + MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, + const boost::uuids::uuid id = boost::uuids::nil_uuid()); + + + const char* getTypeName() const override { return "slam3d::MultiScanMeasurement"; } + + /** + * @brief returns the combined cloud (makes it compatible with PointCloudSensor functions) + * + * @return const PointCloud::Ptr + */ + const PointCloud::Ptr getCombinedPointCloud(const std::string& annotation = ""); + + + const std::vector getCloudsByAnnotation(const std::string &annotation); + + +// protected: + friend class boost::serialization::access; + std::vector clouds; + + std::vector annotations; + + std::map cloudByUuid; + + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + // Tell boost::serialization that this is derived from Measurement. + // It is required because we don't explicitely call Measurement::serialize() + // from within PointCloudMeasurement::serialize(). + + boost::serialization::void_cast_register( + static_cast(NULL), + static_cast(NULL)); + } + + // TODO serialize + + }; + +class MultiScanSensor : public slam3d::ScanSensor { + public: + MultiScanSensor(const std::string& n, Logger* l): ScanSensor(n,l) {}; + ~MultiScanSensor() {}; + + + /** + * @brief Sets parameters for the internal pointcloud registration. + * The standard set is always used to calculate the final transformation. + * A coarse set is used to initialize and verify loop-closures. + * @param param new configuration paramerters + * @param coarse whether to set coarse parameter set + */ + void setRegistrationParameters(const RegistrationParameters& param, bool coarse); + + /** + * @brief Set density of points in accumulated map cloud. + * @param r + */ + void setMapResolution(double r); + + /** + * @brief Set parameters for outlier removal. + * @details An outlier is a point that has less then n neighbors within radius r. + * @param r + * @param n + */ + void setMapOutlierRemoval(double r, unsigned n); + + + /** + * @brief Transform source cloud by given transformation. + * @param source + * @param tf + */ + PointCloud::Ptr transform(PointCloud::ConstPtr source, const Transform tf) const; + + /** + * @brief Creates a single point cloud that contains all measurements in vertices. + * @details The individual point clouds are transformed by their current pose in the graph, + * no additional alignement or optimization is performed during this. + * @param vertices + * @return accumulated pointcloud + * @throw BadMeasurementType + */ + PointCloud::Ptr getAccumulatedCloud(const VertexObjectList& vertices) const; + + + /** + * @brief Create a virtual measurement by accumulating scans from given vertices. + * @param vertices list of vertices that should contain a Measurement of this sensor + * @param pose origin of the accumulated scan + * @throw BadMeasurementType + */ + virtual Measurement::Ptr createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const; + + + /** + * @brief Create a constraint between two measurements. + * @details The odometry transformation and the resulting constraint are + * with regards to the robot coordinate system. Make sure that sensor_pose + * is properly set within the measurements. + * @param source + * @param target + * @param odometry + */ + virtual Constraint::Ptr createConstraint(const Measurement::Ptr& source, + const Measurement::Ptr& target, + const Transform& odometry, + bool loop); + + protected: + RegistrationParameters mFineConfiguration; + RegistrationParameters mCoarseConfiguration; + + double mMapResolution; + double mMapOutlierRadius; + unsigned mMapOutlierNeighbors; + + +}; + +} // namespace slam3d + +namespace boost { +namespace serialization { + + template + inline void save_construct_data(Archive & ar, const slam3d::MultiScanMeasurement * m, const unsigned int file_version) + { + // save data required to construct instance + ar << m->clouds; + ar << m->annotations; + ar << m->cloudByUuid; + ar << m->getRobotName(); + ar << m->getSensorName(); + ar << m->getSensorPose(); + ar << m->getUniqueId(); + } + + template + inline void load_construct_data(Archive & ar, slam3d::MultiScanMeasurement * t, const unsigned int file_version) + { + // retrieve data from archive required to construct new instance + + std::vector clouds; + std::vector annotations; + std::map cloudByUuid; + + std::string robot; + std::string sensor; + slam3d::Transform pose; + boost::uuids::uuid id; + ar >> clouds; + ar >> annotations; + ar >> cloudByUuid; + + ar >> robot; + ar >> sensor; + ar >> pose; + ar >> id; + + // invoke inplace constructor to initialize instance of PointCloudMeasurement + ::new(t)slam3d::MultiScanMeasurement(clouds, robot, sensor, id); + } + +} // namespace serialization +} // namespace boost + +BOOST_CLASS_EXPORT_KEY(slam3d::MultiScanMeasurement) diff --git a/slam3d/sensor/pcl/PointCloudSensor.hpp b/slam3d/sensor/pcl/PointCloudSensor.hpp index ea429cf..87be09c 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.hpp +++ b/slam3d/sensor/pcl/PointCloudSensor.hpp @@ -124,7 +124,7 @@ namespace slam3d * @param pose origin of the accumulated pointcloud * @throw BadMeasurementType */ - Measurement::Ptr createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const override; + virtual Measurement::Ptr createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const override; /** * @brief Create an ICP constraint between two point clouds. diff --git a/slam3d/serialization/GraphSerialization.cpp b/slam3d/serialization/GraphSerialization.cpp index 2e3c6dc..bf51f38 100644 --- a/slam3d/serialization/GraphSerialization.cpp +++ b/slam3d/serialization/GraphSerialization.cpp @@ -39,6 +39,6 @@ bool GraphSerialization::fromFile(Graph* graph, const std::string &graphfile) // optimize locations graph->reloadToSolver(); - graph->optimize(); + // graph->optimize(); return true; } From cb08b6cd4fa02422fbf4670243d37601668d58b2 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Wed, 10 Jun 2026 17:30:43 +0200 Subject: [PATCH 02/14] comment unused function in multi measurement --- slam3d/sensor/pcl/MultiScanSensor.cpp | 66 +++++++++++++++------------ slam3d/sensor/pcl/MultiScanSensor.hpp | 4 +- 2 files changed, 38 insertions(+), 32 deletions(-) diff --git a/slam3d/sensor/pcl/MultiScanSensor.cpp b/slam3d/sensor/pcl/MultiScanSensor.cpp index b94e5d9..fd60549 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.cpp +++ b/slam3d/sensor/pcl/MultiScanSensor.cpp @@ -150,25 +150,25 @@ MultiScanMeasurement::MultiScanMeasurement(const std::vectorgetPointCloud()); - } - } - return combined; -} - -const std::vector MultiScanMeasurement::getCloudsByAnnotation(const std::string &annotation) { - std::vector result; - for (unsigned int i = 0; i < clouds.size(); ++i) { - if (annotation == annotations[i]) { - result.push_back(clouds[i]->getPointCloud()); - } - } - return result; -} +// const PointCloud::Ptr MultiScanMeasurement::getCombinedPointCloud(const std::string& annotation) { +// PointCloud::Ptr combined(new PointCloud); +// for (unsigned int i = 0; i < clouds.size(); ++i) { +// if (annotation == "" || annotation == annotations[i]) { +// *combined += *(clouds[i]->getPointCloud()); +// } +// } +// return combined; +// } + +// const std::vector MultiScanMeasurement::getCloudsByAnnotation(const std::string &annotation) { +// std::vector result; +// for (unsigned int i = 0; i < clouds.size(); ++i) { +// if (annotation == annotations[i]) { +// result.push_back(clouds[i]->getPointCloud()); +// } +// } +// return result; +// } PointCloud::Ptr MultiScanSensor::transform(PointCloud::ConstPtr source, const Transform tf) const { PointCloud::Ptr transformedCloud(new PointCloud); @@ -180,17 +180,23 @@ PointCloud::Ptr MultiScanSensor::transform(PointCloud::ConstPtr source, const Tr PointCloud::Ptr MultiScanSensor::getAccumulatedCloud(const VertexObjectList& vertices) const { PointCloud::Ptr accu(new PointCloud); - for (VertexObjectList::const_reverse_iterator it = vertices.rbegin(); it != vertices.rend(); it++) { - Measurement::Ptr m = mMapper->getGraph()->getMeasurement(it->measurementUuid); - MultiScanMeasurement::Ptr pcl = boost::dynamic_pointer_cast(m); - if (!pcl) { - mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a MultiScanMeasurement cloud!"); - throw slam3d::BadMeasurementType(); - } - PointCloud::Ptr tempCloud = this->transform(pcl->getCombinedPointCloud(), (it->correctedPose * pcl->getSensorPose())); - *accu += *tempCloud; - } - return accu; + #pragma omp parallel for + for (size_t i = 0; i < vertices.size(); ++i) + { + Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertices[i].measurementUuid); + PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast(m); + if(!pcl) + { + mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!"); + throw BadMeasurementType(); + } + + PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), (vertices[i].correctedPose)); + + #pragma omp critical + *accu += *tempCloud; + } + return accu; } Measurement::Ptr MultiScanSensor::createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const { diff --git a/slam3d/sensor/pcl/MultiScanSensor.hpp b/slam3d/sensor/pcl/MultiScanSensor.hpp index 5418558..d6f5dfa 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.hpp +++ b/slam3d/sensor/pcl/MultiScanSensor.hpp @@ -29,10 +29,10 @@ class MultiScanMeasurement : public slam3d::PointCloudMeasurement { * * @return const PointCloud::Ptr */ - const PointCloud::Ptr getCombinedPointCloud(const std::string& annotation = ""); + // const PointCloud::Ptr getCombinedPointCloud(const std::string& annotation = ""); - const std::vector getCloudsByAnnotation(const std::string &annotation); + // const std::vector getCloudsByAnnotation(const std::string &annotation); // protected: From 6dcafae275aa79ada3a417ccf472cbe5d4526e16 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Mon, 15 Jun 2026 17:05:21 +0200 Subject: [PATCH 03/14] allow storing sub-measurements in VertexOjects --- slam3d/core/Graph.cpp | 4 +- slam3d/core/Graph.hpp | 2 +- slam3d/core/Mapper.cpp | 4 +- slam3d/core/Mapper.hpp | 2 +- slam3d/core/ScanSensor.cpp | 8 +- slam3d/core/ScanSensor.hpp | 4 +- slam3d/core/Types.hpp | 56 ++++++-- slam3d/sensor/pcl/MultiScanSensor.cpp | 117 ++++++++-------- slam3d/sensor/pcl/MultiScanSensor.hpp | 194 +++++++++++++------------- slam3d/serialization/YamlTypes.hpp | 35 +++++ 10 files changed, 244 insertions(+), 182 deletions(-) diff --git a/slam3d/core/Graph.cpp b/slam3d/core/Graph.cpp index da686a2..c14c0aa 100644 --- a/slam3d/core/Graph.cpp +++ b/slam3d/core/Graph.cpp @@ -158,12 +158,12 @@ bool Graph::optimized() } } -IdType Graph::addVertex(Measurement::Ptr m, const Transform &corrected) +IdType Graph::addVertex(Measurement::Ptr m, const Transform &corrected, const std::vector submeasurements) { // Create the new VertexObject and add it to the PoseGraph IdType id = mIndexer.getNext(); VertexObject vo; - vo.init(m, id); + vo.init(m, id, submeasurements); vo.correctedPose = corrected; vo.fixed = mFixNext; mFixNext = false; diff --git a/slam3d/core/Graph.hpp b/slam3d/core/Graph.hpp index ced3b4f..c97726f 100644 --- a/slam3d/core/Graph.hpp +++ b/slam3d/core/Graph.hpp @@ -243,7 +243,7 @@ namespace slam3d * @param m measurement * @param corrected initial pose for the new vertex */ - IdType addVertex(Measurement::Ptr m, const Transform &corrected); + IdType addVertex(Measurement::Ptr m, const Transform &corrected, const std::vector submeasurements = std::vector()); /** * @brief Add a placeholder constraint diff --git a/slam3d/core/Mapper.cpp b/slam3d/core/Mapper.cpp index 9ee98ed..f07731b 100644 --- a/slam3d/core/Mapper.cpp +++ b/slam3d/core/Mapper.cpp @@ -81,12 +81,12 @@ Transform Mapper::getCurrentPose() return mStartPose; } -IdType Mapper::addMeasurement(Measurement::Ptr m) +IdType Mapper::addMeasurement(Measurement::Ptr m, const std::vector submeasurements) { const bool first = mLastIndex == 0; // Add the vertex to the pose graph mLogger->message(DEBUG, (boost::format("Add reading from own Sensor '%1%'.") % m->getSensorName()).str()); - mLastIndex = mGraph->addVertex(m, getCurrentPose()); + mLastIndex = mGraph->addVertex(m, getCurrentPose(), submeasurements); // Call all registered PoseSensor's on the new vertex for(PoseSensorList::iterator ps = mPoseSensors.begin(); ps != mPoseSensors.end(); ps++) diff --git a/slam3d/core/Mapper.hpp b/slam3d/core/Mapper.hpp index c76feb3..6abcd8e 100644 --- a/slam3d/core/Mapper.hpp +++ b/slam3d/core/Mapper.hpp @@ -75,7 +75,7 @@ namespace slam3d * @param m pointer to a new measurement * @return id of the newly added vertex */ - IdType addMeasurement(Measurement::Ptr m); + IdType addMeasurement(Measurement::Ptr m, const std::vector submeasurements = std::vector()); /** * @brief Add a new measurement from another robot. diff --git a/slam3d/core/ScanSensor.cpp b/slam3d/core/ScanSensor.cpp index 9a0494a..c02c44f 100644 --- a/slam3d/core/ScanSensor.cpp +++ b/slam3d/core/ScanSensor.cpp @@ -46,11 +46,11 @@ ScanSensor::~ScanSensor() { } -bool ScanSensor::addMeasurement(const Measurement::Ptr& m) +bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const std::vector submeasurements) { if(mLastVertex == 0) { - mLastVertex = mMapper->addMeasurement(m); + mLastVertex = mMapper->addMeasurement(m, submeasurements); return true; } @@ -91,11 +91,11 @@ bool ScanSensor::checkMeasurementDistance(const Transform& odom) return false; } -bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom) +bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom, const std::vector submeasurements) { if(mLastVertex == 0) { - mLastVertex = mMapper->addMeasurement(m); + mLastVertex = mMapper->addMeasurement(m, submeasurements); mLastOdometry = odom; return true; } diff --git a/slam3d/core/ScanSensor.hpp b/slam3d/core/ScanSensor.hpp index afd5a15..d2ba32b 100644 --- a/slam3d/core/ScanSensor.hpp +++ b/slam3d/core/ScanSensor.hpp @@ -81,14 +81,14 @@ namespace slam3d * @brief Add a new measurement from this sensor. * @param scan */ - bool addMeasurement(const Measurement::Ptr& scan); + bool addMeasurement(const Measurement::Ptr& scan, const std::vector submeasurements = std::vector()); /** * @brief Add a new measurement from this sensor together with an odometry pose. * @param scan * @param odom */ - bool addMeasurement(const Measurement::Ptr& scan, const Transform& odom); + bool addMeasurement(const Measurement::Ptr& scan, const Transform& odom, const std::vector submeasurements = std::vector()); /** * @brief Check if a new measurement with given odometry would be added. diff --git a/slam3d/core/Types.hpp b/slam3d/core/Types.hpp index 8081e57..b75fc91 100644 --- a/slam3d/core/Types.hpp +++ b/slam3d/core/Types.hpp @@ -124,6 +124,12 @@ namespace slam3d virtual const char* getTypeName() const = 0; + std::vector getTags() { return tags; } + void addTag(const std::string &tag) + { + tags.push_back(tag); + } + protected: timeval mStamp; std::string mRobotName; @@ -132,6 +138,7 @@ namespace slam3d Transform mSensorPose; Transform mInverseSensorPose; + std::vector tags; }; enum ConstraintType {TENTATIVE, SE3, GRAVITY, POSITION, ORIENTATION, POSE}; @@ -296,38 +303,57 @@ namespace slam3d const char* getTypeName() { return "Tentative"; } }; - /** - * @struct VertexObject - * @brief Object attached to a vertex in the pose graph. - * @details It contains a pointer to an abstract measurement, which could - * be anything, e.g. a range scan, point cloud or image. - */ - struct VertexObject + + struct VertexData { - void init(const Measurement::Ptr m, IdType i) + void init(const Measurement::Ptr m, const IdType i) { index = i; - robotName = m->getRobotName(); sensorName = m->getSensorName(); typeName = m->getTypeName(); timestamp = m->getTimestamp(); measurementUuid = m->getUniqueId(); - + tags = m->getTags(); + robotName = m->getRobotName(); boost::format frm("%1%:%2%(%3%)"); frm % robotName % sensorName % index; label = frm.str(); } - - std::string label; IdType index; + timeval timestamp; + std::string label; std::string robotName; std::string sensorName; std::string typeName; - timeval timestamp; - bool fixed = false; + + boost::uuids::uuid measurementUuid; + std::vector tags; + + }; + + /** + * @struct VertexObject + * @brief Object attached to a vertex in the pose graph. + * @details It contains a pointer to an abstract measurement, which could + * be anything, e.g. a range scan, point cloud or image. + */ + struct VertexObject : public VertexData + { + void init(const Measurement::Ptr m, IdType i, const std::vector subMeasurements = std::vector()) + { + VertexData::init(m, i); + for (const auto &sub : subMeasurements) { + VertexData svd; + svd.init(sub, index); + subVertices.push_back(svd); + } + } + + bool fixed = false; Transform correctedPose; - boost::uuids::uuid measurementUuid; + + std::vector subVertices; }; /** diff --git a/slam3d/sensor/pcl/MultiScanSensor.cpp b/slam3d/sensor/pcl/MultiScanSensor.cpp index fd60549..5b8f926 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.cpp +++ b/slam3d/sensor/pcl/MultiScanSensor.cpp @@ -84,8 +84,8 @@ Transform doNDT(PointCloud::Ptr source, return Transform(tf_matrix); } -Transform align(MultiScanMeasurement::Ptr source, - MultiScanMeasurement::Ptr target, +Transform align(PointCloudMeasurement::Ptr source, + PointCloudMeasurement::Ptr target, const Transform& guess, const RegistrationParameters& config) { @@ -129,25 +129,25 @@ Transform align(MultiScanMeasurement::Ptr source, } -MultiScanMeasurement::MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, const boost::uuids::uuid id) : PointCloudMeasurement(std::make_shared(),r, s, slam3d::Transform::Identity(), id), clouds(clouds) { +// MultiScanMeasurement::MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, const boost::uuids::uuid id) : PointCloudMeasurement(std::make_shared(),r, s, slam3d::Transform::Identity(), id), clouds(clouds) { - // todo make this transform if frames differ - for (const auto& subcloud : clouds) { - PointCloud::Ptr tempCloud; - if (!subcloud->getSensorPose().isApprox(slam3d::Transform::Identity())) { - tempCloud = PointCloud::Ptr(new PointCloud); - pcl::transformPointCloud(*subcloud->getPointCloud(), *tempCloud, subcloud->getSensorPose().matrix()); - } else { - tempCloud = subcloud->getPointCloud(); - } - *mPointCloud.get() += *tempCloud.get(); - } +// // todo make this transform if frames differ +// for (const auto& subcloud : clouds) { +// PointCloud::Ptr tempCloud; +// if (!subcloud->getSensorPose().isApprox(slam3d::Transform::Identity())) { +// tempCloud = PointCloud::Ptr(new PointCloud); +// pcl::transformPointCloud(*subcloud->getPointCloud(), *tempCloud, subcloud->getSensorPose().matrix()); +// } else { +// tempCloud = subcloud->getPointCloud(); +// } +// *mPointCloud.get() += *tempCloud.get(); +// } - mPointCloud->header = clouds[0]->getPointCloud()->header; - mStamp.tv_sec = clouds[0]->getPointCloud()->header.stamp / 1000000; - mStamp.tv_usec = clouds[0]->getPointCloud()->header.stamp % 1000000; -} +// mPointCloud->header = clouds[0]->getPointCloud()->header; +// mStamp.tv_sec = clouds[0]->getPointCloud()->header.stamp / 1000000; +// mStamp.tv_usec = clouds[0]->getPointCloud()->header.stamp % 1000000; +// } // const PointCloud::Ptr MultiScanMeasurement::getCombinedPointCloud(const std::string& annotation) { @@ -208,8 +208,11 @@ Measurement::Ptr MultiScanSensor::createCombinedMeasurement(const VertexObjectLi std::vector clouds; PointCloudMeasurement::Ptr pcm (new PointCloudMeasurement(shifted, "AccumulatedPointcloud", mName, Transform::Identity())); clouds.push_back(pcm); - Measurement::Ptr m(new MultiScanMeasurement(clouds, "AccumulatedPointcloud", mName)); - return m; + + + + // Measurement::Ptr m(new PointCloudMeasurement(clouds, "AccumulatedPointcloud", mName, Transform::Identity())); + return pcm; } @@ -223,10 +226,10 @@ Constraint::Ptr MultiScanSensor::createConstraint(const Measurement::Ptr& source Transform guess = source->getInverseSensorPose() * odometry * target->getSensorPose(); // Cast to this sensors measurement type - MultiScanMeasurement::Ptr sourceCloud = boost::dynamic_pointer_cast(source); - MultiScanMeasurement::Ptr targetCloud = boost::dynamic_pointer_cast(target); + PointCloudMeasurement::Ptr sourceCloud = boost::dynamic_pointer_cast(source); + PointCloudMeasurement::Ptr targetCloud = boost::dynamic_pointer_cast(target); if (!sourceCloud || !targetCloud) { - mLogger->message(ERROR, "Measurement given to createConstraint() is not a MultiScanMeasurement!"); + mLogger->message(ERROR, "Measurement given to createConstraint() is not a PointCloudMeasurement!"); throw BadMeasurementType(); } @@ -247,43 +250,41 @@ Constraint::Ptr MultiScanSensor::createConstraint(const Measurement::Ptr& source } +// void MultiScanSensor::setRegistrationParameters(const RegistrationParameters& conf, bool coarse) +// { +// if(coarse) +// { +// mLogger->message(INFO, " = RegistrationParameters (Coarse) ="); +// mCoarseConfiguration = conf; +// }else +// { +// mLogger->message(INFO, " = RegistrationParameters (Fine) ="); +// mFineConfiguration = conf; +// } +// mLogger->message(INFO, (boost::format("correspondence_randomness: %1%") % conf.correspondence_randomness).str()); +// mLogger->message(INFO, (boost::format("euclidean_fitness_epsilon: %1%") % conf.euclidean_fitness_epsilon).str()); +// mLogger->message(INFO, (boost::format("max_correspondence_distance: %1%") % conf.max_correspondence_distance).str()); +// mLogger->message(INFO, (boost::format("max_fitness_score: %1%") % conf.max_fitness_score).str()); +// mLogger->message(INFO, (boost::format("maximum_iterations: %1%") % conf.maximum_iterations).str()); +// mLogger->message(INFO, (boost::format("maximum_optimizer_iterations: %1%") % conf.maximum_optimizer_iterations).str()); +// mLogger->message(INFO, (boost::format("point_cloud_density: %1%") % conf.point_cloud_density).str()); +// mLogger->message(INFO, (boost::format("rotation_epsilon: %1%") % conf.rotation_epsilon).str()); +// mLogger->message(INFO, (boost::format("transformation_epsilon: %1%") % conf.transformation_epsilon).str()); +// } +// void MultiScanSensor::setMapResolution(double r) +// { +// mLogger->message(INFO, (boost::format("map_resolution: %1%") % r).str()); +// mMapResolution = r; +// } -void MultiScanSensor::setRegistrationParameters(const RegistrationParameters& conf, bool coarse) -{ - if(coarse) - { - mLogger->message(INFO, " = RegistrationParameters (Coarse) ="); - mCoarseConfiguration = conf; - }else - { - mLogger->message(INFO, " = RegistrationParameters (Fine) ="); - mFineConfiguration = conf; - } - mLogger->message(INFO, (boost::format("correspondence_randomness: %1%") % conf.correspondence_randomness).str()); - mLogger->message(INFO, (boost::format("euclidean_fitness_epsilon: %1%") % conf.euclidean_fitness_epsilon).str()); - mLogger->message(INFO, (boost::format("max_correspondence_distance: %1%") % conf.max_correspondence_distance).str()); - mLogger->message(INFO, (boost::format("max_fitness_score: %1%") % conf.max_fitness_score).str()); - mLogger->message(INFO, (boost::format("maximum_iterations: %1%") % conf.maximum_iterations).str()); - mLogger->message(INFO, (boost::format("maximum_optimizer_iterations: %1%") % conf.maximum_optimizer_iterations).str()); - mLogger->message(INFO, (boost::format("point_cloud_density: %1%") % conf.point_cloud_density).str()); - mLogger->message(INFO, (boost::format("rotation_epsilon: %1%") % conf.rotation_epsilon).str()); - mLogger->message(INFO, (boost::format("transformation_epsilon: %1%") % conf.transformation_epsilon).str()); -} - -void MultiScanSensor::setMapResolution(double r) -{ - mLogger->message(INFO, (boost::format("map_resolution: %1%") % r).str()); - mMapResolution = r; -} - -void MultiScanSensor::setMapOutlierRemoval(double r, unsigned n) -{ - mLogger->message(INFO, (boost::format("map_outlier_radius: %1%") % r).str()); - mLogger->message(INFO, (boost::format("map_outlier_neighbors: %1%") % n).str()); - mMapOutlierRadius = r; - mMapOutlierNeighbors = n; -} +// void MultiScanSensor::setMapOutlierRemoval(double r, unsigned n) +// { +// mLogger->message(INFO, (boost::format("map_outlier_radius: %1%") % r).str()); +// mLogger->message(INFO, (boost::format("map_outlier_neighbors: %1%") % n).str()); +// mMapOutlierRadius = r; +// mMapOutlierNeighbors = n; +// } } // namespace slam3d diff --git a/slam3d/sensor/pcl/MultiScanSensor.hpp b/slam3d/sensor/pcl/MultiScanSensor.hpp index d6f5dfa..f3cf9ed 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.hpp +++ b/slam3d/sensor/pcl/MultiScanSensor.hpp @@ -13,54 +13,54 @@ namespace slam3d { -class MultiScanMeasurement : public slam3d::PointCloudMeasurement { - public: - typedef boost::shared_ptr Ptr; +// class MultiScanMeasurement : public slam3d::PointCloudMeasurement { +// public: +// typedef boost::shared_ptr Ptr; - MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, - const boost::uuids::uuid id = boost::uuids::nil_uuid()); +// MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, +// const boost::uuids::uuid id = boost::uuids::nil_uuid()); - const char* getTypeName() const override { return "slam3d::MultiScanMeasurement"; } +// const char* getTypeName() const override { return "slam3d::MultiScanMeasurement"; } - /** - * @brief returns the combined cloud (makes it compatible with PointCloudSensor functions) - * - * @return const PointCloud::Ptr - */ - // const PointCloud::Ptr getCombinedPointCloud(const std::string& annotation = ""); +// /** +// * @brief returns the combined cloud (makes it compatible with PointCloudSensor functions) +// * +// * @return const PointCloud::Ptr +// */ +// // const PointCloud::Ptr getCombinedPointCloud(const std::string& annotation = ""); - // const std::vector getCloudsByAnnotation(const std::string &annotation); +// // const std::vector getCloudsByAnnotation(const std::string &annotation); -// protected: - friend class boost::serialization::access; - std::vector clouds; +// // protected: +// friend class boost::serialization::access; +// std::vector clouds; - std::vector annotations; +// std::vector annotations; - std::map cloudByUuid; +// std::map cloudByUuid; - private: - friend class boost::serialization::access; - template - void serialize(Archive &ar, const unsigned int version) - { - // Tell boost::serialization that this is derived from Measurement. - // It is required because we don't explicitely call Measurement::serialize() - // from within PointCloudMeasurement::serialize(). +// private: +// friend class boost::serialization::access; +// template +// void serialize(Archive &ar, const unsigned int version) +// { +// // Tell boost::serialization that this is derived from Measurement. +// // It is required because we don't explicitely call Measurement::serialize() +// // from within PointCloudMeasurement::serialize(). - boost::serialization::void_cast_register( - static_cast(NULL), - static_cast(NULL)); - } +// boost::serialization::void_cast_register( +// static_cast(NULL), +// static_cast(NULL)); +// } - // TODO serialize +// // TODO serialize - }; +// }; class MultiScanSensor : public slam3d::ScanSensor { public: @@ -68,28 +68,28 @@ class MultiScanSensor : public slam3d::ScanSensor { ~MultiScanSensor() {}; - /** - * @brief Sets parameters for the internal pointcloud registration. - * The standard set is always used to calculate the final transformation. - * A coarse set is used to initialize and verify loop-closures. - * @param param new configuration paramerters - * @param coarse whether to set coarse parameter set - */ - void setRegistrationParameters(const RegistrationParameters& param, bool coarse); + // /** + // * @brief Sets parameters for the internal pointcloud registration. + // * The standard set is always used to calculate the final transformation. + // * A coarse set is used to initialize and verify loop-closures. + // * @param param new configuration paramerters + // * @param coarse whether to set coarse parameter set + // */ + // void setRegistrationParameters(const RegistrationParameters& param, bool coarse); - /** - * @brief Set density of points in accumulated map cloud. - * @param r - */ - void setMapResolution(double r); + // /** + // * @brief Set density of points in accumulated map cloud. + // * @param r + // */ + // void setMapResolution(double r); - /** - * @brief Set parameters for outlier removal. - * @details An outlier is a point that has less then n neighbors within radius r. - * @param r - * @param n - */ - void setMapOutlierRemoval(double r, unsigned n); + // /** + // * @brief Set parameters for outlier removal. + // * @details An outlier is a point that has less then n neighbors within radius r. + // * @param r + // * @param n + // */ + // void setMapOutlierRemoval(double r, unsigned n); /** @@ -146,49 +146,49 @@ class MultiScanSensor : public slam3d::ScanSensor { } // namespace slam3d -namespace boost { -namespace serialization { - - template - inline void save_construct_data(Archive & ar, const slam3d::MultiScanMeasurement * m, const unsigned int file_version) - { - // save data required to construct instance - ar << m->clouds; - ar << m->annotations; - ar << m->cloudByUuid; - ar << m->getRobotName(); - ar << m->getSensorName(); - ar << m->getSensorPose(); - ar << m->getUniqueId(); - } - - template - inline void load_construct_data(Archive & ar, slam3d::MultiScanMeasurement * t, const unsigned int file_version) - { - // retrieve data from archive required to construct new instance +// namespace boost { +// namespace serialization { + +// template +// inline void save_construct_data(Archive & ar, const slam3d::MultiScanMeasurement * m, const unsigned int file_version) +// { +// // save data required to construct instance +// ar << m->clouds; +// ar << m->annotations; +// ar << m->cloudByUuid; +// ar << m->getRobotName(); +// ar << m->getSensorName(); +// ar << m->getSensorPose(); +// ar << m->getUniqueId(); +// } + +// template +// inline void load_construct_data(Archive & ar, slam3d::MultiScanMeasurement * t, const unsigned int file_version) +// { +// // retrieve data from archive required to construct new instance - std::vector clouds; - std::vector annotations; - std::map cloudByUuid; - - std::string robot; - std::string sensor; - slam3d::Transform pose; - boost::uuids::uuid id; - ar >> clouds; - ar >> annotations; - ar >> cloudByUuid; - - ar >> robot; - ar >> sensor; - ar >> pose; - ar >> id; - - // invoke inplace constructor to initialize instance of PointCloudMeasurement - ::new(t)slam3d::MultiScanMeasurement(clouds, robot, sensor, id); - } - -} // namespace serialization -} // namespace boost - -BOOST_CLASS_EXPORT_KEY(slam3d::MultiScanMeasurement) +// std::vector clouds; +// std::vector annotations; +// std::map cloudByUuid; + +// std::string robot; +// std::string sensor; +// slam3d::Transform pose; +// boost::uuids::uuid id; +// ar >> clouds; +// ar >> annotations; +// ar >> cloudByUuid; + +// ar >> robot; +// ar >> sensor; +// ar >> pose; +// ar >> id; + +// // invoke inplace constructor to initialize instance of PointCloudMeasurement +// ::new(t)slam3d::MultiScanMeasurement(clouds, robot, sensor, id); +// } + +// } // namespace serialization +// } // namespace boost + +// BOOST_CLASS_EXPORT_KEY(slam3d::MultiScanMeasurement) diff --git a/slam3d/serialization/YamlTypes.hpp b/slam3d/serialization/YamlTypes.hpp index a256cfb..f642aa6 100644 --- a/slam3d/serialization/YamlTypes.hpp +++ b/slam3d/serialization/YamlTypes.hpp @@ -253,6 +253,37 @@ namespace YAML { } }; + template<> struct convert + { + static bool decode(const Node& node, slam3d::VertexData& config) + { + checkAndSet(&config.index, node["index"]); + checkAndSet(&config.timestamp, node["timestamp"]); + checkAndSet(&config.label, node["label"]); + checkAndSet(&config.robotName, node["robotName"]); + checkAndSet(&config.sensorName, node["sensorName"]); + checkAndSet(&config.typeName, node["typeName"]); + checkAndSet(&config.measurementUuid,node["measurementUuid"]); + checkAndSet(&config.tags,node["tags"]); + + return true; + } + static Node encode(const slam3d::VertexData& config) + { + Node node; + node["index"] = config.index; + node["timestamp"] = config.timestamp; + node["label"] = config.label; + node["robotName"] = config.robotName; + node["sensorName"] = config.sensorName; + node["typeName"] = config.typeName; + node["tags"] = config.tags; + + return node; + } + }; + + template<> struct convert { static bool decode(const Node& node, slam3d::VertexObject& config) @@ -266,6 +297,8 @@ namespace YAML { checkAndSet(&config.correctedPose, node["correctedPose"]); checkAndSet(&config.fixed, node["fixed"]); checkAndSet(&config.measurementUuid,node["measurementUuid"]); + checkAndSet(&config.tags,node["tags"]); + checkAndSet(&config.subVertices,node["subVertices"]); return true; } @@ -281,6 +314,8 @@ namespace YAML { node["measurementUuid"] = config.measurementUuid; node["correctedPose"] = config.correctedPose; node["fixed"] = config.fixed; + node["tags"] = config.tags; + node["subVertices"] = config.subVertices; return node; } }; From 76232d276c2f095c8ad3f1d0f5a97e0a8b71802d Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Wed, 17 Jun 2026 13:38:38 +0200 Subject: [PATCH 04/14] rename VertexData to VertexMeasurementData --- slam3d/core/Types.hpp | 19 ++++++++++--------- slam3d/serialization/YamlTypes.hpp | 10 +++++----- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/slam3d/core/Types.hpp b/slam3d/core/Types.hpp index b75fc91..865f95e 100644 --- a/slam3d/core/Types.hpp +++ b/slam3d/core/Types.hpp @@ -304,7 +304,7 @@ namespace slam3d }; - struct VertexData + struct VertexMeasurementData { void init(const Measurement::Ptr m, const IdType i) { @@ -337,23 +337,24 @@ namespace slam3d * @details It contains a pointer to an abstract measurement, which could * be anything, e.g. a range scan, point cloud or image. */ - struct VertexObject : public VertexData + struct VertexObject : public VertexMeasurementData { - void init(const Measurement::Ptr m, IdType i, const std::vector subMeasurements = std::vector()) + void init(const Measurement::Ptr m, IdType i, const std::vector subMeasurementPtrs = std::vector()) { - VertexData::init(m, i); + VertexMeasurementData::init(m, i); - for (const auto &sub : subMeasurements) { - VertexData svd; - svd.init(sub, index); - subVertices.push_back(svd); + for (const auto &sub : subMeasurementPtrs) { + printf("%s:%i\n", __PRETTY_FUNCTION__, __LINE__); + VertexMeasurementData vmd; + vmd.init(sub, index); + subMeasurements.push_back(vmd); } } bool fixed = false; Transform correctedPose; - std::vector subVertices; + std::vector subMeasurements; }; /** diff --git a/slam3d/serialization/YamlTypes.hpp b/slam3d/serialization/YamlTypes.hpp index f642aa6..09c9192 100644 --- a/slam3d/serialization/YamlTypes.hpp +++ b/slam3d/serialization/YamlTypes.hpp @@ -253,9 +253,9 @@ namespace YAML { } }; - template<> struct convert + template<> struct convert { - static bool decode(const Node& node, slam3d::VertexData& config) + static bool decode(const Node& node, slam3d::VertexMeasurementData& config) { checkAndSet(&config.index, node["index"]); checkAndSet(&config.timestamp, node["timestamp"]); @@ -268,7 +268,7 @@ namespace YAML { return true; } - static Node encode(const slam3d::VertexData& config) + static Node encode(const slam3d::VertexMeasurementData& config) { Node node; node["index"] = config.index; @@ -298,7 +298,7 @@ namespace YAML { checkAndSet(&config.fixed, node["fixed"]); checkAndSet(&config.measurementUuid,node["measurementUuid"]); checkAndSet(&config.tags,node["tags"]); - checkAndSet(&config.subVertices,node["subVertices"]); + checkAndSet(&config.subMeasurements,node["subMeasurements"]); return true; } @@ -315,7 +315,7 @@ namespace YAML { node["correctedPose"] = config.correctedPose; node["fixed"] = config.fixed; node["tags"] = config.tags; - node["subVertices"] = config.subVertices; + node["subMeasurements"] = config.subMeasurements; return node; } }; From 457713f1d1729b484399b9f2ce051f4530559454 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Wed, 17 Jun 2026 13:38:53 +0200 Subject: [PATCH 05/14] fix multi-measurement vertices --- slam3d/core/ScanSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/slam3d/core/ScanSensor.cpp b/slam3d/core/ScanSensor.cpp index c02c44f..9564a9d 100644 --- a/slam3d/core/ScanSensor.cpp +++ b/slam3d/core/ScanSensor.cpp @@ -104,7 +104,7 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom mLastTransform = mLastOdometry.inverse() * odom; if(checkMinDistance(mLastTransform)) { - IdType newVertex = mMapper->addMeasurement(m); + IdType newVertex = mMapper->addMeasurement(m, submeasurements); Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex); if(mLinkPrevious) { From f8875bde39b834496be1bbba40741ce032254ca4 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Fri, 19 Jun 2026 12:33:26 +0200 Subject: [PATCH 06/14] make sure cloud exist in storage when adding the vertex --- slam3d/core/Graph.cpp | 6 +++++- slam3d/sensor/pcl/MultiScanSensor.cpp | 3 --- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/slam3d/core/Graph.cpp b/slam3d/core/Graph.cpp index c14c0aa..7ef3eba 100644 --- a/slam3d/core/Graph.cpp +++ b/slam3d/core/Graph.cpp @@ -160,6 +160,11 @@ bool Graph::optimized() IdType Graph::addVertex(Measurement::Ptr m, const Transform &corrected, const std::vector submeasurements) { + // add Measurement to storage first to make sure is is available when the VertexObject enters the Graph + mStorage->add(m); + for (const auto& sub: submeasurements) { + mStorage->add(sub); + } // Create the new VertexObject and add it to the PoseGraph IdType id = mIndexer.getNext(); VertexObject vo; @@ -168,7 +173,6 @@ IdType Graph::addVertex(Measurement::Ptr m, const Transform &corrected, const st vo.fixed = mFixNext; mFixNext = false; addVertex(vo); - mStorage->add(m); mLogger->message(INFO, (boost::format("Created vertex %1% (from %2%:%3%).") % id % m->getRobotName() % m->getSensorName()).str()); // Add it to the uuid-index, so we can find it by its uuid diff --git a/slam3d/sensor/pcl/MultiScanSensor.cpp b/slam3d/sensor/pcl/MultiScanSensor.cpp index 5b8f926..5c434cf 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.cpp +++ b/slam3d/sensor/pcl/MultiScanSensor.cpp @@ -205,10 +205,7 @@ Measurement::Ptr MultiScanSensor::createCombinedMeasurement(const VertexObjectLi PointCloud::Ptr shifted(new PointCloud); pcl::transformPointCloud(*cloud, *shifted, pose.inverse().matrix()); mLogger->message(DEBUG, (boost::format("Patch MultiScanMeasurement has %1% points.") % cloud->size()).str()); - std::vector clouds; PointCloudMeasurement::Ptr pcm (new PointCloudMeasurement(shifted, "AccumulatedPointcloud", mName, Transform::Identity())); - clouds.push_back(pcm); - // Measurement::Ptr m(new PointCloudMeasurement(clouds, "AccumulatedPointcloud", mName, Transform::Identity())); From 08f43923495c938db6738b0e1eee6bfc3028249f Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Fri, 10 Jul 2026 16:34:16 +0200 Subject: [PATCH 07/14] add function to get all tags from graph --- slam3d/core/Graph.cpp | 18 ++++++++++++++++++ slam3d/core/Graph.hpp | 7 +++++++ 2 files changed, 25 insertions(+) diff --git a/slam3d/core/Graph.cpp b/slam3d/core/Graph.cpp index 7ef3eba..2ad6aa3 100644 --- a/slam3d/core/Graph.cpp +++ b/slam3d/core/Graph.cpp @@ -279,3 +279,21 @@ const VertexObjectList Graph::getNearbyVertices(const Transform &tf, float radiu mLogger->message(DEBUG, (boost::format("Neighbor search found %1% vertices nearby.") % result.size()).str()); return result; } + +std::set Graph::getTags(const StringSet& sensors) const +{ + std::set tags; + VertexObjectList allVertices = getVertices(sensors); + + for (const auto& vertex : allVertices) { + for (const auto& tag : vertex.tags) { + tags.insert(tag); + } + for (const auto& sub : vertex.subMeasurements) { + for (const auto tag : sub.tags) { + tags.insert(tag); + } + } + } + return tags; +} diff --git a/slam3d/core/Graph.hpp b/slam3d/core/Graph.hpp index c97726f..5cd1709 100644 --- a/slam3d/core/Graph.hpp +++ b/slam3d/core/Graph.hpp @@ -78,6 +78,7 @@ laser->addMeasurement(m); #include #include +#include namespace slam3d { @@ -448,6 +449,12 @@ namespace slam3d */ virtual float calculateGraphDistance(IdType source, IdType target) const = 0; + /** + * @brief Get a list of tags within the graph by traversing all vertices. + * Graph implemetations may implement a faster solution + */ + virtual std::set getTags(const StringSet& sensors = {}) const; + protected: // Graph access /** From 9718a6406b4f73573c9f35f744c9daa9b6a47d62 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Fri, 10 Jul 2026 16:34:50 +0200 Subject: [PATCH 08/14] rename MultiScanMeasurement to MultiPointCloudSensor --- slam3d/sensor/pcl/CMakeLists.txt | 4 +- ...anSensor.cpp => MultiPointCloudSensor.cpp} | 103 +++++++++++++++--- ...anSensor.hpp => MultiPointCloudSensor.hpp} | 15 ++- 3 files changed, 98 insertions(+), 24 deletions(-) rename slam3d/sensor/pcl/{MultiScanSensor.cpp => MultiPointCloudSensor.cpp} (76%) rename slam3d/sensor/pcl/{MultiScanSensor.hpp => MultiPointCloudSensor.hpp} (91%) diff --git a/slam3d/sensor/pcl/CMakeLists.txt b/slam3d/sensor/pcl/CMakeLists.txt index c17a096..2eb631f 100644 --- a/slam3d/sensor/pcl/CMakeLists.txt +++ b/slam3d/sensor/pcl/CMakeLists.txt @@ -1,6 +1,6 @@ add_library(sensor-pcl SHARED PointCloudSensor.cpp - MultiScanSensor.cpp + MultiPointCloudSensor.cpp ) target_include_directories(sensor-pcl @@ -33,7 +33,7 @@ target_link_libraries(sensor-pcl install( FILES PointCloudSensor.hpp - MultiScanSensor.hpp + MultiPointCloudSensor.hpp RegistrationParameters.hpp DESTINATION include/slam3d/sensor/pcl ) diff --git a/slam3d/sensor/pcl/MultiScanSensor.cpp b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp similarity index 76% rename from slam3d/sensor/pcl/MultiScanSensor.cpp rename to slam3d/sensor/pcl/MultiPointCloudSensor.cpp index 5c434cf..186becb 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.cpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp @@ -1,7 +1,10 @@ -#include "MultiScanSensor.hpp" +#include "MultiPointCloudSensor.hpp" #include +#include +#include + #include @@ -170,36 +173,102 @@ Transform align(PointCloudMeasurement::Ptr source, // return result; // } -PointCloud::Ptr MultiScanSensor::transform(PointCloud::ConstPtr source, const Transform tf) const { +PointCloud::Ptr MultiPointCloudSensor::transform(PointCloud::ConstPtr source, const Transform tf) const { PointCloud::Ptr transformedCloud(new PointCloud); pcl::transformPointCloud(*source, *transformedCloud, tf.matrix()); return transformedCloud; } -PointCloud::Ptr MultiScanSensor::getAccumulatedCloud(const VertexObjectList& vertices) const { +namespace { + +// Returns true if any of the requested tags is contained in 'have'. +bool hasAnyTag(const std::vector& have, const std::vector& requested) +{ + for (const auto& tag : requested) + { + if (std::find(have.begin(), have.end(), tag) != have.end()) + { + return true; + } + } + return false; +} + +} // namespace + +PointCloud::Ptr MultiPointCloudSensor::getAccumulatedCloud(const VertexObjectList& vertices, + const std::vector& tags) const { PointCloud::Ptr accu(new PointCloud); + // Exceptions must not escape an OpenMP structured block, so record a + // bad-measurement error in a shared flag and throw after the parallel region. + bool badMeasurementType = false; + #pragma omp parallel for for (size_t i = 0; i < vertices.size(); ++i) { - Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertices[i].measurementUuid); - PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast(m); - if(!pcl) + // Once a bad measurement was seen, cheaply skip the remaining iterations + // (we cannot break out of an OpenMP for loop). + #pragma omp flush(badMeasurementType) + if (badMeasurementType) { - mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!"); - throw BadMeasurementType(); + continue; } - - PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), (vertices[i].correctedPose)); - #pragma omp critical - *accu += *tempCloud; + const VertexObject& vertex = vertices[i]; + + // Decide which measurements of this vertex to accumulate: + // - No tag filter, or the vertex itself carries a requested tag: + // use the vertex' own (combined) measurement, positioned by correctedPose. + // - Otherwise the vertex is ignored and the tags are searched in its + // subMeasurements; every matching subMeasurement is accumulated, + // positioned by correctedPose * its own sensor pose. + // - Main VertexObject has no measurement and no tags given: use combined subs + std::vector> selected; + Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertex.measurementUuid); + if (m && (tags.empty() || hasAnyTag(vertex.tags, tags))) + { + selected.emplace_back(m, vertex.correctedPose); + } + else + { + for (const auto& sub : vertex.subMeasurements) + { + if ((!m && tags.size() == 0)|| hasAnyTag(sub.tags, tags)) + { + Measurement::Ptr sm = mMapper->getGraph()->getMeasurement(sub.measurementUuid); + selected.emplace_back(sm, vertex.correctedPose * m->getSensorPose()); + } + } + } + + for (const auto& sel : selected) + { + PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast(sel.first); + if(!pcl) + { + mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!"); + #pragma omp atomic write + badMeasurementType = true; + break; + } + + PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), sel.second); + + #pragma omp critical + *accu += *tempCloud; + } + } + + if (badMeasurementType) + { + throw BadMeasurementType(); } return accu; } -Measurement::Ptr MultiScanSensor::createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const { +Measurement::Ptr MultiPointCloudSensor::createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const { printf("%s:%i\n", __PRETTY_FUNCTION__, __LINE__); PointCloud::Ptr cloud = getAccumulatedCloud(vertices); PointCloud::Ptr shifted(new PointCloud); @@ -213,7 +282,7 @@ Measurement::Ptr MultiScanSensor::createCombinedMeasurement(const VertexObjectLi } -Constraint::Ptr MultiScanSensor::createConstraint(const Measurement::Ptr& source, +Constraint::Ptr MultiPointCloudSensor::createConstraint(const Measurement::Ptr& source, const Measurement::Ptr& target, const Transform& odometry, bool loop) @@ -247,7 +316,7 @@ Constraint::Ptr MultiScanSensor::createConstraint(const Measurement::Ptr& source } -// void MultiScanSensor::setRegistrationParameters(const RegistrationParameters& conf, bool coarse) +// void MultiPointCloudSensor::setRegistrationParameters(const RegistrationParameters& conf, bool coarse) // { // if(coarse) // { @@ -269,13 +338,13 @@ Constraint::Ptr MultiScanSensor::createConstraint(const Measurement::Ptr& source // mLogger->message(INFO, (boost::format("transformation_epsilon: %1%") % conf.transformation_epsilon).str()); // } -// void MultiScanSensor::setMapResolution(double r) +// void MultiPointCloudSensor::setMapResolution(double r) // { // mLogger->message(INFO, (boost::format("map_resolution: %1%") % r).str()); // mMapResolution = r; // } -// void MultiScanSensor::setMapOutlierRemoval(double r, unsigned n) +// void MultiPointCloudSensor::setMapOutlierRemoval(double r, unsigned n) // { // mLogger->message(INFO, (boost::format("map_outlier_radius: %1%") % r).str()); // mLogger->message(INFO, (boost::format("map_outlier_neighbors: %1%") % n).str()); diff --git a/slam3d/sensor/pcl/MultiScanSensor.hpp b/slam3d/sensor/pcl/MultiPointCloudSensor.hpp similarity index 91% rename from slam3d/sensor/pcl/MultiScanSensor.hpp rename to slam3d/sensor/pcl/MultiPointCloudSensor.hpp index f3cf9ed..fa85c2a 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.hpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.hpp @@ -6,7 +6,7 @@ #include #include -#include +// #include #include @@ -62,10 +62,10 @@ namespace slam3d { // }; -class MultiScanSensor : public slam3d::ScanSensor { +class MultiPointCloudSensor : public slam3d::ScanSensor { public: - MultiScanSensor(const std::string& n, Logger* l): ScanSensor(n,l) {}; - ~MultiScanSensor() {}; + MultiPointCloudSensor(const std::string& n, Logger* l): ScanSensor(n,l) {}; + ~MultiPointCloudSensor() {}; // /** @@ -104,10 +104,15 @@ class MultiScanSensor : public slam3d::ScanSensor { * @details The individual point clouds are transformed by their current pose in the graph, * no additional alignement or optimization is performed during this. * @param vertices + * @param tags optional list of tags to filter by. If non-empty, only vertices + * carrying at least one of these tags are accumulated. Each tag is looked + * up on the VertexObject itself and, if not found there, in its + * subMeasurements. * @return accumulated pointcloud * @throw BadMeasurementType */ - PointCloud::Ptr getAccumulatedCloud(const VertexObjectList& vertices) const; + PointCloud::Ptr getAccumulatedCloud(const VertexObjectList& vertices, + const std::vector& tags = {}) const; /** From f51a6d57a85529fa89519d35e36f56704d70bead Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Fri, 10 Jul 2026 16:38:29 +0200 Subject: [PATCH 09/14] tag support for getAccumulatedCloud --- slam3d/sensor/pcl/MultiScanSensor.cpp | 89 ++++++++++++++++++++++++--- slam3d/sensor/pcl/MultiScanSensor.hpp | 9 ++- 2 files changed, 86 insertions(+), 12 deletions(-) diff --git a/slam3d/sensor/pcl/MultiScanSensor.cpp b/slam3d/sensor/pcl/MultiScanSensor.cpp index 5c434cf..319f1e1 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.cpp +++ b/slam3d/sensor/pcl/MultiScanSensor.cpp @@ -2,6 +2,9 @@ #include +#include +#include + #include @@ -177,24 +180,90 @@ PointCloud::Ptr MultiScanSensor::transform(PointCloud::ConstPtr source, const Tr } -PointCloud::Ptr MultiScanSensor::getAccumulatedCloud(const VertexObjectList& vertices) const { +namespace { + +// Returns true if any of the requested tags is contained in 'have'. +bool hasAnyTag(const std::vector& have, const std::vector& requested) +{ + for (const auto& tag : requested) + { + if (std::find(have.begin(), have.end(), tag) != have.end()) + { + return true; + } + } + return false; +} + +} // namespace + +PointCloud::Ptr MultiScanSensor::getAccumulatedCloud(const VertexObjectList& vertices, + const std::vector& tags) const { PointCloud::Ptr accu(new PointCloud); + // Exceptions must not escape an OpenMP structured block, so record a + // bad-measurement error in a shared flag and throw after the parallel region. + bool badMeasurementType = false; + #pragma omp parallel for for (size_t i = 0; i < vertices.size(); ++i) { - Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertices[i].measurementUuid); - PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast(m); - if(!pcl) + // Once a bad measurement was seen, cheaply skip the remaining iterations + // (we cannot break out of an OpenMP for loop). + #pragma omp flush(badMeasurementType) + if (badMeasurementType) { - mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!"); - throw BadMeasurementType(); + continue; } - - PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), (vertices[i].correctedPose)); - #pragma omp critical - *accu += *tempCloud; + const VertexObject& vertex = vertices[i]; + + // Decide which measurements of this vertex to accumulate: + // - No tag filter, or the vertex itself carries a requested tag: + // use the vertex' own (combined) measurement, positioned by correctedPose. + // - Otherwise the vertex is ignored and the tags are searched in its + // subMeasurements; every matching subMeasurement is accumulated, + // positioned by correctedPose * its own sensor pose. + // - Main VertexObject has no measurement and no tags given: use combined subs + std::vector> selected; + Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertex.measurementUuid); + if (m && (tags.empty() || hasAnyTag(vertex.tags, tags))) + { + selected.emplace_back(m, vertex.correctedPose); + } + else + { + for (const auto& sub : vertex.subMeasurements) + { + if ((!m && tags.size() == 0)|| hasAnyTag(sub.tags, tags)) + { + Measurement::Ptr sm = mMapper->getGraph()->getMeasurement(sub.measurementUuid); + selected.emplace_back(sm, vertex.correctedPose * m->getSensorPose()); + } + } + } + + for (const auto& sel : selected) + { + PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast(sel.first); + if(!pcl) + { + mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!"); + #pragma omp atomic write + badMeasurementType = true; + break; + } + + PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), sel.second); + + #pragma omp critical + *accu += *tempCloud; + } + } + + if (badMeasurementType) + { + throw BadMeasurementType(); } return accu; } diff --git a/slam3d/sensor/pcl/MultiScanSensor.hpp b/slam3d/sensor/pcl/MultiScanSensor.hpp index f3cf9ed..73b6b02 100644 --- a/slam3d/sensor/pcl/MultiScanSensor.hpp +++ b/slam3d/sensor/pcl/MultiScanSensor.hpp @@ -6,7 +6,7 @@ #include #include -#include +// #include #include @@ -104,10 +104,15 @@ class MultiScanSensor : public slam3d::ScanSensor { * @details The individual point clouds are transformed by their current pose in the graph, * no additional alignement or optimization is performed during this. * @param vertices + * @param tags optional list of tags to filter by. If non-empty, only vertices + * carrying at least one of these tags are accumulated. Each tag is looked + * up on the VertexObject itself and, if not found there, in its + * subMeasurements. * @return accumulated pointcloud * @throw BadMeasurementType */ - PointCloud::Ptr getAccumulatedCloud(const VertexObjectList& vertices) const; + PointCloud::Ptr getAccumulatedCloud(const VertexObjectList& vertices, + const std::vector& tags = {}) const; /** From 71a79ff14fac886361911d4991624a53f0f9900b Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Mon, 13 Jul 2026 09:43:51 +0200 Subject: [PATCH 10/14] cleanup --- slam3d/core/Graph.cpp | 15 --- slam3d/core/Types.hpp | 1 - slam3d/sensor/pcl/MultiPointCloudSensor.cpp | 79 ------------- slam3d/sensor/pcl/MultiPointCloudSensor.hpp | 121 -------------------- 4 files changed, 216 deletions(-) diff --git a/slam3d/core/Graph.cpp b/slam3d/core/Graph.cpp index 2ad6aa3..4291c6f 100644 --- a/slam3d/core/Graph.cpp +++ b/slam3d/core/Graph.cpp @@ -86,26 +86,11 @@ void Graph::reloadToSolver() // add all edges after vertices are defined for (const auto& edge : getEdges()) { - printf("%s:%i %li->%li (%s : %s)\n", __PRETTY_FUNCTION__, __LINE__, edge.source, edge.target, edge.constraint->getTypeName(), edge.constraint->getSensorName().c_str()); if (edge.constraint->getType() != TENTATIVE) { mSolver->addEdge(edge.source, edge.target, edge.constraint); } } - - // add all edges after vertices are defined - // for (const auto& vertex : vertices) - // { - // for (const auto& edge : getOutEdges(vertex.index)) - // { - // if (edge.constraint->getType() != TENTATIVE && edge.source == vertex.index) - // { - // printf("%s:%i %li->%li (%s : %s)\n", __PRETTY_FUNCTION__, __LINE__, edge.source, edge.target, edge.constraint->getTypeName(), edge.constraint->getSensorName().c_str()); - // mSolver->addEdge(edge.source, edge.target, edge.constraint); - // } - // } - // } - } void Graph::writeGraphToFile(const std::string &name) diff --git a/slam3d/core/Types.hpp b/slam3d/core/Types.hpp index 865f95e..710fd5c 100644 --- a/slam3d/core/Types.hpp +++ b/slam3d/core/Types.hpp @@ -344,7 +344,6 @@ namespace slam3d VertexMeasurementData::init(m, i); for (const auto &sub : subMeasurementPtrs) { - printf("%s:%i\n", __PRETTY_FUNCTION__, __LINE__); VertexMeasurementData vmd; vmd.init(sub, index); subMeasurements.push_back(vmd); diff --git a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp index 186becb..0dbc93b 100644 --- a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp @@ -131,48 +131,6 @@ Transform align(PointCloudMeasurement::Ptr source, } } - -// MultiScanMeasurement::MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, const boost::uuids::uuid id) : PointCloudMeasurement(std::make_shared(),r, s, slam3d::Transform::Identity(), id), clouds(clouds) { - - -// // todo make this transform if frames differ -// for (const auto& subcloud : clouds) { -// PointCloud::Ptr tempCloud; -// if (!subcloud->getSensorPose().isApprox(slam3d::Transform::Identity())) { -// tempCloud = PointCloud::Ptr(new PointCloud); -// pcl::transformPointCloud(*subcloud->getPointCloud(), *tempCloud, subcloud->getSensorPose().matrix()); -// } else { -// tempCloud = subcloud->getPointCloud(); -// } -// *mPointCloud.get() += *tempCloud.get(); -// } - -// mPointCloud->header = clouds[0]->getPointCloud()->header; -// mStamp.tv_sec = clouds[0]->getPointCloud()->header.stamp / 1000000; -// mStamp.tv_usec = clouds[0]->getPointCloud()->header.stamp % 1000000; -// } - - -// const PointCloud::Ptr MultiScanMeasurement::getCombinedPointCloud(const std::string& annotation) { -// PointCloud::Ptr combined(new PointCloud); -// for (unsigned int i = 0; i < clouds.size(); ++i) { -// if (annotation == "" || annotation == annotations[i]) { -// *combined += *(clouds[i]->getPointCloud()); -// } -// } -// return combined; -// } - -// const std::vector MultiScanMeasurement::getCloudsByAnnotation(const std::string &annotation) { -// std::vector result; -// for (unsigned int i = 0; i < clouds.size(); ++i) { -// if (annotation == annotations[i]) { -// result.push_back(clouds[i]->getPointCloud()); -// } -// } -// return result; -// } - PointCloud::Ptr MultiPointCloudSensor::transform(PointCloud::ConstPtr source, const Transform tf) const { PointCloud::Ptr transformedCloud(new PointCloud); pcl::transformPointCloud(*source, *transformedCloud, tf.matrix()); @@ -316,43 +274,6 @@ Constraint::Ptr MultiPointCloudSensor::createConstraint(const Measurement::Ptr& } -// void MultiPointCloudSensor::setRegistrationParameters(const RegistrationParameters& conf, bool coarse) -// { -// if(coarse) -// { -// mLogger->message(INFO, " = RegistrationParameters (Coarse) ="); -// mCoarseConfiguration = conf; -// }else -// { -// mLogger->message(INFO, " = RegistrationParameters (Fine) ="); -// mFineConfiguration = conf; -// } -// mLogger->message(INFO, (boost::format("correspondence_randomness: %1%") % conf.correspondence_randomness).str()); -// mLogger->message(INFO, (boost::format("euclidean_fitness_epsilon: %1%") % conf.euclidean_fitness_epsilon).str()); -// mLogger->message(INFO, (boost::format("max_correspondence_distance: %1%") % conf.max_correspondence_distance).str()); -// mLogger->message(INFO, (boost::format("max_fitness_score: %1%") % conf.max_fitness_score).str()); -// mLogger->message(INFO, (boost::format("maximum_iterations: %1%") % conf.maximum_iterations).str()); -// mLogger->message(INFO, (boost::format("maximum_optimizer_iterations: %1%") % conf.maximum_optimizer_iterations).str()); -// mLogger->message(INFO, (boost::format("point_cloud_density: %1%") % conf.point_cloud_density).str()); -// mLogger->message(INFO, (boost::format("rotation_epsilon: %1%") % conf.rotation_epsilon).str()); -// mLogger->message(INFO, (boost::format("transformation_epsilon: %1%") % conf.transformation_epsilon).str()); -// } - -// void MultiPointCloudSensor::setMapResolution(double r) -// { -// mLogger->message(INFO, (boost::format("map_resolution: %1%") % r).str()); -// mMapResolution = r; -// } - -// void MultiPointCloudSensor::setMapOutlierRemoval(double r, unsigned n) -// { -// mLogger->message(INFO, (boost::format("map_outlier_radius: %1%") % r).str()); -// mLogger->message(INFO, (boost::format("map_outlier_neighbors: %1%") % n).str()); -// mMapOutlierRadius = r; -// mMapOutlierNeighbors = n; -// } - - } // namespace slam3d diff --git a/slam3d/sensor/pcl/MultiPointCloudSensor.hpp b/slam3d/sensor/pcl/MultiPointCloudSensor.hpp index fa85c2a..e6e5ec6 100644 --- a/slam3d/sensor/pcl/MultiPointCloudSensor.hpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.hpp @@ -13,85 +13,11 @@ namespace slam3d { -// class MultiScanMeasurement : public slam3d::PointCloudMeasurement { -// public: -// typedef boost::shared_ptr Ptr; - - -// MultiScanMeasurement(const std::vector& clouds, const std::string& r, const std::string& s, -// const boost::uuids::uuid id = boost::uuids::nil_uuid()); - - -// const char* getTypeName() const override { return "slam3d::MultiScanMeasurement"; } - -// /** -// * @brief returns the combined cloud (makes it compatible with PointCloudSensor functions) -// * -// * @return const PointCloud::Ptr -// */ -// // const PointCloud::Ptr getCombinedPointCloud(const std::string& annotation = ""); - - -// // const std::vector getCloudsByAnnotation(const std::string &annotation); - - -// // protected: -// friend class boost::serialization::access; -// std::vector clouds; - -// std::vector annotations; - -// std::map cloudByUuid; - - -// private: -// friend class boost::serialization::access; -// template -// void serialize(Archive &ar, const unsigned int version) -// { -// // Tell boost::serialization that this is derived from Measurement. -// // It is required because we don't explicitely call Measurement::serialize() -// // from within PointCloudMeasurement::serialize(). - -// boost::serialization::void_cast_register( -// static_cast(NULL), -// static_cast(NULL)); -// } - -// // TODO serialize - -// }; - class MultiPointCloudSensor : public slam3d::ScanSensor { public: MultiPointCloudSensor(const std::string& n, Logger* l): ScanSensor(n,l) {}; ~MultiPointCloudSensor() {}; - - // /** - // * @brief Sets parameters for the internal pointcloud registration. - // * The standard set is always used to calculate the final transformation. - // * A coarse set is used to initialize and verify loop-closures. - // * @param param new configuration paramerters - // * @param coarse whether to set coarse parameter set - // */ - // void setRegistrationParameters(const RegistrationParameters& param, bool coarse); - - // /** - // * @brief Set density of points in accumulated map cloud. - // * @param r - // */ - // void setMapResolution(double r); - - // /** - // * @brief Set parameters for outlier removal. - // * @details An outlier is a point that has less then n neighbors within radius r. - // * @param r - // * @param n - // */ - // void setMapOutlierRemoval(double r, unsigned n); - - /** * @brief Transform source cloud by given transformation. * @param source @@ -150,50 +76,3 @@ class MultiPointCloudSensor : public slam3d::ScanSensor { }; } // namespace slam3d - -// namespace boost { -// namespace serialization { - -// template -// inline void save_construct_data(Archive & ar, const slam3d::MultiScanMeasurement * m, const unsigned int file_version) -// { -// // save data required to construct instance -// ar << m->clouds; -// ar << m->annotations; -// ar << m->cloudByUuid; -// ar << m->getRobotName(); -// ar << m->getSensorName(); -// ar << m->getSensorPose(); -// ar << m->getUniqueId(); -// } - -// template -// inline void load_construct_data(Archive & ar, slam3d::MultiScanMeasurement * t, const unsigned int file_version) -// { -// // retrieve data from archive required to construct new instance - -// std::vector clouds; -// std::vector annotations; -// std::map cloudByUuid; - -// std::string robot; -// std::string sensor; -// slam3d::Transform pose; -// boost::uuids::uuid id; -// ar >> clouds; -// ar >> annotations; -// ar >> cloudByUuid; - -// ar >> robot; -// ar >> sensor; -// ar >> pose; -// ar >> id; - -// // invoke inplace constructor to initialize instance of PointCloudMeasurement -// ::new(t)slam3d::MultiScanMeasurement(clouds, robot, sensor, id); -// } - -// } // namespace serialization -// } // namespace boost - -// BOOST_CLASS_EXPORT_KEY(slam3d::MultiScanMeasurement) From 422d10c6212431df84802266af747ce9bbedb7f1 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Mon, 13 Jul 2026 09:59:38 +0200 Subject: [PATCH 11/14] cleanup --- slam3d/sensor/pcl/MultiPointCloudSensor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp index 0dbc93b..dec480e 100644 --- a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp @@ -227,7 +227,6 @@ PointCloud::Ptr MultiPointCloudSensor::getAccumulatedCloud(const VertexObjectLis } Measurement::Ptr MultiPointCloudSensor::createCombinedMeasurement(const VertexObjectList& vertices, Transform pose) const { - printf("%s:%i\n", __PRETTY_FUNCTION__, __LINE__); PointCloud::Ptr cloud = getAccumulatedCloud(vertices); PointCloud::Ptr shifted(new PointCloud); pcl::transformPointCloud(*cloud, *shifted, pose.inverse().matrix()); From 2835f1e1b4cfc921db7701215e12617225eff2b4 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Mon, 13 Jul 2026 10:24:17 +0200 Subject: [PATCH 12/14] anonymous namespace for local functions --- slam3d/sensor/pcl/MultiPointCloudSensor.cpp | 4 +--- slam3d/sensor/pcl/PointCloudSensor.cpp | 4 ++++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp index dec480e..3f6da28 100644 --- a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp @@ -19,6 +19,7 @@ namespace slam3d { +namespace { template Transform doICP(PointCloud::Ptr source, @@ -137,9 +138,6 @@ PointCloud::Ptr MultiPointCloudSensor::transform(PointCloud::ConstPtr source, co return transformedCloud; } - -namespace { - // Returns true if any of the requested tags is contained in 'have'. bool hasAnyTag(const std::vector& have, const std::vector& requested) { diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp index b4791a6..244279f 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.cpp +++ b/slam3d/sensor/pcl/PointCloudSensor.cpp @@ -50,6 +50,8 @@ using namespace slam3d; +namespace { + template Transform doICP(PointCloud::Ptr source, PointCloud::Ptr target, @@ -174,6 +176,8 @@ Transform align(PointCloudMeasurement::Ptr source, return result; } +} + PointCloudSensor::PointCloudSensor(const std::string& n, Logger* l) : ScanSensor(n, l) { From d85a64d4064c18b1c990b57e0346be09ed826119 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Mon, 13 Jul 2026 10:45:57 +0200 Subject: [PATCH 13/14] revert namespace stuff --- slam3d/sensor/pcl/MultiPointCloudSensor.cpp | 4 +++- slam3d/sensor/pcl/PointCloudSensor.cpp | 4 ---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp index 3f6da28..dec480e 100644 --- a/slam3d/sensor/pcl/MultiPointCloudSensor.cpp +++ b/slam3d/sensor/pcl/MultiPointCloudSensor.cpp @@ -19,7 +19,6 @@ namespace slam3d { -namespace { template Transform doICP(PointCloud::Ptr source, @@ -138,6 +137,9 @@ PointCloud::Ptr MultiPointCloudSensor::transform(PointCloud::ConstPtr source, co return transformedCloud; } + +namespace { + // Returns true if any of the requested tags is contained in 'have'. bool hasAnyTag(const std::vector& have, const std::vector& requested) { diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp index 244279f..b4791a6 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.cpp +++ b/slam3d/sensor/pcl/PointCloudSensor.cpp @@ -50,8 +50,6 @@ using namespace slam3d; -namespace { - template Transform doICP(PointCloud::Ptr source, PointCloud::Ptr target, @@ -176,8 +174,6 @@ Transform align(PointCloudMeasurement::Ptr source, return result; } -} - PointCloudSensor::PointCloudSensor(const std::string& n, Logger* l) : ScanSensor(n, l) { From b53e3c628460a7d00d72a9800fde2649fea5c827 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Thu, 20 Aug 2026 18:30:41 +0200 Subject: [PATCH 14/14] allow a max range in registration --- slam3d/sensor/pcl/PointCloudSensor.cpp | 9 ++++++--- slam3d/sensor/pcl/PointCloudSensor.hpp | 2 +- slam3d/sensor/pcl/RegistrationParameters.hpp | 4 ++++ 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp index b4791a6..5c4158c 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.cpp +++ b/slam3d/sensor/pcl/PointCloudSensor.cpp @@ -127,8 +127,8 @@ Transform align(PointCloudMeasurement::Ptr source, PointCloud::Ptr filtered_target = target->getPointCloud(); if(config.point_cloud_density > 0) { - filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density); - filtered_target = PointCloudSensor::downsample(target->getPointCloud(), config.point_cloud_density); + filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density, config.point_cloud_coord_limit); + filtered_target = PointCloudSensor::downsample(target->getPointCloud(), config.point_cloud_density, config.point_cloud_coord_limit); } // Make sure that there are enough points left (ICP will crash if not) @@ -202,12 +202,15 @@ PointCloud::Ptr PointCloudSensor::crop(PointCloud::Ptr in, const Eigen::Vector4f } } -PointCloud::Ptr PointCloudSensor::downsample(PointCloud::Ptr in, double leaf_size) +PointCloud::Ptr PointCloudSensor::downsample(PointCloud::Ptr in, double leaf_size, double maxrange) { if(in->size() > 0 && leaf_size > 0) { PointCloud::Ptr out(new PointCloud); pcl::VoxelGrid grid; + if (maxrange>0) { + grid.setFilterLimits(-maxrange,maxrange); + } grid.setLeafSize (leaf_size, leaf_size, leaf_size); grid.setInputCloud(in); grid.filter(*out); diff --git a/slam3d/sensor/pcl/PointCloudSensor.hpp b/slam3d/sensor/pcl/PointCloudSensor.hpp index 87be09c..44edb85 100644 --- a/slam3d/sensor/pcl/PointCloudSensor.hpp +++ b/slam3d/sensor/pcl/PointCloudSensor.hpp @@ -179,7 +179,7 @@ namespace slam3d * @param source * @param resolution */ - static PointCloud::Ptr downsample(PointCloud::Ptr source, double resolution); + static PointCloud::Ptr downsample(PointCloud::Ptr source, double resolution, double maxrange = -1.0); /** * @brief Crop the source cloud to a square box with given corners. diff --git a/slam3d/sensor/pcl/RegistrationParameters.hpp b/slam3d/sensor/pcl/RegistrationParameters.hpp index 867a57b..376e72d 100644 --- a/slam3d/sensor/pcl/RegistrationParameters.hpp +++ b/slam3d/sensor/pcl/RegistrationParameters.hpp @@ -44,6 +44,10 @@ namespace slam3d // pointclouds will be downsampled to this density before the alignement double point_cloud_density = 0.2; + // point with a coordinage higher than point_cloud_coord_limit or lower than + // -point_cloud_coord_limit are ignored, only active when point_cloud_density > 0 + double point_cloud_coord_limit = 80; + // maximum fitness score (e.g., sum of squared distances from the source to the target) // to accept the registration result double max_fitness_score = 2.0;