fixed runtime errors for single depth camera and stereo

corelib/include/rtabmap/core/CameraModel.h

@@ -137,7 +137,7 @@ public:
 			double baseline,
 			const Transform & localTransform = Transform::getIdentity()) :
 		left_(fx, fy, cx, cy, localTransform),
-		right_(fx, fy, cx, cy, localTransform, baseline*-right_.fx())
+		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
 	{
 	}
 	virtual ~StereoCameraModel() {}

corelib/include/rtabmap/core/OdometryEvent.h

@@ -38,6 +38,20 @@ namespace rtabmap {
 
 class OdometryEvent : public UEvent
 {
+public:
+	static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance)
+	{
+		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
+		UASSERT(uIsFinite(transVariance) && transVariance>0);
+		cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+		covariance.at<double>(0,0) = transVariance;
+		covariance.at<double>(1,1) = transVariance;
+		covariance.at<double>(2,2) = transVariance;
+		covariance.at<double>(3,3) = rotVariance;
+		covariance.at<double>(4,4) = rotVariance;
+		covariance.at<double>(5,5) = rotVariance;
+		return covariance;
+	}
 public:
 	OdometryEvent() :
 		_covariance(cv::Mat::eye(6,6,CV_64FC1))
@@ -69,22 +83,13 @@ public:
 		const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
 			_pose(pose),
-			_covariance(cv::Mat::eye(6,6,CV_64FC1)),
+			_covariance(generateCovarianceMatrix(rotVariance, transVariance)),
 			_info(info)
 	{
-		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-		UASSERT(uIsFinite(transVariance) && transVariance>0);
-		_covariance.at<double>(0,0) = transVariance;
-		_covariance.at<double>(1,1) = transVariance;
-		_covariance.at<double>(2,2) = transVariance;
-		_covariance.at<double>(3,3) = rotVariance;
-		_covariance.at<double>(4,4) = rotVariance;
-		_covariance.at<double>(5,5) = rotVariance;
 	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
-	bool isValid() const {return !_pose.isNull();}
 	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
 	const Transform & pose() const {return _pose;}

corelib/include/rtabmap/core/Rtabmap.h

@@ -159,7 +159,7 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	bool _publishLastSignature;
+	bool _publishLastSignatureData;
 	bool _publishPdf;
 	bool _publishLikelihood;
 	float _maxTimeAllowed; // in ms

corelib/include/rtabmap/core/SensorData.h

@@ -167,7 +167,7 @@ public:
 	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
-	StereoCameraModel stereoCameraModel() const {return _stereoCameraModel;}
+	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
 
 	void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
 	const std::vector<unsigned char> & userData() const {return _userData;}

corelib/include/rtabmap/core/Signature.h

@@ -53,12 +53,10 @@ class RTABMAP_EXP Signature
 public:
 	Signature();
 	Signature(int id,
-			int mapId,
+			int mapId = -1,
-			int weight,
+			int weight = 0,
-			double stamp,
+			double stamp = 0.0,
-			const std::string & label,
+			const std::string & label = std::string(),
-			const std::multimap<int, cv::KeyPoint> & words,
-			const std::multimap<int, pcl::PointXYZ> & words3,
 			const Transform & pose = Transform(),
 			const std::vector<unsigned char> & userData = std::vector<unsigned char>(),
 			const SensorData & sensorData = SensorData());
@@ -141,7 +139,7 @@ private:
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
 	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
-	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
+	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
 

corelib/include/rtabmap/core/Statistics.h

@@ -136,11 +136,7 @@ public:
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
 	void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
 
-	void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
+	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
-	void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
-	void setStamps(const std::map<int, double> & stamps) {_stamps = stamps;}
-	void setUserDatas(const std::map<int, std::vector<unsigned char> > & userDatas) {_userDatas = userDatas;}
-	void setSignature(const Signature & s) {_signature = s;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -159,11 +155,7 @@ public:
 	int loopClosureId() const {return _loopClosureId;}
 	int localLoopClosureId() const {return _localLoopClosureId;}
 
-	const std::map<int, int> & getMapIds() const {return _mapIds;}
+	const std::map<int, Signature> & getSignatures() const {return _signatures;}
-	const std::map<int, std::string> & getLabels() const {return _labels;}
-	const std::map<int, double> & getStamps() const {return _stamps;}
-	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
-	const Signature & getSignature() const {return _signature;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -185,14 +177,7 @@ private:
 	int _loopClosureId;
 	int _localLoopClosureId;
 
-	// extended data start here...
+	std::map<int, Signature> _signatures;
-	std::map<int, int> _mapIds;
-	std::map<int, std::string> _labels;
-	std::map<int, double> _stamps;
-	std::map<int, std::vector<unsigned char> > _userDatas;
-
-	// Signature data
-	Signature _signature;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;

corelib/include/rtabmap/core/Transform.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
@@ -46,25 +47,27 @@ public:
 	Transform(float r11, float r12, float r13, float o14,
 		      float r21, float r22, float r23, float o24,
 			  float r31, float r32, float r33, float o34);
+	// should have 3 rows, 4 cols and type CV_32FC1
+	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
 
-	float r11() const {return data_[0];}
+	float r11() const {return data()[0];}
-	float r12() const {return data_[1];}
+	float r12() const {return data()[1];}
-	float r13() const {return data_[2];}
+	float r13() const {return data()[2];}
-	float r21() const {return data_[4];}
+	float r21() const {return data()[4];}
-	float r22() const {return data_[5];}
+	float r22() const {return data()[5];}
-	float r23() const {return data_[6];}
+	float r23() const {return data()[6];}
-	float r31() const {return data_[8];}
+	float r31() const {return data()[8];}
-	float r32() const {return data_[9];}
+	float r32() const {return data()[9];}
-	float r33() const {return data_[10];}
+	float r33() const {return data()[10];}
 
-	float o14() const {return data_[3];}
+	float o14() const {return data()[3];}
-	float o24() const {return data_[7];}
+	float o24() const {return data()[7];}
-	float o34() const {return data_[11];}
+	float o34() const {return data()[11];}
 
-	float & operator[](int index) {return data_[index];}
+	float & operator[](int index) {return data()[index];}
-	const float & operator[](int index) const {return data_[index];}
+	const float & operator[](int index) const {return data()[index];}
 
 	bool isNull() const;
 	bool isIdentity() const;
@@ -72,16 +75,16 @@ public:
 	void setNull();
 	void setIdentity();
 
-	const float * data() const {return data_.data();}
+	const float * data() const {return (const float *)data_.data;}
-	float * data() {return data_.data();}
+	float * data() {return (float *)data_.data;}
-	int size() const {return (int)data_.size();}
+	int size() const {return 12;}
 
-	float & x() {return data_[3];}
+	float & x() {return data()[3];}
-	float & y() {return data_[7];}
+	float & y() {return data()[7];}
-	float & z() {return data_[11];}
+	float & z() {return data()[11];}
-	const float & x() const {return data_[3];}
+	const float & x() const {return data()[3];}
-	const float & y() const {return data_[7];}
+	const float & y() const {return data()[7];}
-	const float & z() const {return data_[11];}
+	const float & z() const {return data()[11];}
 
 	float theta() const;
 
@@ -121,7 +124,7 @@ public:
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
 
 private:
-	std::vector<float> data_;
+	cv::Mat data_;
 };
 
 RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);

corelib/src/DBDriverSqlite3.cpp

@@ -1349,8 +1349,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						weight,
 						stamp,
 						label,
-						std::multimap<int, cv::KeyPoint>(),
-						std::multimap<int, pcl::PointXYZ>(),
 						pose,
 						userData);
 				s->setSaved(true);

corelib/src/DBReader.cpp

@@ -149,7 +149,7 @@ void DBReader::mainLoopBegin()
 void DBReader::mainLoop()
 {
 	OdometryEvent odom = this->getNextData();
-	if(odom.isValid())
+	if(odom.data().id())
 	{
 		int goalId = 0;
 		double previousStamp = odom.data().stamp();

corelib/src/Memory.cpp

@@ -4027,8 +4027,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			0,
 			data.stamp(),
 			"",
-			words,
-			words3D,
 			pose,
 			data.userData(),
 			stereoCameraModel.isValid()?
@@ -4054,8 +4052,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			0,
 			data.stamp(),
 			"",
-			words,
-			words3D,
 			pose,
 			data.userData(),
 			SensorData(
@@ -4063,6 +4059,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				data.laserScanMaxPts(),
 				cv::Mat(), cv::Mat(), CameraModel(), id));
 	}
+	s->setWords(words);
+	s->setWords3(words3D);
 	if(this->isRawDataKept())
 	{
 		s->sensorData().setImageRaw(image);

corelib/src/Rtabmap.cpp

@@ -73,7 +73,7 @@ namespace rtabmap
 
 Rtabmap::Rtabmap() :
 	_publishStats(Parameters::defaultRtabmapPublishStats()),
-	_publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()),
+	_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
 	_publishPdf(Parameters::defaultRtabmapPublishPdf()),
 	_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
 	_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
@@ -364,7 +364,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 
 	Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
-	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
+	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
 	Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
@@ -2023,44 +2023,6 @@ bool Rtabmap::process(
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
 
-			// Set local graph
-			if(!_rgbdSlamMode)
-			{
-				// no optimization on appearance-only mode, create a local graph
-				std::map<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
-				std::map<int, Transform> poses;
-				std::map<int, int> mapIds;
-				std::map<int, std::string> labels;
-				std::map<int, double> stamps;
-				std::map<int, std::vector<unsigned char> > userDatas;
-				std::multimap<int, Link> constraints;
-				_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
-				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					Transform odomPose;
-					int weight = -1;
-					int mapId = -1;
-					std::string label;
-					double stamp = 0;
-					std::vector<unsigned char> userData;
-					_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
-					mapIds.insert(std::make_pair(iter->first, mapId));
-					labels.insert(std::make_pair(iter->first, label));
-					stamps.insert(std::make_pair(iter->first, stamp));
-					userDatas.insert(std::make_pair(iter->first, userData));
-				}
-				statistics_.setPoses(poses);
-				statistics_.setConstraints(constraints);
-				statistics_.setMapIds(mapIds);
-				statistics_.setLabels(labels);
-				statistics_.setStamps(stamps);
-				statistics_.setUserDatas(userDatas);
-			}
-			else // RGBD-SLAM mode
-			{
-				//see after transfer below
-			}
-
 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
 			statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
@@ -2084,11 +2046,6 @@ bool Rtabmap::process(
 			//Epipolar geometry constraint
 			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 
-			if(_publishLastSignature)
-			{
-				statistics_.setSignature(*signature);
-			}
-
 			if(_publishLikelihood || _publishPdf)
 			{
 				// Child count by parent signature on the root of the memory ... for statistics
@@ -2146,6 +2103,12 @@ bool Rtabmap::process(
 		_memory->deleteLocation(signature->id());
 	}
 
+	Signature lastSignatureData(signature->id());
+	if(_publishLastSignatureData)
+	{
+		lastSignatureData = *signature;
+	}
+
 	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;
 
@@ -2231,15 +2194,27 @@ bool Rtabmap::process(
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
 		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
-		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size());
 
-		if(_rgbdSlamMode)
+		std::map<int, Signature> signatures;
+		if(_publishLastSignatureData)
 		{
-			std::map<int, int> mapIds;
+			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
-			std::map<int, std::string> labels;
+		}
-			std::map<int, double> stamps;
+		// Set local graph
-			std::map<int, std::vector<unsigned char> > userDatas;
+		std::map<int, Transform> poses;
-			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+		std::multimap<int, Link> constraints;
+		if(!_rgbdSlamMode)
+		{
+			// no optimization on appearance-only mode, create a local graph
+			std::map<int, int> ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
+		}
+		else // RGBD-SLAM mode
+		{
+			poses = _optimizedPoses;
+			constraints = _constraints;
+		}
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			Transform odomPose;
 			int weight = -1;
@@ -2247,20 +2222,20 @@ bool Rtabmap::process(
 			std::string label;
 			double stamp = 0;
 			std::vector<unsigned char> userData;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
-				mapIds.insert(std::make_pair(iter->first, mapId));
+			signatures.insert(std::make_pair(iter->first,
-				labels.insert(std::make_pair(iter->first, label));
+					Signature(iter->first,
-				stamps.insert(std::make_pair(iter->first, stamp));
+							mapId,
-				userDatas.insert(std::make_pair(iter->first, userData));
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData)));
 		}
-			statistics_.setPoses(_optimizedPoses);
+		statistics_.setPoses(poses);
-			statistics_.setConstraints(_constraints);
+		statistics_.setConstraints(constraints);
-			statistics_.setMapIds(mapIds);
+		statistics_.setSignatures(signatures);
-			statistics_.setLabels(labels);
+		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
-			statistics_.setStamps(stamps);
-			statistics_.setUserDatas(userDatas);
-		}
-
 	}
 
 	//Start trashing
@@ -2780,8 +2755,6 @@ void Rtabmap::get3DMap(
 							weight,
 							stamp,
 							label,
-							std::multimap<int, cv::KeyPoint>(),
-							std::multimap<int, pcl::PointXYZ>(),
 							odomPose,
 							userData,
 							data)));
