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

Added CameraStereoZedOC (Zed Open Capture driver).

Browse Source 
* Added CameraStereoZedOC (Zed Open Capture driver). Calibration: added stereo baseline option, show warning if fx is very different after stereo calibration.

* Fixed build without Zed Open Capture. Fixed "any" prefix added to all tools when WITH_ZEDOC=ON. UI: fixed Zed Open Capture action not disabled when not built with it.
This commit is contained in:
matlabbe
2021-02-28 10:31:18 -05:00
committed by GitHub
parent 862eb0a90a
commit 967c57d165
21 changed files with 1495 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/CMakeLists.txt
View File

@@ -33,6 +33,7 @@ SET(SRC_FILES
camera/CameraStereoImages.cpp
camera/CameraStereoVideo.cpp
camera/CameraStereoZed.cpp
camera/CameraStereoZedOC.cpp
camera/CameraStereoTara.cpp
camera/CameraVideo.cpp
camera/CameraMyntEye.cpp
@@ -469,6 +470,19 @@ IF(ZED_FOUND)
ENDIF(CUDA_FOUND)
ENDIF(ZED_FOUND)
IF(ZEDOC_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${ZEDOC_INCLUDE_DIRS}
${HIDAPI_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${ZEDOC_LIBRARIES}
${HIDAPI_LIBRARIES}
)
ENDIF(ZEDOC_FOUND)
IF(octomap_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}

6
corelib/src/StereoCameraModel.cpp
View File

@@ -401,7 +401,11 @@ bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
}
else
{
UERROR("Failed saving stereo extrinsics (they are null).");
UERROR("Failed saving stereo extrinsics (they are null):");
std::cout << "R= " << R_ << std::endl;
std::cout << "T= " << T_ << std::endl;
std::cout << "E= " << T_ << std::endl;
std::cout << "F= " << F_ << std::endl;
}
return false;
}

782
corelib/src/camera/CameraStereoZedOC.cpp Normal file
View File

