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

CameraK4A: refactored to unify playback and real device code. Fixed ir option (with rectification).

Browse Source
This commit is contained in:
matlabbe
2020-09-04 12:48:18 -04:00
parent d2784095a3
commit 49cb470b8d
6 changed files with 309 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/CameraModel.cpp
View File

@@ -765,4 +765,16 @@ bool CameraModel::inFrame(int u, int v) const
return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
}
std::ostream& operator<<(std::ostream& os, const CameraModel& model)
{
os << "Name: " << model.name() << std::endl
<< "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
<< "K= " << model.K_raw() << std::endl
<< "D= " << model.D_raw() << std::endl
<< "R= " << model.R() << std::endl
<< "P= " << model.P() << std::endl
<< "LocalTransform= " << model.localTransform();
return os;
}
} /* namespace rtabmap */

11
corelib/src/Transform.cpp
View File

@@ -311,14 +311,9 @@ bool Transform::operator!=(const Transform & t) const
std::ostream& operator<<(std::ostream& os, const Transform& s)
{
for(int i = 0; i < 3; ++i)
{
for(int j = 0; j < 4; ++j)
{
os << std::left << std::setw(12) << s.data()[i*4 + j] << " ";
}
os << std::endl;
}
os << "[" << s.data()[0] << ", " << s.data()[1] << ", " << s.data()[2] << ", " << s.data()[3] << ";" << std::endl
<< " " << s.data()[4] << ", " << s.data()[5] << ", " << s.data()[6] << ", " << s.data()[7] << ";" << std::endl
<< " " << s.data()[8] << ", " << s.data()[9] << ", " << s.data()[10]<< ", " << s.data()[11] << "]";
return os;
}

669
corelib/src/camera/CameraK4A.cpp
View File

