Added CameraRealsSense2 driver (tested only with D435)

CMakeLists.txt

@@ -159,6 +159,7 @@ option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
+option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
 option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
@@ -372,6 +373,13 @@ IF(WITH_REALSENSE)
     ENDIF(RealSenseSlam_FOUND)
 ENDIF(WITH_REALSENSE)
 
+IF(WITH_REALSENSE2)
+    FIND_PACKAGE(realsense2 QUIET)
+    IF(realsense2_FOUND)
+       MESSAGE(STATUS "Found RealSense2: ${realsense2_INCLUDE_DIRS}")
+    ENDIF(realsense2_FOUND)
+ENDIF(WITH_REALSENSE2)
+
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(OCTOMAP QUIET)
     IF(OCTOMAP_FOUND)
@@ -446,7 +454,7 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR realsense2_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -460,7 +468,7 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SL
         message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
       ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR realsense2_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -569,6 +577,11 @@ ENDIF()
 IF(NOT RealSenseSlam_FOUND)
    SET(REALSENSESLAM "//")
 ENDIF(NOT RealSenseSlam_FOUND)
+IF(NOT realsense2_FOUND)
+   SET(REALSENSE2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
+ENDIF()
 IF(NOT OCTOMAP_FOUND)
    SET(OCTOMAP "//")
 ELSE()
@@ -941,6 +954,14 @@ ELSE()
 MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
 ENDIF()
 
+IF(realsense2_FOUND)
+MESSAGE(STATUS "  With RealSense2           = YES (License: Apache-2)")
+ELSEIF(NOT WITH_REALSENSE2)
+MESSAGE(STATUS "  With RealSense2           = NO (WITH_REALSENSE2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With RealSense2           = NO (librealsense2 not found)")
+ENDIF()
+
 IF(OCTOMAP_FOUND)
 MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
 ELSEIF(NOT WITH_OCTOMAP)

Version.h.in

@@ -55,6 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @ZED@#define RTABMAP_ZED
 @REALSENSE@#define RTABMAP_REALSENSE
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
 @OPENCHISEL@#define RTABMAP_OPENCHISEL

corelib/include/rtabmap/core/CameraRGBD.h

@@ -48,6 +48,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
+#ifdef RTABMAP_REALSENSE2
+#include <librealsense2/rs.hpp>
+#endif
+
 #include <boost/signals2/connection.hpp>
 
 namespace openni
@@ -409,6 +413,54 @@ private:
 	USemaphore dataReady_;
 #endif
 };
+/////////////////////////
+// CameraRealSense
+/////////////////////////
+class slam_event_handler;
+class RTABMAP_EXP CameraRealSense2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraRealSense2(
+		int deviceId = 0,
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE2
+
+	void alignFrame(const rs2_intrinsics& from_intrin,
+		const rs2_intrinsics& other_intrin,
+		rs2::frame from_image,
+		uint32_t output_image_bytes_per_pixel,
+		const rs2_extrinsics& from_to_other,
+		cv::Mat & registeredDepth);
+
+	rs2::context ctx_;
+	rs2::device dev_;
+	int deviceId_;
+	rs2::syncer syncer_;
+	float depth_scale_meters_;
+	rs2_intrinsics depthIntrinsics_;
+	rs2_intrinsics rgbIntrinsics_;
+	rs2_extrinsics depthToRGBExtrinsics_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbBuffer_;
+	CameraModel model_;
+#endif
+};
 
 
 /////////////////////////

corelib/src/CMakeLists.txt

@@ -185,6 +185,17 @@ IF(RealSense_FOUND)
 	)
 ENDIF(RealSense_FOUND)
 
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		realsense2
+	)
+ENDIF(realsense2_FOUND)
+
 IF(DC1394_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}

corelib/src/CameraRGBD.cpp

@@ -76,6 +76,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
+#ifdef RTABMAP_REALSENSE2
+#include <librealsense2/rsutil.h>
+#include <librealsense2/hpp/rs_processing.hpp>
+#include <librealsense2/rs_advanced_mode.hpp>
+#endif
+
 #ifdef RTABMAP_OPENNI2
 #include <OniVersion.h>
 #include <OpenNI.h>
