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.physFocalLength= 0.5;
187 intrinsic.focalLengthX= m_dep_w/2;
188 intrinsic.focalLengthY= m_dep_h/2;
189 intrinsic.principalPointX= (m_dep_w-1)/2;
190 intrinsic.principalPointY= (m_dep_h-1)/2;
191 intrinsic.distortionModel.type = CameraDistortionType::YARP_PLUMB_BOB;
192 intrinsic.distortionModel.k1 = 0;
193 intrinsic.distortionModel.k2 = 0;
194 intrinsic.distortionModel.t1 = 0;
195 intrinsic.distortionModel.t2 = 0;
196 intrinsic.distortionModel.k3 = 0;
197 //intrinsic.stamp = yarp::os::Time::now();
198 return ReturnValue_ok;
199}
200
202{
203 std::lock_guard <std::recursive_mutex> lg(m_mutex);
204 return m_dep_h;
205}
206
208{
209 std::lock_guard <std::recursive_mutex> lg(m_mutex);
210 return m_dep_w;
211}
212
213ReturnValue FakeDepthCameraDriver_mini::getDepthFOV(double& horizontalFov, double& verticalFov)
214{
215 std::lock_guard <std::recursive_mutex> lg(m_mutex);
216 horizontalFov = m_dep_Hfov;
217 verticalFov = m_dep_Vfov;
218 return ReturnValue_ok;
219}
220
222{
223 std::lock_guard <std::recursive_mutex> lg(m_mutex);
224 intrinsic.physFocalLength = 0.5;
225 const double deg2rad = 3.1415926535/180.0;
226 double focal_x = m_dep_w / (2*tan(m_dep_Hfov*deg2rad/2));
227 double focal_y = m_dep_h / (2*tan(m_dep_Vfov*deg2rad/2));
228 intrinsic.focalLengthX=focal_x;
229 intrinsic.focalLengthY=focal_y;
230 intrinsic.principalPointX=(m_dep_w-1)/2;
231 intrinsic.principalPointY=(m_dep_h-1)/2;
232 intrinsic.distortionModel.type = CameraDistortionType::YARP_PLUMB_BOB;
233 intrinsic.distortionModel.k1 = 0;
234 intrinsic.distortionModel.k2 = 0;
235 intrinsic.distortionModel.t1 = 0;
236 intrinsic.distortionModel.t2 = 0;
237 intrinsic.distortionModel.k3 = 0;
238 //intrinsic.stamp = yarp::os::Time::now();
239
240 return ReturnValue_ok;
241}
242
244{
245 std::lock_guard <std::recursive_mutex> lg(m_mutex);
247 return ReturnValue_ok;
248}
249
251{
252 std::lock_guard <std::recursive_mutex> lg(m_mutex);
253 nearPlane = m_dep_near;
254 farPlane = m_dep_far;
255 return ReturnValue_ok;
256}
257
259{
260 std::lock_guard <std::recursive_mutex> lg(m_mutex);
261 m_dep_near = nearPlane;
262 m_dep_far = farPlane;
263 return ReturnValue_ok;
264}
265
267{
268 std::lock_guard <std::recursive_mutex> lg(m_mutex);
269 mirror = m_depth_mirror;
270 return ReturnValue_ok;
271}
272
274{
275 std::lock_guard <std::recursive_mutex> lg(m_mutex);
277 return ReturnValue_ok;
278}
279
281{
282 std::lock_guard <std::recursive_mutex> lg(m_mutex);
283 extrinsic.resize(4, 4);
284 extrinsic.zero();
285
286 extrinsic[0][0] = 1;
287 extrinsic[1][1] = 1;
288 extrinsic[2][2] = 1;
289 extrinsic[3][3] = 1;
290 return ReturnValue_ok;
291}
292
294{
295 std::lock_guard <std::recursive_mutex> lg(m_mutex);
297 if (timeStamp) {
298 timeStamp->update(yarp::os::Time::now());
299 }
300 return ReturnValue_ok;
301}
302
304{
305 std::lock_guard <std::recursive_mutex> lg(m_mutex);
307 if (timeStamp) {
308 timeStamp->update(yarp::os::Time::now());
309 }
310 return ReturnValue_ok;
311}
312
314{
315 std::lock_guard <std::recursive_mutex> lg(m_mutex);
316 auto r1 = getRgbImage(colorFrame, colorStamp);
317 auto r2 = getDepthImage(depthFrame, depthStamp);
318 ReturnValue rr = r1 && r2;
319 return rr;
320}
321
323{
324 std::lock_guard <std::recursive_mutex> lg(m_mutex);
325 status = RGBD_SENSOR_OK_IN_USE;
326 return ReturnValue_ok;
327}
328
330{
331 std::lock_guard <std::recursive_mutex> lg(m_mutex);
332 msg = std::string("no error");
333 return ReturnValue_ok;
334}
@ 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.
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.
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 getDepthIntrinsicParam(yarp::sig::IntrinsicParams &intrinsic) override
Get the intrinsic parameters of the depth camera.
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 getRgbIntrinsicParam(yarp::sig::IntrinsicParams &intrinsic) override
Get the intrinsic parameters of the rgb 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.
A mini-server for performing network communication in the background.
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:18
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
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
yarp::sig::DistortionModelData distortionModel
A class for a Matrix.
Definition Matrix.h:40
#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.
constexpr char accuracy[]
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).
#define deg2rad(x)