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

RealSense: fixed color rectification. Fixed ZR300 odometry frame.

Browse Source
This commit is contained in:
matlabbe
2018-09-26 20:51:12 -04:00
parent 0c2287df77
commit 5e08da51aa
7 changed files with 625 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
corelib/src/CameraModel.cpp
View File

@@ -278,26 +278,34 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
std::string distortionModel = (std::string)n["distortion_model"];
if(uStrContains(distortionModel, "fisheye") ||
uStrContains(distortionModel, "equidistant"))
{
D_ = cv::Mat::zeros(1,6,CV_64FC1);
D_.at<double>(0,0) = data[0];
D_.at<double>(0,1) = data[1];
D_.at<double>(0,4) = data[2];
D_.at<double>(0,5) = data[3];
}
else
{
D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
}
n = fs["distortion_model"];
if(n.type() != cv::FileNode::NONE)
{
std::string distortionModel = (std::string)n;
if(D_.cols>=4 &&
(uStrContains(distortionModel, "fisheye") ||
uStrContains(distortionModel, "equidistant")))
{
cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
D.at<double>(0,0) = D_.at<double>(0,0);
D.at<double>(0,1) = D_.at<double>(0,1);
D.at<double>(0,4) = D_.at<double>(0,2);
D.at<double>(0,5) = D_.at<double>(0,3);
D_ = D;
}
}
else
{
UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
}
n = fs["rectification_matrix"];
if(n.type() != cv::FileNode::NONE)
{
@@ -400,7 +408,7 @@ bool CameraModel::save(const std::string & directory) const
// compaibility with ROS
if(D_.cols == 6)
{
fs << "distortion_model" << "fisheye"; // equidistant
fs << "distortion_model" << "equidistant"; // equidistant, fisheye
}
else if(D.cols > 5)
{

626
corelib/src/CameraRGBD.cpp
View File

@@ -2618,6 +2618,7 @@ CameraRealSense::CameraRealSense(
presetDepth_(presetDepth),
computeOdometry_(computeOdometry),
depthScaledToRGBSize_(false),
rgbSource_(kColor),
slam_(0)
#endif
{
@@ -2631,14 +2632,18 @@ CameraRealSense::~CameraRealSense()
UDEBUG("");
if(dev_)
{
if(slam_!=0)
try
{
dev_->stop(rs::source::all_sources);
}
else
{
dev_->stop();
if(slam_!=0)
{
dev_->stop(rs::source::all_sources);
}
else
{
dev_->stop();
}
}
catch(const rs::error & error){UWARN("%s", error.what());}
dev_ = 0;
}
UDEBUG("");
@@ -2685,6 +2690,118 @@ bool setStreamConfigIntrin(
}
#endif
void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
#ifdef RTABMAP_REALSENSE
depthScaledToRGBSize_ = enabled;
#endif
}
void CameraRealSense::setRGBSource(RGBSource source)
{
#ifdef RTABMAP_REALSENSE
rgbSource_ = source;
#endif
}
#ifdef RTABMAP_REALSENSE
template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
{
// Iterate over the pixels of the depth image
#pragma omp parallel for schedule(dynamic)
for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
{
int depth_pixel_index = depth_y * depth_intrin.width;
for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
{
// Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
if(float depth = get_depth(depth_pixel_index))
{
// Map the top-left corner of the depth pixel onto the other image
float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
// Map the bottom-right corner of the depth pixel onto the other image
depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
// Transfer between the depth pixels and the pixels inside the rectangle on the other image
for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
}
}
}
}
typedef uint8_t byte;
void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
{
auto out_z = (uint16_t *)(z_aligned_to_other);
align_images(z_intrin, z_to_other, other_intrin,
[z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
[out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
}
void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
{
auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
align_images(disparity_intrin, disparity_to_other, other_intrin,
[disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
[out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
}
template<int N> struct bytes { char b[N]; };
template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
{
auto in_other = (const bytes<N> *)(other_pixels);
auto out_other = (bytes<N> *)(other_aligned_to_depth);
align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
[out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
}
/////////////////////////
// Image rectification //
/////////////////////////
std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
{
std::vector<int> rectification_table;
rectification_table.resize(rect_intrin.width * rect_intrin.height);
align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
[&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
return rectification_table;
}
template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
{
for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
}
void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
{
switch(format)
{
case RS_FORMAT_Y8:
return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
case RS_FORMAT_Y16: case RS_FORMAT_Z16:
return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
default:
assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
}
}
#endif
bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
@@ -2692,10 +2809,22 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
if(dev_)
{
dev_->stop(rs::source::all_sources);
try
{
if(slam_!=0)
{
dev_->stop(rs::source::all_sources);
}
else
{
dev_->stop();
}
}
catch(const rs::error & error){UWARN("%s", error.what());}
dev_ = 0;
}
bufferedFrames_.clear();
rsRectificationTable_.clear();
#ifdef RTABMAP_REALSENSE_SLAM
motionSeq_[0] = motionSeq_[1] = 0;
@@ -2735,29 +2864,78 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
UINFO(" Preset RGB: %d", presetRGB_);
UINFO(" Preset Depth: %d", presetDepth_);
bool computeOdometry = false;
#ifdef RTABMAP_REALSENSE_SLAM
if (name.find("ZR300") != std::string::npos && computeOdometry_)
{
// Only enable ZR300 functionality if fisheye stream is enabled.
// Accel/Gyro automatically enabled when fisheye requested
computeOdometry = true;
}
#ifndef RTABMAP_REALSENSE_SLAM
computeOdometry_ = false;
#endif
if (name.find("ZR300") == std::string::npos)
{
// Only enable ZR300 functionality odometry if fisheye stream is enabled.
// Accel/Gyro automatically enabled when fisheye requested
computeOdometry_ = false;
// Only ZR300 has fisheye
if(rgbSource_ == kFishEye)
{
UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
rgbSource_ = kColor;
}
}
rs::intrinsics depth_intrin;
rs::intrinsics fisheye_intrin;
rs::intrinsics color_intrin;
// Configure depth and color to run with the device's preferred settings
UINFO("Enabling streams...");
// R200:
// 0=640x480 vs 480x360
// 1=1920x1080 vs 640x480
// 2=640x480 vs 320x240
dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
try {
rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height);
UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height);
// left/rgb stream
if(rgbSource_==kFishEye || computeOdometry_)
{
dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
if(computeOdometry_)
{
// Needed to align image timestamps to common clock-domain with the motion events
dev_->set_option(rs::option::fisheye_strobe, 1);
}
// This option causes the fisheye image to be acquired in-sync with the depth image.
dev_->set_option(rs::option::fisheye_external_trigger, 1);
dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
UINFO(" Fisheye: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
if(rgbSource_==kFishEye)
{
color_intrin = fisheye_intrin; // not rectified
}
}
if(rgbSource_!=kFishEye)
{
dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height);
if(rgbSource_==kInfrared)
{
dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
UINFO(" IR left: %dx%d", color_intrin.width, color_intrin.height);
}
}
dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height);
}
catch(const rs::error & error)
{
UERROR("Failed starting the streams: %s", error.what());
return false;
}
Transform imu2Camera = Transform::getIdentity();
#ifdef RTABMAP_REALSENSE_SLAM
UDEBUG("Setup frame callback");
@@ -2787,9 +2965,50 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
frame.get_stride()
};
cv::Mat image;
if(frame.get_format() == rs::format::raw8)
if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
{
image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
if(frame.get_stream_type() == rs::stream::fisheye)
{
// fisheye always received just after the depth image (doesn't have exact timestamp with depth)
if(bufferedFrames_.size())
{
bufferedFrames_.rbegin()->second.first = image.clone();
UScopeMutex lock(dataMutex_);
bool notify = lastSyncFrames_.first.empty();
lastSyncFrames_ = bufferedFrames_.rbegin()->second;
if(notify)
{
dataReady_.release();
}
bufferedFrames_.clear();
}
}
else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
{
if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
{
bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
UScopeMutex lock(dataMutex_);
bool notify = lastSyncFrames_.first.empty();
lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
if(notify)
{
dataReady_.release();
}
bufferedFrames_.erase(frame.get_timestamp());
}
else
{
bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
}
if(bufferedFrames_.size()>5)
{
UWARN("Frames cannot be synchronized!");
bufferedFrames_.clear();
}
return;
}
}
else if(frame.get_format() == rs::format::z16)
{
@@ -2818,7 +3037,15 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
}
else if(frame.get_format() == rs::format::rgb8)
{
image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
if(rsRectificationTable_.size())
{
image = cv::Mat(height, width, CV_8UC3);
rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
}
else
{
image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
}
if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
{
bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
@@ -2868,26 +3095,27 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
}
};
UDEBUG("");
// Setup stream callback for stream
if(computeOdometry)
if(computeOdometry_ || rgbSource_ == kFishEye)
{
dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
}
dev_->set_frame_callback(rs::stream::depth, frameCallback);
dev_->set_frame_callback(rs::stream::color, frameCallback);
if (computeOdometry)
if(rgbSource_ == kInfrared)
{
dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
UINFO(" Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
dev_->set_frame_callback(rs::stream::infrared, frameCallback);
}
else if(rgbSource_ == kColor)
{
dev_->set_frame_callback(rs::stream::color, frameCallback);
}
// Needed to align image timestamps to common clock-domain with the motion events
dev_->set_option(rs::option::fisheye_strobe, 1);
// This option causes the fisheye image to be aquired in-sync with the depth image.
dev_->set_option(rs::option::fisheye_external_trigger, 1);
dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
dev_->set_frame_callback(rs::stream::depth, frameCallback);
UDEBUG("");
if (computeOdometry_)
{
UDEBUG("Setup motion callback");
//define callback to the motion events and set it.
std::function<void(rs::motion_data)> motion_callback;
@@ -2995,23 +3223,137 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
return false;
}
dev_->start(rs::source::all_sources);
rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
imu2Camera = Transform(
fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
if(rgbSource_ == kInfrared)
{
rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
Transform fisheye2Color = Transform(
color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
imu2Camera *= fisheye2Color;
}
else if(rgbSource_ == kColor)
{
rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
Transform fisheye2Color = Transform(
color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
imu2Camera *= fisheye2Color;
}
UDEBUG("start device!");
try
{
dev_->start(rs::source::all_sources);
}
catch(const rs::error & error)
{
UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
return false;
}
}
else
{
dev_->start();
UDEBUG("start device!");
try
{
dev_->start();
}
catch(const rs::error & error)
{
UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
return false;
}
}
#else
dev_->start();
try {
dev_->start();
dev_->wait_for_frames();
}
catch (const rs::error & e)
{
UERROR("Exception: %s", e.what());
UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
}
#endif
cv::Mat D;
if(rgbSource_ == kFishEye)
{
// ftheta/equidistant model
D = cv::Mat::zeros(1,6,CV_64FC1);
D.at<double>(0,0) = color_intrin.coeffs[0];
D.at<double>(0,1) = color_intrin.coeffs[1];
D.at<double>(0,4) = color_intrin.coeffs[2];
D.at<double>(0,5) = color_intrin.coeffs[3];
}
else
{
// Brown-Conrady / radtan
D = cv::Mat::zeros(1,5,CV_64FC1);
D.at<double>(0,0) = color_intrin.coeffs[0];
D.at<double>(0,1) = color_intrin.coeffs[1];
D.at<double>(0,2) = color_intrin.coeffs[2];
D.at<double>(0,3) = color_intrin.coeffs[3];
D.at<double>(0,4) = color_intrin.coeffs[4];
}
cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
K.at<double>(0,0) = color_intrin.fx;
K.at<double>(1,1) = color_intrin.fy;
K.at<double>(0,2) = color_intrin.ppx;
K.at<double>(1,2) = color_intrin.ppy;
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
cameraModel_ = CameraModel(
dev_->get_name(),
cv::Size(color_intrin.width, color_intrin.height),
K,
D,
R,
P,
this->getLocalTransform()*imu2Camera);
UDEBUG("");
if(rgbSource_ == kColor)
{
rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
}
else if(rgbSource_ == kFishEye)
{
UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
if(!calibrationFolder.empty() && !cameraName.empty())
{
CameraModel model;
if(!model.load(calibrationFolder, cameraName))
{
UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
cameraName.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
model.fx(),
model.fy(),
model.cx(),
model.cy());
cameraModel_ = model;
cameraModel_.setName(cameraName);
cameraModel_.initRectificationMap();
cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
}
}
}
uSleep(1000); // ignore the first frames
UINFO("Enabling streams...done!");
@@ -3073,8 +3415,23 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
// Retrieve camera parameters for mapping between depth and color
rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color);
rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
rs::extrinsics depth_to_color;
rs::intrinsics color_intrin;
if(rgbSource_ == kFishEye)
{
depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
}
else if(rgbSource_ == kInfrared)
{
depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
}
else // color
{
depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
}
#ifdef RTABMAP_REALSENSE_SLAM
if(!dataReady_.acquire(1, 5000))
@@ -3082,6 +3439,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
UWARN("Not received new frames since 5 seconds, end of stream reached!");
return data;
}
{
UScopeMutex lock(dataMutex_);
rgb = lastSyncFrames_.first;
@@ -3106,84 +3464,124 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
// Retrieve our images
depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
#endif
float scale = dev_->get_depth_scale();
// factory registration...
cv::Mat bgr;
cv::cvtColor(rgb, bgr, CV_RGB2BGR);
CameraModel model(
color_intrin.fx, //fx
color_intrin.fy, //fy
color_intrin.ppx, //cx
color_intrin.ppy, //cy
this->getLocalTransform(),
0,
bgr.size());
cv::Mat depth;
if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
depth_intrin.width < color_intrin.width &&
depth_intrin.height < color_intrin.height &&
!depthScaledToRGBSize_)
if(rgbSource_ == kFishEye)
{
//we can keep the depth image size as is
depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
float scaleX = float(depth_intrin.width) / float(color_intrin.width);
float scaleY = float(depth_intrin.height) / float(color_intrin.height);
color_intrin.fx *= scaleX;
color_intrin.fy *= scaleY;
color_intrin.ppx *= scaleX;
color_intrin.ppy *= scaleY;
color_intrin.height = depth_intrin.height;
color_intrin.width = depth_intrin.width;
rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
}
else if(rgbSource_ == kInfrared)
{
rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
}
else
{
//depth to color
depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
}
for (int dy = 0; dy < depth_intrin.height; ++dy)
#endif
// factory registration...
cv::Mat bgr;
if(rgbSource_ != kColor)
{
for (int dx = 0; dx < depth_intrin.width; ++dx)
bgr = rgb;
}
else
{
cv::cvtColor(rgb, bgr, CV_RGB2BGR);
}
bool rectified = false;
if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
{
bgr = cameraModel_.rectifyImage(bgr);
rectified = true;
color_intrin.fx = cameraModel_.fx();
color_intrin.fy = cameraModel_.fy();
color_intrin.ppx = cameraModel_.cx();
color_intrin.ppy = cameraModel_.cy();
UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
}
#ifndef RTABMAP_REALSENSE_SLAM
else if(rgbSource_ != kColor)
{
bgr = bgr.clone();
}
#endif
cv::Mat depth;
if(rgbSource_ != kFishEye || rectified)
{
if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
depth_intrin.width < color_intrin.width &&
depth_intrin.height < color_intrin.height &&
!depthScaledToRGBSize_)
{
// Retrieve the 16-bit depth value and map it into a depth in meters
uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
float depth_in_meters = depth_value * scale;
//we can keep the depth image size as is
depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
float scaleX = float(depth_intrin.width) / float(color_intrin.width);
float scaleY = float(depth_intrin.height) / float(color_intrin.height);
color_intrin.fx *= scaleX;
color_intrin.fy *= scaleY;
color_intrin.ppx *= scaleX;
color_intrin.ppy *= scaleY;
color_intrin.height = depth_intrin.height;
color_intrin.width = depth_intrin.width;
}
else
{
//depth to color
depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
}
// Skip over pixels with a depth value of zero, which is used to indicate no data
if (depth_value == 0 || depth_in_meters>10.0f) continue;
// Map from pixel coordinates in the depth image to pixel coordinates in the color image
rs::float2 depth_pixel = { (float)dx, (float)dy };
rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
rs::float3 color_point = depth_to_color.transform(depth_point);
rs::float2 color_pixel = color_intrin.project(color_point);
int pdx = color_pixel.x;
int pdy = color_pixel.y;
if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
float scale = dev_->get_depth_scale();
for (int dy = 0; dy < depth_intrin.height; ++dy)
{
for (int dx = 0; dx < depth_intrin.width; ++dx)
{
depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
// Retrieve the 16-bit depth value and map it into a depth in meters
uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
float depth_in_meters = depth_value * scale;
// Skip over pixels with a depth value of zero, which is used to indicate no data
if (depth_value == 0 || depth_in_meters>10.0f) continue;
// Map from pixel coordinates in the depth image to pixel coordinates in the color image
int pdx = dx;
int pdy = dy;
if(rgbSource_ == kColor || rgbSource_ == kFishEye)
{
rs::float2 depth_pixel = { (float)dx, (float)dy };
rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
rs::float3 color_point = depth_to_color.transform(depth_point);
rs::float2 color_pixel = color_intrin.project(color_point);
pdx = color_pixel.x;
pdy = color_pixel.y;
}
//else infrared is already registered
if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
{
depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
}
}
}
if (color_intrin.width > depth_intrin.width)
{
// Fill holes
UTimer time;
util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
UDEBUG("Filling depth holes: %fs", time.ticks());
}
}
if (color_intrin.width > depth_intrin.width)
if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
{
// Fill holes
UTimer time;
util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
UDEBUG("Filling depth holes: %fs", time.ticks());
}
if (!bgr.empty() && !depth.empty())
{
data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
#ifdef RTABMAP_REALSENSE_SLAM
if(info && slam_)
{
@@ -3191,8 +3589,20 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
rs::slam::PoseMatrix4f pose;
if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
{
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
/*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
if( accuracy == rs::slam::tracking_accuracy::low ||
accuracy == rs::slam::tracking_accuracy::medium ||
accuracy == rs::slam::tracking_accuracy::high)*/
{
// the pose is in camera link or IMU frame, get pose of the color camera
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
}
/*else
{
UERROR("Odometry failed: accuracy=%d", accuracy);
}*/
}
else
{

2
corelib/src/Memory.cpp
View File

@@ -3822,7 +3822,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
cv::Mat depthMask;
if(!decimatedData.depthRaw().empty() && _depthAsMask)
if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
{
if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
imageMono.cols % decimatedData.depthRaw().cols == 0 &&