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