i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 04:49:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraK4A: added imu support with playback. Fixed windows build error.

Browse Source
This commit is contained in:
matlabbe
2020-09-04 14:10:32 -04:00
parent 49cb470b8d
commit e0973fea92
2 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraModel.h
-2
View File
@@ -146,8 +146,6 @@ public:
void reproject(float x, float y, float z, int & u, int & v) const; void reproject(float x, float y, float z, int & u, int & v) const;
bool inFrame(int u, int v) const; bool inFrame(int u, int v) const;
friend std::ostream& operator<<(std::ostream& os, const CameraModel& model);
private: private:
std::string name_; std::string name_;
cv::Size imageSize_; cv::Size imageSize_;
corelib/src/camera/CameraK4A.cpp
+15 -5
View File
@@ -318,7 +318,11 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
this->getLocalTransform()); this->getLocalTransform());
} }
std::cout << model_ << std::endl; if (ULogger::level() <= ULogger::kInfo)
{
UINFO("K4A calibration:");
std::cout << model_ << std::endl;
}
transformationHandle_ = k4a_transformation_create(&calibration_); transformationHandle_ = k4a_transformation_create(&calibration_);
@@ -416,12 +420,12 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
if(playbackHandle_) if(playbackHandle_)
{ {
k4a_stream_result_t result = K4A_STREAM_RESULT_FAILED; k4a_stream_result_t result = K4A_STREAM_RESULT_FAILED;
while((UTimer::now()-t < 5.0) && while((UTimer::now()-t < 0.1) &&
(K4A_STREAM_RESULT_SUCCEEDED != (result=k4a_playback_get_next_capture(playbackHandle_, &captureHandle_)) || (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)))) ((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
{ {
k4a_capture_release(captureHandle_); k4a_capture_release(captureHandle_);
// the first frame may be null, just retry for 5 seconds // the first frame may be null, just retry for 1 second
} }
if (result == K4A_STREAM_RESULT_EOF) if (result == K4A_STREAM_RESULT_EOF)
{ {
@@ -545,14 +549,20 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
{ {
// Get IMU sample, clear buffer // Get IMU sample, clear buffer
// FIXME: not tested, uncomment when tested. // FIXME: not tested, uncomment when tested.
/*if(K4A_WAIT_RESULT_SUCCEEDED == k4a_playback_get_next_imu_sample(playbackHandle_, imu_sample_)) k4a_playback_seek_timestamp(playbackHandle_, stamp* 1000000+1, K4A_PLAYBACK_SEEK_BEGIN);
if(K4A_STREAM_RESULT_SUCCEEDED == k4a_playback_get_previous_imu_sample(playbackHandle_, &imu_sample_))
{ {
double stmp = ((double)imu_sample_.acc_timestamp_usec) / 1000000;
imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z), 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::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::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), cv::Mat::eye(3, 3, CV_64FC1),
imuLocalTransform_); imuLocalTransform_);
}*/ }
else
{
UWARN("IMU data NULL");
}
} }
else else
{ {