Add Multi-Measurement support in a single vertex

slam3d/core/Graph.cpp

@@ -84,14 +84,11 @@ 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))
+		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);
 		}
 	}
 }
@@ -146,17 +143,21 @@ bool Graph::optimized()
 	}
 }
 
-IdType Graph::addVertex(Measurement::Ptr m, const Transform &corrected)
+IdType Graph::addVertex(Measurement::Ptr m, const Transform &corrected, const std::vector<Measurement::Ptr> 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;
-	vo.init(m, id);
+	vo.init(m, id, submeasurements);
 	vo.correctedPose = corrected;
 	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
@@ -263,3 +264,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<std::string> Graph::getTags(const StringSet& sensors) const
+{
+	std::set<std::string> 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;
+}

slam3d/core/Graph.hpp

@@ -78,6 +78,7 @@ laser->addMeasurement(m);
 #include <slam3d/core/MeasurementStorage.hpp>
 
 #include <map>
+#include <set>
 
 namespace slam3d
 {
@@ -243,7 +244,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<Measurement::Ptr> submeasurements = std::vector<Measurement::Ptr>());
 
 		/**
 		 * @brief Add a placeholder constraint
@@ -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<std::string> getTags(const StringSet& sensors = {}) const;
+
 	protected:
 		// Graph access
 		/**

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<Measurement::Ptr> 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++)

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<Measurement::Ptr> submeasurements = std::vector<Measurement::Ptr>());
 
 		/**
 		 * @brief Add a new measurement from another robot.

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<Measurement::Ptr> 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<Measurement::Ptr> submeasurements)
 {
 	if(mLastVertex == 0)
 	{
-		mLastVertex = mMapper->addMeasurement(m);
+		mLastVertex = mMapper->addMeasurement(m, submeasurements);
 		mLastOdometry = odom;
 		return true;
 	}
@@ -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)
 		{

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<Measurement::Ptr> submeasurements = std::vector<Measurement::Ptr>());
 
 		/**
 		 * @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<Measurement::Ptr> submeasurements = std::vector<Measurement::Ptr>());
 
 		/**
 		 * @brief Check if a new measurement with given odometry would be added.

slam3d/core/Types.hpp

@@ -124,6 +124,12 @@ namespace slam3d
 
 		virtual const char* getTypeName() const = 0;
 
+		std::vector<std::string> 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<std::string> 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 VertexMeasurementData
 	{
-		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<std::string> 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 VertexMeasurementData
+	{
+		void init(const Measurement::Ptr m, IdType i, const std::vector<Measurement::Ptr> subMeasurementPtrs = std::vector<Measurement::Ptr>())
+		{
+			VertexMeasurementData::init(m, i);
 
+			for (const auto &sub : subMeasurementPtrs) {
+				VertexMeasurementData vmd;
+				vmd.init(sub, index);
+				subMeasurements.push_back(vmd);
+			}
+		}
+
+		bool fixed = false;
 		Transform correctedPose;
-		boost::uuids::uuid measurementUuid;
+
+		std::vector<VertexMeasurementData> subMeasurements;
 	};
 
 	/**

slam3d/sensor/pcl/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_library(sensor-pcl SHARED
 	PointCloudSensor.cpp
+	MultiPointCloudSensor.cpp
 )
 
 target_include_directories(sensor-pcl
@@ -32,6 +33,7 @@ target_link_libraries(sensor-pcl
 install(
 	FILES
 		PointCloudSensor.hpp
+		MultiPointCloudSensor.hpp
 		RegistrationParameters.hpp
 	DESTINATION include/slam3d/sensor/pcl
 )