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

feat(calib depth/points) add calib prams in constructed function;

Browse Source
This commit is contained in:
TinyOh 2019-01-09 17:30:38 +08:00
parent 94bf528f8b
commit 733a91c68c
7 changed files with 110 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,7 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE
const char DepthProcessor::NAME[] = "DepthProcessor"; const char DepthProcessor::NAME[] = "DepthProcessor";
DepthProcessor::DepthProcessor(std::int32_t proc_period) DepthProcessor::DepthProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::int32_t proc_period)
: Processor(std::move(proc_period)) { : Processor(std::move(proc_period)) {
VLOG(2) << __func__ << ": proc_period=" << proc_period; VLOG(2) << __func__ << ": proc_period=" << proc_period;
} }

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

@ -18,6 +18,7 @@
#include <string> #include <string>
#include "mynteye/api/processor.h" #include "mynteye/api/processor.h"
#include "mynteye/api/processor/rectify_processor.h"
MYNTEYE_BEGIN_NAMESPACE MYNTEYE_BEGIN_NAMESPACE
@ -25,7 +26,9 @@ class DepthProcessor : public Processor {
public: public:
static const char NAME[]; static const char NAME[];
explicit DepthProcessor(std::int32_t proc_period = 0); explicit DepthProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::int32_t proc_period = 0);
virtual ~DepthProcessor(); virtual ~DepthProcessor();
std::string Name() override; std::string Name() override;

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

