Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Changes enabling using the HighResRangeFinder to more closely simulate rotating laser scanners #54

Open
wants to merge 3 commits into
base: mars2
Choose a base branch
from
Open

Changes enabling using the HighResRangeFinder to more closely simulate rotating laser scanners #54

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
25dd307
HighResRangeFinder: allow adding extra cameras through configuration
pierrewillenbrockdfki Nov 20, 2023
77290a5
HighResRangeFinder: Reorder properties, clarify documentation
pierrewillenbrockdfki Nov 20, 2023
419936e
HighResRangeFinder: Allow choosing sample points like a rotating scanner
pierrewillenbrockdfki Nov 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions highresrangefinderTypes.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@

#ifndef MARS_HIGHRESRANGEFINDER_TYPES_HH
#define MARS_HIGHRESRANGEFINDER_TYPES_HH

#include <string>

namespace mars
{

struct HighResRangeFinderCamera {
std::string name; ///<Name of the camera within the scene file
double orientation; ///<To get a full 360 degree view you have to add four 90-cameras with 0, 90, 180 and 270
};

}
#endif


16 changes: 11 additions & 5 deletions mars.orogen
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ import_types_from "poseType.hpp"
import_types_from "objectDetectionTypes.hpp"
import_types_from "wrenchTypes.hpp"
import_types_from "opaqueTypes.hpp"
import_types_from "highresrangefinderTypes.hpp"
import_types_from "base"
import_types_from "mars/plugins/tether_simulation/Settings.h"
import_types_from "maps"
Expand Down Expand Up @@ -192,20 +193,25 @@ 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('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')
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</mars/HighResRangeFinderCamera>').
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').
Expand Down
236 changes: 173 additions & 63 deletions tasks/HighResRangeFinder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -52,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();
}

}


Expand All @@ -116,7 +191,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<mars::sim::CameraSensor *>(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<mars::sim::CameraSensor *>(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;
}

Expand All @@ -131,6 +240,12 @@ void HighResRangeFinder::errorHook()
void HighResRangeFinder::stopHook()
{
HighResRangeFinderBase::stopHook();

std::vector<Camera*>::iterator it = cameras.begin();
for(; it != cameras.end(); ++it) {
delete *it;
}
cameras.clear();
}
void HighResRangeFinder::cleanupHook()
{
Expand All @@ -139,34 +254,29 @@ void HighResRangeFinder::cleanupHook()

void HighResRangeFinder::getData()
{
Eigen::Matrix<double, 3, 1> scene_p;
base::samples::Pointcloud pointcloud;
pointcloud.time = getTime();
size_t x_t = 0, y_t = 0;
int counter = 0;
std::vector<Camera*>::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<double>( (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<double, 3, 1> scene_p;
float pt = image->data[x_t+y_t*image->width];
if(pt >= _minimum_distance.get() &&
pt <= _maximum_distance.get() &&
image->getScenePoint<double>( 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 <<
Expand Down
14 changes: 6 additions & 8 deletions tasks/HighResRangeFinder.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -72,18 +76,12 @@ namespace mars {
// Rotation of the camera around the y-axis within the camera frame.
Eigen::Quaternion<double, Eigen::DontAlign> 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<LookupPoint> points;
std::string name;
};

std::vector<Camera*> cameras;
std::vector<HighResRangeFinderCamera> addedCameras;

/**
* Loads another camera from the scene file which will be used for pointcloud creation.
Expand Down