Main MRPT website > C++ reference for MRPT 1.5.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-2017, 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 
15 // Universal include for all versions of OpenCV
16 #include <mrpt/otherlibs/do_opencv_includes.h>
17 
18 #if MRPT_HAS_SVS
19 # include <svsclass.h>
20 # include <dcs.h>
21 #endif
22 
23 using namespace std;
24 using namespace mrpt;
25 using namespace mrpt::obs;
26 using namespace mrpt::hwdrivers;
27 
28 /*-------------------------------------------------------------
29  Constructor
30  -------------------------------------------------------------*/
31 CStereoGrabber_SVS::CStereoGrabber_SVS( int cameraIndex, const TCaptureOptions_SVS &options ) :
32  m_bInitialized(false),
33  m_videoObject(NULL),
34  m_stereoImage(NULL),
35  m_disparityParams(NULL),
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)->SetSize(m_resolutionX,m_resolutionY); // width x height image
57 
58 
59  m_disparityParams = static_cast<svsVideoImages*>(m_videoObject)->GetDP();
60  static_cast<svsVideoImages*>(m_videoObject)->SetNDisp(m_options.m_NDisp); // 32 disparities
61  static_cast<svsVideoImages*>(m_videoObject)->SetCorrsize(m_options.m_Corrsize); // correlation window size
62  static_cast<svsVideoImages*>(m_videoObject)->SetLR(m_options.m_LR); // no left-right check, not available
63  static_cast<svsVideoImages*>(m_videoObject)->SetThresh(m_options.m_Thresh); // texture filter
64  static_cast<svsVideoImages*>(m_videoObject)->SetUnique(m_options.m_Unique); // uniqueness filter
65  static_cast<svsVideoImages*>(m_videoObject)->SetHoropter(m_options.m_Horopter); // horopter offset
66 
67  if(!(static_cast<svsVideoImages*>(m_videoObject)->SetExposure(0,0,true,true)))
68  {
69  cout<<"Can't set Auto exposure"<<endl;
70  }else
71  {
72  cout<<"Autoexposure set to 0 0"<<endl;
73  }
74 
75  /* videoObject->SetBrightness(true, 0);
76  videoObject->SetAutoExpParams(0.0, -2.0);
77  */
78  ///videoObject->SetGamma(); search for auto gamma ?
79 
80  static_cast<svsVideoImages*>(m_videoObject)->SetRate(m_options.framerate);
81  static_cast<svsVideoImages*>(m_videoObject)->SetSpeckleSize(m_options.m_SpeckleSize); //TODO add in config
82 
83  //TODO call CheckParam
84  if(static_cast<svsVideoImages*>(m_videoObject)->CheckParams())
85  {
86  cout<<"Params OK !"<<endl;
87  m_initialized =true;
88  m_status = true;
89  bool ret = static_cast<svsVideoImages*>(m_videoObject)->Start();
90  if (ret){
91 
92  cout<<" Start Continuous mode"<<endl;
93 
94  // NOTE: to do rectification, we have to turn it on...
95  // Here we optionally set up acquisition to rectify the image
96 
97  ret = static_cast<svsVideoImages*>(m_videoObject)->SetRect(true);
98  if (ret){
99  cout<<"Images will be rectified"<<endl;
100  }else{
101  cout<<"Can't set rectification"<<endl;
102  }
103  if(m_procesOnChip)
104  {
105  // NOTE: for STOC device, turn on stereo processing on-chip
106  if (static_cast<svsVideoImages*>(m_videoObject) && static_cast<svsVideoImages*>(m_videoObject)->is_proc_capable) // can we process on-camera?
107  {
108  static_cast<svsVideoImages*>(m_videoObject)->SetProcMode(PROC_MODE_DISPARITY);
109  cout<<"Setting STOC disparity mode"<<endl;
110  }
111  }
112  else
113  {
114  if (m_processObject) m_processObject = new svsStereoProcess();
115  // NOTE: for STOC device, turn off stereo processing on-chip
116  if (static_cast<svsVideoImages*>(m_videoObject) && static_cast<svsVideoImages*>(m_videoObject)->is_proc_capable) // can we process on-camera?
117  {
118  static_cast<svsVideoImages*>(m_videoObject)->SetProcMode(PROC_MODE_OFF);
119  cout<<"Setting STOC stereo mode"<<endl;
120  }
121  }
122  }
123  else
124  {
125  cout<<"Can't start continuous capture"<<endl;
126  m_status = false;
127 
128  }
129 
130 
131 
132 
133  }
134  else
135  {
136  m_initialized =false;
137  m_status = false;
138  cout<<"Params Unconsistents !"<<endl;
139  }
140 
141  }
142  else
143 
144  //TODO essayer de faire un close...
145  cout<<"Can't open stereo device"<<endl;
146 
147  m_status = false;
148 
149 #else
150  MRPT_UNUSED_PARAM(cameraIndex);
151  THROW_EXCEPTION("This class requires MRPT built with Videre SVS library.")
152 #endif
153 }
154 
155 /*-------------------------------------------------------------
156  Destructor
157  -------------------------------------------------------------*/
159 {
160 #if MRPT_HAS_SVS
161  static_cast<svsVideoImages*>(m_videoObject)->Close();
162 #endif // No need to raise an exception on "#else" since it's already raised upon construction.
163 }
164 
165 /*-------------------------------------------------------------
166  get the image
167  -------------------------------------------------------------*/
169 {
170 #if MRPT_HAS_SVS
171  if ( (m_stereoImage = static_cast<svsVideoImages*>(m_videoObject)->GetImage(500)) && static_cast<svsStereoImage*>(m_stereoImage)->haveImages ) // 500 ms timeout //TODO adjust timeout with framerate
172  {
173 
174  //get disparity params
175  m_disparityParams = static_cast<svsVideoImages*>(m_videoObject)->GetDP();
176 
177  const size_t sizeOfMat = m_resolutionX * m_resolutionY;
178 
179  IplImage* ImageLeft = cvCreateImageHeader(cvSize(m_resolutionX,m_resolutionY),IPL_DEPTH_8U,1);
180 
181  ImageLeft->widthStep=ImageLeft->width; // JL: The next line assumes this
182  ImageLeft->imageData =(char*) static_cast<svsStereoImage*>(m_stereoImage)->Left();
183 
184  if(m_procesOnChip)
185  {
186  IplImage* ImageDisparity = cvCreateImage(cvSize(m_resolutionX,m_resolutionY),IPL_DEPTH_8U,1);
187 
188  unsigned char *ptrOutDisp;
189  short int *ptrDisp;
190 
191  ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
192  ptrOutDisp = (unsigned char*) ImageDisparity->imageData;
193 
194  ASSERT_(ImageDisparity->widthStep==ImageDisparity->width); // JL: The code below assumes image_width == widthStep
195 
196  for(int pix = 0;pix<sizeOfMat;pix++,ptrOutDisp++,ptrDisp++ )
197  {
198  if(*(ptrDisp)>0)
199  *(ptrOutDisp) = (unsigned char)((*(ptrDisp)>>2)&0x00FF);
200  else
201  *(ptrOutDisp) = 0;
202  }
203 
204  // Create the object to be return (it will have a fresh timestamp if it's created now and here):
205  CObservationStereoImages ret_obj(
206  cvCloneImage( ImageLeft ), // Create a new IplImage* which will be owned by the observation object.
207  NULL /*has no right*/,
208  ImageDisparity,
209  true /* own the memory, so we don't have to free it here */);
210 
211  out_observation.swap(ret_obj); // Send as output (faster than a "=").
212  }
213  else if (m_calDisparity)
214  {
215  static_cast<svsStereoProcess*>(m_processObject)->CalcStereo(static_cast<svsStereoImage*>(m_stereoImage));
216  IplImage* ImageDisparity = cvCreateImage(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_(ImageDisparity->widthStep==ImageDisparity->width); // JL: The code below assumes image_width == widthStep
225 
226  for(int pix = 0;pix<sizeOfMat;pix++,ptrOutDisp++,ptrDisp++ )
227  {
228  if(*(ptrDisp)>0)
229  *(ptrOutDisp) = (unsigned char)((*(ptrDisp)>>2)&0x00FF);
230  else
231  *(ptrOutDisp) = 0;
232  }
233 
234  // Create the object to be return (it will have a fresh timestamp if it's created now and here):
235  CObservationStereoImages ret_obj(
236  cvCloneImage( ImageLeft ), // Create a new IplImage* which will be owned by the observation object.
237  NULL /*has no right*/,
238  ImageDisparity,
239  true /* own the memory, so we don't have to free it here */);
240 
241  out_observation.swap(ret_obj); // Send as output (faster than a "=").
242 
243  }
244  else
245  {
246 
247  IplImage* ImageRight = cvCreateImageHeader(cvSize(m_resolutionX,m_resolutionY),IPL_DEPTH_8U,1);
248  ImageRight->widthStep=ImageRight->width; // JL: The next line assumes this
249  ImageRight->imageData =(char*) static_cast<svsStereoImage*>(m_stereoImage)->Right();
250 
251  // Create the object to be return (it will have a fresh timestamp if it's created now and here):
252  CObservationStereoImages ret_obj(
253  cvCloneImage( ImageLeft ), // Create a new IplImage* which will be owned by the observation object.
254  cvCloneImage( ImageRight ),
255  NULL /*has no disparity*/,
256  true /* own the memory, so we don't have to free it here */);
257 
258  out_observation.swap(ret_obj); // Send as output (faster than a "=").
259 
260  }
261 
262 
263 
264  // cvReleaseImage(&Image Disparity); // No need anymore to release images...
265  return true;
266  }
267 
268  return false;
269  // All ok
270 #else
271  MRPT_UNUSED_PARAM(out_observation);
272  // No need to raise an exception on "#else" since it's already raised upon construction.
273  return false; // This shouldn't actually be never reached, just to please the compiler.
274 #endif
275 }
276 
277 /*-------------------------------------------------------------
278  TCaptureOptions_bumblebee Constructor
279  -------------------------------------------------------------*/
280 TCaptureOptions_SVS::TCaptureOptions_SVS(int _frame_width, int _frame_height , double _framerate, int _NDisp,
281  int _Corrsize, int _LR , int _Thresh, int _Unique, int _Horopter,int _SpeckleSize,bool _procesOnChip,bool _calDisparity)
282 {
283  frame_width = _frame_width;
284  frame_height = _frame_height;
285  framerate = _framerate;
286  m_NDisp = _NDisp; // 32 disparities
287  m_Corrsize =_Corrsize; // correlation window size
288  m_LR =_LR;// no left-right check, not available
289  m_Thresh =_Thresh; // texture filter
290  m_Unique =_Unique; // uniqueness filter
291  m_Horopter =_Horopter;
292  m_SpeckleSize =_SpeckleSize;
293  m_procesOnChip =_procesOnChip;
294  m_calDisparity =_calDisparity;
295 
296 }
297 
virtual ~CStereoGrabber_SVS(void)
Destructor.
int m_NDisp
number of STOC&#39;s disparities (Default: 64 )
#define THROW_EXCEPTION(msg)
Contains classes for various device interfaces.
STL namespace.
double framerate
STOC camera frame rate (Default: 30 fps)
int frame_height
Capture resolution (Default: 640x480)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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)
#define ASSERT_(f)
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 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020