@@ -2842,11 +2815,8 @@ void Rtabmap::getGraph(
 							weight,
 							stamp,
 							label,
-							std::multimap<int, cv::KeyPoint>(),
-							std::multimap<int, pcl::PointXYZ>(),
 							odomPose,
-							userData,
+							userData)));
-							SensorData())));
 			}
 		}
 	}

corelib/src/RtabmapThread.cpp

@@ -295,7 +295,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 	{
 		UDEBUG("OdometryEvent");
 		OdometryEvent * e = (OdometryEvent*)event;
-		if(e->isValid())
+		if(!e->pose().isNull())
 		{
 			this->addData(*e);
 		}
@@ -476,7 +476,7 @@ void RtabmapThread::process()
 {
 	OdometryEvent data;
 	getData(data);
-	if(data.isValid() && _state.empty())
+	if(data.data().isValid() && _state.empty())
 	{
 		if(_rtabmap->getMemory())
 		{
@@ -499,12 +499,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
-		if(!odomEvent.isValid())
-		{
-			ULOGGER_ERROR("data not valid !?");
-			return;
-		}
-
 		if(_rate>0.0f)
 		{
 			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
@@ -552,6 +546,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			{
 				_transVariance = 1.0;
 			}
+			UDEBUG("Added data %d", odomEvent.data().id());
 			_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
 			_rotVariance = 0;
 			_transVariance = 0;

corelib/src/SensorData.cpp

@@ -169,15 +169,15 @@ SensorData::SensorData(
 				depth.type() == CV_16UC1); // Depth in millimetre
 		_depthOrRightRaw = depth;
 	}
-	if(laserScan.rows == 1)
+
+	if(laserScan.type() == CV_32FC2)
 	{
-		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanRaw = laserScan;
-		_laserScanCompressed = laserScan;
 	}
 	else if(!laserScan.empty())
 	{
-		UASSERT(laserScan.type() == CV_32FC2);
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
-		_laserScanRaw = laserScan;
+		_laserScanCompressed = laserScan;
 	}
 }
 
@@ -262,15 +262,14 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.rows == 1)
+	if(laserScan.type() == CV_32FC2)
 	{
-		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanRaw = laserScan;
-		_laserScanCompressed = laserScan;
 	}
 	else if(!laserScan.empty())
 	{
-		UASSERT(laserScan.type() == CV_32FC2);
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
-		_laserScanRaw = laserScan;
+		_laserScanCompressed = laserScan;
 	}
 
 	for(unsigned int i=0; i<cameraModels.size(); ++i)
@@ -355,15 +354,14 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}
 
-	if(laserScan.rows == 1)
+	if(laserScan.type() == CV_32FC2)
 	{
-		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanRaw = laserScan;
-		_laserScanCompressed = laserScan;
 	}
 	else if(!laserScan.empty())
 	{
-		UASSERT(laserScan.type() == CV_32FC2);
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
-		_laserScanRaw = laserScan;
+		_laserScanCompressed = laserScan;
 	}
 }
 

