15 #include <mrpt/3rdparty/do_opencv_includes.h>
30 CStereoGrabber_SVS::CStereoGrabber_SVS(
31 [[maybe_unused]]
int cameraIndex,
33 : m_resolutionX(options.frame_width),
34 m_resolutionY(options.frame_height),
35 m_procesOnChip(options.m_procesOnChip),
36 m_calDisparity(options.m_calDisparity),
42 m_videoObject =
static_cast<svsVideoImages*
>(getVideoObject());
43 cout <<
"Using live images:" << endl;
44 cout <<
"svsVideoIdent" << endl;
48 ret =
static_cast<svsVideoImages*
>(m_videoObject)->Open();
51 cout <<
" stereo device Opened" << endl;
53 static_cast<svsVideoImages*
>(m_videoObject)
54 ->SetSize(m_resolutionX, m_resolutionY);
57 static_cast<svsVideoImages*
>(m_videoObject)->GetDP();
58 static_cast<svsVideoImages*
>(m_videoObject)
59 ->SetNDisp(m_options.m_NDisp);
60 static_cast<svsVideoImages*
>(m_videoObject)
61 ->SetCorrsize(m_options.m_Corrsize);
62 static_cast<svsVideoImages*
>(m_videoObject)
63 ->SetLR(m_options.m_LR);
64 static_cast<svsVideoImages*
>(m_videoObject)
65 ->SetThresh(m_options.m_Thresh);
66 static_cast<svsVideoImages*
>(m_videoObject)
67 ->SetUnique(m_options.m_Unique);
68 static_cast<svsVideoImages*
>(m_videoObject)
69 ->SetHoropter(m_options.m_Horopter);
71 if (!(
static_cast<svsVideoImages*
>(m_videoObject)
72 ->SetExposure(0, 0,
true,
true)))
74 cout <<
"Can't set Auto exposure" << endl;
78 cout <<
"Autoexposure set to 0 0" << endl;
86 static_cast<svsVideoImages*
>(m_videoObject)
87 ->SetRate(m_options.framerate);
88 static_cast<svsVideoImages*
>(m_videoObject)
89 ->SetSpeckleSize(m_options.m_SpeckleSize);
92 if (
static_cast<svsVideoImages*
>(m_videoObject)->CheckParams())
94 cout <<
"Params OK !" << endl;
97 bool ret =
static_cast<svsVideoImages*
>(m_videoObject)->Start();
100 cout <<
" Start Continuous mode" << endl;
106 static_cast<svsVideoImages*
>(m_videoObject)->SetRect(
true);
109 cout <<
"Images will be rectified" << endl;
113 cout <<
"Can't set rectification" << endl;
118 if (
static_cast<svsVideoImages*
>(m_videoObject) &&
119 static_cast<svsVideoImages*
>(m_videoObject)
122 static_cast<svsVideoImages*
>(m_videoObject)
123 ->SetProcMode(PROC_MODE_DISPARITY);
124 cout <<
"Setting STOC disparity mode" << endl;
130 m_processObject =
new svsStereoProcess();
132 if (
static_cast<svsVideoImages*
>(m_videoObject) &&
133 static_cast<svsVideoImages*
>(m_videoObject)
136 static_cast<svsVideoImages*
>(m_videoObject)
137 ->SetProcMode(PROC_MODE_OFF);
138 cout <<
"Setting STOC stereo mode" << endl;
144 cout <<
"Can't start continuous capture" << endl;
150 m_initialized =
false;
152 cout <<
"Params Unconsistents !" << endl;
158 cout <<
"Can't open stereo device" << endl;
163 THROW_EXCEPTION(
"This class requires MRPT built with Videre SVS library.");
174 #endif // No need to raise an exception on "#else" since it's already raised
186 static_cast<svsVideoImages*
>(
m_videoObject)->GetImage(500)) &&
201 IplImage* ImageLeft = cvCreateImageHeader(
204 ImageLeft->widthStep =
206 ImageLeft->imageData =
211 IplImage* ImageDisparity = cvCreateImage(
214 unsigned char* ptrOutDisp;
217 ptrDisp =
static_cast<svsStereoImage*
>(
m_stereoImage)->Disparity();
218 ptrOutDisp = (
unsigned char*)ImageDisparity->imageData;
221 ImageDisparity->widthStep ==
222 ImageDisparity->width);
225 for (
int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
228 *(ptrOutDisp) = (
unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
236 cvCloneImage(ImageLeft),
238 nullptr , ImageDisparity,
241 out_observation.swap(
248 IplImage* ImageDisparity = cvCreateImage(
251 unsigned char* ptrOutDisp;
254 ptrDisp =
static_cast<svsStereoImage*
>(
m_stereoImage)->Disparity();
255 ptrOutDisp = (
unsigned char*)ImageDisparity->imageData;
258 ImageDisparity->widthStep ==
259 ImageDisparity->width);
262 for (
int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
265 *(ptrOutDisp) = (
unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
273 cvCloneImage(ImageLeft),
275 nullptr , ImageDisparity,
278 out_observation.swap(
283 IplImage* ImageRight = cvCreateImageHeader(
285 ImageRight->widthStep =
287 ImageRight->imageData =
293 cvCloneImage(ImageLeft),
295 cvCloneImage(ImageRight),
nullptr ,
298 out_observation.swap(
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)