YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
laserFromPointCloud.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
6#define _USE_MATH_DEFINES
7
9
10#include <yarp/os/Time.h>
11#include <yarp/os/Log.h>
12#include <yarp/os/LogStream.h>
14#include <yarp/math/Math.h>
15
16#include <cmath>
17#include <cstring>
18#include <cstdlib>
19#include <iostream>
20#include <limits>
21#include <mutex>
22
23
24#ifndef DEG2RAD
25#define DEG2RAD M_PI/180.0
26#endif
27
28using namespace yarp::sig;
29
30YARP_LOG_COMPONENT(LASER_FROM_POINTCLOUD, "yarp.devices.laserFromPointCloud")
31
32/*
33
34yarpdev --device rangefinder2D_nws_yarp --subdevice laserFromPointCloud \
35 --SENSOR \
36 --RGBD_SENSOR_CLIENT::localImagePort /clientRgbPort:i \
37 --RGBD_SENSOR_CLIENT::localDepthPort /clientDepthPort:i \
38 --RGBD_SENSOR_CLIENT::localRpcPort /clientRpcPort \
39 --RGBD_SENSOR_CLIENT::remoteImagePort /SIM_CER_ROBOT/depthCamera/rgbImage:o \
40 --RGBD_SENSOR_CLIENT::remoteDepthPort /SIM_CER_ROBOT/depthCamera/depthImage:o \
41 --RGBD_SENSOR_CLIENT::remoteRpcPort /SIM_CER_ROBOT/depthCamera/rpc:i \
42 --TRANSFORM_CLIENT::local /laserFromDepth/tfClient \
43 --TRANSFORM_CLIENT::remote /transformServer \
44 --Z_CLIPPING_PLANES::floor_height 0.15 \
45 --Z_CLIPPING_PLANES::ceiling_height 3.0 \
46 --Z_CLIPPING_PLANES::camera_frame_id depth_center \
47 --Z_CLIPPING_PLANES::ground_frame_id ground_link \
48 --period 0.10 \
49 --name /outlaser:o
50*/
51
52//-------------------------------------------------------------------------------------
53
55{
57 m_info = "LaserFromPointCloud device";
58
59#ifdef LASER_DEBUG
60 yCDebug(LASER_FROM_POINTCLOUD) << "%s\n", config.toString().c_str();
61#endif
62
63 m_min_distance = 0.1; //m
64 m_max_distance = 5.0; //m
65 m_floor_height = 0.1; //m
66 m_pointcloud_max_distance = 10.0; //m
67 m_ceiling_height = 2; //m
68
69 m_sensorsNum = 360;
70 m_resolution = 1;
71 m_laser_data.resize(m_sensorsNum, 0.0);
72 m_max_angle = 360;
73 m_min_angle = 0;
74
75 m_pc_stepx=2;
76 m_pc_stepy=2;
77
78 m_transform_mtrx.resize(4,4);
79 m_transform_mtrx.eye();
80
81
82 m_ground_frame_id = "/ground_frame";
83 m_camera_frame_id = "/depth_camera_frame";
84
85
86 bool bpcr = config.check("POINTCLOUD_QUALITY");
87 if (bpcr != false)
88 {
89 yarp::os::Searchable& pointcloud_quality_config = config.findGroup("POINTCLOUD_QUALITY");
90 if (pointcloud_quality_config.check("x_step")) { m_pc_stepx = pointcloud_quality_config.find("x_step").asFloat64(); }
91 if (pointcloud_quality_config.check("y_step")) { m_pc_stepy = pointcloud_quality_config.find("y_step").asFloat64(); }
92 yCInfo(LASER_FROM_POINTCLOUD) << "Pointcloud decimation step set to:" << m_pc_stepx << m_pc_stepy;
93 }
94
95 bool bpc = config.check("Z_CLIPPING_PLANES");
96 if (bpc != false)
97 {
98 yarp::os::Searchable& pointcloud_clip_config = config.findGroup("Z_CLIPPING_PLANES");
99 if (pointcloud_clip_config.check("floor_height")) {m_floor_height = pointcloud_clip_config.find("floor_height").asFloat64();}
100 if (pointcloud_clip_config.check("ceiling_height")) {m_ceiling_height=pointcloud_clip_config.find("ceiling_height").asFloat64();}
101 if (pointcloud_clip_config.check("max_distance")) { m_pointcloud_max_distance = pointcloud_clip_config.find("max_distance").asFloat64(); }
102 if (pointcloud_clip_config.check("ground_frame_id")) {m_ground_frame_id = pointcloud_clip_config.find("ground_frame_id").asString();}
103 if (pointcloud_clip_config.check("camera_frame_id")) {m_camera_frame_id = pointcloud_clip_config.find("camera_frame_id").asString();}
104 }
105 yCInfo(LASER_FROM_POINTCLOUD) <<"Z clipping planes (floor,ceiling) have been set to ("<< m_floor_height <<","<< m_ceiling_height <<")";
106
107 if (this->parseConfiguration(config) == false)
108 {
109 yCError(LASER_FROM_POINTCLOUD) << "Error parsing parameters";
110 return false;
111 }
112
113 //open the tc client
115 if(!config.check("TRANSFORM_CLIENT"))
116 {
117 yCError(LASER_FROM_POINTCLOUD) << "missing TRANSFORM_CLIENT section in configuration file!";
118 return false;
119 }
120 tcprop.fromString(config.findGroup("TRANSFORM_CLIENT").toString());
121 tcprop.put("device", "frameTransformClient");
122
123 m_tc_driver.open(tcprop);
124 if (!m_tc_driver.isValid())
125 {
126 yCError(LASER_FROM_POINTCLOUD) << "Error opening PolyDriver check parameters";
127 return false;
128 }
129 m_tc_driver.view(m_iTc);
130 if (!m_iTc)
131 {
132 yCError(LASER_FROM_POINTCLOUD) << "Error opening iFrameTransform interface. Device not available";
133 return false;
134 }
135
136 // This block here is just for debug/testing purposes, to avoid the need of a transform server.
137 // It is used by CI only.
138 {
139 bool testWithIndentityTransform = config.check("testWithIndentityTransform");
141 {
142 yarp::sig::Matrix T(4, 4) ;
143 T(0,0) = 0; T(0,1) = 0; T(0,2) = 1; T(0,3) = 0;
144 T(1,0) = -1; T(1,1) = 0; T(1,2) = 0; T(1,3) = 0;
145 T(2,0) = 0; T(2,1) = -1; T(2,2) = 0; T(2,3) = 0;
146 T(3,0) = 0; T(3,1) = 0; T(3,2) = 0; T(3,3) = 1;
147 m_iTc->setTransformStatic(m_camera_frame_id, m_ground_frame_id, T);
148 }
149 }
150
152
153
154 //open the rgbd client
155 Property prop;
156 if(!config.check("RGBD_SENSOR_CLIENT"))
157 {
158 yCError(LASER_FROM_POINTCLOUD) << "missing RGBD_SENSOR_CLIENT section in configuration file!";
159 return false;
160 }
161 prop.fromString(config.findGroup("RGBD_SENSOR_CLIENT").toString());
162 prop.put("device", "RGBDSensor_nwc_yarp");
163 m_rgbd_driver.open(prop);
164 if (!m_rgbd_driver.isValid())
165 {
166 yCError(LASER_FROM_POINTCLOUD) << "Error opening PolyDriver check parameters";
167 return false;
168 }
169 m_rgbd_driver.view(m_iRGBD);
170 if (!m_iRGBD)
171 {
172 yCError(LASER_FROM_POINTCLOUD) << "Error opening iRGBD interface. Device not available";
173 return false;
174 }
176
177 //get parameters data from the camera
178 m_depth_width = m_iRGBD->getRgbWidth(); //@@@ this is horrible! See yarp issue: https://github.com/robotology/yarp/issues/2290
179 m_depth_height = m_iRGBD->getRgbHeight(); //@@@ this is horrible! See yarp issue: https://github.com/robotology/yarp/issues/2290
180 bool propintr = m_iRGBD->getDepthIntrinsicParam(m_propIntrinsics);
182 yCInfo(LASER_FROM_POINTCLOUD) << "Depth Intrinsics:" << m_propIntrinsics.toString();
183 m_intrinsics.fromProperty(m_propIntrinsics);
184
186 yCInfo(LASER_FROM_POINTCLOUD) << "Sensor ready";
187
188 return true;
189}
190
192{
194
195 if (m_rgbd_driver.isValid()) {
197 }
198
199 if (m_tc_driver.isValid()) {
201 }
202
203 yCInfo(LASER_FROM_POINTCLOUD) << "closed";
204 return true;
205}
206
208{
209 std::lock_guard<std::mutex> guard(m_mutex);
210 m_min_distance = min;
211 m_max_distance = max;
212 return ReturnValue_ok;
213}
214
215
217{
218 std::lock_guard<std::mutex> guard(m_mutex);
219 yCWarning(LASER_FROM_POINTCLOUD,"setScanLimits not yet implemented");
220 return ReturnValue_ok;
221}
222
224{
225 std::lock_guard<std::mutex> guard(m_mutex);
226 yCWarning(LASER_FROM_POINTCLOUD,"setHorizontalResolution not yet implemented");
227 return ReturnValue_ok;
228}
229
231{
232 std::lock_guard<std::mutex> guard(m_mutex);
233 yCWarning(LASER_FROM_POINTCLOUD,"setScanRate not yet implemented");
234 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
235}
236
237
239{
240#ifdef LASER_DEBUG
241 yCDebug(LASER_FROM_POINTCLOUD) << "thread initialising...\n";
242 yCDebug(LASER_FROM_POINTCLOUD) << "... done!\n");
243#endif
244
245 return true;
246}
247
249{
250 for (size_t i=0; i<pc.size(); i++)
251 {
252 auto v1 = pc(i).toVector4();
253 auto v2 = m*v1;
254 pc(i).x=v2(0);
255 pc(i).y=v2(1);
256 pc(i).z=v2(2);
257 }
258}
259
260bool LaserFromPointCloud::utility_insideCameraFov(size_t i, double first_elem, double second_elem)
261{
262 if (first_elem <= second_elem)
263 return i >= first_elem && i <= second_elem;
264 else
265 return i >= first_elem || i <= second_elem;
266}
267
268double computeHFOV(double fx, int width)
269{
270 return 2.0 * std::atan(width / (2.0 * fx)); // radians
271}
272
273double computeHFOVdeg(double fx, int width)
274{
275 return computeHFOV(fx, width) * 180.0 / M_PI;
276}
277
279{
280#ifdef DEBUG_TIMING
281 static double t3 = yarp::os::Time::now();
282 double t1 = yarp::os::Time::now();
283 yCDebug(LASER_FROM_POINTCLOUD) << "thread period:" << t1 - t3;
285#endif
286
288 if (depth_ok == false)
289 {
290 yCError(LASER_FROM_POINTCLOUD) << "getDepthImage failed";
291 return false;
292 }
293
294 if (m_depth_image.getRawImage() == nullptr)
295 {
296 yCError(LASER_FROM_POINTCLOUD) << "invalid image received";
297 return false;
298 }
299
302 {
303 yCError(LASER_FROM_POINTCLOUD) << "invalid image size: (" << m_depth_image.width() << " " << m_depth_image.height() << ") vs (" << m_depth_width << " " << m_depth_height << ")";
304 return false;
305 }
306
307 const double myinf = std::numeric_limits<double>::infinity();
308 const double mynan = std::nan("");
309
310 //compute the point cloud: from the camera u,v camera system,
311 //to the xyz world system
312 //To reduce the computational load, decimation of the pointcloud is done using m_pc_stepx, m_pc_stepy
314
315
316#ifdef TEST_M
317 //yCDebug(LASER_FROM_POINTCLOUD) << "pc size:" << pc.size();
318#endif
319
320 //we compute the transformation matrix from the camera to the laser reference frame
321#ifdef TEST_M
323 vvv(0) = -1.57;
324 vvv(1) = 0;
325 vvv(2) = -1.57;
327 m(2, 3) = 1.2; //z translation
328#else
330 if (frame_exists == false)
331 {
332 yCWarning(LASER_FROM_POINTCLOUD) << "Unable to found m matrix";
333 }
334#endif
335
336 //we rototranslate the full pointcloud from the camera to the world(lidar) frame.
338
339 //just for debug puporses
340#if 0
341 {
342 //These numbers are expressed in degrees
345 double g_hfov;
346 double g_vfov;
348 //hfov = 36 degrees -> +-18 on each side of the camera center
349 }
350#endif
351
352 // compute the leftmost and rightmost points of the image
353 // to know which angles are not covered by the camera.
354 // the formula uses is the same used in yarp::sig::utils::depthToPC
355 // it is called pin-hole equation.
356 // The y coordinate is set to the middle of the image height, to avoid distortion effects on the borders.
357 double pointx = 0;
358 double pointy = m_depth_image.height()/2.0;
359 yarp::sig::Vector left(4);
362 left[2] = 1000; //useless z
363 left[3] = 1;
364
366 yarp::sig::Vector right(4);
369 right[2] = 1000; //useless z
370 right[3] = 1;
371
372 double leftmost_dist_dummy;
373 double leftmost_theta;
375 double rightmost_theta;
376 left = m_transform_mtrx * left; //only x and y are used, z is useless
377 right = m_transform_mtrx * right; //only x and y are used, z is useless
378
379//just for debug puporses
380#if 0
381 {
383 double testdummy_dist=0;
384 double testdummy_theta = 0;
385 ldata.set_cartesian(1, 0);
387 //testdummy_theta should be 0
388 ldata.set_cartesian(0, 1);
390 //testdummy_theta should be 90
391 ldata.set_cartesian(0, -1);
393 //testdummy_theta should be -90
394 }
395#endif
396
398 double tmp_lx=left[0];
399 double tmp_ly=left[1];
400 data_left.set_cartesian(tmp_lx, tmp_ly);
402
404 double tmp_rx=right[0];
405 double tmp_ry=right[1];
406 data_right.set_cartesian(tmp_rx, tmp_ry);
408
409 auto normalize_angle = [](double angle_deg)
410 {
411 while (angle_deg < 0)
412 {
413 angle_deg += 360.0;
414 }
415 while (angle_deg >= 360.0)
416 {
417 angle_deg -= 360.0;
418 }
419 return angle_deg;
420 };
421
422 // radians -> degrees
427
428 // angle -> laser index
429 size_t leftmost_elem = static_cast<size_t>(leftmost_theta / m_resolution);
430 size_t rightmost_elem = static_cast<size_t>(rightmost_theta / m_resolution);
431
432#ifdef DEBUG_TIMING
433 double t4 = yarp::os::Time::now();
434#endif
435
436 //prepare an empty laserscan vector with the resolution we want
437 for (auto it = m_laser_data.begin(); it != m_laser_data.end(); it++)
438 {
439 *it = mynan;
440 }
441
442 // we set to infinity the angles that are inside the camera fov
443 // a value will be set later for inf value, while nan value are no more modifiable
444 for (size_t i = 0; i < m_sensorsNum; i++)
445 {
446 if (utility_insideCameraFov(i, rightmost_elem, leftmost_elem))
447 { m_laser_data[i] = myinf; }
448 }
449
450 //Scan for each point in the pointcloud...
451 for (size_t i = 0; i < pc.size(); i++)
452 {
453
454#ifdef TEST_M
455 //yCDebug(LASER_FROM_POINTCLOUD) << pc(i).toString(5,5);
456#endif
457
458 //we obtain a point (xyz) from the point cloud
459 double point_x = pc(i).toVector4()[0];
460 double point_y = pc(i).toVector4()[1];
461 double point_z = pc(i).toVector4()[2];
462
463 //we check if the point (xyz) is in the volume that we want to consider as possible obstacle
466 {
467#ifdef TEST_M
468 // yCDebug(LASER_FROM_POINTCLOUD) << "This point is ok:" << i <<"its z is:" << tvec[2];
469#endif
470 //by removing z, we project the 3d point on the 2D plane on which the laser works.
471 //we use LaserMeasurementData struct to easily obtain a polar representation from a cartesian representation
474
475 //get the polar representation
476 double distance;
477 double theta;
478 data.get_polar(distance, theta);
479
480 //compute the id of the vector where to put distance data.
481 //This is done by clustering angles, depending on the laser resolution.
482 theta = theta * 180 / M_PI;
483 if (theta < 0) {
484 theta += 360;
485 } else if (theta > 360) {
486 theta -= 360;
487 }
488 size_t elem = theta / m_resolution;
489 if (elem >= m_laser_data.size())
490 {
491 yCError(LASER_FROM_POINTCLOUD) << "Error in computing elem" << i << ">" << m_laser_data.size();
492 continue;
493 }
494
495#ifdef TEST_M
496 // yCDebug(LASER_FROM_POINTCLOUD) <<theta << elem <<distance;
497#endif
498 //update the vector of measurements, putting the NEAREST obstacle in correct element of the vector.
500 {
502 }
503 }
504 else
505 {
506#ifdef TEST_M
507 //yCDebug(LASER_FROM_POINTCLOUD) << "this point is out of considered volume:" <<i << " z:" << vec[2];
508#endif
509 }
510 }
511
512
513#ifdef DEBUG_TIMING
514 double t2 = yarp::os::Time::now();
515 yCDebug(LASER_FROM_POINTCLOUD) << "tot run:" << t2 - t1 << "crit run:" << t2 - t4;
516#endif
517
518 return true;
519}
520
522{
523 bool b = updateLidarData();
524 YARP_UNUSED(b);
525}
526
528{
529#ifdef LASER_DEBUG
530 yCDebug(LASER_FROM_POINTCLOUD) << "Thread releasing...";
531 yCDebug(LASER_FROM_POINTCLOUD) << "... done.";
532#endif
533
534 return;
535}
#define ReturnValue_ok
Definition ReturnValue.h:82
#define M_PI
laserFromPointCloud: Documentation to be added
yarp::dev::ReturnValue setScanLimits(double min, double max) override
set the scan angular range.
void run() override
Loop function.
yarp::dev::ReturnValue setScanRate(double rate) override
set the scan rate (scans per seconds)
yarp::sig::IntrinsicParams m_intrinsics
yarp::sig::utils::PCL_ROI m_pc_roi
yarp::sig::Matrix m_transform_mtrx
IFrameTransform * m_iTc
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue setDistanceRange(double min, double max) override
set the device detection range.
yarp::sig::ImageOf< float > m_depth_image
void threadRelease() override
Release method.
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
yarp::dev::ReturnValue setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
bool threadInit() override
Initialization method.
virtual yarp::dev::ReturnValue getDepthFOV(double &horizontalFov, double &verticalFov)=0
Get the field of view (FOV) of the depth camera.
virtual yarp::dev::ReturnValue getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform)=0
Get the transform between two frames.
virtual yarp::dev::ReturnValue getDepthImage(yarp::sig::ImageOf< yarp::sig::PixelFloat > &depthImage, yarp::os::Stamp *timeStamp=nullptr)=0
Get the depth frame from the device.
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
bool close() override
Close the DeviceDriver.
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition Property.h:33
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
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
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:462
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
void set_cartesian(const double x, const double y)
void get_polar(double &rho, double &theta)
A class for a Matrix.
Definition Matrix.h:39
The PointCloud class.
Definition PointCloud.h:21
size_t size() const
Definition Vector.h:351
iterator begin() noexcept
Returns an iterator to the beginning of the VectorOf.
Definition Vector.h:487
iterator end() noexcept
Returns an iterator to the end of the VectorOf.
Definition Vector.h:494
const yarp::os::LogComponent & LASER_FROM_POINTCLOUD()
double computeHFOV(double fx, int width)
double computeHFOVdeg(double fx, int width)
void rotate_pc(yarp::sig::PointCloud< yarp::sig::DataXYZ > &pc, const yarp::sig::Matrix &m)
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
yarp::sig::PointCloud< yarp::sig::DataXYZ > depthToPC(const yarp::sig::ImageOf< yarp::sig::PixelFloat > &depth, const yarp::sig::IntrinsicParams &intrinsic)
depthToPC, compute the PointCloud given depth image and the intrinsic parameters of the camera.
The main, catch-all namespace for YARP.
Definition dirs.h:16
double focalLengthY
Result of the product of the physical focal length(mm) and the size sy of the individual imager eleme...
double focalLengthX
Result of the product of the physical focal length(mm) and the size sx of the individual imager eleme...
double principalPointX
Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge.
double principalPointY
Vertical coordinate of the principal point of the image, as a pixel offset from the top edge.
#define YARP_UNUSED(var)
Definition api.h:162