0.11.7: Added ZED sdk support

corelib/src/CMakeLists.txt

@@ -209,6 +209,27 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
+IF(ZED_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${ZED_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${ZED_LIBRARIES}
+	)
+	IF(CUDA_FOUND)
+		SET(INCLUDE_DIRS
+			${INCLUDE_DIRS}
+			${CUDA_INCLUDE_DIRS}
+		)
+		SET(LIBRARIES
+			${LIBRARIES}
+			${CUDA_LIBRARIES}
+		)
+	ENDIF(CUDA_FOUND)
+ENDIF(ZED_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/CameraRGB.cpp

@@ -722,7 +722,7 @@ CameraVideo::~CameraVideo()
 
 bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-	_guid.clear();
+	_guid = cameraName;
 	if(_capture.isOpened())
 	{
 		_capture.release();
@@ -750,19 +750,22 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 	}
 	else
 	{
-		unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
-		if(guid != 0 && guid != 0xffffffff)
+		if (_guid.empty())
 		{
-			_guid = uFormat("%08x", guid);
+			unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				_guid = uFormat("%08x", guid);
+			}
 		}
 
 		// look for calibration files
-		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
+		if(!calibrationFolder.empty() && !_guid.empty())
 		{
-			if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName)))
+			if(!_model.load(calibrationFolder, _guid))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());
+						_guid.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{

corelib/src/CameraStereo.cpp

@@ -48,6 +48,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fc2triclops.h>
 #endif
 
+#ifdef RTABMAP_ZED
+#include <zed/Camera.hpp>
+#endif
+
 namespace rtabmap
 {
 
@@ -731,6 +735,152 @@ SensorData CameraStereoFlyCapture2::captureImage()
 	return data;
 }
 
+//
+// CameraStereoZED
+//
+bool CameraStereoZed::available()
+{
+#ifdef RTABMAP_ZED
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(bool rgbdMode, float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		zed_(0),
+		rgbdMode_(rgbdMode)
+{
+}
+
+CameraStereoZed::~CameraStereoZed()
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+	}
+#endif
+}
+
+bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+		zed_ = 0;
+	}
+	
+	if(zed_->isZEDconnected())
+	{
+		zed_ = new sl::zed::Camera(sl::zed::HD720); // Use in Live Mode
+		//zed_ = new sl::zed::Camera(argv[1]); // Use in SVO playback mode
+
+		int width = zed_->getImageSize().width;
+		int height = zed_->getImageSize().height;
+
+		//init WITH self-calibration (- last parameter to false -)
+		sl::zed::ERRCODE err = zed_->init(sl::zed::MODE::PERFORMANCE, 0, true, false, false);
+
+		// Quit if an error occurred
+		if (err != sl::zed::SUCCESS)
+		{
+			UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err));
+			delete zed_;
+			zed_ = 0;
+			return false;
+		}
+	}
+	else
+	{
+		UERROR("ZED camera initialization failed: ZED is not connected!");
+		return false;
+	}
+	
+	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
+	sl::zed::resolution res = zed_->getImageSize();
+				
+	stereoModel_ = StereoCameraModel(
+		stereoParams->LeftCam.fx, 
+		stereoParams->LeftCam.fy, 
+		stereoParams->LeftCam.cx, 
+		stereoParams->LeftCam.cy, 
+		stereoParams->baseline/1000.0f,
+		this->getLocalTransform(),
+		cv::Size(res.width, res.height));
+
+	return true;
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return false;
+}
+
+bool CameraStereoZed::isCalibrated() const
+{
+	return stereoModel_.isValidForProjection();
+}
+
+std::string CameraStereoZed::getSerial() const
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		return uFormat("%x", zed_->getZEDSerial());
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoZed::captureImage()
+{
+	SensorData data;
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		sl::zed::SENSING_MODE dm_type = sl::zed::RAW;
+		bool res = zed_->grab(dm_type);
+
+		if(!res)
+		{
+			// get left image
+			cv::Mat rgbaLeft = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_LEFT)));
+			cv::Mat left;
+			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
+
+			if(rgbdMode_)
+			{
+				// get depth image
+				cv::Mat depth;
+				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
+				depth /= 1000.0;
+
+				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
+			}
+			else
+			{
+				// get right image
+				cv::Mat rgbaRight = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_RIGHT)));
+				cv::Mat right;
+				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+			
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
+			}
+		}
+		else
+		{
+			UERROR("CameraStereoZed: Failed to grab images!");
+		}
+	}
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return data;
+}
+
+
 //
 // CameraStereoImages
 //
@@ -921,7 +1071,21 @@ CameraStereoVideo::CameraStereoVideo(
 		const Transform & localTransform) :
 		Camera(imageRate, localTransform),
 		path_(path),