corelib/src/Signature.cpp

@@ -39,8 +39,7 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
-	_stamp(0.0),
+	_weight(0),
-	_weight(-1),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
@@ -54,11 +53,9 @@ Signature::Signature(
 		int weight,
 		double stamp,
 		const std::string & label,
-		const std::multimap<int, cv::KeyPoint> & words,
-		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
 		const std::vector<unsigned char> & userData,
-		const SensorData & sensorData) :
+		const SensorData & sensorData):
 	_id(id),
 	_mapId(mapId),
 	_stamp(stamp),
@@ -68,12 +65,15 @@ Signature::Signature(
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_words(words),
-	_words3(words3),
 	_enabled(false),
 	_pose(pose),
 	_sensorData(sensorData)
 {
+	if(_sensorData.id() == 0)
+	{
+		_sensorData.setId(id);
+	}
+	UASSERT(_sensorData.id() == _id);
 }
 
 Signature::~Signature()

corelib/src/Transform.cpp

@@ -31,44 +31,33 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
 #include <iomanip>
 
 namespace rtabmap {
 
-Transform::Transform() : data_(12)
+Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1))
 {
-	data_[0] = 0.0f;
-	data_[1] = 0.0f;
-	data_[2] = 0.0f;
-	data_[3] = 0.0f;
-	data_[4] = 0.0f;
-	data_[5] = 0.0f;
-	data_[6] = 0.0f;
-	data_[7] = 0.0f;
-	data_[8] = 0.0f;
-	data_[9] = 0.0f;
-	data_[10] = 0.0f;
-	data_[11] = 0.0f;
 }
 
 // rotation matrix r## and origin o##
