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

L515: downscale depth image if it has been upscaled during registration, fixed depth not correctly scaled in IR mode

Browse Source
This commit is contained in:
matlabbe
2021-01-18 11:33:10 -05:00
parent 814a243693
commit 70e9dff7da
1 changed files with 32 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -25,6 +25,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <rtabmap/core/camera/CameraRealSense2.h> #include <rtabmap/core/camera/CameraRealSense2.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThreadC.h> #include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
@@ -102,6 +103,8 @@ CameraRealSense2::~CameraRealSense2()
{ {
UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i]->query_sensors().size(), (int)i); UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i]->query_sensors().size(), (int)i);
for(rs2::sensor _sensor : dev_[i]->query_sensors()) for(rs2::sensor _sensor : dev_[i]->query_sensors())
{
if(!_sensor.get_active_streams().empty())
{ {
try try
{ {
@@ -113,6 +116,7 @@ CameraRealSense2::~CameraRealSense2()
UWARN("%s", error.what()); UWARN("%s", error.what());
} }
} }
}
#ifdef WIN32 #ifdef WIN32
dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
// Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted // Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted
@@ -1298,7 +1302,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
if(is_rgb_arrived && is_depth_arrived) if(is_rgb_arrived && is_depth_arrived)
{ {
cv::Mat depth; cv::Mat depth;
if(ir_) if(ir_ && !irDepth_)
{ {
depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone(); depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
} }
@@ -1307,18 +1311,30 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
rs2::align align(rgb_frame.get_profile().stream_type()); rs2::align align(rgb_frame.get_profile().stream_type());
rs2::frameset processed = frameset.apply_filter(align); rs2::frameset processed = frameset.apply_filter(align);
rs2::depth_frame aligned_depth_frame = processed.get_depth_frame(); rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();
if(frameset.get_depth_frame().get_width() < aligned_depth_frame.get_width() &&
frameset.get_depth_frame().get_height() < aligned_depth_frame.get_height())
{
int decimationWidth = int(float(aligned_depth_frame.get_width())/float(frameset.get_depth_frame().get_width())+0.5f);
int decimationHeight = int(float(aligned_depth_frame.get_height())/float(frameset.get_depth_frame().get_height())+0.5f);
if(decimationWidth>1 || decimationHeight>1)
{
depth = util2d::decimate(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()), decimationWidth>decimationHeight?decimationWidth:decimationHeight);
}
else
{
depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone(); depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
}
}
else
{
depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
}
if(depth_scale_meters_ != 0.001f) if(depth_scale_meters_ != 0.001f)
{ // convert to mm { // convert to mm
if(depth.type() == CV_16UC1) if(depth.type() == CV_16UC1)
{ {
float scale = depth_scale_meters_ / 0.001f; float scaleMM = depth_scale_meters_ / 0.001f;
uint16_t *p = depth.ptr<uint16_t>(); depth = scaleMM * depth;
int buffSize = depth.rows * depth.cols;
#pragma omp parallel for
for(int i = 0; i < buffSize; ++i) {
p[i] *= scale;
}
} }
} }
} }