fixed runtime errors for single depth camera and stereo

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,36 +2194,48 @@ 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;
-			std::map<int, std::string> labels;
-			std::map<int, double> stamps;
-			std::map<int, std::vector<unsigned char> > userDatas;
-			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.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, true);
-				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(_optimizedPoses);
-			statistics_.setConstraints(_constraints);
-			statistics_.setMapIds(mapIds);
-			statistics_.setLabels(labels);
-			statistics_.setStamps(stamps);
-			statistics_.setUserDatas(userDatas);
+			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
 		}
-
+		// Set local graph
+		std::map<int, Transform> poses;
+		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;
+			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);
+			signatures.insert(std::make_pair(iter->first,
+					Signature(iter->first,
+							mapId,
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData)));
+		}
+		statistics_.setPoses(poses);
+		statistics_.setConstraints(constraints);
+		statistics_.setSignatures(signatures);
+		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
 	}
 
 	//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,
-							SensorData())));
+							userData)));
 			}
 		}
 	}

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
-		_laserScanCompressed = laserScan;
+		_laserScanRaw = laserScan;
 	}
 	else if(!laserScan.empty())
 	{
-		UASSERT(laserScan.type() == CV_32FC2);
-		_laserScanRaw = laserScan;
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_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
-		_laserScanCompressed = laserScan;
+		_laserScanRaw = laserScan;
 	}
 	else if(!laserScan.empty())
 	{
-		UASSERT(laserScan.type() == CV_32FC2);
-		_laserScanRaw = laserScan;
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_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
-		_laserScanCompressed = laserScan;
+		_laserScanRaw = laserScan;
 	}
 	else if(!laserScan.empty())
 	{
-		UASSERT(laserScan.type() == CV_32FC2);
-		_laserScanRaw = laserScan;
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
 	}
 }
 

corelib/src/Signature.cpp

@@ -39,8 +39,7 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
-	_stamp(0.0),
-	_weight(-1),
+	_weight(0),
 	_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,
-				     float r21, float r22, float r23, float o24,
-				     float r31, float r32, float r33, float o34) :
-	data_(12)
+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)
 {
-	data_[0] = r11;
-	data_[1] = r12;
-	data_[2] = r13;
-	data_[3] = o14;
-	data_[4] = r21;
-	data_[5] = r22;
-	data_[6] = r23;
-	data_[7] = o24;
-	data_[8] = r31;
-	data_[9] = r32;
-	data_[10] = r33;
-	data_[11] = o34;
+	data_ = (cv::Mat_<float>(3,4) <<
+			r11, r12, r13, o14,
+			r21, r22, r23, o24,
+			r31, r32, r33, o34);
+}
+
+Transform::Transform(const cv::Mat & transformationMatrix)
+{
+	UASSERT(transformationMatrix.cols == 4 &&
+			transformationMatrix.rows == 3 &&
+			transformationMatrix.type() == CV_32FC1);
+	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 &&
-			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) ||
-			uIsNan(data_[0]) ||
-			uIsNan(data_[1]) ||
-			uIsNan(data_[2]) ||
-			uIsNan(data_[3]) ||
-			uIsNan(data_[4]) ||
-			uIsNan(data_[5]) ||
-			uIsNan(data_[6]) ||
-			uIsNan(data_[7]) ||
-			uIsNan(data_[8]) ||
-			uIsNan(data_[9]) ||
-			uIsNan(data_[10]) ||
-			uIsNan(data_[11]);
+	return (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) ||
+			uIsNan(data()[0]) ||
+			uIsNan(data()[1]) ||
+			uIsNan(data()[2]) ||
+			uIsNan(data()[3]) ||
+			uIsNan(data()[4]) ||
+			uIsNan(data()[5]) ||
+			uIsNan(data()[6]) ||
+			uIsNan(data()[7]) ||
+			uIsNan(data()[8]) ||
+			uIsNan(data()[9]) ||
+			uIsNan(data()[10]) ||
+			uIsNan(data()[11]);
 }
 
 bool Transform::isIdentity() const
 {
-	return data_[0] == 1.0f &&
-			data_[1] == 0.0f &&
-			data_[2] == 0.0f &&
-			data_[3] == 0.0f &&
-			data_[4] == 0.0f &&
-			data_[5] == 1.0f &&
-			data_[6] == 0.0f &&
-			data_[7] == 0.0f &&
-			data_[8] == 0.0f &&
-			data_[9] == 0.0f &&
-			data_[10] == 1.0f &&
-			data_[11] == 0.0f;
+	return data()[0] == 1.0f &&
+			data()[1] == 0.0f &&
+			data()[2] == 0.0f &&
+			data()[3] == 0.0f &&
+			data()[4] == 0.0f &&
+			data()[5] == 1.0f &&
+			data()[6] == 0.0f &&
+			data()[7] == 0.0f &&
+			data()[8] == 0.0f &&
+			data()[9] == 0.0f &&
+			data()[10] == 1.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,
-					 data_[4], data_[5], data_[6], 0,
-					 data_[8], data_[9], data_[10], 0);
+	return Transform(
+			data()[0], data()[1], data()[2], 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],
-					 0,1,0, data_[7],
-					 0,0,1, data_[11]);
+	return Transform(1,0,0, data()[3],
+					 0,1,0, data()[7],
+					 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],
-		 data_[4], data_[5], data_[6], data_[7],
-		 data_[8], data_[9], data_[10], data_[11],
+	m << data()[0], data()[1], data()[2], data()[3],
+		 data()[4], data()[5], data()[6], data()[7],
+		 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],
-		 data_[4], data_[5], data_[6], data_[7],
-		 data_[8], data_[9], data_[10], data_[11],
+	m << data()[0], data()[1], data()[2], data()[3],
+		 data()[4], data()[5], data()[6], data()[7],
+		 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(),