-Transform::Transform(float r11, float r12, float r13, float o14,
+Transform::Transform(
+		float r11, float r12, float r13, float o14,
 		float r21, float r22, float r23, float o24,
-				     float r31, float r32, float r33, float o34) :
+		float r31, float r32, float r33, float o34)
-	data_(12)
 {
-	data_[0] = r11;
+	data_ = (cv::Mat_<float>(3,4) <<
-	data_[1] = r12;
+			r11, r12, r13, o14,
-	data_[2] = r13;
+			r21, r22, r23, o24,
-	data_[3] = o14;
+			r31, r32, r33, o34);
-	data_[4] = r21;
+}
-	data_[5] = r22;
+
-	data_[6] = r23;
+Transform::Transform(const cv::Mat & transformationMatrix)
-	data_[7] = o24;
+{
-	data_[8] = r31;
+	UASSERT(transformationMatrix.cols == 4 &&
-	data_[9] = r32;
+			transformationMatrix.rows == 3 &&
-	data_[10] = r33;
+			transformationMatrix.type() == CV_32FC1);
-	data_[11] = o34;
+	data_ = transformationMatrix;
 }
 
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
@@ -79,46 +68,46 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 
 bool Transform::isNull() const
 {
-	return (data_[0] == 0.0f &&
+	return (data()[0] == 0.0f &&
-			data_[1] == 0.0f &&
+			data()[1] == 0.0f &&
-			data_[2] == 0.0f &&
+			data()[2] == 0.0f &&
-			data_[3] == 0.0f &&
+			data()[3] == 0.0f &&
-			data_[4] == 0.0f &&
+			data()[4] == 0.0f &&
-			data_[5] == 0.0f &&
+			data()[5] == 0.0f &&
-			data_[6] == 0.0f &&
+			data()[6] == 0.0f &&
-			data_[7] == 0.0f &&
+			data()[7] == 0.0f &&
-			data_[8] == 0.0f &&
+			data()[8] == 0.0f &&
-			data_[9] == 0.0f &&
+			data()[9] == 0.0f &&
-			data_[10] == 0.0f &&
+			data()[10] == 0.0f &&
-			data_[11] == 0.0f) ||
+			data()[11] == 0.0f) ||
-			uIsNan(data_[0]) ||
+			uIsNan(data()[0]) ||
-			uIsNan(data_[1]) ||
+			uIsNan(data()[1]) ||
-			uIsNan(data_[2]) ||
+			uIsNan(data()[2]) ||
-			uIsNan(data_[3]) ||
+			uIsNan(data()[3]) ||
-			uIsNan(data_[4]) ||
+			uIsNan(data()[4]) ||
-			uIsNan(data_[5]) ||
+			uIsNan(data()[5]) ||
-			uIsNan(data_[6]) ||
+			uIsNan(data()[6]) ||
-			uIsNan(data_[7]) ||
+			uIsNan(data()[7]) ||
-			uIsNan(data_[8]) ||
+			uIsNan(data()[8]) ||
-			uIsNan(data_[9]) ||
+			uIsNan(data()[9]) ||
-			uIsNan(data_[10]) ||
+			uIsNan(data()[10]) ||
-			uIsNan(data_[11]);
+			uIsNan(data()[11]);
 }
 
 bool Transform::isIdentity() const
 {
-	return data_[0] == 1.0f &&
+	return data()[0] == 1.0f &&
-			data_[1] == 0.0f &&
+			data()[1] == 0.0f &&
-			data_[2] == 0.0f &&
+			data()[2] == 0.0f &&
-			data_[3] == 0.0f &&
+			data()[3] == 0.0f &&
-			data_[4] == 0.0f &&
+			data()[4] == 0.0f &&
-			data_[5] == 1.0f &&
+			data()[5] == 1.0f &&
-			data_[6] == 0.0f &&
+			data()[6] == 0.0f &&
-			data_[7] == 0.0f &&
+			data()[7] == 0.0f &&
-			data_[8] == 0.0f &&
+			data()[8] == 0.0f &&
-			data_[9] == 0.0f &&
+			data()[9] == 0.0f &&
-			data_[10] == 1.0f &&
+			data()[10] == 1.0f &&
-			data_[11] == 0.0f;
+			data()[11] == 0.0f;
 }
 
 void Transform::setNull()
