mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 09:30:25 +08:00
CameraK4A: refactored to unify playback and real device code. Fixed ir option (with rectification).
This commit is contained in:
matlabbe
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user