From 25dd3074dc19419dc0fdbbcf396b8236ffc6ce11 Mon Sep 17 00:00:00 2001 From: Pierre Willenbrock Date: Mon, 20 Nov 2023 11:02:56 +0100 Subject: [PATCH 1/3] HighResRangeFinder: allow adding extra cameras through configuration --- highresrangefinderTypes.hpp | 18 ++++++++++++ mars.orogen | 4 +++ tasks/HighResRangeFinder.cpp | 57 ++++++++++++++++++++++++++++++++---- tasks/HighResRangeFinder.hpp | 1 + 4 files changed, 75 insertions(+), 5 deletions(-) create mode 100644 highresrangefinderTypes.hpp diff --git a/highresrangefinderTypes.hpp b/highresrangefinderTypes.hpp new file mode 100644 index 00000000..9906ec02 --- /dev/null +++ b/highresrangefinderTypes.hpp @@ -0,0 +1,18 @@ + +#ifndef MARS_HIGHRESRANGEFINDER_TYPES_HH +#define MARS_HIGHRESRANGEFINDER_TYPES_HH + +#include + +namespace mars +{ + +struct HighResRangeFinderCamera { + std::string name; ///'). + doc('Extra set of cameras, described as pairs of name within scene file and orientation. Note that the camera from the "name" property always goes at orientation 0.0.') + operation('addCamera'). doc('Adds another camera which will be used for pointcloud creation'). returns("bool"). diff --git a/tasks/HighResRangeFinder.cpp b/tasks/HighResRangeFinder.cpp index f5c638bf..07dcbc7c 100644 --- a/tasks/HighResRangeFinder.cpp +++ b/tasks/HighResRangeFinder.cpp @@ -38,10 +38,17 @@ bool HighResRangeFinder::addCamera(::std::string const & name, double orientatio return false; } - Camera* camera = new Camera(sensor_id, cam_sensor, orientation); - camera->name = name; - calcCamParameters(camera); - cameras.push_back(camera); + HighResRangeFinderCamera cam = { name, orientation }; + addedCameras.push_back(cam); + + //using !cameras.empty() as a proxy for the task being started. In a started task, + //there will always be at least one camera. + if(!cameras.empty()) { + Camera* camera = new Camera(sensor_id, cam_sensor, orientation); + camera->name = name; + calcCamParameters(camera); + cameras.push_back(camera); + } return true; } @@ -116,7 +123,41 @@ bool HighResRangeFinder::startHook() Camera* camera = new Camera(sensor_id, this->camera, 0.0); calcCamParameters(camera); cameras.push_back(camera); - + + for(auto &c : addedCameras) { + long sensor_id = control->sensors->getSensorID( c.name ); + if( !sensor_id ){ + RTT::log(RTT::Error) << "There is no camera by the name of " << c.name << " in the scene" << RTT::endlog(); + } + + mars::sim::CameraSensor* cam_sensor = dynamic_cast(control->sensors->getSimSensor(sensor_id)); + if( !cam_sensor){ + RTT::log(RTT::Error) << "CameraPlugin: Given sensor name is not a camera" << RTT::endlog(); + } + + Camera* camera = new Camera(sensor_id, cam_sensor, c.orientation); + camera->name = c.name; + calcCamParameters(camera); + cameras.push_back(camera); + } + + for(auto &c : _extra_cameras.get()) { + long sensor_id = control->sensors->getSensorID( c.name ); + if( !sensor_id ){ + RTT::log(RTT::Error) << "There is no camera by the name of " << c.name << " in the scene" << RTT::endlog(); + } + + mars::sim::CameraSensor* cam_sensor = dynamic_cast(control->sensors->getSimSensor(sensor_id)); + if( !cam_sensor){ + RTT::log(RTT::Error) << "CameraPlugin: Given sensor name is not a camera" << RTT::endlog(); + } + + Camera* camera = new Camera(sensor_id, cam_sensor, c.orientation); + camera->name = c.name; + calcCamParameters(camera); + cameras.push_back(camera); + } + return true; } @@ -131,6 +172,12 @@ void HighResRangeFinder::errorHook() void HighResRangeFinder::stopHook() { HighResRangeFinderBase::stopHook(); + + std::vector::iterator it = cameras.begin(); + for(; it != cameras.end(); ++it) { + delete *it; + } + cameras.clear(); } void HighResRangeFinder::cleanupHook() { diff --git a/tasks/HighResRangeFinder.hpp b/tasks/HighResRangeFinder.hpp index d343d683..99f48527 100644 --- a/tasks/HighResRangeFinder.hpp +++ b/tasks/HighResRangeFinder.hpp @@ -84,6 +84,7 @@ namespace mars { }; std::vector cameras; + std::vector addedCameras; /** * Loads another camera from the scene file which will be used for pointcloud creation. From 77290a5a575e288c2ca1a7238fe4a88e629c9f0e Mon Sep 17 00:00:00 2001 From: Pierre Willenbrock Date: Mon, 20 Nov 2023 11:11:08 +0100 Subject: [PATCH 2/3] HighResRangeFinder: Reorder properties, clarify documentation left/right_limit and resolution_horizontal belong together as do upper/lower_limit and resolution_vertical. Both resolutions had "Vertical angular resolution" for their documentation. This laser scanner does not check distances against the distance to the actual device but to the camera image plane(that crosses the actual device position). --- mars.orogen | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mars.orogen b/mars.orogen index 30fdf16f..9768347c 100755 --- a/mars.orogen +++ b/mars.orogen @@ -193,20 +193,20 @@ task_context "HighResRangeFinder", subclasses: "CameraPlugin" do doc('Left limit of the vertical opening angle in radians') property('right_limit', 'double', Math::PI/4.0). doc('Right limit of the vertical opening angle in radians') - property('resolution_vertical', 'double', Math::PI/180.0). - doc('Vertical angular resolution in radians') + property('resolution_horizontal', 'double', Math::PI/180.0). + doc('Horizontal angular resolution in radians') property('upper_limit', 'double', Math::PI/4.0). doc('Upper limit of the horizontal opening angle in radians') property('lower_limit', 'double', -Math::PI/4.0). doc('Lower limit of the horizontal opening angle in radians') - property('resolution_horizontal', 'double', Math::PI/180.0). + property('resolution_vertical', 'double', Math::PI/180.0). doc('Vertical angular resolution in radians') property('minimum_distance', 'double', 1.0). - doc('Smaller distances (meter) are discarded') + doc('Smaller distances from camera plane (meter) are discarded') property('maximum_distance', 'double', 80.0). - doc('Larger distances (meter) are discarded') + doc('Larger distances from camera plane (meter) are discarded') property('extra_cameras', '/std/vector'). doc('Extra set of cameras, described as pairs of name within scene file and orientation. Note that the camera from the "name" property always goes at orientation 0.0.') From 419936e01eaa68ca7c59c79531595571d403f49d Mon Sep 17 00:00:00 2001 From: Pierre Willenbrock Date: Mon, 20 Nov 2023 11:15:27 +0100 Subject: [PATCH 3/3] HighResRangeFinder: Allow choosing sample points like a rotating scanner Internally, this precalculates all sampling points instead of calculating them on the fly. --- mars.orogen | 2 + tasks/HighResRangeFinder.cpp | 179 +++++++++++++++++++++++------------ tasks/HighResRangeFinder.hpp | 13 +-- 3 files changed, 128 insertions(+), 66 deletions(-) diff --git a/mars.orogen b/mars.orogen index 9768347c..934b3f63 100755 --- a/mars.orogen +++ b/mars.orogen @@ -195,6 +195,8 @@ task_context "HighResRangeFinder", subclasses: "CameraPlugin" do doc('Right limit of the vertical opening angle in radians') property('resolution_horizontal', 'double', Math::PI/180.0). doc('Horizontal angular resolution in radians') + property('rotational_horizontal', 'bool', false). + doc('Rotate the beams for the horizontal direction instead of tracking a line') property('upper_limit', 'double', Math::PI/4.0). doc('Upper limit of the horizontal opening angle in radians') diff --git a/tasks/HighResRangeFinder.cpp b/tasks/HighResRangeFinder.cpp index 07dcbc7c..1d692bdc 100644 --- a/tasks/HighResRangeFinder.cpp +++ b/tasks/HighResRangeFinder.cpp @@ -59,47 +59,115 @@ void HighResRangeFinder::calcCamParameters(Camera* camera) { int height = camera->cam_sensor_info.height; double opening_width = camera->cam_sensor_info.opening_width; double opening_height = camera->cam_sensor_info.opening_height; - + + // These conversions are not entirely correct since the image pixels + // are linear on a straight line, while angles are linear on a circumference. + // see below for the correct calculations. + // Calculate starting pixel (bottom left within the image plane) - camera->pixel_per_rad_horizontal = width / ((opening_width / 180.0) * M_PI); - camera->pixel_per_rad_vertical = height / ((opening_height / 180.0) * M_PI); - - // Sets the borders. - camera->lower_pixel = _lower_limit.get() * camera->pixel_per_rad_vertical + height/2.0; - camera->upper_pixel = _upper_limit.get() * camera->pixel_per_rad_vertical + height/2.0; - camera->left_pixel = _left_limit.get() * camera->pixel_per_rad_horizontal + width/2.0; - camera->right_pixel = _right_limit.get() * camera->pixel_per_rad_horizontal + width/2.0; - - if(camera->lower_pixel < 0) { - RTT::log(RTT::Warning) << "Lower limit exceeds the image plane, will be scaled down by " << - std::fabs(camera->lower_pixel) << " pixel" << RTT::endlog(); - camera->lower_pixel = 0; - } - if(camera->upper_pixel > height) { - RTT::log(RTT::Warning) << "Upper limit exceeds the image plane, will be scaled down by " << - camera->upper_pixel - height << " pixel" << RTT::endlog(); - camera->upper_pixel = height; - } - if(camera->left_pixel < 0) { - RTT::log(RTT::Warning) << "Left limit exceeds the image plane, will be scaled down by " << - std::fabs(camera->left_pixel) << " pixel" << RTT::endlog(); - camera->left_pixel = 0; - } - if(camera->right_pixel > width) { - RTT::log(RTT::Warning) << "Right limit exceeds the image plane, will be scaled down by " << - camera->right_pixel - width << " pixel" << RTT::endlog(); - camera->right_pixel = width; - } - - camera->v_steps = _resolution_vertical.get() * camera->pixel_per_rad_vertical; - camera->h_steps = _resolution_horizontal.get() * camera->pixel_per_rad_horizontal; - + double pixel_per_rad_horizontal = width / ((opening_width / 180.0) * M_PI); + double pixel_per_rad_vertical = height / ((opening_height / 180.0) * M_PI); + + double pixel_per_tan_rad_horizontal = width / 2.0 / tan( opening_width / 180.0 * M_PI / 2.0 ); + double pixel_per_tan_rad_vertical = height / 2.0 / tan( opening_height / 180.0 * M_PI / 2.0 ); + RTT::log(RTT::Info) << "Camera " << camera->sensor_id << " (" << camera->name << ") added" << RTT::endlog(); RTT::log(RTT::Info) << "opening_width " << opening_width << ", opening_height " << opening_height << RTT::endlog(); - RTT::log(RTT::Info) << "Horizontal: Every " << camera->h_steps << " pixel " << " will be used from " << - camera->left_pixel << " to " << camera->right_pixel << RTT::endlog(); - RTT::log(RTT::Info) << "Vertical: Every " << camera->v_steps << " pixel " << " will be used from " << - camera->lower_pixel << " to " << camera->upper_pixel << RTT::endlog(); + + camera->points.clear(); + + if(!_rotational_horizontal.get()) { + // Sets the borders. + double lower_pixel = _lower_limit.get() * pixel_per_rad_vertical + height/2.0; + double upper_pixel = _upper_limit.get() * pixel_per_rad_vertical + height/2.0; + double left_pixel = _left_limit.get() * pixel_per_rad_horizontal + width/2.0; + double right_pixel = _right_limit.get() * pixel_per_rad_horizontal + width/2.0; + + if(lower_pixel < 0) { + RTT::log(RTT::Warning) << "Lower limit exceeds the image plane, will be scaled down by " << + std::fabs(lower_pixel) << " pixel" << RTT::endlog(); + lower_pixel = 0; + } + if(upper_pixel > height) { + RTT::log(RTT::Warning) << "Upper limit exceeds the image plane, will be scaled down by " << + upper_pixel - height << " pixel" << RTT::endlog(); + upper_pixel = height; + } + if(left_pixel < 0) { + RTT::log(RTT::Warning) << "Left limit exceeds the image plane, will be scaled down by " << + std::fabs(left_pixel) << " pixel" << RTT::endlog(); + left_pixel = 0; + } + if(right_pixel > width) { + RTT::log(RTT::Warning) << "Right limit exceeds the image plane, will be scaled down by " << + right_pixel - width << " pixel" << RTT::endlog(); + right_pixel = width; + } + + double v_steps = _resolution_vertical.get() * pixel_per_rad_vertical; + double h_steps = _resolution_horizontal.get() * pixel_per_rad_horizontal; + + for(double y = lower_pixel; y < upper_pixel; y += v_steps) { + for(double x = left_pixel; x < right_pixel; x += h_steps) { + // Pixel contains a distance value between min and max + // and lies within the image plane. + LookupPoint pt = {(size_t) x, (size_t) y}; + camera->points.push_back(pt); + } + } + + RTT::log(RTT::Info) << "Horizontal: Every " << h_steps << " pixel " << " will be used from " << + left_pixel << " to " << right_pixel << RTT::endlog(); + RTT::log(RTT::Info) << "Vertical: Every " << v_steps << " pixel " << " will be used from " << + lower_pixel << " to " << upper_pixel << RTT::endlog(); + } else { + //this is basically a vertical cylinder; the sensor could be a rotating line-ccd + + // Sets the borders. + double lower_pixel = tan(_lower_limit.get()) * pixel_per_tan_rad_vertical + height/2.0; + double upper_pixel = tan(_upper_limit.get()) * pixel_per_tan_rad_vertical + height/2.0; + + if(lower_pixel < 0) { + RTT::log(RTT::Warning) << "Lower limit exceeds the image plane, will be scaled down by " << + std::fabs(lower_pixel) << " pixel" << RTT::endlog(); + lower_pixel = 0; + } + if(upper_pixel > height) { + RTT::log(RTT::Warning) << "Upper limit exceeds the image plane, will be scaled down by " << + upper_pixel - height << " pixel" << RTT::endlog(); + upper_pixel = height; + } + + double v_steps = _resolution_vertical.get() * pixel_per_rad_vertical; + + for(double y = lower_pixel; y < upper_pixel; y += v_steps) { + for(double x_rad = _left_limit.get(); + x_rad < _right_limit.get(); + x_rad += _resolution_horizontal.get()) { + LookupPoint pt = { + (size_t) (tan(x_rad) * pixel_per_tan_rad_horizontal + width/2.0 ), + (size_t) ((y-height/2.0)/cos(x_rad) + height/2.0) }; + if(pt.x >= 0 && pt.y >= 0 && + pt.x < width && pt.y < height) { + bool found = false; + for(auto &p : camera->points) { + if(p.x == pt.x && p.y == pt.y) { + found = true; + break; + } + } + if(!found) + camera->points.push_back(pt); + } + } + } + + RTT::log(RTT::Info) << "Horizontal: Every " << _resolution_horizontal.get() << " rad " << " will be used from " << + _left_limit.get() << " to " << _right_limit.get() << RTT::endlog(); + RTT::log(RTT::Info) << "Vertical: Every " << v_steps << " pixel " << " will be used from " << + lower_pixel << " to " << upper_pixel << RTT::endlog(); + } + } @@ -186,34 +254,29 @@ void HighResRangeFinder::cleanupHook() void HighResRangeFinder::getData() { - Eigen::Matrix scene_p; base::samples::Pointcloud pointcloud; pointcloud.time = getTime(); - size_t x_t = 0, y_t = 0; - int counter = 0; std::vector::iterator it = cameras.begin(); for(; it < cameras.end(); ++it) { - counter = 0; + int counter = 0; // Request image and store it within the base distance image. (*it)->camera_sensor->getDepthImage((*it)->image->data); base::samples::DistanceImage* image = (*it)->image; - for(double y = (*it)->lower_pixel; y < (*it)->upper_pixel; y += (*it)->v_steps) { - for(double x = (*it)->left_pixel; x < (*it)->right_pixel; x += (*it)->h_steps) { - // Pixel contains a distance value between min and max - // and lies within the image plane. - x_t = (size_t) x; - y_t = (size_t) y; - if((*it)->image->data[x_t+y_t*(*it)->image->width] >= _minimum_distance.get() && - (*it)->image->data[x_t+y_t*(*it)->image->width] <= _maximum_distance.get() && - (*it)->image->getScenePoint( (size_t) x, (size_t) y, scene_p )) { - // Rotate camera around the y axis. - scene_p = (*it)->orientation * scene_p; - // Transforms to robot frame (x: front, z: up) and adds to the pointcloud. - scene_p = base::Vector3d(scene_p[2], -scene_p[0], -scene_p[1]); - pointcloud.points.push_back(scene_p); - counter++; - } + for(auto &p : (*it)->points) { + size_t x_t = p.x; + size_t y_t = p.y; + Eigen::Matrix scene_p; + float pt = image->data[x_t+y_t*image->width]; + if(pt >= _minimum_distance.get() && + pt <= _maximum_distance.get() && + image->getScenePoint( x_t, y_t, scene_p )) { + // Rotate camera around the y axis. + scene_p = (*it)->orientation * scene_p; + // Transforms to robot frame (x: front, z: up) and adds to the pointcloud. + scene_p = base::Vector3d(scene_p[2], -scene_p[0], -scene_p[1]); + pointcloud.points.push_back(scene_p); + counter++; } } RTT::log(RTT::Info) << counter << " points have been added by camera " << (*it)->name << diff --git a/tasks/HighResRangeFinder.hpp b/tasks/HighResRangeFinder.hpp index 99f48527..bfa25abc 100644 --- a/tasks/HighResRangeFinder.hpp +++ b/tasks/HighResRangeFinder.hpp @@ -30,6 +30,10 @@ namespace mars { { friend class HighResRangeFinderBase; protected: + struct LookupPoint { + size_t x; + size_t y; + }; struct Camera { /** * Stores the informations of all cameras which are used to simulate this sensor. @@ -72,14 +76,7 @@ namespace mars { // Rotation of the camera around the y-axis within the camera frame. Eigen::Quaternion orientation; - double pixel_per_rad_horizontal; - double pixel_per_rad_vertical; - double lower_pixel; - double upper_pixel; - double left_pixel; - double right_pixel; - double v_steps; - double h_steps; + std::vector points; std::string name; };