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 weight,
-			double stamp,
-			const std::string & label,
-			const std::multimap<int, cv::KeyPoint> & words,
-			const std::multimap<int, pcl::PointXYZ> & words3,
+			int mapId = -1,
+			int weight = 0,
+			double stamp = 0.0,
+			const std::string & label = std::string(),
 			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 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 setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
 
 	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, 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, Signature> & getSignatures() const {return _signatures;}
 
 	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, 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, Signature> _signatures;
 
 	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 r12() const {return data_[1];}
-	float r13() const {return data_[2];}
-	float r21() const {return data_[4];}
-	float r22() const {return data_[5];}
-	float r23() const {return data_[6];}
-	float r31() const {return data_[8];}
-	float r32() const {return data_[9];}
-	float r33() const {return data_[10];}
+	float r11() const {return data()[0];}
+	float r12() const {return data()[1];}
+	float r13() const {return data()[2];}
+	float r21() const {return data()[4];}
+	float r22() const {return data()[5];}
+	float r23() const {return data()[6];}
+	float r31() const {return data()[8];}
+	float r32() const {return data()[9];}
+	float r33() const {return data()[10];}
 
-	float o14() const {return data_[3];}
-	float o24() const {return data_[7];}
-	float o34() const {return data_[11];}
+	float o14() const {return data()[3];}
+	float o24() const {return data()[7];}
+	float o34() const {return data()[11];}
 
-	float & operator[](int index) {return data_[index];}
-	const float & operator[](int index) const {return data_[index];}
+	float & operator[](int index) {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();}
-	float * data() {return data_.data();}
-	int size() const {return (int)data_.size();}
+	const float * data() const {return (const float *)data_.data;}
+	float * data() {return (float *)data_.data;}
+	int size() const {return 12;}
 
-	float & x() {return data_[3];}
-	float & y() {return data_[7];}
-	float & z() {return data_[11];}
-	const float & x() const {return data_[3];}
-	const float & y() const {return data_[7];}
-	const float & z() const {return data_[11];}
+	float & x() {return data()[3];}
+	float & y() {return data()[7];}
+	float & z() {return data()[11];}
+	const float & x() const {return data()[3];}
+	const float & y() const {return data()[7];}
+	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);