Debian Codegraph @

libs/hwdrivers/src/CStereoGrabber_SVS.cpp

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