@ -52,7 +52,9 @@ struct DepthTraits<float> {
const char PointsProcessor::NAME[] = "PointsProcessor"; const char PointsProcessor::NAME[] = "PointsProcessor";
PointsProcessor::PointsProcessor(std::int32_t proc_period) PointsProcessor::PointsProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::int32_t proc_period)
: Processor(std::move(proc_period)) { : Processor(std::move(proc_period)) {
VLOG(2) << __func__ << ": proc_period=" << proc_period; VLOG(2) << __func__ << ": proc_period=" << proc_period;
} }
@ -95,7 +97,6 @@ bool PointsProcessor::OnProcess(
int height = static_cast<int>(output->value.rows); int height = static_cast<int>(output->value.rows);
int width = static_cast<int>(output->value.cols); int width = static_cast<int>(output->value.cols);
for (int v = 0; v < height; ++v) { for (int v = 0; v < height; ++v) {
cv::Vec3f *dptr = output->value.ptr<cv::Vec3f>(v); cv::Vec3f *dptr = output->value.ptr<cv::Vec3f>(v);
for (int u = 0; u < width; ++u) { for (int u = 0; u < width; ++u) {
float depth = input->value.at<float>(v, u); float depth = input->value.at<float>(v, u);

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

@ -20,6 +20,7 @@
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
#include "mynteye/api/processor.h" #include "mynteye/api/processor.h"
#include "mynteye/api/processor/rectify_processor.h"
MYNTEYE_BEGIN_NAMESPACE MYNTEYE_BEGIN_NAMESPACE
@ -27,7 +28,9 @@ class PointsProcessor : public Processor {
public: public:
static const char NAME[]; static const char NAME[];
explicit PointsProcessor(std::int32_t proc_period = 0); explicit PointsProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::int32_t proc_period = 0);
virtual ~PointsProcessor(); virtual ~PointsProcessor();
std::string Name() override; std::string Name() override;
@ -38,7 +41,6 @@ class PointsProcessor : public Processor {
Object *const in, Object *const out, Processor *const parent) override; Object *const in, Object *const out, Processor *const parent) override;
private: private:
cv::Mat Q_;
}; };
MYNTEYE_END_NAMESPACE MYNTEYE_END_NAMESPACE

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

@ -18,38 +18,10 @@
#include <opencv2/calib3d/calib3d.hpp> #include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
#include "mynteye/logger.h" #include "mynteye/logger.h"
#include "mynteye/device/device.h"
#include <camodocal/camera_models/Camera.h>
#include <camodocal/camera_models/CameraFactory.h>
#include <camodocal/camera_models/CataCamera.h>
#include <camodocal/camera_models/EquidistantCamera.h>
#include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/gpl/gpl.h>
#include <camodocal/camera_models/Camera.h>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
struct camera_info { MYNTEYE_BEGIN_NAMESPACE
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_mat_info_pair { cv::Mat RectifyProcessor::rectifyrad(const cv::Mat& R) {
struct camera_info left;
struct camera_info right;
};
cv::Mat rectifyrad(const cv::Mat& R) {
cv::Mat r_vec; cv::Mat r_vec;
cv::Rodrigues(R, r_vec); cv::Rodrigues(R, r_vec);
// pi/180 = x/179 ==> x = 3.1241 // pi/180 = x/179 ==> x = 3.1241
@ -65,13 +37,12 @@ cv::Mat rectifyrad(const cv::Mat& R) {
return R.clone(); return R.clone();
} }
void stereoRectify(camodocal::CameraPtr leftOdo, void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2,
const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* D1, const CvMat* D2, CvSize imageSize,
const CvMat* matR, const CvMat* matT, const CvMat* matR, const CvMat* matT,
CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, int flags, double alpha, CvSize newImgSize) {
CvSize newImgSize = cv::Size()) {
double _om[3], _t[3] = {0}, _uu[3]={0, 0, 0}, _r_r[3][3], _pp[3][4]; double _om[3], _t[3] = {0}, _uu[3]={0, 0, 0}, _r_r[3][3], _pp[3][4];
double _ww[3], _wr[3][3], _z[3] = {0, 0, 0}, _ri[3][3], _w3[3]; double _ww[3], _wr[3][3], _z[3] = {0, 0, 0}, _ri[3][3], _w3[3];
cv::Rect_<float> inner1, inner2, outer1, outer2; cv::Rect_<float> inner1, inner2, outer1, outer2;
@ -229,7 +200,7 @@ void stereoRectify(camodocal::CameraPtr leftOdo,
} }
} }
Eigen::Matrix4d loadT(const mynteye::Extrinsics& in) { Eigen::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics& in) {
Eigen::Matrix3d R; Eigen::Matrix3d R;
R<< R<<
in.rotation[0][0], in.rotation[0][1], in.rotation[0][2], in.rotation[0][0], in.rotation[0][1], in.rotation[0][2],
@ -249,17 +220,18 @@ Eigen::Matrix4d loadT(const mynteye::Extrinsics& in) {
return T; return T;
} }
void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT void RectifyProcessor::loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT
cv::Size& image_size, // NOLINT cv::Size& image_size, // NOLINT
struct camera_info& calib_data) { // NOLINT struct camera_calib_info& calib_data) { // NOLINT
K = cv::Mat(3, 3, CV_64F, calib_data.K); K = cv::Mat(3, 3, CV_64F, calib_data.K);
std::size_t d_length = 4; std::size_t d_length = 4;
D = cv::Mat(1, d_length, CV_64F, calib_data.D); D = cv::Mat(1, d_length, CV_64F, calib_data.D);
image_size = cv::Size(calib_data.width, calib_data.height); image_size = cv::Size(calib_data.width, calib_data.height);
} }
struct camera_info getCalibMatData(const mynteye::IntrinsicsEquidistant& in) { struct camera_calib_info RectifyProcessor::getCalibMatData(
struct camera_info calib_mat_data; const mynteye::IntrinsicsEquidistant& in) {
struct camera_calib_info calib_mat_data;
calib_mat_data.distortion_model = "KANNALA_BRANDT"; calib_mat_data.distortion_model = "KANNALA_BRANDT";
calib_mat_data.height = in.height; calib_mat_data.height = in.height;
calib_mat_data.width = in.width; calib_mat_data.width = in.width;
@ -276,7 +248,7 @@ struct camera_info getCalibMatData(const mynteye::IntrinsicsEquidistant& in) {
return calib_mat_data; return calib_mat_data;
} }
struct camera_mat_info_pair stereoRectify( std::shared_ptr<struct camera_calib_info_pair> RectifyProcessor::stereoRectify(
camodocal::CameraPtr leftOdo, camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo, camodocal::CameraPtr rightOdo,
mynteye::IntrinsicsEquidistant in_left, mynteye::IntrinsicsEquidistant in_left,
@ -291,8 +263,8 @@ struct camera_mat_info_pair stereoRectify(
cv::Mat K1, D1, K2, D2; cv::Mat K1, D1, K2, D2;
cv::Size image_size1, image_size2; cv::Size image_size1, image_size2;
struct camera_info calib_mat_data_left = getCalibMatData(in_left); struct camera_calib_info calib_mat_data_left = getCalibMatData(in_left);
struct camera_info calib_mat_data_right = getCalibMatData(in_right); struct camera_calib_info calib_mat_data_right = getCalibMatData(in_right);
loadCameraMatrix(K1, D1, image_size1, calib_mat_data_left); loadCameraMatrix(K1, D1, image_size1, calib_mat_data_left);
loadCameraMatrix(K2, D2, image_size2, calib_mat_data_right); loadCameraMatrix(K2, D2, image_size2, calib_mat_data_right);
@ -335,11 +307,12 @@ struct camera_mat_info_pair stereoRectify(
calib_mat_data_right.R[i*3 +j] = R2.at<double>(i, j); calib_mat_data_right.R[i*3 +j] = R2.at<double>(i, j);
} }
} }
struct camera_mat_info_pair res = {calib_mat_data_left, calib_mat_data_right}; struct camera_calib_info_pair res =
return res; {calib_mat_data_left, calib_mat_data_right};
return std::make_shared<struct camera_calib_info_pair>(res);
} }
camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( camodocal::CameraPtr RectifyProcessor::generateCameraFromIntrinsicsEquidistant(
const mynteye::IntrinsicsEquidistant & in) { const mynteye::IntrinsicsEquidistant & in) {
camodocal::EquidistantCameraPtr camera( camodocal::EquidistantCameraPtr camera(
new camodocal::EquidistantCamera("KANNALA_BRANDT", new camodocal::EquidistantCamera("KANNALA_BRANDT",
@ -356,8 +329,6 @@ camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant(
return camera; return camera;
} }
MYNTEYE_BEGIN_NAMESPACE
void RectifyProcessor::InitParams( void RectifyProcessor::InitParams(
IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_left,
IntrinsicsEquidistant in_right, IntrinsicsEquidistant in_right,
@ -367,7 +338,7 @@ void RectifyProcessor::InitParams(
generateCameraFromIntrinsicsEquidistant(in_left); generateCameraFromIntrinsicsEquidistant(in_left);
camodocal::CameraPtr camera_odo_ptr_right = camodocal::CameraPtr camera_odo_ptr_right =
generateCameraFromIntrinsicsEquidistant(in_right); generateCameraFromIntrinsicsEquidistant(in_right);
struct camera_mat_info_pair calib_info_pair = calib_infos =
stereoRectify(camera_odo_ptr_left, stereoRectify(camera_odo_ptr_left,
camera_odo_ptr_right, camera_odo_ptr_right,
in_left, in_left,
@ -378,18 +349,18 @@ void RectifyProcessor::InitParams(
cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F);
for (size_t i = 0; i < 3; i++) { for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) { for (size_t j = 0; j < 3; j++) {
rect_R_l.at<float>(i, j) = calib_info_pair.left.R[i*3+j]; rect_R_l.at<float>(i, j) = calib_infos->left.R[i*3+j];
rect_R_r.at<float>(i, j) = calib_info_pair.right.R[i*3+j]; rect_R_r.at<float>(i, j) = calib_infos->right.R[i*3+j];
} }
} }
double left_f[] = double left_f[] =
{calib_info_pair.left.P[0], calib_info_pair.left.P[5]}; {calib_infos->left.P[0], calib_infos->left.P[5]};
double left_center[] = double left_center[] =
{calib_info_pair.left.P[2], calib_info_pair.left.P[6]}; {calib_infos->left.P[2], calib_infos->left.P[6]};
double right_f[] = double right_f[] =
{calib_info_pair.right.P[0], calib_info_pair.right.P[5]}; {calib_infos->right.P[0], calib_infos->right.P[5]};
double right_center[] = double right_center[] =
{calib_info_pair.right.P[2], calib_info_pair.right.P[6]}; {calib_infos->right.P[2], calib_infos->right.P[6]};
camera_odo_ptr_left->initUndistortRectifyMap( camera_odo_ptr_left->initUndistortRectifyMap(
map11, map12, left_f[0], left_f[1], map11, map12, left_f[0], left_f[1],
cv::Size(0, 0), left_center[0], cv::Size(0, 0), left_center[0],

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

@ -22,9 +22,39 @@
#include "mynteye/types.h" #include "mynteye/types.h"
#include "mynteye/api/processor.h" #include "mynteye/api/processor.h"
#include "mynteye/device/device.h"
#include <camodocal/camera_models/Camera.h>
#include <camodocal/camera_models/CameraFactory.h>
#include <camodocal/camera_models/CataCamera.h>
#include <camodocal/camera_models/EquidistantCamera.h>
#include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/gpl/gpl.h>
#include <camodocal/camera_models/Camera.h>
#include <opencv2/core/eigen.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
MYNTEYE_BEGIN_NAMESPACE 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;
};
class Device; class Device;
class RectifyProcessor : public Processor { class RectifyProcessor : public Processor {
@ -45,6 +75,10 @@ class RectifyProcessor : public Processor {
cv::Mat R1, P1, R2, P2, Q; cv::Mat R1, P1, R2, P2, Q;
cv::Mat map11, map12, map21, map22; cv::Mat map11, map12, map21, map22;
inline std::shared_ptr<struct camera_calib_info_pair> getCalibInfoPair() {
return calib_infos;
}
protected: protected:
Object *OnCreateOutput() override; Object *OnCreateOutput() override;
bool OnProcess( bool OnProcess(
@ -54,10 +88,39 @@ class RectifyProcessor : public Processor {
void InitParams(IntrinsicsEquidistant in_left, void InitParams(IntrinsicsEquidistant in_left,
IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left); IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left);
cv::Mat rectifyrad(const cv::Mat& R);
void stereoRectify(camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2,
const CvMat* D1, const CvMat* D2, CvSize imageSize,
const CvMat* matR, const CvMat* matT,
CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1,
CvSize newImgSize = cv::Size());
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 camera_calib_info getCalibMatData(
const mynteye::IntrinsicsEquidistant& in);
std::shared_ptr<struct camera_calib_info_pair> stereoRectify(
camodocal::CameraPtr leftOdo,
camodocal::CameraPtr rightOdo,
mynteye::IntrinsicsEquidistant in_left,
mynteye::IntrinsicsEquidistant in_right,
mynteye::Extrinsics ex_right_to_left);
camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant(
const mynteye::IntrinsicsEquidistant & in);
std::shared_ptr<IntrinsicsBase> intr_left_; std::shared_ptr<IntrinsicsBase> intr_left_;
std::shared_ptr<IntrinsicsBase> intr_right_; std::shared_ptr<IntrinsicsBase> intr_right_;
std::shared_ptr<Extrinsics> extr_; std::shared_ptr<Extrinsics> extr_;
CalibrationModel calib_model; CalibrationModel calib_model;
std::shared_ptr<struct camera_calib_info_pair> calib_infos;
}; };
MYNTEYE_END_NAMESPACE MYNTEYE_END_NAMESPACE

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

@ -570,6 +570,9 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) {
void Synthetic::InitProcessors() { void Synthetic::InitProcessors() {
std::shared_ptr<Processor> rectify_processor = nullptr; std::shared_ptr<Processor> rectify_processor = nullptr;
#ifdef WITH_CAM_MODELS
std::shared_ptr<RectifyProcessor> rectify_processor_imp = nullptr;
#endif
cv::Mat Q; cv::Mat Q;
if (calib_model_ == CalibrationModel::PINHOLE) { if (calib_model_ == CalibrationModel::PINHOLE) {
auto &&rectify_processor_ocv = auto &&rectify_processor_ocv =
@ -579,9 +582,10 @@ void Synthetic::InitProcessors() {
Q = rectify_processor_ocv->Q; Q = rectify_processor_ocv->Q;
#ifdef WITH_CAM_MODELS #ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
rectify_processor = rectify_processor_imp =
std::make_shared<RectifyProcessor>(intr_left_, intr_right_, extr_, std::make_shared<RectifyProcessor>(intr_left_, intr_right_, extr_,
RECTIFY_PROC_PERIOD); RECTIFY_PROC_PERIOD);
rectify_processor = rectify_processor_imp;
#endif #endif
} else { } else {
LOG(ERROR) << "Unknow calib model type in device: " LOG(ERROR) << "Unknow calib model type in device: "
@ -603,6 +607,7 @@ void Synthetic::InitProcessors() {
#ifdef WITH_CAM_MODELS #ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
points_processor = std::make_shared<PointsProcessor>( points_processor = std::make_shared<PointsProcessor>(
rectify_processor_imp -> getCalibInfoPair(),
POINTS_PROC_PERIOD); POINTS_PROC_PERIOD);
#endif #endif
} else { } else {
@ -614,7 +619,9 @@ void Synthetic::InitProcessors() {
depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD); depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
#ifdef WITH_CAM_MODELS #ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
depth_processor = std::make_shared<DepthProcessor>(DEPTH_PROC_PERIOD); depth_processor = std::make_shared<DepthProcessor>(
rectify_processor_imp -> getCalibInfoPair(),
DEPTH_PROC_PERIOD);
#endif #endif
} else { } else {
depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD); depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);