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

change for zed sdk 2.0

Browse Source
This commit is contained in:
akria_you
2017-04-18 15:03:10 +09:00
parent 6f148c47e2
commit b402ce1564
2 changed files with 78 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/include/rtabmap/core/CameraStereo.h
View File

@@ -42,11 +42,8 @@ class Camera;
namespace sl
{
namespace zed
{
class Camera;
}
}
namespace rtabmap
{
@@ -148,7 +145,7 @@ protected:
private:
#ifdef RTABMAP_ZED
sl::zed::Camera * zed_;
sl::Camera * zed_;
StereoCameraModel stereoModel_;
CameraVideo::Source src_;
int usbDevice_;

126
corelib/src/CameraStereo.cpp
View File

@@ -49,7 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#endif
#ifdef RTABMAP_ZED
#include <zed/Camera.hpp>
#include <sl/Camera.hpp>
#endif
namespace rtabmap
@@ -785,9 +785,9 @@ CameraStereoZed::CameraStereoZed(
{
UDEBUG("");
#ifdef RTABMAP_ZED
UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
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_FILL && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif
}
@@ -819,9 +819,9 @@ CameraStereoZed::CameraStereoZed(
{
UDEBUG("");
#ifdef RTABMAP_ZED
UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
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_FILL && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif
}
@@ -850,56 +850,53 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
if(src_ == CameraVideo::kVideoFile)
{
UINFO("svo file = %s", svoFilePath_.c_str());
zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
zed_ = new sl::Camera(); // Use in SVO playback mode
sl::InitParameters param;
param.svo_input_filename=svoFilePath_.c_str();
zed_->open(param);
}
else
{
UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode
zed_ = new sl::Camera(); // Use in Live Mode
sl::InitParameters param;
param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
param.camera_fps=getImageRate();
param.camera_linux_id=usbDevice_;
param.depth_mode=(sl::DEPTH_MODE)quality_;
param.coordinate_units=sl::UNIT_METER;
param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
param.sdk_verbose=false;
param.sdk_gpu_id=-1;
param.depth_minimum_distance=-1;
param.camera_disable_self_calib=!selfCalibration_;
zed_->open(param);
}
sl::zed::InitParams parameters(
(sl::zed::MODE)quality_, //MODE
(sl::zed::UNIT)sl::zed::METER, //UNIT
(sl::zed::COORDINATE_SYSTEM)sl::zed::IMAGE, //COORDINATE_SYSTEM
false, // verbose
-1, //device (GPU)
-1., //minDist
!selfCalibration_, //disableSelfCalib: false = self calibrated
false); //vflip
UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
quality_, sl::zed::METER, sl::zed::IMAGE, selfCalibration_?"true":"false");
sl::zed::ERRCODE err = zed_->init(parameters);
quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
UDEBUG("");
// Quit if an error occurred
if (err != sl::zed::SUCCESS)
{
UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err).c_str());
delete zed_;
zed_ = 0;
return false;
}
zed_->setConfidenceThreshold(confidenceThr_);
if (computeOdometry_)
{
Eigen::Matrix4f initPose;
initPose.setIdentity(4, 4);
zed_->enableTracking(initPose, false);
sl::TrackingParameters tparam;
tparam.enable_spatial_memory=false;
zed_->enableTracking(tparam);
}
sl::zed::StereoParameters * stereoParams = zed_->getParameters();
sl::zed::resolution res = zed_->getImageSize();
sl::CameraInformation infos = zed_->getCameraInformation();
sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
sl::Resolution res = stereoParams->left_cam.image_size;
stereoModel_ = StereoCameraModel(
stereoParams->LeftCam.fx,
stereoParams->LeftCam.fy,
stereoParams->LeftCam.cx,
stereoParams->LeftCam.cy,
stereoParams->baseline,
stereoParams->left_cam.fx,
stereoParams->left_cam.fy,
stereoParams->left_cam.cx,
stereoParams->left_cam.cy,
stereoParams->T[0],//baseline
this->getLocalTransform(),
cv::Size(res.width, res.height));
@@ -924,7 +921,7 @@ std::string CameraStereoZed::getSerial() const
#ifdef RTABMAP_ZED
if(zed_)
{
return uFormat("%x", zed_->getZEDSerial());
return uFormat("%x", zed_->getCameraInformation ().serial_number);
}
#endif
return "";
@@ -938,25 +935,51 @@ bool CameraStereoZed::odomProvided() const
return false;
#endif
}
#ifdef RTABMAP_ZED
static cv::Mat slMat2cvMat(sl::Mat& input) {
//convert MAT_TYPE to CV_TYPE
int cv_type = -1;
switch (input.getDataType()) {
case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
case sl::MAT_TYPE_8U_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
SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZED
sl::RuntimeParameters rparam;
rparam.sensing_mode=(sl::SENSING_MODE)sensingMode_;
rparam.enable_depth=quality_ > 0;
rparam.enable_point_cloud=quality_ > 0;
rparam.move_point_cloud_to_world_frame=false;
if(zed_)
{
UTimer timer;
bool res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
bool res = zed_->grab(rparam);
while (src_ == CameraVideo::kUsbDevice && res && timer.elapsed() < 2.0)
{
// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
uSleep(10);
res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
res = zed_->grab(rparam);
}
if(!res)
{
// get left image
cv::Mat rgbaLeft = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::LEFT)));
sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_LEFT);
cv::Mat rgbaLeft = slMat2cvMat(tmp);
cv::Mat left;
cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
@@ -965,14 +988,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
// get depth image
cv::Mat depth;
slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
sl::Mat tmp;
zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
slMat2cvMat(tmp).copyTo(depth);
data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
}
else
{
// get right image
cv::Mat rgbaRight = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::RIGHT)));
sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
cv::Mat rgbaRight = slMat2cvMat(tmp);
cv::Mat right;
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
@@ -981,12 +1007,14 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
if (computeOdometry_ && info)
{
Eigen::Matrix4f path;
int trackingConfidence = zed_->getTrackingConfidence();
sl::Pose pose;
zed_->getPosition(pose);
int trackingConfidence = pose.pose_confidence;
if (trackingConfidence)
{
zed_->getPosition(path);
info->odomPose = Transform::fromEigen4f(path);
Transform t;
for(int i=0;i<16;i++)t.data()[i]=pose.pose_data.m[i];
info->odomPose = t;
if (!info->odomPose.isNull())
{
//transform x->forward, y->left, z->up