|
|
|
@@ -65,6 +65,10 @@ CameraDepthAI::CameraDepthAI(
|
|
|
|
|
|
|
|
|
|
|
|
CameraDepthAI::~CameraDepthAI()
|
|
|
|
CameraDepthAI::~CameraDepthAI()
|
|
|
|
{
|
|
|
|
{
|
|
|
|
|
|
|
|
if(device_.get())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
device_->close();
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
|
|
|
|
void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
|
|
|
|
@@ -80,91 +84,6 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
std::vector<unsigned char> convertCalibration(const StereoCameraModel & stereoModel)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UDEBUG("");
|
|
|
|
|
|
|
|
// Calibration
|
|
|
|
|
|
|
|
// https://github.com/luxonis/depthai/blob/39852dcb9fe349476c30d0ed90d3750bb2a53e26/depthai_helpers/calibration_utils.py#L97-L109
|
|
|
|
|
|
|
|
std::vector<unsigned char> data;
|
|
|
|
|
|
|
|
cv::Mat tmp;
|
|
|
|
|
|
|
|
int ptr;
|
|
|
|
|
|
|
|
// R1_fp32
|
|
|
|
|
|
|
|
stereoModel.left().R().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// R2_fp32
|
|
|
|
|
|
|
|
stereoModel.right().R().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// M1_fp32
|
|
|
|
|
|
|
|
stereoModel.left().K_raw().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// M2_fp32
|
|
|
|
|
|
|
|
stereoModel.right().K_raw().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// R_fp32
|
|
|
|
|
|
|
|
stereoModel.R().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// T_fp32
|
|
|
|
|
|
|
|
stereoModel.T().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// M3_fp32
|
|
|
|
|
|
|
|
tmp = cv::Mat::zeros(3,3,CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// R_rgb_fp32
|
|
|
|
|
|
|
|
tmp = cv::Mat::eye(3,3,CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// T_rgb_fp32
|
|
|
|
|
|
|
|
tmp = cv::Mat::zeros(1,3,CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// d1_coeff_fp32
|
|
|
|
|
|
|
|
stereoModel.left().D_raw().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
data.resize(data.size() + (14-tmp.total())*sizeof(float), 0); // padding
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// d2_coeff_fp32
|
|
|
|
|
|
|
|
stereoModel.right().D_raw().convertTo(tmp, CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
data.resize(data.size() + (14-tmp.total())*sizeof(float), 0); // padding
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// d3_coeff_fp32
|
|
|
|
|
|
|
|
tmp = cv::Mat::zeros(1,14,CV_32FC1);
|
|
|
|
|
|
|
|
ptr = data.size();
|
|
|
|
|
|
|
|
data.resize(data.size() + tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return data;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
|
|
|
|
bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
|
|
|
|
{
|
|
|
|
{
|
|
|
|
UDEBUG("");
|
|
|
|
UDEBUG("");
|
|
|
|
@@ -176,6 +95,13 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|
|
|
return false;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if(device_.get())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
device_->close();
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
accBuffer_.clear();
|
|
|
|
|
|
|
|
gyroBuffer_.clear();
|
|
|
|
|
|
|
|
|
|
|
|
dai::DeviceInfo deviceToUse;
|
|
|
|
dai::DeviceInfo deviceToUse;
|
|
|
|
if(deviceSerial_.empty())
|
|
|
|
if(deviceSerial_.empty())
|
|
|
|
deviceToUse = devices[0];
|
|
|
|
deviceToUse = devices[0];
|
|
|
|
@@ -201,77 +127,21 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|
|
|
|
|
|
|
|
|
|
|
// look for calibration files
|
|
|
|
// look for calibration files
|
|
|
|
stereoModel_ = StereoCameraModel();
|
|
|
|
stereoModel_ = StereoCameraModel();
|
|
|
|
if(!calibrationFolder.empty())
|
|
|
|
cv::Size targetSize(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
|
|
|
|
{
|
|
|
|
|
|
|
|
std::string name = cameraName.empty()?deviceSerial_:cameraName;
|
|
|
|
|
|
|
|
if(!stereoModel_.load(calibrationFolder, name, false))
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
|
|
|
|
|
|
|
|
name.c_str(), calibrationFolder.c_str());
|
|
|
|
|
|
|
|
outputDepth_ = false;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
|
|
|
|
|
|
|
|
stereoModel_.left().fx(),
|
|
|
|
|
|
|
|
stereoModel_.left().cx(),
|
|
|
|
|
|
|
|
stereoModel_.left().cy(),
|
|
|
|
|
|
|
|
stereoModel_.baseline());
|
|
|
|
|
|
|
|
stereoModel_.setLocalTransform(this->getLocalTransform());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
cv::Size target(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if(stereoModel_.left().imageWidth() != target.width)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
//adjust scale if resolution is not the same used than in calibration
|
|
|
|
|
|
|
|
UWARN("Loaded calibration has different resolution (%dx%d) than "
|
|
|
|
|
|
|
|
"the selected device resolution (%dx%d). We will scale the calibration "
|
|
|
|
|
|
|
|
"for convenience.",
|
|
|
|
|
|
|
|
stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
|
|
|
|
|
|
|
|
target.width, target.height);
|
|
|
|
|
|
|
|
stereoModel_.scale(double(target.width)/double(stereoModel_.left().imageWidth()));
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if(stereoModel_.left().imageHeight() != target.height)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
// Ratio not the same, adjust cy
|
|
|
|
|
|
|
|
cv::Rect roi(0, (stereoModel_.left().imageHeight()-target.height)/2, target.width, target.height);
|
|
|
|
|
|
|
|
UWARN("Loaded calibration has different height (%dx%d) than "
|
|
|
|
|
|
|
|
"the selected device resolution (%dx%d). We will crop the calibration "
|
|
|
|
|
|
|
|
"for convenience.",
|
|
|
|
|
|
|
|
stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
|
|
|
|
|
|
|
|
target.width, target.height);
|
|
|
|
|
|
|
|
stereoModel_.roi(roi);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if(ULogger::level() <= ULogger::kInfo)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UINFO("Calibration:");
|
|
|
|
|
|
|
|
std::cout << stereoModel_ << std::endl;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if(!stereoModel_.isValidForRectification())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UINFO("Disabling outputDepth as no valid calibration has been loaded.");
|
|
|
|
|
|
|
|
outputDepth_ = false;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
stereoModel_.initRectificationMap();
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
dai::Pipeline p;
|
|
|
|
dai::Pipeline p;
|
|
|
|
auto monoLeft = p.create<dai::node::MonoCamera>();
|
|
|
|
auto monoLeft = p.create<dai::node::MonoCamera>();
|
|
|
|
auto monoRight = p.create<dai::node::MonoCamera>();
|
|
|
|
auto monoRight = p.create<dai::node::MonoCamera>();
|
|
|
|
auto stereo = p.create<dai::node::StereoDepth>();
|
|
|
|
auto stereo = p.create<dai::node::StereoDepth>();
|
|
|
|
|
|
|
|
auto imu = p.create<dai::node::IMU>();
|
|
|
|
auto xoutLeft = p.create<dai::node::XLinkOut>();
|
|
|
|
auto xoutLeft = p.create<dai::node::XLinkOut>();
|
|
|
|
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
|
|
|
|
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
|
|
|
|
|
|
|
|
auto xoutIMU = p.create<dai::node::XLinkOut>();
|
|
|
|
|
|
|
|
|
|
|
|
// XLinkOut
|
|
|
|
// XLinkOut
|
|
|
|
xoutLeft->setStreamName(outputDepth_/*stereoModel_.isValidForRectification()*/?"rectified_left":"left");
|
|
|
|
xoutLeft->setStreamName("rectified_left");
|
|
|
|
xoutDepthOrRight->setStreamName(outputDepth_?"depth"/*:stereoModel_.isValidForRectification()?"rectified_right"*/:"right");
|
|
|
|
xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
|
|
|
|
|
|
|
|
xoutIMU->setStreamName("imu");
|
|
|
|
|
|
|
|
|
|
|
|
// MonoCamera
|
|
|
|
// MonoCamera
|
|
|
|
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
|
|
|
|
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
|
|
|
|
@@ -285,9 +155,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// StereoDepth
|
|
|
|
// StereoDepth
|
|
|
|
stereo->setOutputDepth(outputDepth_);
|
|
|
|
stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
|
|
|
|
stereo->setOutputRectified(stereoModel_.isValidForRectification());
|
|
|
|
|
|
|
|
stereo->setConfidenceThreshold(depthConfidence_);
|
|
|
|
|
|
|
|
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
|
|
|
|
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
|
|
|
|
stereo->setRectifyMirrorFrame(false);
|
|
|
|
stereo->setRectifyMirrorFrame(false);
|
|
|
|
stereo->setLeftRightCheck(false);
|
|
|
|
stereo->setLeftRightCheck(false);
|
|
|
|
@@ -303,42 +171,55 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|
|
|
stereo->rectifiedLeft.link(xoutLeft->input);
|
|
|
|
stereo->rectifiedLeft.link(xoutLeft->input);
|
|
|
|
stereo->depth.link(xoutDepthOrRight->input);
|
|
|
|
stereo->depth.link(xoutDepthOrRight->input);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/*else if(stereoModel_.isValidForRectification())
|
|
|
|
else
|
|
|
|
{
|
|
|
|
{
|
|
|
|
stereo->rectifiedLeft.link(xoutLeft->input);
|
|
|
|
stereo->rectifiedLeft.link(xoutLeft->input);
|
|
|
|
stereo->rectifiedRight.link(xoutDepthOrRight->input);
|
|
|
|
stereo->rectifiedRight.link(xoutDepthOrRight->input);
|
|
|
|
}*/
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
stereo->syncedLeft.link(xoutLeft->input);
|
|
|
|
|
|
|
|
stereo->syncedRight.link(xoutDepthOrRight->input);
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
|
|
|
|
|
|
|
|
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
|
|
|
|
|
|
|
|
// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
|
|
|
|
|
|
|
|
imu->setBatchReportThreshold(1);
|
|
|
|
|
|
|
|
// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
|
|
|
|
|
|
|
|
// if lower or equal to batchReportThreshold then the sending is always blocking on device
|
|
|
|
|
|
|
|
// useful to reduce device's CPU load and number of lost packets, if CPU load is high on device side due to multiple nodes
|
|
|
|
|
|
|
|
imu->setMaxBatchReports(10);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Link plugins IMU -> XLINK
|
|
|
|
|
|
|
|
imu->out.link(xoutIMU->input);
|
|
|
|
|
|
|
|
|
|
|
|
if(stereoModel_.isValidForRectification())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
// FIXME: What is the exact format for the calibration stream?
|
|
|
|
|
|
|
|
//std::vector<unsigned char> data = convertCalibration(stereoModel_);
|
|
|
|
|
|
|
|
//stereo->loadCalibrationData(data);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
device_.reset(new dai::Device(p, deviceToUse));
|
|
|
|
device_.reset(new dai::Device(p, deviceToUse));
|
|
|
|
|
|
|
|
|
|
|
|
UDEBUG("");
|
|
|
|
UINFO("Loading eeprom calibration data");
|
|
|
|
if(outputDepth_)
|
|
|
|
dai::CalibrationHandler calibHandler = device_->readCalibration();
|
|
|
|
{
|
|
|
|
std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height));
|
|
|
|
leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
|
|
|
|
double fx = matrix[0][0];
|
|
|
|
rightOrDepthQueue_ = device_->getOutputQueue("depth", 8, false);
|
|
|
|
double fy = matrix[1][1];
|
|
|
|
}
|
|
|
|
double cx = matrix[0][2];
|
|
|
|
else
|
|
|
|
double cy = matrix[1][2];
|
|
|
|
{
|
|
|
|
matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT);
|
|
|
|
UDEBUG("");
|
|
|
|
double baseline = matrix[0][3]/100.0;
|
|
|
|
leftQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_left":*/"left", 8, false);
|
|
|
|
UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
|
|
|
|
UDEBUG("");
|
|
|
|
stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
|
|
|
|
rightOrDepthQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_right":*/"right", 8, false);
|
|
|
|
|
|
|
|
UDEBUG("");
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
device_->startPipeline();
|
|
|
|
// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
|
|
|
|
|
|
|
|
//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
|
|
|
|
|
|
|
|
//imuLocalTransform_ = Transform(
|
|
|
|
|
|
|
|
// matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
|
|
|
|
|
|
|
|
// matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
|
|
|
|
|
|
|
|
// matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
|
|
|
|
|
|
|
|
// Hard-coded acc: x->left, y->up, z->forward
|
|
|
|
|
|
|
|
// Hard-coded gyro: x->down, y->left, z->forward
|
|
|
|
|
|
|
|
imuLocalTransform_ = Transform(
|
|
|
|
|
|
|
|
0, 0, 1, 0,
|
|
|
|
|
|
|
|
1, 0, 0, 0,
|
|
|
|
|
|
|
|
0 ,1, 0, 0);
|
|
|
|
|
|
|
|
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
|
|
|
|
|
|
|
|
rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
|
|
|
|
|
|
|
|
imuQueue_ = device_->getOutputQueue("imu", 50, false);
|
|
|
|
|
|
|
|
|
|
|
|
uSleep(2000); // avoid bad frames on start
|
|
|
|
uSleep(2000); // avoid bad frames on start
|
|
|
|
|
|
|
|
|
|
|
|
@@ -374,6 +255,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|
|
|
cv::Mat left, depthOrRight;
|
|
|
|
cv::Mat left, depthOrRight;
|
|
|
|
auto rectifL = leftQueue_->get<dai::ImgFrame>();
|
|
|
|
auto rectifL = leftQueue_->get<dai::ImgFrame>();
|
|
|
|
auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
|
|
|
|
auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
|
|
|
|
|
|
|
|
|
|
|
|
if(rectifL.get() && rectifRightOrDepth.get())
|
|
|
|
if(rectifL.get() && rectifRightOrDepth.get())
|
|
|
|
{
|
|
|
|
{
|
|
|
|
auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
|
|
|
|
auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
|
|
|
|
@@ -399,10 +281,141 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|
|
|
data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
|
|
|
|
data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if(stampLeft != stampRight)
|
|
|
|
if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms
|
|
|
|
{
|
|
|
|
{
|
|
|
|
UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
|
|
|
|
UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//get imu
|
|
|
|
|
|
|
|
int added= 0;
|
|
|
|
|
|
|
|
while(1)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
auto imuData = imuQueue_->get<dai::IMUData>();
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
auto imuPackets = imuData->packets;
|
|
|
|
|
|
|
|
double accStamp = 0.0;
|
|
|
|
|
|
|
|
double gyroStamp = 0.0;
|
|
|
|
|
|
|
|
for(auto& imuPacket : imuPackets) {
|
|
|
|
|
|
|
|
auto& acceleroValues = imuPacket.acceleroMeter;
|
|
|
|
|
|
|
|
auto& gyroValues = imuPacket.gyroscope;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
|
|
|
|
|
|
|
|
gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
|
|
|
|
|
|
|
|
accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
|
|
|
|
|
|
|
|
gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
|
|
|
|
|
|
|
|
if(accBuffer_.size() > 1000)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
accBuffer_.erase(accBuffer_.begin());
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(gyroBuffer_.size() > 1000)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
gyroBuffer_.erase(gyroBuffer_.begin());
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
++added;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(accStamp >= stamp && gyroStamp >= stamp)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
cv::Vec3d acc, gyro;
|
|
|
|
|
|
|
|
bool valid = !accBuffer_.empty() && !gyroBuffer_.empty();
|
|
|
|
|
|
|
|
//acc
|
|
|
|
|
|
|
|
if(!accBuffer_.empty())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
|
|
|
|
|
|
|
|
std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
|
|
|
|
|
|
|
|
if(iterA != accBuffer_.begin())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
iterA = --iterA;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(iterB == accBuffer_.end())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
iterB = --iterB;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(iterA == iterB && stamp == iterA->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
acc[0] = iterA->second[0];
|
|
|
|
|
|
|
|
acc[1] = iterA->second[1];
|
|
|
|
|
|
|
|
acc[2] = iterA->second[2];
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else if(stamp >= iterA->first && stamp <= iterB->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
|
|
|
|
|
|
|
|
acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
|
|
|
|
|
|
|
|
acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
|
|
|
|
|
|
|
|
acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
valid = false;
|
|
|
|
|
|
|
|
if(stamp < iterA->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else if(stamp > iterB->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
//gyro
|
|
|
|
|
|
|
|
if(!gyroBuffer_.empty())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
|
|
|
|
|
|
|
|
std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
|
|
|
|
|
|
|
|
if(iterA != gyroBuffer_.begin())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
iterA = --iterA;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(iterB == gyroBuffer_.end())
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
iterB = --iterB;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(iterA == iterB && stamp == iterA->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
gyro[0] = iterA->second[0];
|
|
|
|
|
|
|
|
gyro[1] = iterA->second[1];
|
|
|
|
|
|
|
|
gyro[2] = iterA->second[2];
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else if(stamp >= iterA->first && stamp <= iterB->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
|
|
|
|
|
|
|
|
gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
|
|
|
|
|
|
|
|
gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
|
|
|
|
|
|
|
|
gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
valid = false;
|
|
|
|
|
|
|
|
if(stamp < iterA->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else if(stamp > iterB->first)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
// Rotate gyro frame (x->down, y->left, z->forward) in acc frame (x->left, y->up, z->forward)
|
|
|
|
|
|
|
|
double tmp = gyro[0];
|
|
|
|
|
|
|
|
gyro[0] = gyro[1];
|
|
|
|
|
|
|
|
gyro[1] = -tmp;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if(valid)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
else
|
|
|
|
|