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

Merge branch 'develop' into feature/android

Browse Source 
* develop:
  fix(processor): fix reload image params bug
  fix(device): fix intrinsic updating bug
  fix(api.h): remove mynteye/device.h
  fix(mynteye.launch): add device options
  fix(device.cc): get img params bug
  fix(ros): rename rect topic name in rviz
  fix(depth): delete 1000
  fix(points cloud): show the points clopud in normal size
  (fix): points cloud img ptr null
  fix(depth): fix mistake
  fix(channels.cc): compatible s_color and s
  fix(depth): add disparity limit
  feat(calib models): float change to ushort ,fix the T_mul_f number
This commit is contained in:
John Zhao 2019-01-14 12:07:07 +08:00
commit 5d5b5cf476
12 changed files with 103 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,7 +25,6 @@
#include "mynteye/mynteye.h"
#include "mynteye/types.h"
#include "mynteye/device.h"
MYNTEYE_BEGIN_NAMESPACE

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

@ -21,6 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE
const char DepthProcessor::NAME[] = "DepthProcessor";
const int DISPARITY_MIN = 0;
const int DISPARITY_MAX = 64;
DepthProcessor::DepthProcessor(
std::shared_ptr<struct camera_calib_info_pair> calib_infos,
std::int32_t proc_period)
@ -48,15 +51,16 @@ bool DepthProcessor::OnProcess(
ObjMat *output = Object::Cast<ObjMat>(out);
int rows = input->value.rows;
int cols = input->value.cols;
std::cout << calib_infos_->T_mul_f << std::endl;
// std::cout << calib_infos_->T_mul_f << std::endl;
// 0.0793434
// TODO(MYNTEYE): Put the corresponding parameters(T,f)
cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F);
cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U);
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
float disparity_value = input->value.at<float>(i, j);
float depth = calib_infos_->T_mul_f * 1000.0 / disparity_value ;
depth_mat.at<float>(i, j) = depth;
if (disparity_value < DISPARITY_MAX && disparity_value > DISPARITY_MIN) {
float depth = calib_infos_->T_mul_f / disparity_value;
depth_mat.at<ushort>(i, j) = depth;
}
}
}
output->value = depth_mat;

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

