tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: add ROS camera info api.

Browse Source
This commit is contained in:
TinyOh 2019-03-26 13:58:20 +08:00
parent de24292cca
commit d36a288336
15 changed files with 200 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/mynteye/api/api.h
View File

@ -363,6 +363,11 @@ class MYNTEYE_API API {
/** Enable process mode, e.g. imu assembly, temp_drift */
void EnableProcessMode(const std::int32_t& mode);
/**
* Get ROS need camera info struct
*/
std::shared_ptr<struct CameraROSMsgInfoPair> GetCameraROSMsgInfoPair();
private:
std::shared_ptr<Device> device_;

34
include/mynteye/types.h
View File

@ -16,6 +16,7 @@
#pragma once
#include <memory.h>
#include <string>
#include <cstdint>
@ -670,7 +671,6 @@ struct MYNTEYE_API DisparityParamsBM : public DisparityParamsBase {
}
};
/**
* @defgroup datatypes Datatypes
* @brief Public data types.
@ -764,6 +764,38 @@ struct MYNTEYE_API OptionInfo {
MYNTEYE_API
std::ostream &operator<<(std::ostream &os, const OptionInfo &info);
/**
* @ingroup datatypes
* ROS camera info.
*/
struct MYNTEYE_API CameraROSMsgInfo {
/** height */
unsigned int height = 0;
/** width */
unsigned int width = 0;
/** calib model */
std::string distortion_model = "null";
double D[5] = {0};
double K[9] = {0};
double R[9] = {0};
double P[12] = {0};
};
MYNTEYE_API
std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfo &info);
struct CameraROSMsgInfoPair {
inline bool isEmpty() {
return left.height * left.width * right.height * right.width <= 0;
}
struct CameraROSMsgInfo left;
struct CameraROSMsgInfo right;
double T_mul_f = -1.f;
double R[9] = {0};
double P[12] = {0};
};
MYNTEYE_API
std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfoPair &info);
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_TYPES_H_

5
samples/tutorials/data/get_img_params.cc
View File

@ -21,6 +21,11 @@ int main(int argc, char *argv[]) {
auto &&api = API::Create(argc, argv);
if (!api) return 1;
auto info = api->GetCameraROSMsgInfoPair();
if (!info->isEmpty())
std::cout << "ROSMsgInfoPair:"<< std::endl << *info << std::endl;
bool ok;
auto &&request = api->SelectStreamRequest(&ok);
if (!ok) return 1;

4
src/mynteye/api/api.cc
View File

@ -600,4 +600,8 @@ void API::EnableProcessMode(const std::int32_t& mode) {
device_->EnableProcessMode(mode);
}
std::shared_ptr<struct CameraROSMsgInfoPair> API::GetCameraROSMsgInfoPair() {
return synthetic_->GetCameraROSMsgInfoPair();
}
MYNTEYE_END_NAMESPACE

2
src/mynteye/api/processor/depth_processor.cc
View File

@ -25,7 +25,7 @@ const int DISPARITY_MIN = 0;
const int DISPARITY_MAX = 64;
DepthProcessor::DepthProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
std::int32_t proc_period)
: Processor(std::move(proc_period)),
calib_infos_(calib_infos) {

5
src/mynteye/api/processor/depth_processor.h
View File

@ -19,6 +19,7 @@
#include "mynteye/api/processor.h"
#include "mynteye/api/processor/rectify_processor.h"
#include "mynteye/types.h"
MYNTEYE_BEGIN_NAMESPACE
@ -27,7 +28,7 @@ class DepthProcessor : public Processor {
static const char NAME[];
explicit DepthProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
std::int32_t proc_period = 0);
virtual ~DepthProcessor();
@ -42,7 +43,7 @@ class DepthProcessor : public Processor {
Object *const in, Object *const out,
std::shared_ptr<Processor> const parent) override;
private:
std::shared_ptr<struct camera_calib_info_pair> calib_infos_;
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos_;
};
MYNTEYE_END_NAMESPACE

2
src/mynteye/api/processor/points_processor.cc
View File

