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_intrinsics);
182 yCInfo(LASER_FROM_POINTCLOUD) << "Depth Intrinsics:" << m_intrinsics.toString();
183
185 yCInfo(LASER_FROM_POINTCLOUD) << "Sensor ready";
186
187 return true;
188}
189
191{
193
194 if (m_rgbd_driver.isValid()) {
196 }
197
198 if (m_tc_driver.isValid()) {
200 }
201
202 yCInfo(LASER_FROM_POINTCLOUD) << "closed";
203 return true;
204}
205
207{
208 std::lock_guard<std::mutex> guard(m_mutex);
209 m_min_distance = min;
210 m_max_distance = max;
211 return ReturnValue_ok;
212}
213
214
216{
217 std::lock_guard<std::mutex> guard(m_mutex);
218 yCWarning(LASER_FROM_POINTCLOUD,"setScanLimits not yet implemented");
219 return ReturnValue_ok;
220}
221
223{
224 std::lock_guard<std::mutex> guard(m_mutex);
225 yCWarning(LASER_FROM_POINTCLOUD,"setHorizontalResolution not yet implemented");
226 return ReturnValue_ok;
227}
228
230{
231 std::lock_guard<std::mutex> guard(m_mutex);
232 yCWarning(LASER_FROM_POINTCLOUD,"setScanRate not yet implemented");
233 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
234}
235
236
238{
239#ifdef LASER_DEBUG
240 yCDebug(LASER_FROM_POINTCLOUD) << "thread initialising...\n";
241 yCDebug(LASER_FROM_POINTCLOUD) << "... done!\n");
242#endif
243
244 return true;
245}
246
248{
249 for (size_t i=0; i<pc.size(); i++)
250 {
251 auto v1 = pc(i).toVector4();
252 auto v2 = m*v1;
253 pc(i).x=v2(0);
254 pc(i).y=v2(1);
255 pc(i).z=v2(2);
256 }
257}
258
259bool LaserFromPointCloud::utility_insideCameraFov(size_t i, double first_elem, double second_elem)
260{
261 if (first_elem <= second_elem)
262 return i >= first_elem && i <= second_elem;
263 else
264 return i >= first_elem || i <= second_elem;
265}
266
267double computeHFOV(double fx, int width)
268{
269 return 2.0 * std::atan(width / (2.0 * fx)); // radians
270}
271
272double computeHFOVdeg(double fx, int width)
273{
274 return computeHFOV(fx, width) * 180.0 / M_PI;
275}
276
278{
279#ifdef DEBUG_TIMING
280 static double t3 = yarp::os::Time::now();
281 double t1 = yarp::os::Time::now();
282 yCDebug(LASER_FROM_POINTCLOUD) << "thread period:" << t1 - t3;
284#endif
285
287 if (depth_ok == false)
288 {
289 yCError(LASER_FROM_POINTCLOUD) << "getDepthImage failed";
290 return false;
291 }
292
293 if (m_depth_image.getRawImage() == nullptr)
294 {
295 yCError(LASER_FROM_POINTCLOUD) << "invalid image received";
296 return false;
297 }
298
301 {
302 yCError(LASER_FROM_POINTCLOUD) << "invalid image size: (" << m_depth_image.width() << " " << m_depth_image.height() << ") vs (" << m_depth_width << " " << m_depth_height << ")";
303 return false;
304 }
305
306 const double myinf = std::numeric_limits<double>::infinity();
307 const double mynan = std::nan("");
308
309 //compute the point cloud: from the camera u,v camera system,
310 //to the xyz world system
311 //To reduce the computational load, decimation of the pointcloud is done using m_pc_stepx, m_pc_stepy
313
314
315#ifdef TEST_M
316 //yCDebug(LASER_FROM_POINTCLOUD) << "pc size:" << pc.size();
317#endif
318
319 //we compute the transformation matrix from the camera to the laser reference frame
320#ifdef TEST_M
322 vvv(0) = -1.57;
323 vvv(1) = 0;
324 vvv(2) = -1.57;
326 m(2, 3) = 1.2; //z translation
327#else
329 if (frame_exists == false)
330 {
331 yCWarning(LASER_FROM_POINTCLOUD) << "Unable to found m matrix";
332 }
333#endif
334
335 //we rototranslate the full pointcloud from the camera to the world(lidar) frame.
337
338 //just for debug puporses
339#if 0
340 {
341 //These numbers are expressed in degrees
344 double g_hfov;
345 double g_vfov;
347 //hfov = 36 degrees -> +-18 on each side of the camera center
348 }
349#endif
350
351 // compute the leftmost and rightmost points of the image
352 // to know which angles are not covered by the camera.
353 // the formula uses is the same used in yarp::sig::utils::depthToPC
354 // it is called pin-hole equation.
355 // The y coordinate is set to the middle of the image height, to avoid distortion effects on the borders.
356 double pointx = 0;
357 double pointy = m_depth_image.height()/2.0;
358 yarp::sig::Vector left(4);
361 left[2] = 1000; //useless z
362 left[3] = 1;
363
365 yarp::sig::Vector right(4);
368 right[2] = 1000; //useless z
369 right[3] = 1;
370
371 double leftmost_dist_dummy;
372 double leftmost_theta;
374 double rightmost_theta;
375 left = m_transform_mtrx * left; //only x and y are used, z is useless
376 right = m_transform_mtrx * right; //only x and y are used, z is useless
377
378//just for debug puporses
379#if 0
380 {
382 double testdummy_dist=0;
383 double testdummy_theta = 0;
384 ldata.set_cartesian(1, 0);
386 //testdummy_theta should be 0
387 ldata.set_cartesian(0, 1);
389 //testdummy_theta should be 90
390 ldata.set_cartesian(0, -1);
392 //testdummy_theta should be -90
393 }
394#endif
395
397 double tmp_lx=left[0];
398 double tmp_ly=left[1];
399 data_left.set_cartesian(tmp_lx, tmp_ly);
401
403 double tmp_rx=right[0];
404 double tmp_ry=right[1];
405 data_right.set_cartesian(tmp_rx, tmp_ry);
407
408 auto normalize_angle = [](double angle_deg)
409 {
410 while (angle_deg < 0)
411 {
412 angle_deg += 360.0;
413 }
414 while (angle_deg >= 360.0)
415 {
416 angle_deg -= 360.0;
417 }
418 return angle_deg;
419 };
420
421 // radians -> degrees
426
427 // angle -> laser index
428 size_t leftmost_elem = static_cast<size_t>(leftmost_theta / m_resolution);
429 size_t rightmost_elem = static_cast<size_t>(rightmost_theta / m_resolution);
430
431#ifdef DEBUG_TIMING
432 double t4 = yarp::os::Time::now();
433#endif
434
435 //prepare an empty laserscan vector with the resolution we want
436 for (auto it = m_laser_data.begin(); it != m_laser_data.end(); it++)
437 {
438 *it = mynan;
439 }
440
441 // we set to infinity the angles that are inside the camera fov
442 // a value will be set later for inf value, while nan value are no more modifiable
443 for (size_t i = 0; i < m_sensorsNum; i++)
444 {
445 if (utility_insideCameraFov(i, rightmost_elem, leftmost_elem))
446 { m_laser_data[i] = myinf; }
447 }
448
449 //Scan for each point in the pointcloud...
450 for (size_t i = 0; i < pc.size(); i++)
451 {
452
453#ifdef TEST_M
454 //yCDebug(LASER_FROM_POINTCLOUD) << pc(i).toString(5,5);
455#endif
456
457 //we obtain a point (xyz) from the point cloud
458 double point_x = pc(i).toVector4()[0];
459 double point_y = pc(i).toVector4()[1];
460 double point_z = pc(i).toVector4()[2];
461
462 //we check if the point (xyz) is in the volume that we want to consider as possible obstacle
465 {
466#ifdef TEST_M
467 // yCDebug(LASER_FROM_POINTCLOUD) << "This point is ok:" << i <<"its z is:" << tvec[2];
468#endif
469 //by removing z, we project the 3d point on the 2D plane on which the laser works.
470 //we use LaserMeasurementData struct to easily obtain a polar representation from a cartesian representation
473
474 //get the polar representation
475 double distance;
476 double theta;
477 data.get_polar(distance, theta);
478
479 //compute the id of the vector where to put distance data.
480 //This is done by clustering angles, depending on the laser resolution.
481 theta = theta * 180 / M_PI;
482 if (theta < 0) {
483 theta += 360;
484 } else if (theta > 360) {
485 theta -= 360;
486 }
487 size_t elem = theta / m_resolution;
488 if (elem >= m_laser_data.size())
489 {
490 yCError(LASER_FROM_POINTCLOUD) << "Error in computing elem" << i << ">" << m_laser_data.size();
491 continue;
492 }
493
494#ifdef TEST_M
495 // yCDebug(LASER_FROM_POINTCLOUD) <<theta << elem <<distance;
496#endif
497 //update the vector of measurements, putting the NEAREST obstacle in correct element of the vector.
499 {
501 }
502 }
503 else
504 {
505#ifdef TEST_M
506 //yCDebug(LASER_FROM_POINTCLOUD) << "this point is out of considered volume:" <<i << " z:" << vec[2];
507#endif
508 }
509 }
510
511
512#ifdef DEBUG_TIMING
513 double t2 = yarp::os::Time::now();
514 yCDebug(LASER_FROM_POINTCLOUD) << "tot run:" << t2 - t1 << "crit run:" << t2 - t4;
515#endif
516
517 return true;
518}
519
521{
522 bool b = updateLidarData();
523 YARP_UNUSED(b);
524}
525
527{
528#ifdef LASER_DEBUG
529 yCDebug(LASER_FROM_POINTCLOUD) << "Thread releasing...";
530 yCDebug(LASER_FROM_POINTCLOUD) << "... done.";
531#endif
532
533 return;
534}
#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:32
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:980
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:40
The PointCloud class.
Definition PointCloud.h:21
size_t size() const
Returns the number of elements in the vector.
Definition VectorBase.h:167
iterator begin() noexcept
Returns an iterator to the beginning of the Vector.
Definition VectorBase.h:305
iterator end() noexcept
Definition VectorBase.h:311
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
#define YARP_UNUSED(var)
Definition api.h:162