@@ -55,12 +55,11 @@ CameraK4A::CameraK4A(
Camera(imageRate, localTransform)
#ifdef RTABMAP_K4A
,
device_(NULL),
deviceHandle_(NULL),
config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
transformation_(NULL),
capture_(NULL),
playbackHandle_(NULL),
transformationHandle_(NULL),
captureHandle_(NULL),
playbackHandle_(NULL),
deviceId_(deviceId),
rgb_resolution_(0),
framerate_(2),
@@ -78,11 +77,10 @@ CameraK4A::CameraK4A(
Camera(imageRate, localTransform)
#ifdef RTABMAP_K4A
,
device_(NULL),
transformation_(NULL),
capture_(NULL),
playbackHandle_(NULL),
deviceHandle_(NULL),
transformationHandle_(NULL),
captureHandle_(NULL),
playbackHandle_(NULL),
deviceId_(-1),
fileName_(fileName),
rgb_resolution_(0),
@@ -102,37 +100,25 @@ CameraK4A::~CameraK4A()
void CameraK4A::close()
{
#ifdef RTABMAP_K4A
if (!fileName_.empty())
if (playbackHandle_ != NULL)
{
if (playbackHandle_ != NULL)
{
k4a_playback_close((k4a_playback_t)playbackHandle_);
playbackHandle_ = NULL;
}
if (transformationHandle_ != NULL)
{
k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
transformationHandle_ = NULL;
}
k4a_playback_close((k4a_playback_t)playbackHandle_);
playbackHandle_ = NULL;
}
else
else if (deviceHandle_ != NULL)
{
if (device_ != NULL)
{
k4a_device_stop_imu(device_);
k4a_device_stop_imu(deviceHandle_);
if (transformation_ != NULL)
{
k4a_transformation_destroy(transformation_);
transformation_ = NULL;
}
k4a_device_stop_cameras(deviceHandle_);
k4a_device_close(deviceHandle_);
deviceHandle_ = NULL;
config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
}
k4a_device_stop_cameras(device_);
k4a_device_close(device_);
device_ = NULL;
config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
}
if (transformationHandle_ != NULL)
{
k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
transformationHandle_ = NULL;
}
#endif
}
@@ -171,50 +157,17 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
uint64_t recording_length = k4a_playback_get_last_timestamp_usec((k4a_playback_t)playbackHandle_);
UINFO("Recording is %lld seconds long", recording_length / 1000000);
k4a_calibration_t calibration;
if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration))
if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration_))
{
UERROR("Failed to get calibration");
close();
return false;
}
if (ir_)
{
model_ = CameraModel(
calibration.depth_camera_calibration.intrinsics.parameters.param.fx,
calibration.depth_camera_calibration.intrinsics.parameters.param.fy,
calibration.depth_camera_calibration.intrinsics.parameters.param.cx,
calibration.depth_camera_calibration.intrinsics.parameters.param.cy,
this->getLocalTransform(),
0,
cv::Size(calibration.depth_camera_calibration.resolution_width, calibration.depth_camera_calibration.resolution_height));
}
else
{
model_ = CameraModel(
calibration.color_camera_calibration.intrinsics.parameters.param.fx,
calibration.color_camera_calibration.intrinsics.parameters.param.fy,
calibration.color_camera_calibration.intrinsics.parameters.param.cx,
calibration.color_camera_calibration.intrinsics.parameters.param.cy,
this->getLocalTransform(),
0,
cv::Size(calibration.color_camera_calibration.resolution_width, calibration.color_camera_calibration.resolution_height));
transformationHandle_ = k4a_transformation_create(&calibration);
}
k4a_record_configuration_t config;
if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
{
UERROR("Failed to getting recording configuration");
close();
return false;
}
}
else if (deviceId_ >= 0)
{
if(device_!=NULL)
if(deviceHandle_!=NULL)
{
this->close();
}
@@ -265,7 +218,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
UINFO("CameraK4A found %d k4a device(s) attached", device_count);
// Open the first plugged in Kinect device
if (K4A_FAILED(k4a_device_open(deviceId_, &device_)))
if (K4A_FAILED(k4a_device_open(deviceId_, &deviceHandle_)))
{
UERROR("Failed to open k4a device!");
return false;
@@ -273,18 +226,18 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
// Get the size of the serial number
size_t serial_size = 0;
k4a_device_get_serialnum(device_, NULL, &serial_size);
k4a_device_get_serialnum(deviceHandle_, NULL, &serial_size);
// Allocate memory for the serial, then acquire it
char *serial = (char*)(malloc(serial_size));
k4a_device_get_serialnum(device_, serial, &serial_size);
k4a_device_get_serialnum(deviceHandle_, serial, &serial_size);
serial_number_.assign(serial, serial_size);
free(serial);
UINFO("Opened K4A device: %s", serial_number_.c_str());
// Start the camera with the given configuration
if (K4A_FAILED(k4a_device_start_cameras(device_, &config_)))
if (K4A_FAILED(k4a_device_start_cameras(deviceHandle_, &config_)))
{
UERROR("Failed to start cameras!");
close();
@@ -293,59 +246,117 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
UINFO("K4A camera started successfully");
if (K4A_FAILED(k4a_device_get_calibration(device_, config_.depth_mode, config_.color_resolution, &calibration_)))
if (K4A_FAILED(k4a_device_get_calibration(deviceHandle_, config_.depth_mode, config_.color_resolution, &calibration_)))
{
UERROR("k4a_device_get_calibration() failed!");
close();
return false;
}
}
else
{
UERROR("k4a_device_get_calibration() no file and no valid device id!");
return false;
}
if (ir_)
if (ir_)
{
cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
K.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fx;
K.at<double>(1,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fy;
K.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cx;
K.at<double>(1,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cy;
cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
D.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k1;
D.at<double>(0,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k2;
D.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p1;
D.at<double>(0,3) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p2;
D.at<double>(0,4) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k3;
D.at<double>(0,5) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k4;
D.at<double>(0,6) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k5;
D.at<double>(0,7) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k6;
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
P.at<double>(0,0) = K.at<double>(0,0);
P.at<double>(1,1) = K.at<double>(1,1);
P.at<double>(0,2) = K.at<double>(0,2);
P.at<double>(1,2) = K.at<double>(1,2);
model_ = CameraModel(
"k4a_ir",
cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height),
K,D,R,P,
this->getLocalTransform());
UASSERT(model_.isValidForRectification());
model_.initRectificationMap();
}
else
{
cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
K.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.fx;
K.at<double>(1,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.fy;
K.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cx;
K.at<double>(1,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cy;
cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
D.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.k1;
D.at<double>(0,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.k2;
D.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.p1;
D.at<double>(0,3) = calibration_.color_camera_calibration.intrinsics.parameters.param.p2;
D.at<double>(0,4) = calibration_.color_camera_calibration.intrinsics.parameters.param.k3;
D.at<double>(0,5) = calibration_.color_camera_calibration.intrinsics.parameters.param.k4;
D.at<double>(0,6) = calibration_.color_camera_calibration.intrinsics.parameters.param.k5;
D.at<double>(0,7) = calibration_.color_camera_calibration.intrinsics.parameters.param.k6;
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
P.at<double>(0,0) = K.at<double>(0,0);
P.at<double>(1,1) = K.at<double>(1,1);
P.at<double>(0,2) = K.at<double>(0,2);
P.at<double>(1,2) = K.at<double>(1,2);
model_ = CameraModel(
"k4a_color",
cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height),
K,D,R,P,
this->getLocalTransform());
}
std::cout << model_ << std::endl;
transformationHandle_ = k4a_transformation_create(&calibration_);
// Get imu transform
k4a_calibration_extrinsics_t* imu_extrinsics;
if(ir_)
{
imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
}
else
{
imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
}
imuLocalTransform_ = Transform(
imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
// Start playback or camera
if (!fileName_.empty())
{
k4a_record_configuration_t config;
if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
{
model_ = CameraModel(
calibration_.depth_camera_calibration.intrinsics.parameters.param.fx,
calibration_.depth_camera_calibration.intrinsics.parameters.param.fy,
calibration_.depth_camera_calibration.intrinsics.parameters.param.cx,
calibration_.depth_camera_calibration.intrinsics.parameters.param.cy,
this->getLocalTransform(),
0,
cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height));
UERROR("Failed to getting recording configuration");
close();
return false;
}
else
{
model_ = CameraModel(
calibration_.color_camera_calibration.intrinsics.parameters.param.fx,
calibration_.color_camera_calibration.intrinsics.parameters.param.fy,
calibration_.color_camera_calibration.intrinsics.parameters.param.cx,
calibration_.color_camera_calibration.intrinsics.parameters.param.cy,
this->getLocalTransform(),
0,
cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height));
}
transformation_ = k4a_transformation_create(&calibration_);
// Get imu transform
k4a_calibration_extrinsics_t* imu_extrinsics;
if(ir_)
{
imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
}
else
{
imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
}
imuLocalTransform_ = Transform(
imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
if (K4A_FAILED(k4a_device_start_imu(device_)))
}
else
{
if (K4A_FAILED(k4a_device_start_imu(deviceHandle_)))
{
UERROR("Failed to start K4A IMU");
close();
@@ -355,9 +366,9 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
UINFO("K4a IMU started successfully");
// Get an initial capture to put the camera in the right state
if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE))
{
k4a_capture_release(capture_);
k4a_capture_release(captureHandle_);
return true;
}
@@ -395,197 +406,23 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
#ifdef RTABMAP_K4A
if (playbackHandle_ != NULL)
k4a_image_t ir_image_ = NULL;
k4a_image_t rgb_image_ = NULL;
k4a_imu_sample_t imu_sample_;
double t = UTimer::now();
bool captured = false;
if(playbackHandle_)
{
k4a_capture_t capture = NULL;
k4a_stream_result_t result = K4A_STREAM_RESULT_SUCCEEDED;
// wait to get all frames
UTimer time;
while (result == K4A_STREAM_RESULT_SUCCEEDED && time.elapsed() < 5.0)
k4a_stream_result_t result = K4A_STREAM_RESULT_FAILED;
while((UTimer::now()-t < 5.0) &&
(K4A_STREAM_RESULT_SUCCEEDED != (result=k4a_playback_get_next_capture(playbackHandle_, &captureHandle_)) ||
((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
{
result = k4a_playback_get_next_capture((k4a_playback_t)playbackHandle_, &capture);
if (result == K4A_STREAM_RESULT_SUCCEEDED)
{
cv::Mat bgrCV;
cv::Mat depthCV;
double stamp = 0;
// Process capture here
if (ir_)
{
k4a_image_t ir = k4a_capture_get_ir_image(capture);
if (ir != NULL)
{
/*UDEBUG("ir res:%4dx%4d stride:%5d format:%d stamp=%f",
k4a_image_get_height_pixels(ir),
k4a_image_get_width_pixels(ir),
k4a_image_get_stride_bytes(ir),
k4a_image_get_format(ir),
double(k4a_image_get_timestamp_usec(ir)) / 1000000.0);*/
UASSERT(k4a_image_get_format(ir) == K4A_IMAGE_FORMAT_IR16);
cv::Mat bgrCV16(k4a_image_get_height_pixels(ir), k4a_image_get_width_pixels(ir), CV_16UC1, (void*)k4a_image_get_buffer(ir));
bgrCV16.convertTo(bgrCV, CV_8U);
// Release the image
k4a_image_release(ir);
}
}
else
{
k4a_image_t color = k4a_capture_get_color_image(capture);
if (color != NULL)
{
/*UDEBUG("Color res:%4dx%4d stride:%5d format:%d stamp=%f",
k4a_image_get_height_pixels(color),
k4a_image_get_width_pixels(color),
k4a_image_get_stride_bytes(color),
k4a_image_get_format(color),
double(k4a_image_get_timestamp_usec(color)) / 1000000.0);*/
UASSERT(k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG || k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_BGRA32);
if (k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG)
{
bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(color), CV_8UC1, (void*)k4a_image_get_buffer(color)));
//UDEBUG("Uncompressed = %d %d %d", bgrCV.rows, bgrCV.cols, bgrCV.channels());
}
else
{
cv::Mat bgra(k4a_image_get_height_pixels(color), k4a_image_get_width_pixels(color), CV_8UC4, (void*)k4a_image_get_buffer(color));
cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
}
// Release the image
k4a_image_release(color);
}
}
if (!bgrCV.empty())
{
k4a_image_t depth = k4a_capture_get_depth_image(capture);
if (depth != NULL)
{
/*UDEBUG("Depth16 res:%4dx%4d stride:%5d format:%d stamp=%f",
k4a_image_get_height_pixels(depth),
k4a_image_get_width_pixels(depth),
k4a_image_get_stride_bytes(depth),
k4a_image_get_format(depth),
double(k4a_image_get_timestamp_usec(depth)) / 1000000.0);*/
UASSERT(k4a_image_get_format(depth) == K4A_IMAGE_FORMAT_DEPTH16);
stamp = ((double)k4a_image_get_timestamp_usec(depth)) / 1000000;
if (ir_)
{
depthCV = cv::Mat(k4a_image_get_height_pixels(depth), k4a_image_get_width_pixels(depth), CV_16UC1, (void*)k4a_image_get_buffer(depth)).clone();
}
else
{
k4a_image_t transformedDepth;
if (k4a_image_create(k4a_image_get_format(depth), bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
{
if (k4a_transformation_depth_image_to_color_camera((k4a_transformation_t)transformationHandle_, depth, transformedDepth) == K4A_RESULT_SUCCEEDED)
{
depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth), k4a_image_get_width_pixels(transformedDepth), CV_16UC1, (void*)k4a_image_get_buffer(transformedDepth)).clone();
}
else
{
UERROR("Failed registration!");
}
k4a_image_release(transformedDepth);
}
else
{
UERROR("Failed allocating depth registered! (%d %d %d)", bgrCV.cols, bgrCV.rows, bgrCV.cols * 2);
}
}
// Release the image
k4a_image_release(depth);
}
}
k4a_capture_release(capture);
IMU imu;
// FIXME: local imu transform missing
/*k4a_imu_sample_t imuSample;
if (k4a_playback_get_next_imu_sample((k4a_playback_t)playbackHandle_, &imuSample) == K4A_STREAM_RESULT_SUCCEEDED)
{
// K4A IMU Co-ordinates
// x+ = "backwards"
// y+ = "left"
// z+ = "down"
//
// ROS Standard co-ordinates:
// x+ = "forward"
// y+ = "left"
// z+ = "up"
//
// Remap K4A IMU to ROS co-ordinate system:
// ROS_X+ = K4A_X-
// ROS_Y+ = K4A_Y+
// ROS_Z+ = K4A_Z-
imu = IMU(
cv::Vec3d(-1*imuSample.gyro_sample.xyz.x, imuSample.gyro_sample.xyz.y, -1 * imuSample.gyro_sample.xyz.z),
cv::Mat::eye(3, 3, CV_64FC1),
cv::Vec3d(-1 * imuSample.acc_sample.xyz.x, imuSample.acc_sample.xyz.y, -1 * imuSample.acc_sample.xyz.z),
cv::Mat::eye(3, 3, CV_64FC1),
Transform::getIdentity());
}*/
if (!bgrCV.empty() && !depthCV.empty())
{
data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
data.setIMU(imu);
// Frame rate
if (this->getImageRate() < 0.0f)
{
if (stamp == 0)
{
UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
}
else if (previousStamp_ > 0)
{
float ratio = -this->getImageRate();
int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
if (sleepTime > 10000)
{
UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
sleepTime / 1000, previousStamp_, stamp);
sleepTime = 10000;
}
if (sleepTime > 2)
{
uSleep(sleepTime - 2);
}
// Add precision at the cost of a small overhead
while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
{
//
}
double slept = timer_.getElapsedTime();
timer_.start();
UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
}
previousStamp_ = stamp;
}
break;
}
}
k4a_capture_release(captureHandle_);
// the first frame may be null, just retry for 5 seconds
}
if (result == K4A_STREAM_RESULT_EOF)
{
// End of file reached
@@ -595,112 +432,132 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
{
UERROR("Failed to read entire recording");
}
captured = result == K4A_STREAM_RESULT_SUCCEEDED;
}
else
else // device
{
k4a_image_t ir_image_ = NULL;
k4a_image_t rgb_image_ = NULL;
k4a_imu_sample_t imu_sample_;
double t = UTimer::now();
k4a_wait_result_t result = K4A_WAIT_RESULT_FAILED;
while((UTimer::now()-t < 5.0) &&
(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE)) ||
((ir_ && (ir_image_=k4a_capture_get_ir_image(capture_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(capture_)) == NULL))))
(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE)) ||
((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
{
k4a_capture_release(capture_);
k4a_capture_release(captureHandle_);
// the first frame may be null, just retry for 5 seconds
}
captured = result == K4A_WAIT_RESULT_SUCCEEDED;
}
if (result == K4A_WAIT_RESULT_SUCCEEDED && (rgb_image_!=NULL || ir_image_!=NULL))
if (captured && (rgb_image_!=NULL || ir_image_!=NULL))
{
cv::Mat bgrCV;
cv::Mat depthCV;
IMU imu;
if (ir_image_ != NULL)
{
cv::Mat bgrCV;
cv::Mat depthCV;
IMU imu;
// Convert IR image
cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
k4a_image_get_width_pixels(ir_image_),
CV_16UC1,
(void*)k4a_image_get_buffer(ir_image_));
if (ir_image_ != NULL)
bgrCV16.convertTo(bgrCV, CV_8U);
bgrCV = model_.rectifyImage(bgrCV);
// Release the image
k4a_image_release(ir_image_);
}
else
{
// Convert RGB image
if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
{
// Convert IR image
cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
k4a_image_get_width_pixels(ir_image_),
CV_16UC1,
(void*)k4a_image_get_buffer(ir_image_));
bgrCV16.convertTo(bgrCV, CV_8U);
// Release the image
k4a_image_release(ir_image_);
bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
CV_8UC1,
(void*)k4a_image_get_buffer(rgb_image_)));
}
else
{
// Convert RGB image
if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
k4a_image_get_width_pixels(rgb_image_),
CV_8UC4,
(void*)k4a_image_get_buffer(rgb_image_));
cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
}
// Release the image
k4a_image_release(rgb_image_);
}
double stamp = UTimer::now();
if(!bgrCV.empty())
{
// Retrieve depth image from capture
k4a_image_t depth_image_ = k4a_capture_get_depth_image(captureHandle_);
if (depth_image_ != NULL)
{
stamp = ((double)k4a_image_get_timestamp_usec(depth_image_)) / 1000000;
if (ir_)
{
bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
CV_8UC1,
(void*)k4a_image_get_buffer(rgb_image_)));
depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
k4a_image_get_width_pixels(depth_image_),
CV_16UC1,
(void*)k4a_image_get_buffer(depth_image_));
depthCV = model_.rectifyDepth(depthCV);
}
else
{
cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
k4a_image_get_width_pixels(rgb_image_),
CV_8UC4,
(void*)k4a_image_get_buffer(rgb_image_));
cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
}
// Release the image
k4a_image_release(rgb_image_);
}
if(!bgrCV.empty())
{
// Retrieve depth image from capture
k4a_image_t depth_image_ = k4a_capture_get_depth_image(capture_);
if (depth_image_ != NULL)
{
if (ir_)
k4a_image_t transformedDepth = NULL;
if (k4a_image_create(k4a_image_get_format(depth_image_),
bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
{
depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
k4a_image_get_width_pixels(depth_image_),
CV_16UC1,
(void*)k4a_image_get_buffer(depth_image_)).clone();
}
else
{
k4a_image_t transformedDepth = NULL;
if (k4a_image_create(k4a_image_get_format(depth_image_),
bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
if(k4a_transformation_depth_image_to_color_camera(transformationHandle_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
{
if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
{
depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
k4a_image_get_width_pixels(transformedDepth),
CV_16UC1,
(void*)k4a_image_get_buffer(transformedDepth)).clone();
}
else
{
UERROR("K4A failed to register depth image");
}
k4a_image_release(transformedDepth);
depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
k4a_image_get_width_pixels(transformedDepth),
CV_16UC1,
(void*)k4a_image_get_buffer(transformedDepth)).clone();
}
else
{
UERROR("K4A failed to allocate registered depth image");
UERROR("K4A failed to register depth image");
}
k4a_image_release(transformedDepth);
}
else
{
UERROR("K4A failed to allocate registered depth image");
}
k4a_image_release(depth_image_);
}
k4a_image_release(depth_image_);
}
}
k4a_capture_release(capture_);
k4a_capture_release(captureHandle_);
if(playbackHandle_)
{
// Get IMU sample, clear buffer
if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(device_, &imu_sample_, 60))
// FIXME: not tested, uncomment when tested.
/*if(K4A_WAIT_RESULT_SUCCEEDED == k4a_playback_get_next_imu_sample(playbackHandle_, imu_sample_))
{
imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
cv::Mat::eye(3, 3, CV_64FC1),
cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
cv::Mat::eye(3, 3, CV_64FC1),
imuLocalTransform_);
}*/
}
else
{
// Get IMU sample, clear buffer
if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(deviceHandle_, &imu_sample_, 60))
{
imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
cv::Mat::eye(3, 3, CV_64FC1),
@@ -712,13 +569,51 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
{
UERROR("IMU data NULL");
}
}
// Relay the data to rtabmap
if (!bgrCV.empty() && !depthCV.empty())
// Relay the data to rtabmap
if (!bgrCV.empty() && !depthCV.empty())
{
data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
if(!imu.empty())
{
data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), UTimer::now());
data.setIMU(imu);
}
// Frame rate
if (playbackHandle_ && this->getImageRate() < 0.0f)
{
if (stamp == 0)
{
UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
}
else if (previousStamp_ > 0)
{
float ratio = -this->getImageRate();
int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
if (sleepTime > 10000)
{
UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
sleepTime / 1000, previousStamp_, stamp);
sleepTime = 10000;
}
if (sleepTime > 2)
{
uSleep(sleepTime - 2);
}
// Add precision at the cost of a small overhead
while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
{
//
}
double slept = timer_.getElapsedTime();
timer_.start();
UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
}
previousStamp_ = stamp;
}
}
}
#else