i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed building with RealSense2 on Windows

Browse Source
This commit is contained in:
matlabbe
2018-07-10 13:09:52 -04:00
parent ee00f81b5b
commit 35bc2d06a6
6 changed files with 134 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@@ -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)
FIND_PACKAGE(realsense2 QUIET)
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)

41
cmake_modules/FindRealSense2.cmake Normal file
View File

@@ -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)

33
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -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
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_;
rs2::context * ctx_;
rs2::device * dev_;
std::string deviceId_;
rs2::syncer syncer_;
rs2::syncer * syncer_;
float depth_scale_meters_;
rs2_intrinsics depthIntrinsics_;
rs2_intrinsics rgbIntrinsics_;
rs2_extrinsics depthToRGBExtrinsics_;
rs2_intrinsics * depthIntrinsics_;
rs2_intrinsics * rgbIntrinsics_;
rs2_extrinsics * depthToRGBExtrinsics_;
cv::Mat depthBuffer_;
cv::Mat rgbBuffer_;
CameraModel model_;

15
corelib/src/CMakeLists.txt
View File

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

60
corelib/src/CameraRGBD.cpp
View File

@@ -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());

23
corelib/src/VWDictionary.cpp
View File

@@ -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));