-		rectifyImages_(rectifyImages)
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int device,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	path_(""),
+	rectifyImages_(false),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(device)
 {
 }
 
@@ -932,29 +1096,51 @@ CameraStereoVideo::~CameraStereoVideo()
 
 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	cameraName_ = cameraName;
 	if(capture_.isOpened())
 	{
 		capture_.release();
 	}
-	ULOGGER_DEBUG("Camera: filename=\"%s\"", path_.c_str());
-	capture_.open(path_.c_str());
+
+	if (src_ == CameraVideo::kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		capture_.open(usbDevice_);
+	}
+	else if (src_ == CameraVideo::kVideoFile)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+		capture_.open(path_.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
+	}
 
 	if(!capture_.isOpened())
 	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
 		return false;
 	}
 	else
 	{
-		// look for calibration files
-		cameraName_ = cameraName;
-		if(!calibrationFolder.empty() && !cameraName.empty())
+		if (cameraName_.empty())
 		{
-			if(!stereoModel_.load(calibrationFolder, cameraName))
+			unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				cameraName_ = uFormat("%08x", guid);
+			}
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && !cameraName_.empty())
+		{
+			if(!stereoModel_.load(calibrationFolder, cameraName_))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.c_str(), calibrationFolder.c_str());
+					cameraName_.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{
@@ -1007,7 +1193,7 @@ SensorData CameraStereoVideo::captureImage()
 				rightCvt = true;
 			}
 
-			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+			if((src_ != CameraVideo::kVideoFile || rectifyImages_) && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);
 				rightImage = stereoModel_.right().rectifyImage(rightImage);

corelib/src/Memory.cpp

@@ -3425,7 +3425,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
 		float decimationRatio = preDecimation / _imagePostDecimation;
-		double log2value = log(preDecimation)/log(2);
+		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			cv::KeyPoint kpt = keypoints[i];

corelib/src/Odometry.cpp

@@ -253,7 +253,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 
 		// transform back the keypoints in the original image
 		std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
-		double log2value = log(_imageDecimation)/log(2);
+		double log2value = log(double(_imageDecimation))/log(2.0);
 		for(unsigned int i=0; i<kpts.size(); ++i)
 		{
 			kpts[i].pt.x *= _imageDecimation;

corelib/src/util2d.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/StereoDense.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
@@ -724,12 +725,11 @@ void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextIm
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type)
+		const ParametersMap & parameters)
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty());
 	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
 	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
-	UASSERT(type == CV_32FC1 || type == CV_16SC1);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -741,32 +741,9 @@ cv::Mat disparityFromStereoImages(
 		leftMono = leftImage;
 	}
 	cv::Mat disparity;
-#if CV_MAJOR_VERSION < 3
-	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
-	stereo.state->SADWindowSize = 15;
-	stereo.state->minDisparity = 0;
-	stereo.state->numberOfDisparities = 64;
-	stereo.state->preFilterSize = 9;
-	stereo.state->preFilterCap = 31;
-	stereo.state->uniquenessRatio = 15;
-	stereo.state->textureThreshold = 10;
-	stereo.state->speckleWindowSize = 100;
-	stereo.state->speckleRange = 4;
-	stereo(leftMono, rightImage, disparity, type);
-#else
-	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
-	stereo->setBlockSize(15);
-	stereo->setMinDisparity(0);
-	stereo->setNumDisparities(64);
-	stereo->setPreFilterSize(9);
-	stereo->setPreFilterCap(31);
-	stereo->setUniquenessRatio(15);
-	stereo->setTextureThreshold(10);
-	stereo->setSpeckleWindowSize(100);
-	stereo->setSpeckleRange(4);
-	stereo->compute(leftMono, rightImage, disparity);
-#endif
-	return disparity;
+
+	StereoBM stereo(parameters);
+	return stereo.computeDisparity(leftMono, rightImage);
 }
 
 cv::Mat depthFromDisparity(const cv::Mat & disparity,

corelib/src/util3d.cpp

@@ -588,7 +588,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
@@ -623,7 +624,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 
 	return cloudFromDisparityRGB(
 			leftColor,
-			util2d::disparityFromStereoImages(leftMono, rightMono),
+			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
 			modelDecimation,
 			decimation,
 			maxDepth,
@@ -636,7 +637,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -696,7 +698,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 		cloud = cloudFromDisparity(
-				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
 				sensorData.stereoCameraModel(),
 				decimation,
 				maxDepth,
@@ -719,7 +721,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!sensorData.imageRaw().empty());
 	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -804,7 +807,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				decimation,
 				maxDepth,
 				minDepth,
-				validIndices);
+				validIndices,
+				parameters);
 
 		if(cloud->size())
 		{