MRPT  1.9.9
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-2019, 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/otherlibs/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  int cameraIndex, const TCaptureOptions_SVS& options)
32  : m_resolutionX(options.frame_width),
33  m_resolutionY(options.frame_height),
34  m_procesOnChip(options.m_procesOnChip),
35  m_calDisparity(options.m_calDisparity),
36  m_options(options)
37 {
38 #if MRPT_HAS_SVS
39 
40  // get the svsVideoImages object from the currently loaded camera interface
41  m_videoObject = static_cast<svsVideoImages*>(getVideoObject());
42  cout << "Using live images:" << endl;
43  cout << "svsVideoIdent" << endl;
44 
45  // Open the stereo device
46  bool ret;
47  ret = static_cast<svsVideoImages*>(m_videoObject)->Open();
48  if (ret)
49  {
50  cout << " stereo device Opened" << endl;
51 
52  static_cast<svsVideoImages*>(m_videoObject)
53  ->SetSize(m_resolutionX, m_resolutionY); // width x height image
54 
56  static_cast<svsVideoImages*>(m_videoObject)->GetDP();
57  static_cast<svsVideoImages*>(m_videoObject)
58  ->SetNDisp(m_options.m_NDisp); // 32 disparities
59  static_cast<svsVideoImages*>(m_videoObject)
60  ->SetCorrsize(m_options.m_Corrsize); // correlation window size
61  static_cast<svsVideoImages*>(m_videoObject)
62  ->SetLR(m_options.m_LR); // no left-right check, not available
63  static_cast<svsVideoImages*>(m_videoObject)
64  ->SetThresh(m_options.m_Thresh); // texture filter
65  static_cast<svsVideoImages*>(m_videoObject)
66  ->SetUnique(m_options.m_Unique); // uniqueness filter
67  static_cast<svsVideoImages*>(m_videoObject)
68  ->SetHoropter(m_options.m_Horopter); // horopter offset
69 
70  if (!(static_cast<svsVideoImages*>(m_videoObject)
71  ->SetExposure(0, 0, true, true)))
72  {
73  cout << "Can't set Auto exposure" << endl;
74  }
75  else
76  {
77  cout << "Autoexposure set to 0 0" << endl;
78  }
79 
80  /* videoObject->SetBrightness(true, 0);
81  videoObject->SetAutoExpParams(0.0, -2.0);
82  */
83  /// videoObject->SetGamma(); search for auto gamma ?
84 
85  static_cast<svsVideoImages*>(m_videoObject)
86  ->SetRate(m_options.framerate);
87  static_cast<svsVideoImages*>(m_videoObject)
88  ->SetSpeckleSize(m_options.m_SpeckleSize); // TODO add in config
89 
90  // TODO call CheckParam
91  if (static_cast<svsVideoImages*>(m_videoObject)->CheckParams())
92  {
93  cout << "Params OK !" << endl;
94  m_initialized = true;
95  m_status = true;
96  bool ret = static_cast<svsVideoImages*>(m_videoObject)->Start();
97  if (ret)
98  {
99  cout << " Start Continuous mode" << endl;
100 
101  // NOTE: to do rectification, we have to turn it on...
102  // Here we optionally set up acquisition to rectify the image
103 
104  ret =
105  static_cast<svsVideoImages*>(m_videoObject)->SetRect(true);
106  if (ret)
107  {
108  cout << "Images will be rectified" << endl;
109  }
110  else
111  {
112  cout << "Can't set rectification" << endl;
113  }
114  if (m_procesOnChip)
115  {
116  // NOTE: for STOC device, turn on stereo processing on-chip
117  if (static_cast<svsVideoImages*>(m_videoObject) &&
118  static_cast<svsVideoImages*>(m_videoObject)
119  ->is_proc_capable) // can we process on-camera?
120  {
121  static_cast<svsVideoImages*>(m_videoObject)
122  ->SetProcMode(PROC_MODE_DISPARITY);
123  cout << "Setting STOC disparity mode" << endl;
124  }
125  }
126  else
127  {
128  if (m_processObject)
129  m_processObject = new svsStereoProcess();
130  // NOTE: for STOC device, turn off stereo processing on-chip
131  if (static_cast<svsVideoImages*>(m_videoObject) &&
132  static_cast<svsVideoImages*>(m_videoObject)
133  ->is_proc_capable) // can we process on-camera?
134  {
135  static_cast<svsVideoImages*>(m_videoObject)
136  ->SetProcMode(PROC_MODE_OFF);
137  cout << "Setting STOC stereo mode" << endl;
138  }
139  }
140  }
141  else
142  {
143  cout << "Can't start continuous capture" << endl;
144  m_status = false;
145  }
146  }
147  else
148  {
149  m_initialized = false;
150  m_status = false;
151  cout << "Params Unconsistents !" << endl;
152  }
153  }
154  else
155 
156  // TODO essayer de faire un close...
157  cout << "Can't open stereo device" << endl;
158 
159  m_status = false;
160 
161 #else
162  MRPT_UNUSED_PARAM(cameraIndex);
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  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  MRPT_UNUSED_PARAM(out_observation);
311  // No need to raise an exception on "#else" since it's already raised upon
312  // construction.
313  return false; // This shouldn't actually be never reached, just to please
314 // the compiler.
315 #endif
316 }
317 
318 /*-------------------------------------------------------------
319  TCaptureOptions_bumblebee Constructor
320  -------------------------------------------------------------*/
322  int _frame_width, int _frame_height, double _framerate, int _NDisp,
323  int _Corrsize, int _LR, int _Thresh, int _Unique, int _Horopter,
324  int _SpeckleSize, bool _procesOnChip, bool _calDisparity)
325 {
326  frame_width = _frame_width;
327  frame_height = _frame_height;
328  framerate = _framerate;
329  m_NDisp = _NDisp; // 32 disparities
330  m_Corrsize = _Corrsize; // correlation window size
331  m_LR = _LR; // no left-right check, not available
332  m_Thresh = _Thresh; // texture filter
333  m_Unique = _Unique; // uniqueness filter
334  m_Horopter = _Horopter;
335  m_SpeckleSize = _SpeckleSize;
336  m_procesOnChip = _procesOnChip;
337  m_calDisparity = _calDisparity;
338 }
#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:21
void swap(CObservationStereoImages &o)
Do an efficient swap of all data members of this object with "o".
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019