Skip to content

Commit

Permalink
0.20.3: CameraRealSense2: Fixed D435i accel/gyro profile selection to…
Browse files Browse the repository at this point in the history 
… avoid crash. Fixed Integrated D415 fix (#469)
  • Loading branch information
@matlabbe
matlabbe committed Aug 2, 2020
1 parent 07d21e6 commit 0a9d237
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 18 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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})

Expand Down
70 changes: 53 additions & 17 deletions corelib/src/camera/CameraRealSense2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -633,12 +633,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
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;
Expand All @@ -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())
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -755,12 +777,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
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;
}
Expand Down Expand Up @@ -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<rs2::video_stream_profile>();
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<rs2::depth_sensor>())
{
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 0a9d237

Please sign in to comment.