@ -32,7 +32,7 @@ struct DepthTraits<uint16_t> {
static inline bool valid(uint16_t depth) { return depth != 0; }
static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm
static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; }
static inline void initializeBuffer(std::vector<uint8_t>& buffer) {} // Do nothing - already zero-filled
static inline void initializeBuffer(std::vector<uint16_t>& buffer) {} // Do nothing - already zero-filled
};
template<>
@ -100,19 +100,19 @@ bool PointsProcessor::OnProcess(
for (int v = 0; v < height; ++v) {
cv::Vec3f *dptr = output->value.ptr<cv::Vec3f>(v);
for (int u = 0; u < width; ++u) {
float depth = input->value.at<float>(v, u);
float depth = input->value.at<uint16_t>(v, u);
// Missing points denoted by NaNs
if (!DepthTraits<float>::valid(depth)) {
if (!DepthTraits<uint16_t>::valid(depth)) {
continue;
}
dptr[u][0] = (u - center_x) * depth * constant_x;
dptr[u][1] = (v - center_y) * depth * constant_y;
dptr[u][2] = DepthTraits<float>::toMeters(depth);
dptr[u][0] = (u - center_x) * depth * constant_x ;
dptr[u][1] = (v - center_y) * depth * constant_y ;
dptr[u][2] = depth ;
}
}
output->id = input->id;
output->data = input->data;
return true;
}

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

@ -167,7 +167,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
_pp[0][2] = cc_new[1].x;
_pp[1][2] = cc_new[1].y;
_pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
*T_mul_f = 0. - _t[idx];
*T_mul_f = 0. - _t[idx] * fc_new;
cvConvert(&pp, _P2);
alpha = MIN(alpha, 1.);
@ -381,11 +381,11 @@ RectifyProcessor::RectifyProcessor(
std::int32_t proc_period)
: Processor(std::move(proc_period)),
calib_model(CalibrationModel::UNKNOW) {
intr_left_ = intr_left;
intr_right_ = intr_right;
extr_ = extr;
VLOG(2) << __func__ << ": proc_period=" << proc_period;
NotifyImageParamsChanged();
InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),
*extr);
}
RectifyProcessor::~RectifyProcessor() {
@ -396,11 +396,14 @@ std::string RectifyProcessor::Name() {
return NAME;
}
void RectifyProcessor::NotifyImageParamsChanged() {
void RectifyProcessor::ReloadImageParams(
std::shared_ptr<IntrinsicsBase> intr_left,
std::shared_ptr<IntrinsicsBase> intr_right,
std::shared_ptr<Extrinsics> extr) {
InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left_),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right_),
*extr_);
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),
*extr);
}
Object *RectifyProcessor::OnCreateOutput() {

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

@ -71,7 +71,10 @@ class RectifyProcessor : public Processor {
std::string Name() override;
void NotifyImageParamsChanged();
void ReloadImageParams(
std::shared_ptr<IntrinsicsBase> intr_left,
std::shared_ptr<IntrinsicsBase> intr_right,
std::shared_ptr<Extrinsics> extr);
cv::Mat R1, P1, R2, P2, Q;
cv::Mat map11, map12, map21, map22;
@ -117,9 +120,6 @@ class RectifyProcessor : public Processor {
camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant(
const mynteye::IntrinsicsEquidistant & in);
std::shared_ptr<IntrinsicsBase> intr_left_;
std::shared_ptr<IntrinsicsBase> intr_right_;
std::shared_ptr<Extrinsics> extr_;
CalibrationModel calib_model;
std::shared_ptr<struct camera_calib_info_pair> calib_infos;
};

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

@ -32,10 +32,10 @@ RectifyProcessorOCV::RectifyProcessorOCV(
: Processor(std::move(proc_period)),
calib_model(CalibrationModel::UNKNOW) {
VLOG(2) << __func__ << ": proc_period=" << proc_period;
intr_left_ = intr_left;
intr_right_ = intr_right;
extr_ = extr;
NotifyImageParamsChanged();
InitParams(
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right),
*extr);
}
RectifyProcessorOCV::~RectifyProcessorOCV() {
@ -46,11 +46,14 @@ std::string RectifyProcessorOCV::Name() {
return NAME;
}
void RectifyProcessorOCV::NotifyImageParamsChanged() {
void RectifyProcessorOCV::ReloadImageParams(
std::shared_ptr<IntrinsicsBase> intr_left,
std::shared_ptr<IntrinsicsBase> intr_right,
std::shared_ptr<Extrinsics> extr) {
InitParams(
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left_),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right_),
*extr_);
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right),
*extr);
}
Object *RectifyProcessorOCV::OnCreateOutput() {

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

@ -40,7 +40,10 @@ class RectifyProcessorOCV : public Processor {
std::string Name() override;
void NotifyImageParamsChanged();
void ReloadImageParams(
std::shared_ptr<IntrinsicsBase> intr_left,
std::shared_ptr<IntrinsicsBase> intr_right,
std::shared_ptr<Extrinsics> extr);
cv::Mat R1, P1, R2, P2, Q;
cv::Mat map11, map12, map21, map22;
@ -54,10 +57,6 @@ class RectifyProcessorOCV : public Processor {
void InitParams(IntrinsicsPinhole in_left,
IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
std::shared_ptr<IntrinsicsBase> intr_left_;
std::shared_ptr<IntrinsicsBase> intr_right_;
std::shared_ptr<Extrinsics> extr_;
CalibrationModel calib_model;
};

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

@ -111,19 +111,23 @@ Synthetic::~Synthetic() {
}
void Synthetic::NotifyImageParamsChanged() {
intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT);
intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT);
extr_ = std::make_shared<Extrinsics>(
api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
if (calib_model_ == CalibrationModel::PINHOLE) {
auto &&processor = find_processor<RectifyProcessorOCV>(processor_);
if (processor) processor->NotifyImageParamsChanged();
if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
#ifdef WITH_CAM_MODELS
} else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
auto &&processor = find_processor<RectifyProcessor>(processor_);
if (processor) processor->NotifyImageParamsChanged();
if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
#endif
} else {
LOG(ERROR) << "Unknow calib model type in device: "
<< calib_model_ << ", use default pinhole model";
auto &&processor = find_processor<RectifyProcessorOCV>(processor_);
if (processor) processor->NotifyImageParamsChanged();
if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
}
}

7
src/mynteye/device/channel/channels.cc
View File

@ -758,11 +758,14 @@ std::set<Resolution> ChannelsAdapter::GetResolutionSupports() {
std::set<Resolution> res;
auto requests_map = stream_requests_map.at(model_);
for (auto&& r_map : requests_map) {
if (r_map.first == Capabilities::STEREO ||
r_map.first == Capabilities::STEREO_COLOR) {
if (r_map.first == Capabilities::STEREO) {
for (auto&& r : r_map.second) {
res.insert({r.width, r.height});
}
} else if (r_map.first == Capabilities::STEREO_COLOR) {
for (auto&& r : r_map.second) {
res.insert({static_cast<std::uint16_t>(r.width / 2), r.height});
}
}
}
return res;

10
src/mynteye/device/device.cc
View File

@ -640,7 +640,15 @@ void Device::UpdateStreamIntrinsics(
for (auto &&params : all_img_params_) {
auto &&img_res = params.first;
auto &&img_params = params.second;
if (img_params.ok && img_res == request.GetResolution()) {
bool ok = false;
if (capability == Capabilities::STEREO_COLOR) {
ok = img_params.ok &&
img_res.height == request.GetResolution().height &&
img_res.width == request.GetResolution().width / 2;
} else if (capability == Capabilities::STEREO) {
ok = img_params.ok && img_res == request.GetResolution();
}
if (ok) {
SetIntrinsics(Stream::LEFT, img_params.in_left);
SetIntrinsics(Stream::RIGHT, img_params.in_right);
SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params.ex_right_to_left);

33
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -254,6 +254,22 @@
<!-- stream request index -->
<param name="request_index" value="$(arg request_index)" />
<!-- device options of standard-->
<param name="sstandard/gain" value="$(arg standard/gain)" />
<param name="standard/brightness" value="$(arg standard/brightness)" />
<param name="standard/contrast" value="$(arg standard/contrast)" />
<param name="standard/frame_rate" value="$(arg standard/frame_rate)" />
<param name="standard/imu_frequency" value="$(arg standard/imu_frequency)" />
<param name="standard/exposure_mode" value="$(arg standard/exposure_mode)" />
<param name="standard/max_gain" value="$(arg standard/max_gain)" />
<param name="standard/max_exposure_time" value="$(arg standard/max_exposure_time)" />
<param name="standard/desired_brightness" value="$(arg standard/desired_brightness)" />
<param name="standard/ir_control" value="$(arg standard/ir_control)" />
<param name="standard/hdr_mode" value="$(arg standard/hdr_mode)" />
<param name="standard/accel_range" value="$(arg standard/accel_range)" />
<param name="standard/gyro_range" value="$(arg standard/gyro_range)" />
<!-- device options of standard2-->
<param name="standard2/brightness" value="$(arg standard2/brightness)" />
@ -261,11 +277,26 @@
<param name="standard2/max_gain" value="$(arg standard2/max_gain)" />
<param name="standard2/max_exposure_time" value="$(arg standard2/max_exposure_time)" />
<param name="standard2/desired_brightness" value="$(arg standard2/desired_brightness)" />
<param name="standard2/min_exposure_time" value="$(arg standard2/max_exposure_time)" />
<param name="standard2/min_exposure_time" value="$(arg standard2/min_exposure_time)" />
<param name="standard2/ir_control" value="$(arg standard2/ir_control)" />
<param name="standard2/accel_range" value="$(arg standard2/accel_range)" />
<param name="standard2/gyro_range" value="$(arg standard2/gyro_range)" />
<param name="standard2/accel_low_filter" value="$(arg standard2/accel_low_filter)" />
<param name="standard2/gyro_low_filter" value="$(arg standard2/gyro_low_filter)" />
<!-- device options of standard210a-->
<param name="standard210a/brightness" value="$(arg standard210a/brightness)" />
<param name="standard210a/exposure_mode" value="$(arg standard210a/exposure_mode)" />
<param name="standard210a/max_gain" value="$(arg standard210a/max_gain)" />
<param name="standard210a/max_exposure_time" value="$(arg standard210a/max_exposure_time)" />
<param name="standard210a/desired_brightness" value="$(arg standard210a/desired_brightness)" />
<param name="standard210a/min_exposure_time" value="$(arg standard210a/min_exposure_time)" />
<param name="standard210a/accel_range" value="$(arg standard210a/accel_range)" />
<param name="standard210a/gyro_range" value="$(arg standard210a/gyro_range)" />
<param name="standard210a/accel_low_filter" value="$(arg standard210a/accel_low_filter)" />
<param name="standard210a/gyro_low_filter" value="$(arg standard210a/gyro_low_filter)" />
</node>
<!-- disable compressed depth plugin for image topics -->

4
wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz
View File

@ -82,7 +82,7 @@ Visualization Manager:
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /mynteye/left/image_rect
Image Topic: /mynteye/left_rect/image_rect
Max Value: 1
Median window: 5
Min Value: 0
@ -94,7 +94,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /mynteye/right/image_rect
Image Topic: /mynteye/right_rect/image_rect
Max Value: 1
Median window: 5
Min Value: 0