@@ -0,0 +1,782 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraStereoZedOC.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_ZEDOC
#define VIDEO_MOD_AVAILABLE
#define SENSORS_MOD_AVAILABLE
#include <zed-open-capture/videocapture.hpp>
#include <zed-open-capture/sensorcapture.hpp>
#include "SimpleIni.h"
///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2018, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////
inline std::vector<std::string> &split(const std::string &s, char delim, std::vector<std::string> &elems) {
std::stringstream ss(s);
std::string item;
while (getline(ss, item, delim)) {
elems.push_back(item);
}
return elems;
}
inline std::vector<std::string> split(const std::string &s, char delim) {
std::vector<std::string> elems;
split(s, delim, elems);
return elems;
}
class ConfManager {
public:
ConfManager(std::string filename) {
filename_ = filename;
ini_.SetUnicode();
SI_Error rc = ini_.LoadFile(filename_.c_str());
is_opened_ = !(rc < 0);
}
~ConfManager() {
//if (is_opened_) ini_.SaveFile(filename_.c_str());
}
float getValue(std::string key, float default_value = -1) {
if (is_opened_) {
std::vector<std::string> elems;
split(key, ':', elems);
return atof(ini_.GetValue(elems.front().c_str(), elems.back().c_str(), std::to_string(default_value).c_str()));
} else
return -1.f;
}
void setValue(std::string key, float value) {
if (is_opened_) {
std::vector<std::string> elems;
split(key, ':', elems);
/*SI_Error rc = */ini_.SetValue(elems.front().c_str(), elems.back().c_str(), std::to_string(value).c_str());
}
}
inline bool isOpened() {
return is_opened_;
}
private:
std::string filename_;
bool is_opened_;
CSimpleIniA ini_;
};
bool checkFile(std::string path) {
std::ifstream f(path.c_str());
return f.good();
}
static inline std::string getRootHiddenDir() {
#ifdef WIN32
#ifdef UNICODE
wchar_t szPath[MAX_PATH];
#else
TCHAR szPath[MAX_PATH];
#endif
if (!SUCCEEDED(SHGetFolderPath(NULL, CSIDL_COMMON_APPDATA, NULL, 0, szPath)))
return "";
char snfile_path[MAX_PATH];
#ifndef UNICODE
size_t newsize = strlen(szPath) + 1;
wchar_t * wcstring = new wchar_t[newsize];
// Convert char* string to a wchar_t* string.
size_t convertedChars = 0;
mbstowcs_s(&convertedChars, wcstring, newsize, szPath, _TRUNCATE);
wcstombs(snfile_path, wcstring, MAX_PATH);
#else
wcstombs(snfile_path, szPath, MAX_PATH);
#endif
std::string filename(snfile_path);
filename += "\\Stereolabs\\";
#else //LINUX
std::string homepath = getenv("HOME");
std::string filename = homepath + "/zed/";
#endif
return filename;
}
/*return the path to the Sl ZED hidden dir*/
static inline std::string getHiddenDir() {
std::string filename = getRootHiddenDir();
#ifdef WIN32
filename += "settings\\";
#else //LINUX
filename += "settings/";
#endif
return filename;
}
bool downloadCalibrationFile(unsigned int serial_number, std::string &calibration_file) {
#ifndef _WIN32
std::string path = getHiddenDir();
char specific_name[128];
sprintf(specific_name, "SN%d.conf", serial_number);
calibration_file = path + specific_name;
if (!checkFile(calibration_file)) {
std::string cmd;
int res;
// Create download folder
cmd = "mkdir -p " + path;
res = system(cmd.c_str());
// Download the file
std::string url("'https://calib.stereolabs.com/?SN=");
cmd = "wget " + url + std::to_string(serial_number) + "' -O " + calibration_file;
std::cout << cmd << std::endl;
res = system(cmd.c_str());
if( res == EXIT_FAILURE )
{
std::cerr << "Error downloading the calibration file" << std::endl;
return false;
}
if (!checkFile(calibration_file)) {
std::cerr << "Invalid calibration file" << std::endl;
return false;
}
}
#else
std::string path = getHiddenDir();
char specific_name[128];
sprintf(specific_name, "SN%d.conf", serial_number);
calibration_file = path + specific_name;
if (!checkFile(calibration_file)) {
TCHAR *settingFolder = new TCHAR[path.size() + 1];
settingFolder[path.size()] = 0;
std::copy(path.begin(), path.end(), settingFolder);
SHCreateDirectoryEx(NULL, settingFolder, NULL); //recursive creation
std::string url("https://calib.stereolabs.com/?SN=");
url += std::to_string(serial_number);
TCHAR *address = new TCHAR[url.size() + 1];
address[url.size()] = 0;
std::copy(url.begin(), url.end(), address);
TCHAR *calibPath = new TCHAR[calibration_file.size() + 1];
calibPath[calibration_file.size()] = 0;
std::copy(calibration_file.begin(), calibration_file.end(), calibPath);
HRESULT hr = URLDownloadToFile(NULL, address, calibPath, 0, NULL);
if (hr != 0) {
std::cout << "Fail to download calibration file" << std::endl;
return false;
}
if (!checkFile(calibration_file)) {
std::cout << "Invalid calibration file" << std::endl;
return false;
}
}
#endif
return true;
}
bool initCalibration(std::string calibration_file, cv::Size image_size, rtabmap::StereoCameraModel & model, const rtabmap::Transform & localTransform) {
if (!checkFile(calibration_file)) {
std::cout << "Calibration file missing." << std::endl;
return false;
}
// Open camera configuration file
ConfManager camerareader(calibration_file.c_str());
if (!camerareader.isOpened())
return false;
std::string resolution_str;
switch ((int) image_size.width) {
case 2208:
resolution_str = "2k";
break;
case 1920:
resolution_str = "fhd";
break;
case 1280:
resolution_str = "hd";
break;
case 672:
resolution_str = "vga";
break;
default:
resolution_str = "hd";
break;
}
// Get translations
float T_[3];
T_[0] = camerareader.getValue("stereo:baseline", 0.0f);
T_[1] = camerareader.getValue("stereo:ty_" + resolution_str, 0.f);
if(T_[1] == 0.f)
{
T_[1] = camerareader.getValue("stereo:ty", 0.f);
}
T_[2] = camerareader.getValue("stereo:tz_" + resolution_str, 0.f);
if(T_[2] == 0.f)
{
T_[2] = camerareader.getValue("stereo:tz", 0.f);
}
// Get left parameters
float left_cam_cx = camerareader.getValue("left_cam_" + resolution_str + ":cx", 0.0f);
float left_cam_cy = camerareader.getValue("left_cam_" + resolution_str + ":cy", 0.0f);
float left_cam_fx = camerareader.getValue("left_cam_" + resolution_str + ":fx", 0.0f);
float left_cam_fy = camerareader.getValue("left_cam_" + resolution_str + ":fy", 0.0f);
float left_cam_k1 = camerareader.getValue("left_cam_" + resolution_str + ":k1", 0.0f);
float left_cam_k2 = camerareader.getValue("left_cam_" + resolution_str + ":k2", 0.0f);
float left_cam_p1 = camerareader.getValue("left_cam_" + resolution_str + ":p1", 0.0f);
float left_cam_p2 = camerareader.getValue("left_cam_" + resolution_str + ":p2", 0.0f);
float left_cam_k3 = camerareader.getValue("left_cam_" + resolution_str + ":k3", 0.0f);
// Get right parameters
float right_cam_cx = camerareader.getValue("right_cam_" + resolution_str + ":cx", 0.0f);
float right_cam_cy = camerareader.getValue("right_cam_" + resolution_str + ":cy", 0.0f);
float right_cam_fx = camerareader.getValue("right_cam_" + resolution_str + ":fx", 0.0f);
float right_cam_fy = camerareader.getValue("right_cam_" + resolution_str + ":fy", 0.0f);
float right_cam_k1 = camerareader.getValue("right_cam_" + resolution_str + ":k1", 0.0f);
float right_cam_k2 = camerareader.getValue("right_cam_" + resolution_str + ":k2", 0.0f);
float right_cam_p1 = camerareader.getValue("right_cam_" + resolution_str + ":p1", 0.0f);
float right_cam_p2 = camerareader.getValue("right_cam_" + resolution_str + ":p2", 0.0f);
float right_cam_k3 = camerareader.getValue("right_cam_" + resolution_str + ":k3", 0.0f);
// (Linux only) Safety check A: Wrong "." or "," reading in file conf.
#ifndef _WIN32
if (right_cam_k1 == 0 && left_cam_k1 == 0 && left_cam_k2 == 0 && right_cam_k2 == 0) {
std::cout << "ZED File invalid" << std::endl;
std::string cmd = "rm " + calibration_file;
int res = system(cmd.c_str());
if( res == EXIT_FAILURE )
{
return false;
}
return false;
}
#endif
// Get rotations
cv::Mat R_zed = (cv::Mat_<double>(1, 3) << camerareader.getValue("stereo:rx_" + resolution_str, 0.f), camerareader.getValue("stereo:cv_" + resolution_str, 0.f), camerareader.getValue("stereo:rz_" + resolution_str, 0.f));
//R_zed *= -1.f; // FIXME: we had to invert T below, do we need to do this with R? I don't see much difference looking at the disparity image
cv::Mat R;
cv::Rodrigues(R_zed /*in*/, R /*out*/);
cv::Mat distCoeffs_left, distCoeffs_right;
// Left
cv::Mat cameraMatrix_left = (cv::Mat_<double>(3, 3) << left_cam_fx, 0, left_cam_cx, 0, left_cam_fy, left_cam_cy, 0, 0, 1);
distCoeffs_left = (cv::Mat_<double>(1, 5) << left_cam_k1, left_cam_k2, left_cam_p1, left_cam_p2, left_cam_k3);
// Right
cv::Mat cameraMatrix_right = (cv::Mat_<double>(3, 3) << right_cam_fx, 0, right_cam_cx, 0, right_cam_fy, right_cam_cy, 0, 0, 1);
distCoeffs_right = (cv::Mat_<double>(1, 5) << right_cam_k1, right_cam_k2, right_cam_p1, right_cam_p2, right_cam_k3);
// Stereo
cv::Mat T = (cv::Mat_<double>(3, 1) << T_[0], T_[1], T_[2]);
T /= -1000.f; // convert in meters, inverted to get positive baseline
//std::cout << " Camera Matrix L: \n" << cameraMatrix_left << std::endl << std::endl;
//std::cout << " Camera Matrix R: \n" << cameraMatrix_right << std::endl << std::endl;
//std::cout << " Camera Rotation: \n" << R << std::endl << std::endl;
//std::cout << " Camera Translation: \n" << T << std::endl << std::endl;
cv::Mat R1, R2, P1, P2, Q;
cv::stereoRectify(cameraMatrix_left, distCoeffs_left, cameraMatrix_right, distCoeffs_right, image_size, R, T,
R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 0, image_size);
model = rtabmap::StereoCameraModel("zed",
image_size, cameraMatrix_left, distCoeffs_left, R1, P1,
image_size, cameraMatrix_right, distCoeffs_right, R2, P2,
R, T, cv::Mat(), cv::Mat(), localTransform);
return true;
}
#endif
namespace rtabmap
{
#ifdef RTABMAP_ZEDOC
class ZedOCThread: public UThread
{
public:
ZedOCThread(sl_oc::sensors::SensorCapture* sensCap, const Transform & imuLocalTransform)
{
sensCap_= sensCap;
imuLocalTransform_ = imuLocalTransform;
}
void getIMU(
const double & stamp,
IMU & imu,
int maxWaitTimeMs)
{
imu = IMU();
if(imuBuffer_.empty())
{
return;
}
// Interpolate imu
cv::Vec3d acc;
cv::Vec3d gyr;
int waitTry = 0;
imuMutex_.lock();
while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
{
imuMutex_.unlock();
++waitTry;
uSleep(1);
imuMutex_.lock();
}
bool set = false;
if(imuBuffer_.rbegin()->first < stamp)
{
if(maxWaitTimeMs>0)
{
UWARN("Could not find imu data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, imuBuffer_.rbegin()->first);
}
}
else
{
std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterB = imuBuffer_.lower_bound(stamp);
std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterA = iterB;
if(iterA != imuBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == imuBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
acc[0] = iterA->second.first[0];
acc[1] = iterA->second.first[1];
acc[2] = iterA->second.first[2];
gyr[0] = iterA->second.second[0];
gyr[1] = iterA->second.second[1];
gyr[2] = iterA->second.second[2];
set = true;
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
acc[0] = iterA->second.first[0] + t*(iterB->second.first[0] - iterA->second.first[0]);
acc[1] = iterA->second.first[1] + t*(iterB->second.first[1] - iterA->second.first[1]);
acc[2] = iterA->second.first[2] + t*(iterB->second.first[2] - iterA->second.first[2]);
gyr[0] = iterA->second.second[0] + t*(iterB->second.second[0] - iterA->second.second[0]);
gyr[1] = iterA->second.second[1] + t*(iterB->second.second[1] - iterA->second.second[1]);
gyr[2] = iterA->second.second[2] + t*(iterB->second.second[2] - iterA->second.second[2]);
set = true;
}
else
{
if(stamp < iterA->first)
{
UDEBUG("Could not find imu data to interpolate at image time %f (earliest is %f). Are sensors synchronized? (may take some time to be synchronized...)", stamp, iterA->first);
}
else
{
UDEBUG("Could not find imu data to interpolate at image time %f (between %f and %f). Are sensors synchronized? (may take some time to be synchronized...)", stamp, iterA->first, iterB->first);
}
}
}
imuMutex_.unlock();
if(set)
{
imu = IMU(gyr, cv::Mat::eye(3, 3, CV_64FC1),
acc, cv::Mat::eye(3, 3, CV_64FC1),
imuLocalTransform_);
}
return;
}
private:
virtual void mainLoop()
{
// ----> Get IMU data
const sl_oc::sensors::data::Imu imuData = sensCap_->getLastIMUData(2000);
// Process data only if valid
if(imuData.valid == sl_oc::sensors::data::Imu::NEW_VAL ) // Uncomment to use only data syncronized with the video frames
{
UScopeMutex sm(imuMutex_);
static double deg2rad = 0.017453293;
std::pair<cv::Vec3d, cv::Vec3d> imu(
cv::Vec3d(imuData.aX, imuData.aY, imuData.aZ),
cv::Vec3d(imuData.gX*deg2rad, imuData.gY*deg2rad, imuData.gZ*deg2rad));
double stamp = double(imuData.timestamp)/10e8;
if(!imuBuffer_.empty() && imuBuffer_.rbegin()->first > stamp)
{
UWARN("IMU data not received in order, reset buffer! (previous=%f new=%f)", imuBuffer_.rbegin()->first, stamp);
imuBuffer_.clear();
}
imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, imu));
if(imuBuffer_.size() > 1000)
{
imuBuffer_.erase(imuBuffer_.begin());
}
}
}
sl_oc::sensors::SensorCapture* sensCap_;
Transform imuLocalTransform_;
UMutex imuMutex_;
std::map<double, std::pair<cv::Vec3f, cv::Vec3f> > imuBuffer_;
};
#endif
bool CameraStereoZedOC::available()
{
#ifdef RTABMAP_ZEDOC
return true;
#else
return false;
#endif
}
CameraStereoZedOC::CameraStereoZedOC(
int deviceId,
int resolution,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_ZEDOC
,
zed_(0),
sensors_(0),
imuThread_(0),
usbDevice_(deviceId),
resolution_(resolution),
lastStamp_(0)
#endif
{
UDEBUG("");
#ifdef RTABMAP_ZEDOC
sl_oc::video::RESOLUTION res = static_cast<sl_oc::video::RESOLUTION>(resolution_);
UASSERT(res >= sl_oc::video::RESOLUTION::HD2K && res < sl_oc::video::RESOLUTION::LAST);
#endif
}
CameraStereoZedOC::~CameraStereoZedOC()
{
#ifdef RTABMAP_ZEDOC
if(imuThread_)
{
imuThread_->join(true);
delete imuThread_;
}
delete zed_;
delete sensors_;
#endif
}
bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
#ifdef RTABMAP_ZEDOC
if(imuThread_)
{
imuThread_->join(true);
delete imuThread_;
imuThread_=0;
}
if(zed_)
{
delete zed_;
zed_ = 0;
}
if(sensors_)
{
delete sensors_;
sensors_ = 0;
}
lastStamp_ = 0;
// ----> Set Video parameters
sl_oc::video::VideoParams params;
params.res = static_cast<sl_oc::video::RESOLUTION>(resolution_);
params.fps = sl_oc::video::FPS::FPS_15;
if(this->getImageRate() > 60)
{
params.fps = sl_oc::video::FPS::FPS_100;
}
else if(this->getImageRate() > 30)
{
params.fps = sl_oc::video::FPS::FPS_60;
}
else if(this->getImageRate() > 15)
{
params.fps = sl_oc::video::FPS::FPS_30;
}
if(ULogger::level() <= ULogger::kInfo)
{
params.verbose = sl_oc::VERBOSITY::INFO;
}
else if(ULogger::level() <= ULogger::kWarning)
{
params.verbose = sl_oc::VERBOSITY::WARNING;
}
// <---- Set Video parameters
// ----> Create Video Capture
zed_ = new sl_oc::video::VideoCapture(params);
if( !zed_->initializeVideo(usbDevice_) )
{
std::cerr << "Cannot open camera video capture" << std::endl;
std::cerr << "See verbosity level for more details." << std::endl;
delete zed_;
zed_ = 0;
return false;
}
int sn = zed_->getSerialNumber();
std::cout << "Connected to camera sn: " << sn << std::endl;
// <---- Create Video Capture
// ----> Retrieve calibration file from Stereolabs server
std::string calibration_file;
// ZED Calibration
unsigned int serial_number = sn;
// Download camera calibration file
if( !downloadCalibrationFile(serial_number, calibration_file) )
{
std::cerr << "Could not load calibration file from Stereolabs servers" << std::endl;
delete zed_;
zed_ = 0;
return false;
}
std::cout << "Calibration file found. Loading..." << std::endl;
// ----> Frame size
int w,h;
zed_->getFrameSize(w,h);
// <---- Frame size
// ----> Initialize calibration
if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform()))
{
std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
stereoModel_.initRectificationMap();
}
// ----> Create a Sensors Capture object
sensors_ = new sl_oc::sensors::SensorCapture((sl_oc::VERBOSITY)params.verbose);
if( !sensors_->initializeSensors(serial_number) ) // Note: we use the serial number acquired by the VideoCapture object
{
std::cerr << "Cannot open sensors capture" << std::endl;
std::cerr << "Try to enable verbose to get more info" << std::endl;
delete sensors_;
sensors_ = 0;
}
else
{
std::cout << "Sensors Capture connected to camera sn: " << sensors_->getSerialNumber() << std::endl;
// Check is IMU data is available
const sl_oc::sensors::data::Imu& imu = sensors_->getLastIMUData();
if(imu.valid != sl_oc::sensors::data::Imu::NEW_VAL)
{
UINFO("CameraStereoZEDOC: Camera doesn't have IMU sensor");
delete sensors_;
sensors_ = 0;
}
else
{
// Start the sensor capture thread. Note: since sensor data can be retrieved at 400Hz and video data frequency is
// minor (max 100Hz), we use a separated thread for sensors.
// Transform based on ZED2: x->down, y->right, z->backward
Transform imuLocalTransform_ = this->getLocalTransform() * Transform(0,1,0,0, 1,0,0,0, 0,0,-1,0);
std::cout << imuLocalTransform_ << std::endl;
imuThread_ = new ZedOCThread(sensors_, imuLocalTransform_);
// <---- Create Sensors Capture
// ----> Enable video/sensors synchronization
if(!zed_->enableSensorSync(sensors_))
{
UWARN("CameraStereoZEDOC: Failed to start synchronization");
}
// <---- Enable video/sensors synchronization
imuThread_->start();
}
}
return true;
#else
UERROR("CameraStereoZEDOC: RTAB-Map is not built with ZED Open Capture support!");
#endif
return false;
}
bool CameraStereoZedOC::isCalibrated() const
{
#ifdef RTABMAP_ZEDOC
return stereoModel_.isValidForProjection();
#else
return false;
#endif
}
std::string CameraStereoZedOC::getSerial() const
{
#ifdef RTABMAP_ZEDOC
if(zed_)
{
return uFormat("%x", zed_->getSerialNumber());
}
#endif
return "";
}
SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZEDOC
// Get a new frame from camera
if(zed_)
{
UTimer timer;
bool imuReceived = true;
do
{
const sl_oc::video::Frame frame = zed_->getLastFrame();
if(frame.data!=nullptr && frame.timestamp!=lastStamp_)
{
lastStamp_ = frame.timestamp;
double stamp = double(lastStamp_)/10e8;
// If the sensor supports IMU, wait IMU to be available before sending data.
IMU imu;
if(imuThread_)
{
imuThread_->getIMU(stamp, imu, 10);
imuReceived = !imu.empty();
}
if(imuReceived)
{
cv::Mat frameBGR, left, right;
// ----> Conversion from YUV 4:2:2 to BGR for visualization
cv::Mat frameYUV = cv::Mat( frame.height, frame.width, CV_8UC2, frame.data );
cv::cvtColor(frameYUV,frameBGR,cv::COLOR_YUV2BGR_YUYV);
// <---- Conversion from YUV 4:2:2 to BGR for visualization
// ----> Extract left and right images from side-by-side
left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows));
cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
// <---- Extract left and right images from side-by-side
if(stereoModel_.isValidForRectification())
{
left = stereoModel_.left().rectifyImage(left);
right = stereoModel_.right().rectifyImage(right);
}
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
if(!imu.empty())
{
data.setIMU(imu);
}
}
}
else
{
UERROR("CameraStereoZEDOC: Cannot get frame from the camera since 100 msec!");
imuReceived = true;
}
}
while(!imuReceived && timer.elapsed() < 15.0);
// ----> If the frame is valid we can convert, rectify and display it
if(!imuReceived)
{
UERROR("CameraStereoZEDOC: Cannot get synchronized IMU with camera for 15 sec!");
}
}
#else
UERROR("CameraStereoZEDOC: RTAB-Map is not built with ZED Open Capture support!");
#endif
return data;
}
} // namespace rtabmap