MRPT  2.0.1
CStereoGrabber_SVS.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 
13 
14 // Universal include for all versions of OpenCV
15 #include <mrpt/3rdparty/do_opencv_includes.h>
16 
17 #if MRPT_HAS_SVS
18 #include <dcs.h>
19 #include <svsclass.h>
20 #endif
21 
22 using namespace std;
23 using namespace mrpt;
24 using namespace mrpt::obs;
25 using namespace mrpt::hwdrivers;
26 
27 /*-------------------------------------------------------------
28  Constructor
29  -------------------------------------------------------------*/
30 CStereoGrabber_SVS::CStereoGrabber_SVS(
31  [[maybe_unused]] int cameraIndex,
32  [[maybe_unused]] const TCaptureOptions_SVS& options)
33  : m_resolutionX(options.frame_width),
34  m_resolutionY(options.frame_height),
35  m_procesOnChip(options.m_procesOnChip),
36  m_calDisparity(options.m_calDisparity),
37  m_options(options)
38 {
39 #if MRPT_HAS_SVS
40 
41  // get the svsVideoImages object from the currently loaded camera interface
42  m_videoObject = static_cast<svsVideoImages*>(getVideoObject());
43  cout << "Using live images:" << endl;
44  cout << "svsVideoIdent" << endl;
45 
46  // Open the stereo device
47  bool ret;
48  ret = static_cast<svsVideoImages*>(m_videoObject)->Open();
49  if (ret)
50  {
51  cout << " stereo device Opened" << endl;
52 
53  static_cast<svsVideoImages*>(m_videoObject)
54  ->SetSize(m_resolutionX, m_resolutionY); // width x height image
55 
56  m_disparityParams =
57  static_cast<svsVideoImages*>(m_videoObject)->GetDP();
58  static_cast<svsVideoImages*>(m_videoObject)
59  ->SetNDisp(m_options.m_NDisp); // 32 disparities
60  static_cast<svsVideoImages*>(m_videoObject)
61  ->SetCorrsize(m_options.m_Corrsize); // correlation window size
62  static_cast<svsVideoImages*>(m_videoObject)
63  ->SetLR(m_options.m_LR); // no left-right check, not available
64  static_cast<svsVideoImages*>(m_videoObject)
65  ->SetThresh(m_options.m_Thresh); // texture filter
66  static_cast<svsVideoImages*>(m_videoObject)
67  ->SetUnique(m_options.m_Unique); // uniqueness filter
68  static_cast<svsVideoImages*>(m_videoObject)
69  ->SetHoropter(m_options.m_Horopter); // horopter offset
70 
71  if (!(static_cast<svsVideoImages*>(m_videoObject)
72  ->SetExposure(0, 0, true, true)))
73  {
74  cout << "Can't set Auto exposure" << endl;
75  }
76  else
77  {
78  cout << "Autoexposure set to 0 0" << endl;
79  }
80 
81  /* videoObject->SetBrightness(true, 0);
82  videoObject->SetAutoExpParams(0.0, -2.0);
83  */
84  /// videoObject->SetGamma(); search for auto gamma ?
85 
86  static_cast<svsVideoImages*>(m_videoObject)
87  ->SetRate(m_options.framerate);
88  static_cast<svsVideoImages*>(m_videoObject)
89  ->SetSpeckleSize(m_options.m_SpeckleSize); // TODO add in config
90 
91  // TODO call CheckParam
92  if (static_cast<svsVideoImages*>(m_videoObject)->CheckParams())
93  {
94  cout << "Params OK !" << endl;
95  m_initialized = true;
96  m_status = true;
97  bool ret = static_cast<svsVideoImages*>(m_videoObject)->Start();
98  if (ret)
99  {
100  cout << " Start Continuous mode" << endl;
101 
102  // NOTE: to do rectification, we have to turn it on...
103  // Here we optionally set up acquisition to rectify the image
104 
105  ret =
106  static_cast<svsVideoImages*>(m_videoObject)->SetRect(true);
107  if (ret)
108  {
109  cout << "Images will be rectified" << endl;
110  }
111  else
112  {
113  cout << "Can't set rectification" << endl;
114  }
115  if (m_procesOnChip)
116  {
117  // NOTE: for STOC device, turn on stereo processing on-chip
118  if (static_cast<svsVideoImages*>(m_videoObject) &&
119  static_cast<svsVideoImages*>(m_videoObject)
120  ->is_proc_capable) // can we process on-camera?
121  {
122  static_cast<svsVideoImages*>(m_videoObject)
123  ->SetProcMode(PROC_MODE_DISPARITY);
124  cout << "Setting STOC disparity mode" << endl;
125  }
126  }
127  else
128  {
129  if (m_processObject)
130  m_processObject = new svsStereoProcess();
131  // NOTE: for STOC device, turn off stereo processing on-chip
132  if (static_cast<svsVideoImages*>(m_videoObject) &&
133  static_cast<svsVideoImages*>(m_videoObject)
134  ->is_proc_capable) // can we process on-camera?
135  {
136  static_cast<svsVideoImages*>(m_videoObject)
137  ->SetProcMode(PROC_MODE_OFF);
138  cout << "Setting STOC stereo mode" << endl;
139  }
140  }
141  }
142  else
143  {
144  cout << "Can't start continuous capture" << endl;
145  m_status = false;
146  }
147  }
148  else
149  {
150  m_initialized = false;
151  m_status = false;
152  cout << "Params Unconsistents !" << endl;
153  }
154  }
155  else
156 
157  // TODO essayer de faire un close...
158  cout << "Can't open stereo device" << endl;
159 
160  m_status = false;
161 
162 #else
163  THROW_EXCEPTION("This class requires MRPT built with Videre SVS library.");
164 #endif
165 }
166 
167 /*-------------------------------------------------------------
168  Destructor
169  -------------------------------------------------------------*/
171 {
172 #if MRPT_HAS_SVS
173  static_cast<svsVideoImages*>(m_videoObject)->Close();
174 #endif // No need to raise an exception on "#else" since it's already raised
175  // upon construction.
176 }
177 
178 /*-------------------------------------------------------------
179  get the image
180  -------------------------------------------------------------*/
182  [maybe_unused]] mrpt::obs::CObservationStereoImages& out_observation)
183 {
184 #if MRPT_HAS_SVS
185  if ((m_stereoImage =
186  static_cast<svsVideoImages*>(m_videoObject)->GetImage(500)) &&
187  static_cast<svsStereoImage*>(m_stereoImage)->haveImages) // 500 ms
188  // timeout
189  // //TODO
190  // adjust
191  // timeout
192  // with
193  // framerate
194  {
195  // get disparity params
197  static_cast<svsVideoImages*>(m_videoObject)->GetDP();
198 
199  const size_t sizeOfMat = m_resolutionX * m_resolutionY;
200 
201  IplImage* ImageLeft = cvCreateImageHeader(
202  cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);
203 
204  ImageLeft->widthStep =
205  ImageLeft->width; // JL: The next line assumes this
206  ImageLeft->imageData =
207  (char*)static_cast<svsStereoImage*>(m_stereoImage)->Left();
208 
209  if (m_procesOnChip)
210  {
211  IplImage* ImageDisparity = cvCreateImage(
212  cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);
213 
214  unsigned char* ptrOutDisp;
215  short int* ptrDisp;
216 
217  ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
218  ptrOutDisp = (unsigned char*)ImageDisparity->imageData;
219 
220  ASSERT_(
221  ImageDisparity->widthStep ==
222  ImageDisparity->width); // JL: The code below assumes
223  // image_width == widthStep
224 
225  for (int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
226  {
227  if (*(ptrDisp) > 0)
228  *(ptrOutDisp) = (unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
229  else
230  *(ptrOutDisp) = 0;
231  }
232 
233  // Create the object to be return (it will have a fresh timestamp if
234  // it's created now and here):
235  CObservationStereoImages ret_obj(
236  cvCloneImage(ImageLeft), // Create a new IplImage* which will
237  // be owned by the observation object.
238  nullptr /*has no right*/, ImageDisparity,
239  true /* own the memory, so we don't have to free it here */);
240 
241  out_observation.swap(
242  ret_obj); // Send as output (faster than a "=").
243  }
244  else if (m_calDisparity)
245  {
246  static_cast<svsStereoProcess*>(m_processObject)
247  ->CalcStereo(static_cast<svsStereoImage*>(m_stereoImage));
248  IplImage* ImageDisparity = cvCreateImage(
249  cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);
250 
251  unsigned char* ptrOutDisp;
252  short int* ptrDisp;
253 
254  ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
255  ptrOutDisp = (unsigned char*)ImageDisparity->imageData;
256 
257  ASSERT_(
258  ImageDisparity->widthStep ==
259  ImageDisparity->width); // JL: The code below assumes
260  // image_width == widthStep
261 
262  for (int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
263  {
264  if (*(ptrDisp) > 0)
265  *(ptrOutDisp) = (unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
266  else
267  *(ptrOutDisp) = 0;
268  }
269 
270  // Create the object to be return (it will have a fresh timestamp if
271  // it's created now and here):
272  CObservationStereoImages ret_obj(
273  cvCloneImage(ImageLeft), // Create a new IplImage* which will
274  // be owned by the observation object.
275  nullptr /*has no right*/, ImageDisparity,
276  true /* own the memory, so we don't have to free it here */);
277 
278  out_observation.swap(
279  ret_obj); // Send as output (faster than a "=").
280  }
281  else
282  {
283  IplImage* ImageRight = cvCreateImageHeader(
284  cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);
285  ImageRight->widthStep =
286  ImageRight->width; // JL: The next line assumes this
287  ImageRight->imageData =
288  (char*)static_cast<svsStereoImage*>(m_stereoImage)->Right();
289 
290  // Create the object to be return (it will have a fresh timestamp if
291  // it's created now and here):
292  CObservationStereoImages ret_obj(
293  cvCloneImage(ImageLeft), // Create a new IplImage* which will
294  // be owned by the observation object.
295  cvCloneImage(ImageRight), nullptr /*has no disparity*/,
296  true /* own the memory, so we don't have to free it here */);
297 
298  out_observation.swap(
299  ret_obj); // Send as output (faster than a "=").
300  }
301 
302  // cvReleaseImage(&Image Disparity); // No need anymore to release
303  // images...
304  return true;
305  }
306 
307  return false;
308 // All ok
309 #else
310  // No need to raise an exception on "#else" since it's already raised upon
311  // construction.
312  return false; // This shouldn't actually be never reached, just to please
313 // the compiler.
314 #endif
315 }
316 
317 /*-------------------------------------------------------------
318  TCaptureOptions_bumblebee Constructor
319  -------------------------------------------------------------*/
321  int _frame_width, int _frame_height, double _framerate, int _NDisp,
322  int _Corrsize, int _LR, int _Thresh, int _Unique, int _Horopter,
323  int _SpeckleSize, bool _procesOnChip, bool _calDisparity)
324 {
325  frame_width = _frame_width;
326  frame_height = _frame_height;
327  framerate = _framerate;
328  m_NDisp = _NDisp; // 32 disparities
329  m_Corrsize = _Corrsize; // correlation window size
330  m_LR = _LR; // no left-right check, not available
331  m_Thresh = _Thresh; // texture filter
332  m_Unique = _Unique; // uniqueness filter
333  m_Horopter = _Horopter;
334  m_SpeckleSize = _SpeckleSize;
335  m_procesOnChip = _procesOnChip;
336  m_calDisparity = _calDisparity;
337 }
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
int m_NDisp
number of STOC&#39;s disparities (Default: 64 )
Contains classes for various device interfaces.
STL namespace.
double framerate
STOC camera frame rate (Default: 30 fps)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
int frame_width
Capture resolution (Default: 640x480)
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
This namespace contains representation of robot actions and observations.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Options used when creating a STOC Videre Design camera capture object.
struct _IplImage IplImage
Definition: img/CImage.h:22
TCaptureOptions_SVS(int _frame_width=640, int _frame_height=480, double _framerate=30, int _NDisp=64, int _Corrsize=15, int _LR=false, int _Thresh=10, int _Unique=13, int _Horopter=0, int _SpeckleSize=100, bool _procesOnChip=true, bool _calDisparity=true)
bool getStereoObservation(mrpt::obs::CObservationStereoImages &out_observation)
Grab stereo images, and return the pair of rectified images.



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