@@ -145,16 +134,17 @@ Transform Transform::inverse() const
 
 Transform Transform::rotation() const
 {
-	return Transform(data_[0], data_[1], data_[2], 0,
+	return Transform(
-					 data_[4], data_[5], data_[6], 0,
+			data()[0], data()[1], data()[2], 0,
-					 data_[8], data_[9], data_[10], 0);
+			data()[4], data()[5], data()[6], 0,
+			data()[8], data()[9], data()[10], 0);
 }
 
 Transform Transform::translation() const
 {
-	return Transform(1,0,0, data_[3],
+	return Transform(1,0,0, data()[3],
-					 0,1,0, data_[7],
+					 0,1,0, data()[7],
-					 0,0,1, data_[11]);
+					 0,0,1, data()[11]);
 }
 
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
@@ -215,7 +205,7 @@ Transform & Transform::operator*=(const Transform & t)
 
 bool Transform::operator==(const Transform & t) const
 {
-	return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
+	return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0;
 }
 
 bool Transform::operator!=(const Transform & t) const
@@ -239,18 +229,18 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 Eigen::Matrix4f Transform::toEigen4f() const
 {
 	Eigen::Matrix4f m;
-	m << data_[0], data_[1], data_[2], data_[3],
+	m << data()[0], data()[1], data()[2], data()[3],
-		 data_[4], data_[5], data_[6], data_[7],
+		 data()[4], data()[5], data()[6], data()[7],
-		 data_[8], data_[9], data_[10], data_[11],
+		 data()[8], data()[9], data()[10], data()[11],
 		 0,0,0,1;
 	return m;
 }
 Eigen::Matrix4d Transform::toEigen4d() const
 {
 	Eigen::Matrix4d m;
-	m << data_[0], data_[1], data_[2], data_[3],
+	m << data()[0], data()[1], data()[2], data()[3],
-		 data_[4], data_[5], data_[6], data_[7],
+		 data()[4], data()[5], data()[6], data()[7],
-		 data_[8], data_[9], data_[10], data_[11],
+		 data()[8], data()[9], data()[10], data()[11],
 		 0,0,0,1;
 	return m;
 }

