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

Compilation fixes (#501)

Browse Source 
Great! Thx a lot!
It should fix issue #499 as well.
This commit is contained in:
Walter Lucetti
2020-01-31 20:17:10 +01:00
committed by GitHub
parent f7540196d4
commit 8810c9e694
2 changed files with 169 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/camera/CameraStereoZed.h
View File

@@ -89,7 +89,7 @@ private:
Transform imuLocalTransform_;
CameraVideo::Source src_;
int usbDevice_;
std::string svoFilePath_;
std::string svoFilePath_;
int resolution_;
int quality_;
bool selfCalibration_;

180
corelib/src/camera/CameraStereoZed.cpp
View File

@@ -41,6 +41,7 @@ namespace rtabmap
#ifdef RTABMAP_ZED
static cv::Mat slMat2cvMat(sl::Mat& input) {
#if ZED_SDK_MAJOR_VERSION < 3
//convert MAT_TYPE to CV_TYPE
int cv_type = -1;
switch (input.getDataType()) {
@@ -57,6 +58,24 @@ static cv::Mat slMat2cvMat(sl::Mat& input) {
// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
//cv::Mat and sl::Mat will share the same memory pointer
return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
#else
//convert MAT_TYPE to CV_TYPE
int cv_type = -1;
switch (input.getDataType()) {
case sl::MAT_TYPE::F32_C1: cv_type = CV_32FC1; break;
case sl::MAT_TYPE::F32_C2: cv_type = CV_32FC2; break;
case sl::MAT_TYPE::F32_C3: cv_type = CV_32FC3; break;
case sl::MAT_TYPE::F32_C4: cv_type = CV_32FC4; break;
case sl::MAT_TYPE::U8_C1: cv_type = CV_8UC1; break;
case sl::MAT_TYPE::U8_C2: cv_type = CV_8UC2; break;
case sl::MAT_TYPE::U8_C3: cv_type = CV_8UC3; break;
case sl::MAT_TYPE::U8_C4: cv_type = CV_8UC4; break;
default: break;
}
// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
//cv::Mat and sl::Mat will share the same memory pointer
return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM::CPU));
#endif
}
Transform zedPoseToTransform(const sl::Pose & pose)
@@ -67,6 +86,7 @@ Transform zedPoseToTransform(const sl::Pose & pose)
pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
}
#if ZED_SDK_MAJOR_VERSION < 3
IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform)
{
sl::Orientation orientation = imuData.pose_data.getOrientation();
@@ -104,6 +124,45 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform
accCov,
imuLocalTransform);
}
#else
IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform)
{
sl::Orientation orientation = sensorData.imu.pose.getOrientation();
//Convert zed imu orientation from camera frame to world frame ENU!
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow);
orientationT = opticalTransform * orientationT;
static double deg2rad = 0.017453293;
Eigen::Vector4d accT = Eigen::Vector4d(sensorData.imu.linear_acceleration.v[0], sensorData.imu.linear_acceleration.v[1], sensorData.imu.linear_acceleration.v[2], 1);
Eigen::Vector4d gyrT = Eigen::Vector4d(sensorData.imu.angular_velocity.v[0]*deg2rad, sensorData.imu.angular_velocity.v[1]*deg2rad, sensorData.imu.angular_velocity.v[2]*deg2rad, 1);
cv::Mat orientationCov = (cv::Mat_<double>(3,3)<<
sensorData.imu.pose_covariance.r[0], sensorData.imu.pose_covariance.r[1], sensorData.imu.pose_covariance.r[2],
sensorData.imu.pose_covariance.r[3], sensorData.imu.pose_covariance.r[4], sensorData.imu.pose_covariance.r[5],
sensorData.imu.pose_covariance.r[6], sensorData.imu.pose_covariance.r[7], sensorData.imu.pose_covariance.r[8]);
cv::Mat angCov = (cv::Mat_<double>(3,3)<<
sensorData.imu.angular_velocity_covariance.r[0], sensorData.imu.angular_velocity_covariance.r[1], sensorData.imu.angular_velocity_covariance.r[2],
sensorData.imu.angular_velocity_covariance.r[3], sensorData.imu.angular_velocity_covariance.r[4], sensorData.imu.angular_velocity_covariance.r[5],
sensorData.imu.angular_velocity_covariance.r[6], sensorData.imu.angular_velocity_covariance.r[7], sensorData.imu.angular_velocity_covariance.r[8]);
cv::Mat accCov = (cv::Mat_<double>(3,3)<<
sensorData.imu.linear_acceleration_covariance.r[0], sensorData.imu.linear_acceleration_covariance.r[1], sensorData.imu.linear_acceleration_covariance.r[2],
sensorData.imu.linear_acceleration_covariance.r[3], sensorData.imu.linear_acceleration_covariance.r[4], sensorData.imu.linear_acceleration_covariance.r[5],
sensorData.imu.linear_acceleration_covariance.r[6], sensorData.imu.linear_acceleration_covariance.r[7], sensorData.imu.linear_acceleration_covariance.r[8]);
Eigen::Quaternionf quat = orientationT.getQuaternionf();
return IMU(
cv::Vec4d(quat.x(), quat.y(), quat.z(), quat.w()),
orientationCov,
cv::Vec3d(gyrT[0], gyrT[1], gyrT[2]),
angCov,
cv::Vec3d(accT[0], accT[1], accT[2]),
accCov,
imuLocalTransform);
}
#endif
class ZedIMUThread: public UThread
{
@@ -148,12 +207,21 @@ private:
}
frameRateTimer_.start();
#if ZED_SDK_MAJOR_VERSION < 3
sl::IMUData imudata;
bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
if(res == sl::SUCCESS && imudata.valid)
{
UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now()));
}
#else
sl::SensorsData sensordata;
sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE);
if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available)
{
UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now()));
}
#endif
}
float rate_;
sl::Camera * zed_;
@@ -204,10 +272,21 @@ CameraStereoZed::CameraStereoZed(
{
UDEBUG("");
#ifdef RTABMAP_ZED
#if ZED_SDK_MAJOR_VERSION < 3
UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#else
sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif
#endif
}
@@ -242,10 +321,21 @@ CameraStereoZed::CameraStereoZed(
{
UDEBUG("");
#ifdef RTABMAP_ZED
#if ZED_SDK_MAJOR_VERSION < 3
UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#else
sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif
#endif
}
@@ -288,11 +378,16 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
sl::InitParameters param;
param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
param.camera_fps=getImageRate();
param.camera_linux_id=usbDevice_;
param.camera_fps=getImageRate();
param.depth_mode=(sl::DEPTH_MODE)quality_;
#if ZED_SDK_MAJOR_VERSION < 3
param.camera_linux_id=usbDevice_;
param.coordinate_units=sl::UNIT_METER;
param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
#else
param.coordinate_units=sl::UNIT::METER;
param.coordinate_system=sl::COORDINATE_SYSTEM::IMAGE ;
#endif
param.sdk_verbose=true;
param.sdk_gpu_id=-1;
param.depth_minimum_distance=-1;
@@ -303,11 +398,18 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
{
UINFO("svo file = %s", svoFilePath_.c_str());
zed_ = new sl::Camera(); // Use in SVO playback mode
#if ZED_SDK_MAJOR_VERSION < 3
param.svo_input_filename=svoFilePath_.c_str();
#else
param.input.setFromSVOFile(svoFilePath_.c_str());
#endif
r = zed_->open(param);
}
else
{
#if ZED_SDK_MAJOR_VERSION >= 3
param.input.setFromCameraID(usbDevice_);
#endif
UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
zed_ = new sl::Camera(); // Use in Live Mode
r = zed_->open(param);
@@ -321,22 +423,36 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
return false;
}
#if ZED_SDK_MAJOR_VERSION < 3
UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
UDEBUG("");
if(quality_!=sl::DEPTH_MODE_NONE)
{
zed_->setConfidenceThreshold(confidenceThr_);
}
if(quality_!=sl::DEPTH_MODE_NONE)
{
zed_->setConfidenceThreshold(confidenceThr_);
}
#else
UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
quality_, sl::UNIT::METER, sl::COORDINATE_SYSTEM::IMAGE , selfCalibration_?"true":"false");
#endif
UDEBUG("");
if (computeOdometry_)
{
#if ZED_SDK_MAJOR_VERSION < 3
sl::TrackingParameters tparam;
tparam.enable_spatial_memory=false;
zed_->enableTracking(tparam);
if(r!=sl::ERROR_CODE::SUCCESS)
tparam.enable_spatial_memory=false;
r = zed_->enableTracking(tparam);
#else
sl::PositionalTrackingParameters tparam;
tparam.enable_area_memory=false;
r = zed_->enablePositionalTracking(tparam);
#endif
if(r!=sl::ERROR_CODE::SUCCESS)
{
UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
}
@@ -365,7 +481,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
(int)res.height,
this->getLocalTransform().prettyPrint().c_str());
#if ZED_SDK_MAJOR_VERSION < 3
if(infos.camera_model == sl::MODEL_ZED_M)
#else
if(infos.camera_model != sl::MODEL::ZED)
#endif
{
imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
UINFO("IMU local transform: %s (imu2cam=%s))",
@@ -419,10 +539,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZED
#if ZED_SDK_MAJOR_VERSION < 3
sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
#else
sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME::CAMERA);
rparam.confidence_threshold = confidenceThr_;
#endif
if(zed_)
{
UTimer timer;
#if ZED_SDK_MAJOR_VERSION < 3
bool res = zed_->grab(rparam);
while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
{
@@ -430,11 +557,21 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
uSleep(10);
res = zed_->grab(rparam);
}
if(res==sl::SUCCESS)
if(res==sl::SUCCESS)
#else
sl::ERROR_CODE res = zed_->grab(rparam);
if(res==sl::ERROR_CODE::SUCCESS)
#endif
{
// get left image
sl::Mat tmp;
#if ZED_SDK_MAJOR_VERSION < 3
zed_->retrieveImage(tmp,sl::VIEW_LEFT);
#else
zed_->retrieveImage(tmp,sl::VIEW::LEFT);
#endif
cv::Mat rgbaLeft = slMat2cvMat(tmp);
cv::Mat left;
@@ -445,7 +582,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
// get depth image
cv::Mat depth;
sl::Mat tmp;
#if ZED_SDK_MAJOR_VERSION < 3
zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
#else
zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH);
#endif
slMat2cvMat(tmp).copyTo(depth);
data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
@@ -453,7 +594,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
else
{
// get right image
#if ZED_SDK_MAJOR_VERSION < 3
sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
#else
sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW::RIGHT );
#endif
cv::Mat rgbaRight = slMat2cvMat(tmp);
cv::Mat right;
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
@@ -463,9 +608,15 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
if(imuPublishingThread_ == 0)
{
#if ZED_SDK_MAJOR_VERSION < 3
sl::IMUData imudata;
res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
if(res == sl::SUCCESS && imudata.valid)
#else
sl::SensorsData imudata;
res = zed_->getSensorsData(imudata, sl::TIME_REFERENCE::IMAGE);
if(res == sl::ERROR_CODE::SUCCESS && imudata.imu.is_available)
#endif
{
//ZED-Mini
data.setIMU(zedIMUtoIMU(imudata, imuLocalTransform_));
@@ -475,8 +626,13 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
if (computeOdometry_ && info)
{
sl::Pose pose;
#if ZED_SDK_MAJOR_VERSION < 3
sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
if (tracking_state == sl::TRACKING_STATE_OK)
#else
sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(pose);
if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK)
#endif
{
int trackingConfidence = pose.pose_confidence;
// FIXME What does pose_confidence == -1 mean?