From 0a9d237ac2968463d36c4c9b4436871a6c3ea0ca Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Aug 2020 20:29:47 -0400 Subject: [PATCH] 0.20.3: CameraRealSense2: Fixed D435i accel/gyro profile selection to avoid crash. Fixed Integrated D415 fix (#469) --- CMakeLists.txt | 2 +- corelib/src/camera/CameraRealSense2.cpp | 70 +++++++++++++++++++------ 2 files changed, 54 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 947501f048..1cb24ae36a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 20) -SET(RTABMAP_PATCH_VERSION 2) +SET(RTABMAP_PATCH_VERSION 3) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index 0f6652a17c..135767f52b 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -633,12 +633,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st for (auto& profile : profiles) { auto video_profile = profile.as(); - UINFO("%s %d %d %d %d", rs2_format_to_string( + UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string( video_profile.format()), video_profile.width(), video_profile.height(), video_profile.fps(), - video_profile.stream_index()); + video_profile.stream_index(), + video_profile.stream_name().c_str(), + video_profile.stream_type()); } } int pi = 0; @@ -655,7 +657,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st auto intrinsic = video_profile.get_intrinsics(); // rgb or ir left - if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8) || + if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) || (ir_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 1)) { if(!profilesPerSensor[i].empty()) @@ -696,15 +698,35 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF) { //D435i: - //MOTION_XYZ32F 0 0 200 - //MOTION_XYZ32F 0 0 400 - //MOTION_XYZ32F 0 0 63 - //MOTION_XYZ32F 0 0 250 + //MOTION_XYZ32F 0 0 200 (gyro) + //MOTION_XYZ32F 0 0 400 (gyro) + //MOTION_XYZ32F 0 0 63 6 (accel) + //MOTION_XYZ32F 0 0 250 6 (accel) // or dualMode_ T265: - //MOTION_XYZ32F 0 0 200 - //MOTION_XYZ32F 0 0 62 - //6DOF 0 0 200 - profilesPerSensor[i].push_back(profile); + //MOTION_XYZ32F 0 0 200 5 (gyro) + //MOTION_XYZ32F 0 0 62 6 (accel) + //6DOF 0 0 200 4 (pose) + bool modified = false; + for (size_t j= 0; j < profilesPerSensor[i].size(); ++j) + { + if (profilesPerSensor[i][j].stream_type() == profile.stream_type()) + { + if (profile.stream_type() == RS2_STREAM_ACCEL) + { + if(profile.fps() > profilesPerSensor[i][j].fps()) + profilesPerSensor[i][j] = profile; + modified = true; + } + else if (profile.stream_type() == RS2_STREAM_GYRO) + { + if(profile.fps() < profilesPerSensor[i][j].fps()) + profilesPerSensor[i][j] = profile; + modified = true; + } + } + } + if(!modified) + profilesPerSensor[i].push_back(profile); added = true; } } @@ -755,12 +777,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st for (auto& profile : profiles) { auto video_profile = profile.as(); - UERROR("%s %d %d %d %d", rs2_format_to_string( + UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string( video_profile.format()), video_profile.width(), video_profile.height(), video_profile.fps(), - video_profile.stream_index()); + video_profile.stream_index(), + video_profile.stream_name().c_str(), + video_profile.stream_type()); } return false; } @@ -936,6 +960,18 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st if(profilesPerSensor[i].size()) { UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size()); + for (size_t j = 0; j < profilesPerSensor[i].size(); ++j) + { + auto video_profile = profilesPerSensor[i][j].as(); + UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string( + video_profile.format()), + video_profile.width(), + video_profile.height(), + video_profile.fps(), + video_profile.stream_index(), + video_profile.stream_name().c_str(), + video_profile.stream_type()); + } sensors[i].open(profilesPerSensor[i]); if(sensors[i].is()) { @@ -1212,13 +1248,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) IMU imu; unsigned int confidence = 0; double imuStamp = stamp*1000.0; - UASSERT(info!=0); - getPoseAndIMU(imuStamp, info->odomPose, confidence, imu); + Transform pose; + getPoseAndIMU(imuStamp, pose, confidence, imu); - if(odometryProvided_ && !info->odomPose.isNull()) + if(info && odometryProvided_ && !pose.isNull()) { // Transform in base frame (local transform should contain base to pose transform) - info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse(); + info->odomPose = this->getLocalTransform() * pose * this->getLocalTransform().inverse(); info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001; info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);