corelib/src/util3d.cpp

@@ -569,7 +569,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		{
 			leftMono = sensorData.imageRaw();
 		}
-		return cloudFromDisparity(
+		cloud = cloudFromDisparity(
 				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
 				sensorData.stereoCameraModel().left().cx(),
 				sensorData.stereoCameraModel().left().cy(),

examples/RGBDMapping/MapBuilder.h

@@ -191,9 +191,9 @@ protected slots:
 					}
 					cloudViewer_->setCloudVisibility(cloudName, true);
 				}
-				else if(stats.getSignature().id() == iter->first)
+				else if(uContains(stats.getSignatures(), iter->first))
 				{
-					Signature s = stats.getSignature();
+					Signature s = stats.getSignatures().at(iter->first);
 					s.sensorData().uncompressData(); // make sure data is uncompressed
 					// Add the new cloud
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(

examples/WifiMapping/MapBuilderWifi.h

@@ -78,26 +78,25 @@ protected slots:
 		std::map<double, int> nodeStamps; // <stamp, id>
 		std::map<int, std::pair<int, double> > wifiLevels;
 
-		UASSERT(stats.getStamps().size() == stats.getUserDatas().size());
+		for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
-		std::map<int, double>::const_iterator iterStamps = stats.getStamps().begin();
+			iter!=stats.getSignatures().end();
-		std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = stats.getUserDatas().begin();
+			++iter)
-		for(; iterStamps!=stats.getStamps().end() && iterUserDatas!=stats.getUserDatas().end(); ++iterStamps, ++iterUserDatas)
 		{
-			// Sort stamps by stamps
+			// Sort stamps by stamps->id
-			nodeStamps.insert(std::make_pair(iterStamps->second, iterStamps->first));
+			nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
 
 			// convert userData to wifi levels
-			if(iterUserDatas->second.size())
+			if(iter->second.getUserData().size())
 			{
-				UASSERT(iterUserDatas->second.size() == sizeof(int)+sizeof(double));
+				UASSERT(iter->second.getUserData().size() == sizeof(int)+sizeof(double));
 
 				// format [int level, double stamp]
 				int level;
 				double stamp;
-				memcpy(&level, iterUserDatas->second.data(), sizeof(int));
+				memcpy(&level, iter->second.getUserData().data(), sizeof(int));
-				memcpy(&stamp, iterUserDatas->second.data()+sizeof(int), sizeof(double));
+				memcpy(&stamp, iter->second.getUserData().data()+sizeof(int), sizeof(double));
 
-				wifiLevels.insert(std::make_pair(iterUserDatas->first, std::make_pair(level, stamp)));
+				wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp)));
 			}
 		}
 