@@ -3210,6 +3216,357 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 	return data;
 }
 
+/////////////////////////
+// CameraRealSense2
+/////////////////////////
+bool CameraRealSense2::available()
+{
+#ifdef RTABMAP_REALSENSE2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraRealSense2::CameraRealSense2(
+		int device,
+		float imageRate,
+		const rtabmap::Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_REALSENSE2
+    ,
+	deviceId_(device),
+	depth_scale_meters_(1.0f)
+#endif
+{
+	UDEBUG("");
+}
+
+CameraRealSense2::~CameraRealSense2()
+{
+	UDEBUG("");
+}
+
+void CameraRealSense2::alignFrame(const rs2_intrinsics& from_intrin,
+                                   const rs2_intrinsics& other_intrin,
+                                   rs2::frame from_image,
+                                   uint32_t output_image_bytes_per_pixel,
+                                   const rs2_extrinsics& from_to_other,
+								   cv::Mat & registeredDepth)
+{
+    static const auto meter_to_mm = 0.001f;
+    uint8_t* p_out_frame = registeredDepth.data;
+    auto from_vid_frame = from_image.as<rs2::video_frame>();
+    auto from_bytes_per_pixel = from_vid_frame.get_bytes_per_pixel();
+
+    static const auto blank_color = 0x00;
+    UASSERT(registeredDepth.total()*registeredDepth.channels()*registeredDepth.depth() == other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
+    memset(p_out_frame, blank_color, other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
+
+    auto p_from_frame = reinterpret_cast<const uint8_t*>(from_image.get_data());
+    auto from_stream_type = from_image.get_profile().stream_type();
+    float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)?depth_scale_meters_:1.f);
+#pragma omp parallel for schedule(dynamic)
+    for (int from_y = 0; from_y < from_intrin.height; ++from_y)
+    {
+        int from_pixel_index = from_y * from_intrin.width;
+        for (int from_x = 0; from_x < from_intrin.width; ++from_x, ++from_pixel_index)
+        {
+            // Skip over depth pixels with the value of zero
+            float depth = (from_stream_type == RS2_STREAM_DEPTH)?(depth_units * ((const uint16_t*)p_from_frame)[from_pixel_index]): 1.f;
+            if (depth)
+            {
+                // Map the top-left corner of the depth pixel onto the other image
+                float from_pixel[2] = { from_x - 0.5f, from_y - 0.5f }, from_point[3], other_point[3], other_pixel[2];
+                rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
+                rs2_transform_point_to_point(other_point, &from_to_other, from_point);
+                rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+                const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
+                const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
+
+                // Map the bottom-right corner of the depth pixel onto the other image
+                from_pixel[0] = from_x + 0.5f; from_pixel[1] = from_y + 0.5f;
+                rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
+                rs2_transform_point_to_point(other_point, &from_to_other, from_point);
+                rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+                const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
+                const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
+
+                if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
+                    continue;
+
+                for (int y = other_y0; y <= other_y1; ++y)
+                {
+                    for (int x = other_x0; x <= other_x1; ++x)
+                    {
+                        int out_pixel_index = y * other_intrin.width + x;
+                        //Tranfer n-bit pixel to n-bit pixel
+                        for (int i = 0; i < from_bytes_per_pixel; i++)
+                        {
+                            const auto out_offset = out_pixel_index * output_image_bytes_per_pixel + i;
+                            const auto from_offset = from_pixel_index * output_image_bytes_per_pixel + i;
+                            p_out_frame[out_offset] = p_from_frame[from_offset] * (depth_units / meter_to_mm);
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+bool CameraRealSense2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_REALSENSE2
+
+	UINFO("setupDevice...");
+
+	auto list = ctx_.query_devices();
+	if (0 == list.size())
+	{
+		UERROR("No RealSense2 devices were found!");
+		return false;
+	}
+
+	int count = 0;
+	bool found=false;
+	for (auto&& dev : list)
+	{
+		auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+		auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+		uint16_t pid;
+		std::stringstream ss;
+		ss << std::hex << pid_str;
+		ss >> pid;
+		UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
+		if (deviceId_ == 0 || deviceId_ == count)
+		{
+			dev_ = dev;
+			found=true;
+			break;
+		}
+	}
+
+	if (!found)
+	{
+		UERROR("The requested device %d is NOT found!", deviceId_);
+		return false;
+	}
+
+	ctx_.set_devices_changed_callback([this](rs2::event_information& info)
+	{
+		if (info.was_removed(dev_))
+		{
+			UERROR("The device has been disconnected!");
+		}
+	});
+
+
+	auto camera_name = dev_.get_info(RS2_CAMERA_INFO_NAME);
+	UINFO("Device Name: %s", camera_name);
+
+	auto sn = dev_.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+	UINFO("Device Serial No: %s", sn);
+
+	auto fw_ver = dev_.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
+	UINFO("Device FW version: %s", fw_ver);
+
+	auto pid = dev_.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+	UINFO("Device Product ID: 0x%s", pid);
+
+	auto dev_sensors = dev_.query_sensors();
+
+	UINFO("Device Sensors: ");
+	std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth
+	for(auto&& elem : dev_sensors)
+	{
+		std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
+		if ("Stereo Module" == module_name)
+		{
+			sensors[1] = elem;
+		}
+		else if ("Coded-Light Depth Sensor" == module_name)
+		{
+		}
+		else if ("RGB Camera" == module_name)
+		{
+			sensors[0] = elem;
+		}
+		else if ("Wide FOV Camera" == module_name)
+		{
+		}
+		else if ("Motion Module" == module_name)
+		{
+		}
+		else
+		{
+			UERROR("Module Name \"%s\" isn't supported by LibRealSense!", module_name.c_str());
+			return false;
+		}
+		UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
+	}
+
+	UDEBUG("");
+
+	model_ = CameraModel();
+	rs2::stream_profile depthStreamProfile;
+	rs2::stream_profile rgbStreamProfile;
+	 for (unsigned int i=0; i<sensors.size(); ++i)
+	 {
+		 UDEBUG("i=%d", (int)i);
+		auto profiles = sensors[i].get_stream_profiles();
+		bool added = false;
+		UDEBUG("profiles=%d", (int)profiles.size());
+		for (auto& profile : profiles)
+		{
+			auto video_profile = profile.as<rs2::video_stream_profile>();
+			if (video_profile.format() == (i==1?RS2_FORMAT_Z16:RS2_FORMAT_RGB8) &&
+				video_profile.width()  == 640 &&
+				video_profile.height() == 480 &&
+				video_profile.fps()    == 30)
+			{
+				UDEBUG("");
+				sensors[i].open(profile);
+				auto intrinsic = video_profile.get_intrinsics();
+				if(i==1)
+				{
+					depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
+					auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
+					depth_scale_meters_ = depth_sensor.get_depth_scale();
+					depthStreamProfile = profile;
+					depthIntrinsics_ = intrinsic;
+				}
+				else
+				{
+					rgbBuffer_ = cv::Mat(cv::Size(640, 480), CV_8UC3, cv::Scalar(0, 0, 0));
+					auto intrinsic = video_profile.get_intrinsics();
+					model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
+					rgbStreamProfile = profile;
+					rgbIntrinsics_ = intrinsic;
+				}
+				UDEBUG("");
+				added = true;
+				break;
+			}
+		}
+		if (!added)
+		{
+			UERROR("Given stream configuration is not supported by the device! "
+					"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30);
+			return false;
+		}
+	 }
+
+	 UDEBUG("");
+	 if(!model_.isValidForProjection())
+	 {
+		 UERROR("Calibration info not valid!");
+		 return false;
+	 }
+	 UDEBUG("");
+	 depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
+
+	 for (unsigned int i=0; i<sensors.size(); ++i)
+	 {
+		 sensors[i].start(syncer_);
+	 }
+
+	uSleep(1000); // ignore the first frames
+	UINFO("Enabling streams...done!");
+
+	return true;
+
+#else
+	UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
+	return false;
+#endif
+}
+
+bool CameraRealSense2::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraRealSense2::getSerial() const
+{
+#ifdef RTABMAP_REALSENSE2
+	return dev_.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+#endif
+	return "NA";
+}
+
+SensorData CameraRealSense2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_REALSENSE2
+
+	try{
+		double stamp = UTimer::now();
+		auto frameset = syncer_.wait_for_frames(5000);
+		if (frameset.size())
+		{
+			UDEBUG("Frameset arrived.");
+			bool is_rgb_arrived = false;
+			bool is_depth_arrived = false;
+			rs2::frame rgb_frame;
+			rs2::frame depth_frame;
+			for (auto it = frameset.begin(); it != frameset.end(); ++it)
+			{
+				auto f = (*it);
+				auto stream_type = f.get_profile().stream_type();
+				if (stream_type == RS2_STREAM_COLOR)
+				{
+					rgb_frame = f;
+					is_rgb_arrived = true;
+				}
+				else if (stream_type == RS2_STREAM_DEPTH)
+				{
+					depth_frame = f;
+					is_depth_arrived = true;
+				}
+			}
+
+			if(is_rgb_arrived && is_depth_arrived)
+			{
+				auto from_image_frame = depth_frame.as<rs2::video_frame>();
+				cv::Mat depth(depthBuffer_.size(), depthBuffer_.type());
+				alignFrame(depthIntrinsics_, rgbIntrinsics_,
+						depth_frame, from_image_frame.get_bytes_per_pixel(),
+						depthToRGBExtrinsics_, depth);
+
+				cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
+				cv::Mat bgr;
+				cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+
+				data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
+			}
+			else
+			{
+				UERROR("Not received depth and rgb");
+			}
+		}
+	}
+	catch(const std::exception& ex)
+	{
+		UERROR("An error has occurred during frame callback: %s", ex.what());
+	}
+
+	/*if(!dataReady_.acquire(1, 5000))
+	{
+		UWARN("Not received new frames since 5 seconds, end of stream reached!");
+	}
+	else
+	{
+		UScopeMutex s(dataMutex_);
+		data = data_;
+		data_ = SensorData();
+	}*/
+#else
+	UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
+#endif
+	return data;
+}
+
 //
 // CameraRGBDImages
 //

examples/RGBDMapping/main.cpp

@@ -46,7 +46,7 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-rgbd_mapping driver\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense\n\n");
+			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2\n\n");
 	exit(1);
 }
 
@@ -64,9 +64,9 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 7)
+		if(driver < 0 || driver > 8)
 		{
-			UERROR("driver should be between 0 and 7.");
+			UERROR("driver should be between 0 and 8.");
 			showUsage();
 		}
 	}
@@ -141,6 +141,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
 	}
+	else if (driver == 8)
+	{
+		if (!CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 support...");
+			exit(-1);
+		}
+		camera = new CameraRealSense2(0, 0, opticalRotation);
+	}
 	else
 	{
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);

tools/CameraRGBD/main.cpp

@@ -56,6 +56,7 @@ void showUsage()
 			"                                     8=ZED stereo\n"
 			"                                     9=RealSense\n"
 			"                                     10=Kinect for Windows 2 SDK\n"
+			"                                     11=RealSense2\n"
 			"  Options:\n"
 			"      -rate #.#                      Input rate Hz (default 0=inf)\n"
 			"      -save_stereo \"path\"            Save stereo images in a folder or a video file (side by side *.avi).\n"
@@ -152,9 +153,9 @@ int main(int argc, char * argv[])
 
 			// last
 			driver = atoi(argv[i]);
-			if(driver < 0 || driver > 10)
+			if(driver < 0 || driver > 11)
 			{
-				UERROR("driver should be between 0 and 10.");
+				UERROR("driver should be between 0 and 11.");
 				showUsage();
 			}
 		}
@@ -265,6 +266,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraK4W2(0);
 	}
+	else if (driver == 11)
+	{
+		if (!rtabmap::CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense2(0);
+	}
 	else
 	{
 		UFATAL("");