Fixed building with RealSense2 on Windows

CMakeLists.txt

@@ -246,6 +246,10 @@ IF(WITH_QT)
                 SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
                 SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
             ENDIF(value EQUAL -1)
+			MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")
+			IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
+				ADD_DEFINITIONS("-DVTK_OPENGL2")
+			ENDIF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
         ENDIF()
         ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros
     ENDIF(QT4_FOUND OR Qt5_FOUND)
@@ -382,7 +386,11 @@ IF(WITH_REALSENSE)
 ENDIF(WITH_REALSENSE)
 
 IF(WITH_REALSENSE2)
+    IF(WIN32)
+        FIND_PACKAGE(RealSense2 QUIET)
+	ELSE()
         FIND_PACKAGE(realsense2 QUIET)
+    ENDIF()
     IF(realsense2_FOUND)
        MESSAGE(STATUS "Found RealSense2: ${realsense2_INCLUDE_DIRS}")
     ENDIF(realsense2_FOUND)

cmake_modules/FindRealSense2.cmake

@@ -0,0 +1,41 @@
+# - Find librealsense (https://github.com/IntelRealSense/librealsense)
+#
+#  RealSense2_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  RealSense2_FOUND         - Set to false, or undefined, if RealSense2 isn't found.
+#  RealSense2_INCLUDE_DIRS  - The RealSense2 include directory.
+#  RealSense2_LIBRARIES     - The RealSense2 library to link against.
+
+#RealSense library
+
+find_path(RealSense2_INCLUDE_DIRS NAMES librealsense2/rs.hpp PATHS $ENV{RealSense2_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x64  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x86  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x86)
+endif()
+
+IF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+   SET(RealSense2_FOUND TRUE)
+ENDIF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+
+IF (RealSense2_FOUND)
+   SET(RealSense2_LIBRARIES ${RealSense2_LIBRARY})
+   
+   # Compatibility with linux names
+   SET(realsense2_LIBRARIES ${RealSense2_LIBRARIES})
+   SET(realsense2_INCLUDE_DIRS ${RealSense2_INCLUDE_DIRS})
+   SET(realsense2_FOUND ${RealSense2_FOUND})
+  
+   # show which RealSense was found only if not quiet
+   IF (NOT RealSense2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RealSense: ${RealSense2_LIBRARIES}")
+   ENDIF (NOT RealSense2_FIND_QUIETLY)
+ELSE (RealSense2_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (RealSense2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RealSense2 (librealsense2)")
+   ENDIF (RealSense2_FIND_REQUIRED)
+ENDIF (RealSense2_FOUND)
+

corelib/include/rtabmap/core/CameraRGBD.h

@@ -48,10 +48,6 @@ 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
@@ -83,6 +79,15 @@ namespace rs
 	}
 }
 
+namespace rs2
+{
+	class context;
+	class device;
+	class syncer;
+}
+struct rs2_intrinsics;
+struct rs2_extrinsics;
+
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;
 
@@ -450,22 +455,14 @@ protected:
 
 private:
 #ifdef RTABMAP_REALSENSE2
-
+	rs2::context * ctx_;
-	void alignFrame(const rs2_intrinsics& from_intrin,
+	rs2::device * dev_;
-		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_;
 	std::string deviceId_;
-	rs2::syncer syncer_;
+	rs2::syncer * syncer_;
 	float depth_scale_meters_;
-	rs2_intrinsics depthIntrinsics_;
+	rs2_intrinsics * depthIntrinsics_;
-	rs2_intrinsics rgbIntrinsics_;
+	rs2_intrinsics * rgbIntrinsics_;
-	rs2_extrinsics depthToRGBExtrinsics_;
+	rs2_extrinsics * depthToRGBExtrinsics_;
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
 	CameraModel model_;

corelib/src/CMakeLists.txt

@@ -191,10 +191,17 @@ IF(realsense2_FOUND)
 		${INCLUDE_DIRS}
 		${realsense2_INCLUDE_DIRS}
 	)
+	IF(WIN32)
+	    SET(LIBRARIES
+			${LIBRARIES}
+			${RealSense2_LIBRARIES}
+		)
+	ELSE()
 		SET(LIBRARIES
 			${LIBRARIES}
 			realsense2
 		)
+	ENDIF()
 ENDIF(realsense2_FOUND)
 
 IF(DC1394_FOUND)

corelib/src/CameraRGBD.cpp

@@ -77,6 +77,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 
 #ifdef RTABMAP_REALSENSE2
+#include <librealsense2/rs.hpp>
 #include <librealsense2/rsutil.h>
 #include <librealsense2/hpp/rs_processing.hpp>
 #include <librealsense2/rs_advanced_mode.hpp>
@@ -3237,8 +3238,14 @@ CameraRealSense2::CameraRealSense2(
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_REALSENSE2
     ,
+	ctx_(new rs2::context),
+	dev_(new rs2::device),
+	syncer_(new rs2::syncer),
 	deviceId_(device),
 	depth_scale_meters_(1.0f),
+	depthIntrinsics_(new rs2_intrinsics),
+	rgbIntrinsics_(new rs2_intrinsics),
+	depthToRGBExtrinsics_(new rs2_extrinsics),
 	emitterEnabled_(true),
 	irDepth_(false)
 #endif
@@ -3248,16 +3255,23 @@ CameraRealSense2::CameraRealSense2(
 
 CameraRealSense2::~CameraRealSense2()
 {
+	delete ctx_;
+	delete dev_;
+	delete syncer_;
+	delete depthIntrinsics_;
+	delete rgbIntrinsics_;
+	delete depthToRGBExtrinsics_;
 	UDEBUG("");
 }
 
 #ifdef RTABMAP_REALSENSE2
-void CameraRealSense2::alignFrame(const rs2_intrinsics& from_intrin,
+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)
+								   cv::Mat & registeredDepth,
+								   float depth_scale_meters)
 {
     static const auto meter_to_mm = 0.001f;
     uint8_t* p_out_frame = registeredDepth.data;
@@ -3270,9 +3284,9 @@ void CameraRealSense2::alignFrame(const rs2_intrinsics& from_intrin,
 
     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);
+    float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)? depth_scale_meters:1.f);
     UASSERT(from_stream_type == RS2_STREAM_DEPTH);
-    UASSERT_MSG(depth_units > 0.0f, uFormat("depth_scale_meters_=%f", depth_scale_meters_).c_str());
+    UASSERT_MSG(depth_units > 0.0f, uFormat("depth_scale_meters=%f", depth_scale_meters).c_str());
 #pragma omp parallel for schedule(dynamic)
     for (int from_y = 0; from_y < from_intrin.height; ++from_y)
     {
@@ -3329,7 +3343,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 	UINFO("setupDevice...");
 
-	auto list = ctx_.query_devices();
+	auto list = ctx_->query_devices();
 	if (0 == list.size())
 	{
 		UERROR("No RealSense2 devices were found!");
@@ -3348,7 +3362,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
 		if (deviceId_.empty() || deviceId_ == sn)
 		{
-			dev_ = dev;
+			*dev_ = dev;
 			found=true;
 			break;
 		}
@@ -3360,28 +3374,28 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		return false;
 	}
 
-	ctx_.set_devices_changed_callback([this](rs2::event_information& info)
+	ctx_->set_devices_changed_callback([this](rs2::event_information& info)
 	{
-		if (info.was_removed(dev_))
+		if (info.was_removed(*dev_))
 		{
 			UERROR("The device has been disconnected!");
 		}
 	});
 
 
-	auto camera_name = dev_.get_info(RS2_CAMERA_INFO_NAME);
+	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);
+	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);
+	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);
+	auto pid = dev_->get_info(RS2_CAMERA_INFO_PRODUCT_ID);
 	UINFO("Device Product ID: 0x%s", pid);
 
-	auto dev_sensors = dev_.query_sensors();
+	auto dev_sensors = dev_->query_sensors();
 
 	UINFO("Device Sensors: ");
 	std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth
@@ -3447,14 +3461,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				{
 					depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
 					depthStreamProfile = profile;
-					depthIntrinsics_ = intrinsic;
+					*depthIntrinsics_ = intrinsic;
 				}
 				else
 				{
 					rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
 					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;
+					*rgbIntrinsics_ = intrinsic;
 				}
 				added = true;
 				break;
@@ -3473,7 +3487,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 UERROR("Calibration info not valid!");
 		 return false;
 	 }
