MRPT  2.0.1
CMyntEyeCamera.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <iostream>
15 #include <thread>
16 
17 #if MRPT_HAS_MYNTEYE_D
18 #include <mynteyed/camera.h>
19 #include <mynteyed/utils.h>
20 #endif
21 
22 using namespace mrpt;
23 using namespace mrpt::hwdrivers;
24 
26 {
27 #if MRPT_HAS_MYNTEYE_D
29  std::make_shared<mynteyed::Camera>();
30  mynteyed::DeviceInfo dev_info;
31 #endif
32 };
33 
34 #if MRPT_HAS_MYNTEYE_D
35 static mrpt::img::TCamera convertIntrinsics(const mynteyed::CameraIntrinsics& i)
36 {
38  p.ncols = i.width;
39  p.nrows = i.height;
40 
41  p.cx(i.cx);
42  p.cy(i.cy);
43  p.fx(i.fx);
44  p.fy(i.fy);
45  // The distortion coefficients: k1,k2,p1,p2,k3
46  for (int k = 0; k < 5; k++) p.dist[k] = i.coeffs[k];
47 
48  return p;
49 }
50 #endif
51 
53  : m_capture(mrpt::make_impl<CMyntEyeCamera::Impl>())
54 {
56 #if MRPT_HAS_MYNTEYE_D
57  if (!mynteyed::util::select(*m_capture->cam, &m_capture->dev_info))
58  {
59  THROW_EXCEPTION("No MYNTEYE-D cameras was found!");
60  }
61  mynteyed::util::print_stream_infos(
62  *m_capture->cam, m_capture->dev_info.index);
63 
64  mynteyed::OpenParams params(m_capture->dev_info.index);
65  params.color_mode = mynteyed::ColorMode::COLOR_RECTIFIED;
66  params.stream_mode = mynteyed::StreamMode::STREAM_1280x720;
67  params.ir_intensity = p.ir_intensity;
68 
69  mynteyed::StreamMode stream_mode = params.stream_mode;
70  m_capture->cam->Open(params);
71  std::cout << std::endl;
72  if (!m_capture->cam->IsOpened())
73  {
74  THROW_EXCEPTION("Error: Open camera failed");
75  }
76  std::cout << "[CMyntEyeCamera] Open device successful.\n";
77 
78  mynteyed::StreamIntrinsics si =
79  m_capture->cam->GetStreamIntrinsics(stream_mode);
80 
81  m_intrinsics_left = convertIntrinsics(si.left);
82  m_intrinsics_right = convertIntrinsics(si.right);
83 
84  std::cout << "LEFT camera intrinsics:\n"
85  << m_intrinsics_left.dumpAsText() << "\n";
86  std::cout << "RIGHT camera intrinsics:\n"
87  << m_intrinsics_right.dumpAsText() << "\n";
88 
89  std::cout << "Waiting for streams...\n";
90  m_capture->cam->WaitForStreams();
91  std::cout << "Streams started OK.\n";
92 
93  m_bInitialized = true;
94 
95 #else
96  THROW_EXCEPTION("MRPT was built without MYNTEYE-D SDK");
97 #endif
98  MRPT_END
99 }
100 
102 
104 {
105 #if MRPT_HAS_MYNTEYE_D && MRPT_HAS_OPENCV
107 
108  mynteyed::StreamData image_color, image_depth;
109 
110  for (int nRetries = 0; nRetries < 100; nRetries++)
111  {
112  if (!image_color.img)
113  image_color = m_capture->cam->GetStreamData(
114  mynteyed::ImageType::IMAGE_LEFT_COLOR);
115 
116  if (!image_depth.img)
117  image_depth =
118  m_capture->cam->GetStreamData(mynteyed::ImageType::IMAGE_DEPTH);
119 
120  if (image_color.img && image_depth.img)
121  break;
122  else
123  {
124  std::this_thread::sleep_for(std::chrono::milliseconds(1));
125  }
126  }
127 
128  // Empty output obs:
130 
131  if (!image_color.img && !image_depth.img) return false;
132 
133  // all is good:
134  out.timestamp = mrpt::Clock::now();
135 
136  // RGB= 3xu8
137  if (image_color.img)
138  {
139  auto i = image_color.img->To(mynteyed::ImageFormat::COLOR_BGR);
140 
142  i->data_size(), static_cast<size_t>(i->width() * i->height() * 3));
143 
144  cv::Mat m(i->height(), i->width(), CV_8UC3, i->data());
145 
146  out.hasIntensityImage = true;
147  out.cameraParamsIntensity = m_intrinsics_left;
148  out.intensityImage = mrpt::img::CImage(m, mrpt::img::DEEP_COPY);
149  }
150 
151  // Depth= u16
152  if (image_depth.img)
153  {
154  auto i = image_depth.img->To(mynteyed::ImageFormat::DEPTH_RAW);
155 
156  const auto h = i->height(), w = i->width();
157 
158  out.hasRangeImage = true;
159  out.cameraParams = m_intrinsics_left;
160 
161  out.range_is_depth = true;
162  // This method will try to exploit memory pooling if possible:
163  out.rangeImage_setSize(h, w);
164  out.rangeUnits = 1e-3f; // we use mm as units
165 
167  i->data_size(), static_cast<size_t>(i->width() * i->height() * 2));
168  cv::Mat m(i->height(), i->width(), CV_16UC1, i->data());
169 
170  for (int r = 0; r < h; r++)
171  {
172  for (int c = 0; c < w; c++)
173  {
174  const uint16_t v = m.at<uint16_t>(r, c);
175  out.rangeImage.coeffRef(r, c) = v;
176  }
177  }
178  }
179 
180  return true;
181 #else
182  THROW_EXCEPTION("MRPT was built without MYNTEYE-D SDK or OpenCV");
183 #endif
184 }
185 
187  const mrpt::config::CConfigFileBase& c, const std::string& s)
188 {
190 }
uint32_t nrows
Definition: TCamera.h:40
bool m_bInitialized
Set to false if we could not initialize the camera.
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
#define MRPT_LOAD_CONFIG_VAR_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR() for config file object named c and section string named s ...
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
Contains classes for various device interfaces.
mrpt::vision::TStereoCalibParams params
std::string dumpAsText() const
Dumps all the parameters as a multi-line string, with the same format than saveToConfigFile.
Definition: TCamera.cpp:34
CMyntEyeCamera(const TMyntEyeCameraParameters &params)
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
Definition: Clock.cpp:94
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::img::CImage CImage
Definition: utils/CImage.h:5
This class allows loading and storing values and vectors of different types from a configuration text...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:173
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
bool getObservation(mrpt::obs::CObservation3DRangeScan &out)
Grab an image from the opened camera.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
Open parameters for CMyntEyeCamera.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::uint8_t ir_intensity
IR (Infrared), range [0,10], default 0.
mrpt::vision::TStereoCalibResults out
Deep copy: the copied object has a duplicate of all data, becoming independent.
Definition: img/CImage.h:78
#define MRPT_END
Definition: exceptions.h:245
mrpt::img::TCamera m_intrinsics_right
mrpt::pimpl< Impl > m_capture
pimpl< T > make_impl(Args &&... args)
Definition: pimpl.h:18
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
Wrapper on MYNT-EYE-D cameras.
mrpt::img::TCamera m_intrinsics_left



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020