407 lines
11 KiB
Text
407 lines
11 KiB
Text
/*
|
|
* scanToGrid implementation
|
|
*
|
|
* Copyright (C) Uwe Hebbelmann, Sebastian Stock, Andre Schemschat
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#include <map>
|
|
|
|
#include "grid/scanToGrid.h"
|
|
|
|
using std::vector;
|
|
using std::map;
|
|
|
|
/**
|
|
* CTor.
|
|
*
|
|
* @param resolution the resolution of the grid (centimeters per gridunit)
|
|
* @param minRelevantHeight The minimal height of a scan that is relevant
|
|
* @param maxRelevantHeight The maximal height of a scan that is relevant
|
|
* @param maxDistance The maximal relevant distance
|
|
* @param minimalWeighting The minimal weighting for the waypoints
|
|
* @param createWaypoints If set waypoints will be created
|
|
* @param neighbours If set neighbours will be created
|
|
*/
|
|
scanToGrid::scanToGrid(double resolution,
|
|
double minRelevantHeight, double maxRelevantHeight,
|
|
int maxDistance, int spotRadius,
|
|
bool createWaypoints, bool neighbours)
|
|
{
|
|
this->resolution = resolution;
|
|
this->minRelevantHeight = minRelevantHeight;
|
|
this->maxRelevantHeight = maxRelevantHeight;
|
|
this->waypoints = createWaypoints;
|
|
this->neighbours = neighbours;
|
|
this->spot = (float)spotRadius / 3000.0;
|
|
|
|
// Calculate minimal weighting
|
|
this->minimalWeighting = 1;//(1((float)minimalWeighting-1)/
|
|
// (float)scaleToGrid(maxDistance));
|
|
}
|
|
|
|
/**
|
|
* The method checks if the given point is relevant
|
|
* for creating the grid.
|
|
* It is not relevant if it is below or above a specified
|
|
* height, or if it lies to far away (??)
|
|
*
|
|
* @param p The point to check (not scaled to grid)
|
|
* @return True if the point is relevant
|
|
*/
|
|
bool scanToGrid::isPointRelevant(const Point &p) const
|
|
{
|
|
if(p.y < this->minRelevantHeight || p.y > this->maxRelevantHeight)
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* The method scales the absolute coordinate of the scanpoint
|
|
* to the matching coordinate of the gridpoint
|
|
*
|
|
* @param point the absolute coordinate of the scanpoint
|
|
*
|
|
* @return the matching coordiante of the gridpoint
|
|
*/
|
|
long scanToGrid::scaleToGrid(double point)
|
|
{
|
|
return (long)(point / this->resolution);
|
|
}
|
|
|
|
/**
|
|
* The methods calculates the norm vector of the vector given
|
|
* through x and z.
|
|
* The normvector has one direction oflength 1 or -1 and the other
|
|
* length in [-1,1]
|
|
* @param x The x component of the vector
|
|
* @param z the z component of the vector
|
|
* @param xnorm Referenz to the double containing the normalized x value afterwards
|
|
* @param znorm Referenz to the double containing the normalized z value afterwards
|
|
*/
|
|
void scanToGrid::calculateNormvector(long x, long z,
|
|
double& xnorm, double& znorm)
|
|
{
|
|
xnorm = 0;
|
|
znorm = 0;
|
|
|
|
// if |X| < |Z|, then norm z to 1 and x to x/z
|
|
if(abs(x) < abs(z))
|
|
{
|
|
if(z == 0)
|
|
return;
|
|
|
|
xnorm = (double)x / (double)abs(z);
|
|
znorm = z / abs(z); // now znorm is 1 or -1
|
|
}
|
|
else
|
|
{
|
|
if(x == 0)
|
|
return;
|
|
|
|
xnorm = x / abs(x); // now xnorm is 1 or -1
|
|
znorm = (double)z / (double)abs(x);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* The method creates a new grid. It determines the size and offset
|
|
* of the new grid and calculates the viewpoint of the roboter during
|
|
* the scan.
|
|
* The new scangrid is allocated using new, but the pointer is not stored,
|
|
* so it must be freed by the calling method.
|
|
*
|
|
* The scan has to be transformed to absolute values before this method is
|
|
* called
|
|
*
|
|
* @param scan the scan to be transformed
|
|
* @param transformation the transformationmatrix needed for calculating
|
|
* the viewpoint of the roboter
|
|
*
|
|
* @return pointer to the created grid.
|
|
* (Just gets allocated, does not get freed)
|
|
*/
|
|
scanGrid* scanToGrid::createGrid(Scan& scan,
|
|
const double* transformation)
|
|
{
|
|
// Calculate the viewpoint of the scan
|
|
double rPos[3];
|
|
double rPosTheta[3];
|
|
|
|
Matrix4ToEuler(transformation, rPosTheta, rPos);
|
|
|
|
long vpX = scaleToGrid(rPos[0]);
|
|
long vpZ = scaleToGrid(rPos[2]);
|
|
|
|
// calculate maximal and minimal coordiantes
|
|
double maxX = 0;
|
|
double maxZ = 0;
|
|
double minX = 0;
|
|
double minZ = 0;
|
|
|
|
DataXYZ xyz(scan.get("xyz"));
|
|
|
|
for(size_t i = 0; i < xyz.size(); ++i)
|
|
{
|
|
Point it = xyz[i];
|
|
|
|
// If the scan is not relevant, skip it
|
|
if(!isPointRelevant(it))
|
|
continue;
|
|
|
|
if(it.x < minX) minX = it.x;
|
|
if(it.x > maxX) maxX = it.x;
|
|
if(it.z < minZ) minZ = it.z;
|
|
if(it.z > maxZ) maxZ = it.z;
|
|
|
|
}
|
|
|
|
// get offsets and sizes
|
|
long offsetX = scaleToGrid(minX);
|
|
long offsetZ = scaleToGrid(minZ);
|
|
long sizeX = scaleToGrid(maxX) - offsetX + 1;
|
|
long sizeZ = scaleToGrid(maxZ) - offsetZ + 1;
|
|
|
|
//returning new grid
|
|
return new scanGrid(vpX, vpZ,
|
|
offsetX, offsetZ,
|
|
sizeX, sizeZ);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* !NOTE! Experimental. Currently this is not used! !NOTE!
|
|
*
|
|
* The method iterates through the grid and checks for each point
|
|
* if it has a specified amount of neighbours. If not enough neighbours
|
|
* are found, the point is deleted.
|
|
*
|
|
* @param grid The grid to check
|
|
* @param distance
|
|
* @param neighbours The amount of neighbours needed for not getting deleted
|
|
*/
|
|
void scanToGrid::killAlonePoints(scanGrid* grid,
|
|
int distance, int neighbours)
|
|
{
|
|
std::vector< gridPoint* > exPoints;
|
|
|
|
|
|
for(int i=0; i < grid->getSizeX(); ++i)
|
|
{
|
|
for(int j=0; j < grid->getSizeZ(); ++j)
|
|
{
|
|
int found = 0;
|
|
|
|
for(int a=i - distance; a < i + distance; ++a)
|
|
{
|
|
for(int b=i - distance; b < i + distance; ++b)
|
|
{
|
|
if(a > 0 && b > 0 &&
|
|
a < grid->getSizeX() && b < grid->getSizeZ())
|
|
{
|
|
if(grid->points[a][b]->getPercent() > 0)
|
|
{
|
|
++found;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
// not enough neigbours found
|
|
if(found < neighbours)
|
|
{
|
|
exPoints.push_back(grid->points[i][j]);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
vector<gridPoint*>::iterator it = exPoints.begin();
|
|
vector<gridPoint*>::iterator end = exPoints.end();
|
|
|
|
while(it != end)
|
|
{
|
|
//std::cout << "Deleting " << (*it)->getX() << " " << (*it)->getZ() << " " << std::endl;
|
|
(*it)->setFixed(0, 0);
|
|
++it;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* The method adds additional information about the neighbours of
|
|
* the given point based on some sort of probabilityfunction.
|
|
* Given the degree of the scanner, the function calculates the insecure
|
|
* points and weights them
|
|
*
|
|
* @param grid The grid to add the information to
|
|
* @param x The absolute x coordinate
|
|
* @param z The absolute z coordinate
|
|
* @param weighting The weighting of the point
|
|
*/
|
|
void scanToGrid::createNeighbours(scanGrid *grid, long x, long z, float weighting)
|
|
{
|
|
// Calculate direction of the neighbours
|
|
long dx = - (grid->getViewpointZ() - z);
|
|
long dz = grid->getViewpointX() - x;
|
|
|
|
// calculating the normvector of them
|
|
double xnorm, znorm;
|
|
calculateNormvector(dx, dz, xnorm, znorm);
|
|
|
|
// calculating number of neighbours to weight
|
|
int r = (int) (sqrt((double)(dx*dx + dz*dz)) * this->spot);
|
|
|
|
for(int i = -r; i <= r; ++i)
|
|
{
|
|
// Skip 0 (0 is the actual point on the vector)
|
|
if(i == 0)
|
|
continue;
|
|
|
|
long px = x + (int) (i * xnorm);
|
|
long pz = z + (int) (i * znorm);
|
|
|
|
// if calculated point not in grid, skip
|
|
if( !grid->contains(px, pz))
|
|
continue;
|
|
|
|
float weight = weighting / pow(2.0, abs(i*r));
|
|
|
|
// grid->addPoint(px, pz,
|
|
// SOLIDWEIGHT * weight, SOLIDWEIGHT * weight);
|
|
grid->addPoint(px, pz, SOLIDWEIGHT, SOLIDWEIGHT);
|
|
|
|
if(this->waypoints)
|
|
createWaypoints(grid, px, pz, weight);
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* The method calculates the free waypoints between the robot and
|
|
* the occupied point and adds them to the grid.
|
|
* The way is weighted with the weighting-parameter to avoid
|
|
* that a path overwrites found obstacles
|
|
*
|
|
* @param grid pointer to the grid to which the points will be added
|
|
* @param x the x-coordinate of the occupied point
|
|
* @param z the z-coordinate of the occupied point
|
|
* @param weighting The weighting of the point on which the path is based.
|
|
*/
|
|
void scanToGrid::createWaypoints(scanGrid *grid, long x, long z, float weighting)
|
|
{
|
|
if(x == 0 || z == 0)
|
|
return;
|
|
|
|
long dx = x - grid->getViewpointX();
|
|
long dz = z - grid->getViewpointZ();
|
|
|
|
// calculating the normal vector
|
|
double xnorm, znorm;
|
|
calculateNormvector(dx, dz, xnorm, znorm);
|
|
|
|
int distance = (int)floor(sqrt(pow((double)dx, 2) + pow((double)dz, 2)) /
|
|
sqrt(pow(xnorm, 2) + pow(znorm, 2)));
|
|
|
|
|
|
for(int i=0; i < distance; ++i)
|
|
{
|
|
long px = grid->getViewpointX() + (long)(i * xnorm);
|
|
long pz = grid->getViewpointZ() + (long)(i * znorm);
|
|
|
|
// due to some problems with the viewpoint lieing outside
|
|
// of the grid, first check if point is in grid
|
|
if(grid->contains(px, pz))
|
|
//grid->addPoint(px, pz, (int)(WAYPOINTWEIGHT * weighting), 0);
|
|
grid->addPoint(px, pz, WAYPOINTWEIGHT, 0);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* The method adds the point represented by the 2 coordinates. It weights
|
|
* the point based on weighting.
|
|
*
|
|
* @param grid The grid to add the point to
|
|
* @param x The x coordinate (scaled to grid)
|
|
* @param z The z coordinate (scaled to grid)
|
|
* @param weighting Importance of the point (0 <= w <= 1)
|
|
*/
|
|
void scanToGrid::createPoint(scanGrid *grid, long x, long z, float weighting)
|
|
{
|
|
// grid->addPoint(x, z, SOLIDWEIGHT, (int)(SOLIDWEIGHT * weighting));
|
|
grid->addPoint(x, z, SOLIDWEIGHT, SOLIDWEIGHT);
|
|
}
|
|
|
|
/**
|
|
* Converts a scan(3D) to a grid(2D). It iterates through each
|
|
* found point of the scan, translates it to the grid and adds it
|
|
* to the grid. Points that are somehow not relevant to the grid (e.g. points
|
|
* that are to low or to high are ignored. See isPointRelevant)
|
|
*
|
|
* If waypoints is true, it will also create waypoints from the origin of
|
|
* the scan to the found point and mark the space in between as free.
|
|
* If Neighbours should be created, it will also weight the surrounding
|
|
* spaces of the found point.
|
|
*
|
|
* @param scan The scan to be converted
|
|
* @param transformation The transformationmatrix
|
|
* @return pointer to the grid
|
|
*/
|
|
scanGrid* scanToGrid::convert(Scan& scan, const double* transformation)
|
|
{
|
|
scanGrid* grid = createGrid(scan, transformation);
|
|
|
|
DataXYZ xyz(scan.get("xyz"));
|
|
|
|
// go through all points and create the grid
|
|
for(size_t i = 0; i < xyz.size(); ++i)
|
|
{
|
|
Point p = xyz[i];
|
|
|
|
// if the scan is in the relevant area, create Point for it
|
|
if(isPointRelevant(p))
|
|
{
|
|
long x = scaleToGrid(p.x);
|
|
long z = scaleToGrid(p.z);
|
|
|
|
float weighting = calculateWeighting(x - grid->getViewpointX(),
|
|
z - grid->getViewpointZ());
|
|
createPoint(grid, x, z, weighting);
|
|
|
|
if(this->waypoints)
|
|
createWaypoints(grid, x, z, weighting);
|
|
|
|
if(this->neighbours)
|
|
createNeighbours(grid, x, z, weighting);
|
|
|
|
}
|
|
}
|
|
|
|
// Kill all points which have less then 8 out of 25 neighbours
|
|
/*killAlonePoints(grid, 3, 1);
|
|
|
|
for(int i = 0; i < grid->getSizeX(); ++i)
|
|
{
|
|
for(int j = 0; j < grid->getSizeZ(); ++j)
|
|
{
|
|
gridPoint *p = grid->points[i][j];
|
|
|
|
float weighting = calculateWeighting(p->getX() - grid->getViewpointX(), p->getZ() - grid->getViewpointZ());
|
|
|
|
if(this->waypoints)
|
|
createWaypoints(grid, p->getX(), p->getZ(), weighting);
|
|
|
|
if(this->neighbours)
|
|
createNeighbours(grid, p->getX(), p->getZ(), weighting);
|
|
}
|
|
}*/
|
|
|
|
return grid;
|
|
}
|