Merge pull request #184 from akirayou/zed20

change for zed sdk 2.0

corelib/include/rtabmap/core/CameraStereo.h

@@ -42,11 +42,8 @@ class Camera;
 
 namespace sl
 {
-namespace zed
-{
 class Camera;
 }
-}
 
 namespace rtabmap
 {
@@ -148,7 +145,7 @@ protected:
 
 private:
 #ifdef RTABMAP_ZED
-	sl::zed::Camera * zed_;
+	sl::Camera * zed_;
 	StereoCameraModel stereoModel_;
 	CameraVideo::Source src_;
 	int usbDevice_;

corelib/src/CameraStereo.cpp

@@ -49,7 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 
 #ifdef RTABMAP_ZED
-#include <zed/Camera.hpp>
+#include <sl/Camera.hpp>
 #endif
 
 namespace rtabmap
@@ -785,9 +785,9 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
-	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
+	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
-	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
+	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
-	UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
+	UASSERT(sensingMode_ >= sl::SENSING_MODE_FILL && sensingMode_ <sl::SENSING_MODE_LAST);
 	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
 #endif
 }
@@ -819,9 +819,9 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
-	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
+	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
-	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
+	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
-	UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
+	UASSERT(sensingMode_ >= sl::SENSING_MODE_FILL && sensingMode_ <sl::SENSING_MODE_LAST);
 	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
 #endif
 }
@@ -850,56 +850,53 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	if(src_ == CameraVideo::kVideoFile)
 	{
 		UINFO("svo file = %s", svoFilePath_.c_str());
-		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
+		zed_ = new sl::Camera(); // Use in SVO playback mode
+		sl::InitParameters param;
+		param.svo_input_filename=svoFilePath_.c_str();
+		zed_->open(param);
 	}
 	else
 	{
 		UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
-		zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode
+		zed_ = new sl::Camera(); // Use in Live Mode
+		sl::InitParameters param;
+		param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
+		param.camera_fps=getImageRate();
+		param.camera_linux_id=usbDevice_;
+		param.depth_mode=(sl::DEPTH_MODE)quality_;
+		param.coordinate_units=sl::UNIT_METER;
+		param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
+		param.sdk_verbose=false;
+		param.sdk_gpu_id=-1;
+		param.depth_minimum_distance=-1;
+		param.camera_disable_self_calib=!selfCalibration_;
+		zed_->open(param);
 	}
 
-	sl::zed::InitParams parameters(
-		(sl::zed::MODE)quality_, //MODE
-		(sl::zed::UNIT)sl::zed::METER,  //UNIT
-		(sl::zed::COORDINATE_SYSTEM)sl::zed::IMAGE,  //COORDINATE_SYSTEM
-		false,  // verbose
-		-1,     //device (GPU)
-		-1.,    //minDist
-		!selfCalibration_,  //disableSelfCalib: false = self calibrated
-		false); //vflip
 
 	UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
-			quality_, sl::zed::METER, sl::zed::IMAGE, selfCalibration_?"true":"false");
+	      quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
-	sl::zed::ERRCODE err = zed_->init(parameters);
 	UDEBUG("");
 
-	// Quit if an error occurred
-	if (err != sl::zed::SUCCESS)
-	{
-		UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err).c_str());
-		delete zed_;
-		zed_ = 0;
-		return false;
-	}
-
 	zed_->setConfidenceThreshold(confidenceThr_);
 
 	if (computeOdometry_)
 	{
-		Eigen::Matrix4f initPose;
+		sl::TrackingParameters tparam;
-		initPose.setIdentity(4, 4);
+		tparam.enable_spatial_memory=false;
-		zed_->enableTracking(initPose, false);
+		zed_->enableTracking(tparam);
 	}
 
-	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
+	sl::CameraInformation infos = zed_->getCameraInformation();
-	sl::zed::resolution res = zed_->getImageSize();
+	sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
+	sl::Resolution res = stereoParams->left_cam.image_size;
 				
 	stereoModel_ = StereoCameraModel(
-		stereoParams->LeftCam.fx, 
+		stereoParams->left_cam.fx, 
-		stereoParams->LeftCam.fy, 
+		stereoParams->left_cam.fy, 
-		stereoParams->LeftCam.cx, 
+		stereoParams->left_cam.cx, 
-		stereoParams->LeftCam.cy, 
+		stereoParams->left_cam.cy, 
-		stereoParams->baseline,
+		stereoParams->T[0],//baseline
 		this->getLocalTransform(),
 		cv::Size(res.width, res.height));
 
@@ -924,7 +921,7 @@ std::string CameraStereoZed::getSerial() const
 #ifdef RTABMAP_ZED
 	if(zed_)
 	{
-		return uFormat("%x", zed_->getZEDSerial());
+		return uFormat("%x", zed_->getCameraInformation ().serial_number);
 	}
 #endif
 	return "";
@@ -938,25 +935,51 @@ bool CameraStereoZed::odomProvided() const
 	return false;
 #endif
 }
+#ifdef RTABMAP_ZED
+static cv::Mat slMat2cvMat(sl::Mat& input) {
+	//convert MAT_TYPE to CV_TYPE
+	int cv_type = -1;
+	switch (input.getDataType()) {
+	case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
+	case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
+	case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
+	case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
+	case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
+	case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
+	case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
+	case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
+	default: break;
+	}	
+	// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
+	//cv::Mat and sl::Mat will share the same memory pointer
+	return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
+}
+#endif
 
 SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZED
+	sl::RuntimeParameters rparam;
+	rparam.sensing_mode=(sl::SENSING_MODE)sensingMode_;
+	rparam.enable_depth=quality_ > 0;
+	rparam.enable_point_cloud=quality_ > 0;
+	rparam.move_point_cloud_to_world_frame=false;
 	if(zed_)
 	{
 		UTimer timer;
-		bool res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
+		bool res = zed_->grab(rparam);
 		while (src_ == CameraVideo::kUsbDevice && res && timer.elapsed() < 2.0)
 		{
 			// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
 			uSleep(10);
-			res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
+			res = zed_->grab(rparam);
 		}
 		if(!res)
 		{
 			// get left image
-			cv::Mat rgbaLeft = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::LEFT)));
+			sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_LEFT);
+			cv::Mat rgbaLeft = slMat2cvMat(tmp);
 
 			cv::Mat left;
 			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
@@ -965,14 +988,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 			{
 				// get depth image
 				cv::Mat depth;
-				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
+				sl::Mat tmp;
+				zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
+				slMat2cvMat(tmp).copyTo(depth);
 
 				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
 			}
 			else
 			{
 				// get right image
-				cv::Mat rgbaRight = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::RIGHT)));
+				sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
+				cv::Mat rgbaRight = slMat2cvMat(tmp);
 				cv::Mat right;
 				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
 			
@@ -981,12 +1007,14 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 
 			if (computeOdometry_ && info)
 			{
-				Eigen::Matrix4f path;
+				sl::Pose pose;
-				int trackingConfidence = zed_->getTrackingConfidence();
+				zed_->getPosition(pose);
+				int trackingConfidence = pose.pose_confidence;
 				if (trackingConfidence)
 				{
-					zed_->getPosition(path);
+					Transform t;
-					info->odomPose = Transform::fromEigen4f(path);
+					for(int i=0;i<16;i++)t.data()[i]=pose.pose_data.m[i];
+					info->odomPose = t;
 					if (!info->odomPose.isNull())
 					{
 						//transform x->forward, y->left, z->up