-	 depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
+	 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
 
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
@@ -3486,7 +3500,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
 				 depth_scale_meters_ = depth_sensor.get_depth_scale();
 			 }
-			 sensors[i].start(syncer_);
+			 sensors[i].start(*syncer_);
 		 }
 	 }
 
@@ -3509,7 +3523,7 @@ bool CameraRealSense2::isCalibrated() const
 std::string CameraRealSense2::getSerial() const
 {
 #ifdef RTABMAP_REALSENSE2
-	return dev_.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+	return dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
 #endif
 	return "NA";
 }
@@ -3534,12 +3548,12 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 #ifdef RTABMAP_REALSENSE2
 
 	try{
-		auto frameset = syncer_.wait_for_frames(5000);
+		auto frameset = syncer_->wait_for_frames(5000);
 		UTimer timer;
-		while (frameset.size() != 2 && timer.elapsed() < 2)
+		while (frameset.size() != 2 && timer.elapsed() < 2.0)
 		{
 			// maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
-			frameset = syncer_.wait_for_frames(100);
+			frameset = syncer_->wait_for_frames(100);
 		}
 		if (frameset.size() == 2)
 		{
@@ -3576,9 +3590,9 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				else
 				{
 					depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type());
-					alignFrame(depthIntrinsics_, rgbIntrinsics_,
+					alignFrame(*depthIntrinsics_, *rgbIntrinsics_,
 							depth_frame, from_image_frame.get_bytes_per_pixel(),
-							depthToRGBExtrinsics_, depth);
+							*depthToRGBExtrinsics_, depth, depth_scale_meters_);
 				}
 
 				cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());

corelib/src/VWDictionary.cpp

@@ -802,7 +802,16 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			for(int j=0; j<dists.cols; ++j)
 			{
 				float d = dists.at<float>(i,j);
-				int id = uValue(_mapIndexId, (int)results.at<size_t>(i,j));
+				int index;
+				if (sizeof(size_t) == 8)
+				{
+					index = *((size_t*)&results.at<double>(i, j));
+				}
+				else
+				{
+					index = *((size_t*)&results.at<int>(i, j));
+				}
+				int id = uValue(_mapIndexId, index);
 				if(d >= 0.0f && id > 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
@@ -1150,7 +1159,17 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				for(int j=0; j<dists.cols; ++j)
 				{
 					float d = dists.at<float>(i,j);
-					int id = uValue(_mapIndexId, (int)results.at<size_t>(i,j));
+					int index;
+					UWARN("results elemSize1=%d", results.elemSize1());
+					if (sizeof(size_t) == 8)
+					{
+						index = *((size_t*)&results.at<double>(i, j));
+					}
+					else
+					{
+						index = *((size_t*)&results.at<int>(i, j));
+					}
+					int id = uValue(_mapIndexId, index);
 					if(d >= 0.0f && id > 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));