YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeDepthCameraDriver_mini.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7
9#include <yarp/os/LogStream.h>
10#include <yarp/os/Value.h>
11
12#include <algorithm>
13#include <map>
14#include <cmath>
15
16using namespace yarp::dev;
17using namespace yarp::sig;
18using namespace yarp::os;
19
20namespace {
21YARP_LOG_COMPONENT(FAKEDEPTHCAMERA, "yarp.device.fakeDepthCamera_mini")
22}
23
27
29
31{
32 yCDebug(FAKEDEPTHCAMERA) << "Opening Device";
33 if (!this->parseParams(config)) {return false;}
34
37 return true;
38}
39
41{
42 yCDebug(FAKEDEPTHCAMERA) << "Closing Device";
43 std::lock_guard <std::recursive_mutex> lg(m_mutex);
44 return true;
45}
46
48{
49 std::lock_guard <std::recursive_mutex> lg(m_mutex);
50 return m_rgb_h;
51}
52
54{
55 std::lock_guard <std::recursive_mutex> lg(m_mutex);
56 return m_rgb_w;
57}
58
60{
61 std::lock_guard <std::recursive_mutex> lg(m_mutex);
63 configurations.push_back(cfg);
64 return ReturnValue_ok;
65}
66
68{
69 std::lock_guard <std::recursive_mutex> lg(m_mutex);
70 width = m_rgb_w;
71 height = m_rgb_h;
72 return ReturnValue_ok;
73}
74
76{
77 std::lock_guard <std::recursive_mutex> lg(m_mutex);
82 for (size_t y = 0; y < m_rgb_h; y++)
83 for (size_t x = 0; x < m_rgb_w; x++)
84 {
85 rgbImg.pixel(x, y).r = x * 255.0 / m_rgb_w;
86 rgbImg.pixel(x, y).g = 0;
87 rgbImg.pixel(x, y).b = y * 255.0 / m_rgb_h;
88 }
89}
90
92{
93 std::lock_guard <std::recursive_mutex> lg(m_mutex);
95 size_t totalPixels = m_dep_w * m_dep_h;
96 size_t index = 0;
97 for (size_t y = 0; y < m_dep_h; y++)
98 for (size_t x = 0; x < m_dep_w; x++) {
99 auto& p = m_depthImage.pixel(x, y);
101 {
103 }
104 else
105 {
106 p = static_cast<double>(index) / (totalPixels - 1);
107 }
108 ++index;
109 }
110}
111
113{
114 std::lock_guard <std::recursive_mutex> lg(m_mutex);
115 m_rgb_w = width;
116 m_rgb_h = height;
118 return ReturnValue_ok;
119}
120
122{
123 std::lock_guard <std::recursive_mutex> lg(m_mutex);
124 width = m_dep_w;
125 height = m_dep_h;
126 return ReturnValue_ok;
127}
128
130{
131 std::lock_guard <std::recursive_mutex> lg(m_mutex);
132 m_dep_w = width;
133 m_dep_h = height;
135 return ReturnValue_ok;
136}
137
138ReturnValue FakeDepthCameraDriver_mini::setRgbFOV(double horizontalFov, double verticalFov)
139{
140 std::lock_guard <std::recursive_mutex> lg(m_mutex);
141 m_rgb_Hfov = horizontalFov;
142 m_rgb_Vfov = verticalFov;
143 return ReturnValue_ok;
144}
145
146ReturnValue FakeDepthCameraDriver_mini::setDepthFOV(double horizontalFov, double verticalFov)
147{
148 std::lock_guard <std::recursive_mutex> lg(m_mutex);
149 m_dep_Hfov = horizontalFov;
150 m_dep_Vfov = verticalFov;
151 return ReturnValue_ok;
152}
153
155{
156 std::lock_guard <std::recursive_mutex> lg(m_mutex);
158 return ReturnValue_ok;
159}
160
161ReturnValue FakeDepthCameraDriver_mini::getRgbFOV(double& horizontalFov, double& verticalFov)
162{
163 std::lock_guard <std::recursive_mutex> lg(m_mutex);
164 horizontalFov = m_rgb_Hfov;
165 verticalFov = m_rgb_Vfov;
166 return ReturnValue_ok;
167}
168
170{
171 std::lock_guard <std::recursive_mutex> lg(m_mutex);
172 mirror = m_rgb_mirror;
173 return ReturnValue_ok;
174}
175
177{
178 std::lock_guard <std::recursive_mutex> lg(m_mutex);
179 m_rgb_mirror = mirror;
180 return ReturnValue_ok;
181}
182
184{
185 std::lock_guard <std::recursive_mutex> lg(m_mutex);
186 intrinsic.put("physFocalLength", 0.5);
187 intrinsic.put("focalLengthX", m_dep_w/2);
188 intrinsic.put("focalLengthY", m_dep_h/2);
189 intrinsic.put("principalPointX", (m_dep_w-1)/2);
190 intrinsic.put("principalPointY", (m_dep_h-1)/2);
191 intrinsic.put("distortionModel", "plumb_bob");
192 intrinsic.put("k1", 0);
193 intrinsic.put("k2", 0);
194 intrinsic.put("t1", 0);
195 intrinsic.put("t2", 0);
196 intrinsic.put("k3", 0);
197
198 intrinsic.put("stamp", yarp::os::Time::now());
199 return ReturnValue_ok;
200}
201
203{
204 std::lock_guard <std::recursive_mutex> lg(m_mutex);
205 return m_dep_h;
206}
207
209{
210 std::lock_guard <std::recursive_mutex> lg(m_mutex);
211 return m_dep_w;
212}
213
214ReturnValue FakeDepthCameraDriver_mini::getDepthFOV(double& horizontalFov, double& verticalFov)
215{
216 std::lock_guard <std::recursive_mutex> lg(m_mutex);
217 horizontalFov = m_dep_Hfov;
218 verticalFov = m_dep_Vfov;
219 return ReturnValue_ok;
220}
221
223{
224 std::lock_guard <std::recursive_mutex> lg(m_mutex);
225 intrinsic.put("physFocalLength", 0.5);
226 const double deg2rad = 3.1415926535/180.0;
227 double focal_x = m_dep_w / (2*tan(m_dep_Hfov*deg2rad/2));
228 double focal_y = m_dep_h / (2*tan(m_dep_Vfov*deg2rad/2));
229 intrinsic.put("focalLengthX", focal_x);
230 intrinsic.put("focalLengthY", focal_y);
231 intrinsic.put("principalPointX", (m_dep_w-1)/2);
232 intrinsic.put("principalPointY", (m_dep_h-1)/2);
233 intrinsic.put("distortionModel", "plumb_bob");
234 intrinsic.put("k1", 0);
235 intrinsic.put("k2", 0);
236 intrinsic.put("t1", 0);
237 intrinsic.put("t2", 0);
238 intrinsic.put("k3", 0);
239
240 intrinsic.put("stamp", yarp::os::Time::now());
241 return ReturnValue_ok;
242}
243
245{
246 std::lock_guard <std::recursive_mutex> lg(m_mutex);
248 return ReturnValue_ok;
249}
250
252{
253 std::lock_guard <std::recursive_mutex> lg(m_mutex);
254 nearPlane = m_dep_near;
255 farPlane = m_dep_far;
256 return ReturnValue_ok;
257}
258
260{
261 std::lock_guard <std::recursive_mutex> lg(m_mutex);
262 m_dep_near = nearPlane;
263 m_dep_far = farPlane;
264 return ReturnValue_ok;
265}
266
268{
269 std::lock_guard <std::recursive_mutex> lg(m_mutex);
270 mirror = m_depth_mirror;
271 return ReturnValue_ok;
272}
273
275{
276 std::lock_guard <std::recursive_mutex> lg(m_mutex);
278 return ReturnValue_ok;
279}
280
282{
283 std::lock_guard <std::recursive_mutex> lg(m_mutex);
284 extrinsic.resize(4, 4);
285 extrinsic.zero();
286
287 extrinsic[0][0] = 1;
288 extrinsic[1][1] = 1;
289 extrinsic[2][2] = 1;
290 extrinsic[3][3] = 1;
291 return ReturnValue_ok;
292}
293
295{
296 std::lock_guard <std::recursive_mutex> lg(m_mutex);
298 if (timeStamp) {
299 timeStamp->update(yarp::os::Time::now());
300 }
301 return ReturnValue_ok;
302}
303
305{
306 std::lock_guard <std::recursive_mutex> lg(m_mutex);
308 if (timeStamp) {
309 timeStamp->update(yarp::os::Time::now());
310 }
311 return ReturnValue_ok;
312}
313
315{
316 std::lock_guard <std::recursive_mutex> lg(m_mutex);
317 auto r1 = getRgbImage(colorFrame, colorStamp);
318 auto r2 = getDepthImage(depthFrame, depthStamp);
319 ReturnValue rr = r1 && r2;
320 return rr;
321}
322
324{
325 std::lock_guard <std::recursive_mutex> lg(m_mutex);
326 status = RGBD_SENSOR_OK_IN_USE;
327 return ReturnValue_ok;
328}
329
331{
332 std::lock_guard <std::recursive_mutex> lg(m_mutex);
333 msg = std::string("no error");
334 return ReturnValue_ok;
335}
@ VOCAB_PIXEL_RGB
Definition Image.h:44
#define ReturnValue_ok
Definition ReturnValue.h:82
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue setRgbResolution(int width, int height) override
Set the resolution of the rgb image from the camera.
yarp::dev::ReturnValue getRgbSupportedConfigurations(std::vector< yarp::dev::CameraConfig > &configurations) override
Get the possible configurations of the camera.
yarp::dev::ReturnValue getRgbFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the rgb camera.
yarp::dev::ReturnValue getDepthIntrinsicParam(Property &intrinsic) override
Get the intrinsic parameters of the depth camera.
int getDepthWidth() override
Return the height of each frame.
yarp::dev::ReturnValue getRgbResolution(int &width, int &height) override
Get the resolution of the rgb image from the camera.
yarp::dev::ReturnValue getDepthClipPlanes(double &nearPlane, double &farPlane) override
Get the clipping planes of the sensor.
yarp::dev::ReturnValue setDepthAccuracy(double accuracy) override
Set the minimum detectable variation in distance [meter] when possible.
yarp::dev::ReturnValue getLastErrorMsg(std::string &mesg, Stamp *timeStamp=nullptr) override
Return an error message in case of error.
yarp::dev::ReturnValue getRgbIntrinsicParam(Property &intrinsic) override
Get the intrinsic parameters of the rgb camera.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
yarp::dev::ReturnValue getDepthMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
yarp::dev::ReturnValue setDepthMirroring(bool mirror) override
Set the mirroring setting of the sensor.
yarp::dev::ReturnValue setRgbFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the rgb camera.
int getRgbWidth() override
Return the width of each frame.
int getDepthHeight() override
Return the height of each frame.
yarp::dev::ReturnValue getImages(FlexImage &colorFrame, depthImage &depthFrame, Stamp *colorStamp=nullptr, Stamp *depthStamp=nullptr) override
yarp::dev::ReturnValue getDepthAccuracy(double &accuracy) override
Get the minimum detectable variation in distance [meter].
yarp::dev::ReturnValue getDepthResolution(int &width, int &height) override
Get the resolution of the depth image from the camera.
yarp::dev::ReturnValue getRgbMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
int getRgbHeight() override
Return the height of each frame.
yarp::dev::ReturnValue setDepthResolution(int width, int height) override
Set the resolution of the depth image from the camera.
~FakeDepthCameraDriver_mini() override
yarp::dev::ReturnValue setDepthFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the depth camera.
yarp::dev::ReturnValue getDepthImage(depthImage &depthImage, Stamp *timeStamp=nullptr) override
yarp::dev::ReturnValue setDepthClipPlanes(double nearPlane, double farPlane) override
Set the clipping planes of the sensor.
yarp::dev::ReturnValue getSensorStatus(RGBDSensor_status &status) override
yarp::dev::ReturnValue getDepthFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the depth camera.
yarp::dev::ReturnValue getRgbImage(FlexImage &rgbImage, Stamp *timeStamp=nullptr) override
Get the rgb frame from the device.
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getExtrinsicParam(yarp::sig::Matrix &extrinsic) override
Get the extrinsic parameters from the device.
yarp::dev::ReturnValue setRgbMirroring(bool mirror) override
Set the mirroring setting of the sensor.
Struct describing a possible camera configuration.
A mini-server for performing network communication in the background.
A class for storing options and configuration information.
Definition Property.h:33
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition Property.cpp:987
A base class for nested structures that can be searched.
Definition Searchable.h:31
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
Image class with user control of representation details.
Definition Image.h:361
void setQuantum(size_t imgQuantum)
Definition Image.h:376
void setPixelCode(int imgPixelCode)
Definition Image.h:364
T & pixel(size_t x, size_t y)
Definition Image.h:619
void resize(size_t imgWidth, size_t imgHeight)
Reallocate an image to be of a desired size, throwing away its current contents.
Definition Image.cpp:387
A class for a Matrix.
Definition Matrix.h:39
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13
constexpr char accuracy[]
#define deg2rad(x)