@ -53,7 +53,7 @@ struct DepthTraits<float> {
const char PointsProcessor::NAME[] = "PointsProcessor";
PointsProcessor::PointsProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
std::int32_t proc_period)
: Processor(std::move(proc_period)),
calib_infos_(calib_infos) {

6
src/mynteye/api/processor/points_processor.h
View File

@ -20,7 +20,7 @@
#include <opencv2/core/core.hpp>
#include "mynteye/api/processor.h"
#include "mynteye/api/processor/rectify_processor.h"
#include "mynteye/types.h"
MYNTEYE_BEGIN_NAMESPACE
@ -29,7 +29,7 @@ class PointsProcessor : public Processor {
static const char NAME[];
explicit PointsProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
std::int32_t proc_period = 0);
virtual ~PointsProcessor();
@ -45,7 +45,7 @@ class PointsProcessor : public Processor {
std::shared_ptr<Processor> const parent) override;
private:
std::shared_ptr<struct camera_calib_info_pair> calib_infos_;
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos_;
};
MYNTEYE_END_NAMESPACE

37
src/mynteye/api/processor/rectify_processor.cc
View File

@ -222,16 +222,16 @@ Eigen::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics& in) {
void RectifyProcessor::loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT
cv::Size& image_size, // NOLINT
struct camera_calib_info& calib_data) { // NOLINT
struct CameraROSMsgInfo& calib_data) { // NOLINT
K = cv::Mat(3, 3, CV_64F, calib_data.K);
std::size_t d_length = 4;
D = cv::Mat(1, d_length, CV_64F, calib_data.D);
image_size = cv::Size(calib_data.width, calib_data.height);
}
struct camera_calib_info RectifyProcessor::getCalibMatData(
struct CameraROSMsgInfo RectifyProcessor::getCalibMatData(
const mynteye::IntrinsicsEquidistant& in) {
struct camera_calib_info calib_mat_data;
struct CameraROSMsgInfo calib_mat_data;
calib_mat_data.distortion_model = "KANNALA_BRANDT";
calib_mat_data.height = in.height;
calib_mat_data.width = in.width;
@ -248,7 +248,7 @@ struct camera_calib_info RectifyProcessor::getCalibMatData(
return calib_mat_data;
}
std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo,
mynteye::IntrinsicsEquidistant in_left,
@ -263,8 +263,8 @@ std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
cv::Mat K1, D1, K2, D2;
cv::Size image_size1, image_size2;
struct camera_calib_info calib_mat_data_left = getCalibMatData(in_left);
struct camera_calib_info calib_mat_data_right = getCalibMatData(in_right);
struct CameraROSMsgInfo calib_mat_data_left = getCalibMatData(in_left);
struct CameraROSMsgInfo calib_mat_data_right = getCalibMatData(in_right);
loadCameraMatrix(K1, D1, image_size1, calib_mat_data_left);
loadCameraMatrix(K2, D2, image_size2, calib_mat_data_right);
@ -308,9 +308,26 @@ std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
calib_mat_data_right.R[i*3 +j] = R2.at<double>(i, j);
}
}
struct camera_calib_info_pair res =
{calib_mat_data_left, calib_mat_data_right, T_mul_f};
return std::make_shared<struct camera_calib_info_pair>(res);
struct CameraROSMsgInfoPair info_pair;
info_pair.left = calib_mat_data_left;
info_pair.right = calib_mat_data_right;
info_pair.T_mul_f = T_mul_f;
for (std::size_t i = 0; i< 3 * 4; i++) {
info_pair.P[i] = calib_mat_data_left.P[i];
}
info_pair.R[0] = ex_right_to_left.rotation[0][0];
info_pair.R[1] = ex_right_to_left.rotation[0][1];
info_pair.R[2] = ex_right_to_left.rotation[0][2];
info_pair.R[3] = ex_right_to_left.rotation[1][0];
info_pair.R[4] = ex_right_to_left.rotation[1][1];
info_pair.R[5] = ex_right_to_left.rotation[1][2];
info_pair.R[6] = ex_right_to_left.rotation[2][0];
info_pair.R[7] = ex_right_to_left.rotation[2][1];
info_pair.R[8] = ex_right_to_left.rotation[2][2];
return std::make_shared<struct CameraROSMsgInfoPair>(info_pair);
}
camodocal::CameraPtr RectifyProcessor::generateCameraFromIntrinsicsEquidistant(
@ -382,7 +399,7 @@ RectifyProcessor::RectifyProcessor(
std::int32_t proc_period)
: Processor(std::move(proc_period)),
calib_model(CalibrationModel::UNKNOW) {
calib_infos = std::make_shared<struct camera_calib_info_pair>();
calib_infos = std::make_shared<struct CameraROSMsgInfoPair>();
InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),

28
src/mynteye/api/processor/rectify_processor.h
View File

@ -31,22 +31,6 @@
MYNTEYE_BEGIN_NAMESPACE
struct camera_calib_info {
unsigned int height = 0;
unsigned int width = 0;
std::string distortion_model = "null";
double D[4] = {0};
double K[9] = {0};
double R[9] = {0};
double P[12] = {0};
};
struct camera_calib_info_pair {
struct camera_calib_info left;
struct camera_calib_info right;
double T_mul_f;
};
class Device;
class RectifyProcessor : public Processor {
@ -69,8 +53,8 @@ class RectifyProcessor : public Processor {
cv::Mat R1, P1, R2, P2, Q;
cv::Mat map11, map12, map21, map22;
inline std::shared_ptr<struct camera_calib_info_pair> getCalibInfoPair() {
inline std::shared_ptr<struct CameraROSMsgInfoPair>
getCameraROSMsgInfoPair() {
return calib_infos;
}
@ -103,12 +87,12 @@ class RectifyProcessor : public Processor {
Eigen::Matrix4d loadT(const mynteye::Extrinsics& in);
void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT
cv::Size& image_size, // NOLINT
struct camera_calib_info& calib_data); // NOLINT
struct CameraROSMsgInfo& calib_data); // NOLINT
struct camera_calib_info getCalibMatData(
struct CameraROSMsgInfo getCalibMatData(
const mynteye::IntrinsicsEquidistant& in);
std::shared_ptr<struct camera_calib_info_pair> stereoRectify(
std::shared_ptr<struct CameraROSMsgInfoPair> stereoRectify(
camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo,
mynteye::IntrinsicsEquidistant in_left,
@ -119,7 +103,7 @@ class RectifyProcessor : public Processor {
const mynteye::IntrinsicsEquidistant & in);
CalibrationModel calib_model;
std::shared_ptr<struct camera_calib_info_pair> calib_infos;
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos;
};
MYNTEYE_END_NAMESPACE

53
src/mynteye/api/processor/rectify_processor_ocv.cc
View File

@ -32,6 +32,7 @@ RectifyProcessorOCV::RectifyProcessorOCV(
: Processor(std::move(proc_period)),
calib_model(CalibrationModel::UNKNOW) {
VLOG(2) << __func__ << ": proc_period=" << proc_period;
calib_infos = std::make_shared<struct CameraROSMsgInfoPair>();
InitParams(
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right),
@ -75,6 +76,25 @@ bool RectifyProcessorOCV::OnProcess(
return true;
}
struct CameraROSMsgInfo RectifyProcessorOCV::getCalibMatData(
const mynteye::IntrinsicsPinhole& in) {
struct CameraROSMsgInfo calib_mat_data;
calib_mat_data.distortion_model = "PINHOLE";
calib_mat_data.height = in.height;
calib_mat_data.width = in.width;
for (std::size_t i = 0; i < 5; i++) {
calib_mat_data.D[i] = in.coeffs[i];
}
calib_mat_data.K[0] = in.fx;
calib_mat_data.K[4] = in.cx;
calib_mat_data.K[2] = in.fy;
calib_mat_data.K[5] = in.cy;
calib_mat_data.K[8] = 1;
return calib_mat_data;
}
void RectifyProcessorOCV::InitParams(
IntrinsicsPinhole in_left,
IntrinsicsPinhole in_right,
@ -84,6 +104,9 @@ void RectifyProcessorOCV::InitParams(
in_right.ResizeIntrinsics();
cv::Size size{in_left.width, in_left.height};
struct CameraROSMsgInfoPair info_pair;
info_pair.left = getCalibMatData(in_left);
info_pair.right = getCalibMatData(in_right);
cv::Mat M1 =
(cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy,
in_left.cy, 0, 0, 1);
@ -100,6 +123,16 @@ void RectifyProcessorOCV::InitParams(
ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation);
info_pair.R[0] = ex_right_to_left.rotation[0][0];
info_pair.R[1] = ex_right_to_left.rotation[0][1];
info_pair.R[2] = ex_right_to_left.rotation[0][2];
info_pair.R[3] = ex_right_to_left.rotation[1][0];
info_pair.R[4] = ex_right_to_left.rotation[1][1];
info_pair.R[5] = ex_right_to_left.rotation[1][2];
info_pair.R[6] = ex_right_to_left.rotation[2][0];
info_pair.R[7] = ex_right_to_left.rotation[2][1];
info_pair.R[8] = ex_right_to_left.rotation[2][2];
VLOG(2) << "InitParams size: " << size;
VLOG(2) << "M1: " << M1;
VLOG(2) << "M2: " << M2;
@ -113,6 +146,26 @@ void RectifyProcessorOCV::InitParams(
M1, D1, M2, D2, size, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY,
0, size, &left_roi, &right_roi);
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 4; j++) {
info_pair.left.P[i*4 + j] = P1.at<double>(i, j);
info_pair.right.P[i*4 + j] = P2.at<double>(i, j);
}
}
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
info_pair.left.R[i*3 + j] = R1.at<double>(i, j);
info_pair.right.R[i*3 +j] = R2.at<double>(i, j);
}
}
for (std::size_t i = 0; i< 3 * 4; i++) {
info_pair.P[i] = info_pair.left.P[i];
}
*calib_infos = info_pair;
cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_16SC2, map11, map12);
cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
}

10
src/mynteye/api/processor/rectify_processor_ocv.h
View File

@ -40,6 +40,14 @@ class RectifyProcessorOCV : public Processor {
std::string Name() override;
inline std::shared_ptr<struct CameraROSMsgInfoPair>
getCameraROSMsgInfoPair() {
return calib_infos;
}
struct CameraROSMsgInfo getCalibMatData(
const mynteye::IntrinsicsPinhole& in);
void ReloadImageParams(
std::shared_ptr<IntrinsicsBase> intr_left,
std::shared_ptr<IntrinsicsBase> intr_right,
@ -66,6 +74,8 @@ class RectifyProcessorOCV : public Processor {
IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
CalibrationModel calib_model;
std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos;
};
MYNTEYE_END_NAMESPACE

20
src/mynteye/api/synthetic.cc
View File

@ -352,10 +352,10 @@ void Synthetic::InitProcessors() {
RECTIFY_PROC_PERIOD);
rectify_processor = rectify_processor_imp;
points_processor = std::make_shared<PointsProcessor>(
rectify_processor_imp -> getCalibInfoPair(),
rectify_processor_imp -> getCameraROSMsgInfoPair(),
POINTS_PROC_PERIOD);
depth_processor = std::make_shared<DepthProcessor>(
rectify_processor_imp -> getCalibInfoPair(),
rectify_processor_imp -> getCameraROSMsgInfoPair(),
DEPTH_PROC_PERIOD);
root_processor->AddChild(rectify_processor);
@ -566,4 +566,20 @@ void Synthetic::NotifyStreamData(
}
}
std::shared_ptr<struct CameraROSMsgInfoPair>
Synthetic::GetCameraROSMsgInfoPair() {
if (calib_model_ == CalibrationModel::PINHOLE) {
auto processor = getProcessorWithStream(Stream::LEFT_RECTIFIED);
auto proc = static_cast<RectifyProcessorOCV*>(&(*processor));
return proc->getCameraROSMsgInfoPair();
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
auto processor = getProcessorWithStream(Stream::LEFT_RECTIFIED);
auto proc = static_cast<RectifyProcessor*>(&(*processor));
return proc->getCameraROSMsgInfoPair();
#endif
}
return {};
}
MYNTEYE_END_NAMESPACE

1
src/mynteye/api/synthetic.h
View File

@ -93,6 +93,7 @@ class Synthetic {
std::shared_ptr<Processor> getProcessorWithStream(const Stream& stream);
void SetDisparityComputingMethodType(
const DisparityComputingMethod &MethoType);
std::shared_ptr<struct CameraROSMsgInfoPair> GetCameraROSMsgInfoPair();
private:
void InitCalibInfo();

30
src/mynteye/types.cc
View File

@ -317,4 +317,34 @@ std::ostream &operator<<(std::ostream &os, const OptionInfo &info) {
<< ", def: " << info.def;
}
std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfo &info) {
os << "width: " << info.width << ", height: " << info.height << std::endl
<< "distortion_model: " << info.distortion_model;
os << std::endl << "D: ";
for (size_t i = 0; i < 5; i++)
os << info.D[i] << ",";
os << std::endl << "K: ";
for (size_t i = 0; i < 9; i++)
os << info.K[i] << ",";
os << std::endl << "R: ";
for (size_t i = 0; i < 9; i++)
os << info.R[i] << ",";
os << std::endl << "P: ";
for (size_t i = 0; i < 12; i++)
os << info.P[i] << ",";
os << std::endl;
}
std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfoPair &info) {
os << "left:\n" << info.left << std::endl;
os << "right:\n" << info.right << std::endl;
os << "base R:";
for (size_t i = 0; i < 9; i++)
os << info.R[i] << ",";
os << std::endl << "base P:";
for (size_t i = 0; i < 12; i++)
os << info.P[i] << ",";
os << std::endl;
}
MYNTEYE_END_NAMESPACE