guilib/src/DatabaseViewer.cpp

@@ -2252,12 +2252,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 							float groundNormalMaxAngle = M_PI_4;
 							int minClusterSize = 20;
 							cv::Mat ground, obstacles;
+
 							util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
 									cloud,
 									ground, obstacles,
 									cellSize,
 									groundNormalMaxAngle,
 									minClusterSize);
+
 							if(!ground.empty() || !obstacles.empty())
 							{
 								localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles)));

guilib/src/MainWindow.cpp

@@ -960,8 +960,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	totalTime.start();
 	//Affichage des stats et images
 
-	int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1);
+	int refMapId = -1, loopMapId = -1;
-	int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1));
+	if(uContains(stat.getSignatures(), stat.refImageId()))
+	{
+		refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
+	}
+	if(uContains(stat.getSignatures(), stat.loopClosureId()))
+	{
+		loopMapId = stat.getSignatures().at(stat.loopClosureId()).mapId();
+	}
 
 	_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
 	_ui->label_matchId->clear();
@@ -985,9 +992,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		_ui->imageView_loopClosure->setBackgroundColor(Qt::black);
 
 		// update cache
-		Signature signature = stat.getSignature();
+		Signature signature;
+		if(uContains(stat.getSignatures(), stat.refImageId()))
+		{
+			signature = stat.getSignatures().at(stat.refImageId());
 			signature.sensorData().uncompressData(); // make sure data are uncompressed
-		_cachedSignatures.insert(stat.getSignature().id(), signature);
+			_cachedSignatures.insert(signature.id(), signature);
+		}
 
 		int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
 		int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
@@ -1164,10 +1175,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		if(stat.poses().size())
 		{
 			// update pose only if odometry is not received
+			std::map<int, int> mapIds;
+			for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
+			}
 			updateMapCloud(stat.poses(),
 					_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
 					stat.constraints(),
-					stat.getMapIds());
+					mapIds);
 
 			_odometryReceived = false;