Debian Codegraph @

libs/hwdrivers/src/CImageGrabber_OpenCV.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/3rdparty/do_opencv_includes.h>
13 #include <mrpt/hwdrivers/CImageGrabber_OpenCV.h>
14
15 #include <chrono>
16 #include <thread>
17
18 #ifdef HAVE_OPENCV_VIDEOIO
19 // cv::VideoCapture moved from highgui in opencv2 to videoio in opencv3:
20 #include <opencv2/videoio.hpp>
21 #endif
22
23 using namespace std;
24 using namespace mrpt;
25 using namespace mrpt::hwdrivers;
26
27 struct CImageGrabber_OpenCV::Impl
28 {
29 #if MRPT_HAS_OPENCV
30 cv::VideoCapture cap;
31 #endif
32 };
33
34 /*-------------------------------------------------------------
35 Constructor
36 -------------------------------------------------------------*/
37 CImageGrabber_OpenCV::CImageGrabber_OpenCV(
38 int cameraIndex, TCameraType cameraType, const TCaptureCVOptions& options) :
39 m_capture(mrpt::make_impl<CImageGrabber_OpenCV::Impl>())
40 {
41 MRPT_START
42 m_bInitialized = false;
43
44 #if MRPT_HAS_OPENCV
45 int cv_cap_indx = 0;
46 switch (cameraType)
47 {
48 case CAMERA_CV_AUTODETECT:
49 cv_cap_indx = cv::CAP_ANY;
50 break;
51 case CAMERA_CV_DC1394:
52 cv_cap_indx = cv::CAP_DC1394;
53 break;
54 case CAMERA_CV_VFL:
55 cv_cap_indx = cv::CAP_V4L;
56 break;
57 case CAMERA_CV_DSHOW:
58 cv_cap_indx = cv::CAP_DSHOW;
59 break;
60 default:
61 THROW_EXCEPTION_FMT("Invalid camera type: %i", cameraType);
62 }
63
64 cv_cap_indx += cameraIndex;
65
66 // Open camera:
67 if (!m_capture->cap.open(cv_cap_indx))
68 {
69 cerr << mrpt::format("[CImageGrabber_OpenCV] ERROR: Can't open camera '%i'!!\n", cameraIndex);
70 return;
71 }
72
73 // Set properties:
74 // Based on code from Orocos project. Thanks!
75 // ----------------------------------------
76 // Global settings
77 if (options.gain != 0)
78 {
79 if (!m_capture->cap.set(cv::CAP_PROP_GAIN, options.gain))
80 cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
81 "capturing gain property!"
82 << endl;
83 }
84
85 // Settings only for firewire
86 if (cameraType == CAMERA_CV_DC1394)
87 {
88 if (options.frame_height != 0 && options.frame_width != 0)
89 {
90 // MODE_320x240_YUV422 ****
91 //
92 enum
93 {
94 MY_MODE_160x120_YUV444 = 64,
95 MY_MODE_320x240_YUV422, // ***
96 MY_MODE_640x480_YUV411,
97 MY_MODE_640x480_YUV422, // ***
98 MY_MODE_640x480_RGB, // ?
99 MY_MODE_640x480_MONO, // ***
100 MY_MODE_640x480_MONO16
101 };
102
103 int cvMode1394 = -1;
104 if (options.frame_height == 320 && options.frame_width == 240)
105 cvMode1394 = MY_MODE_320x240_YUV422;
106 else if (
107 options.frame_height == 640 && options.frame_width == 480 && !options.ieee1394_grayscale)
108 cvMode1394 = MY_MODE_640x480_YUV422;
109 else if (
110 options.frame_height == 640 && options.frame_width == 480 && options.ieee1394_grayscale)
111 cvMode1394 = MY_MODE_640x480_MONO;
112
113 if (cvMode1394 > 0)
114 {
115 if (!m_capture->cap.set(cv::CAP_PROP_MODE, cvMode1394))
116 cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
117 "capturing mode "
118 << cvMode1394 << " property!" << endl;
119 }
120 else
121 cerr << "[CImageGrabber_OpenCV] Warning: Not valid combination "
122 "of width x height x color mode for OpenCV/IEEE1394 "
123 "interface"
124 << endl;
125 }
126
127 // Not needed: Default seems to be = 1
128 // if(cvSetCaptureProperty(M_CAPTURE,CV_CAP_PROP_CONVERT_RGB,_capture_convert.value())<1)
129 // cerr << "[CImageGrabber_OpenCV] Warning: Could not set the RGB
130 // conversion property!" << endl;
131
132 if (!m_capture->cap.set(cv::CAP_PROP_FPS, options.ieee1394_fps))
133 cerr << "[CImageGrabber_OpenCV] Warning: Could not set the fps "
134 "property!"
135 << endl;
136 }
137
138 // Settings only for V4L
139 if (cameraType == CAMERA_CV_AUTODETECT || cameraType == CAMERA_CV_VFL ||
140 cameraType == CAMERA_CV_DSHOW)
141 {
142 if (options.frame_width != 0 && options.frame_height != 0)
143 {
144 // First set width then height. The first command always returns a
145 // error!
146 m_capture->cap.set(cv::CAP_PROP_FRAME_WIDTH, options.frame_width);
147 if (!m_capture->cap.set(cv::CAP_PROP_FRAME_HEIGHT, options.frame_height))
148 cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
149 "frame width & height property!"
150 << endl;
151 }
152 }
153
154 // remember that we successfully initialized everything
155 m_bInitialized = true;
156 #else
157 THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
158 #endif
159 MRPT_END
160 }
161
162 CImageGrabber_OpenCV::CImageGrabber_OpenCV(const std::string& AVI_fileName) :
163 m_capture(mrpt::make_impl<CImageGrabber_OpenCV::Impl>())
164 {
165 MRPT_START
166 m_bInitialized = false;
167
168 #if MRPT_HAS_OPENCV
169 if (!m_capture->cap.open(AVI_fileName))
170 {
171 printf("[CImageGrabber_OpenCV] Warning! Can't open AVI file '%s'!!\n", AVI_fileName.c_str());
172 return;
173 }
174
175 // remember that we successfully initialized everything
176 m_bInitialized = true;
177 #else
178 THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
179 #endif
180 MRPT_END
181 }
182
183 /*-------------------------------------------------------------
184 Destructor
185 -------------------------------------------------------------*/
186 CImageGrabber_OpenCV::~CImageGrabber_OpenCV()
187 {
188 #if MRPT_HAS_OPENCV
189 if (m_bInitialized)
190 {
191 m_capture->cap.release();
192 }
193 #endif
194 }
195
196 /*-------------------------------------------------------------
197 get the image
198 -------------------------------------------------------------*/
199 bool CImageGrabber_OpenCV::getObservation(mrpt::obs::CObservationImage& out_observation)
200 {
201 MRPT_START
202
203 if (!m_bInitialized) return false;
204
205 #if MRPT_HAS_OPENCV
206
207 // Capture the image:
208 if (!m_capture->cap.grab()) return false;
209
210 // JL: Sometimes there're errors in some frames: try not to return an error
211 // unless it seems
212 // there's no way:
213 for (int nTries = 0; nTries < 10; nTries++)
214 {
215 cv::Mat capImg;
216 if (m_capture->cap.retrieve(capImg))
217 {
218 // Fill the output class:
219 out_observation.timestamp = mrpt::Clock::now();
220 out_observation.image = mrpt::img::CImage(capImg, mrpt::img::SHALLOW_COPY);
221 return true;
222 }
223 cerr << "[CImageGrabber_OpenCV] WARNING: Ignoring error #" << nTries + 1
224 << " retrieving frame..." << endl;
225 std::this_thread::sleep_for(1ms);
226 }
227 return false;
228 #else
229 THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
230 #endif
231 MRPT_END
232 }