96 lines
3 KiB
C
96 lines
3 KiB
C
|
#ifndef __SCANTOGRID_H_
|
||
|
|
#define __SCANTOGRID_H_
|
||
|
|
|
||
|
|
#include "grid/scanGrid.h"
|
||
|
|
#include "slam6d/scan.h"
|
||
|
|
#include "slam6d/globals.icc"
|
||
|
|
|
||
|
|
#define WAYPOINTWEIGHT 10
|
||
|
|
#define SOLIDWEIGHT 5000
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The class provides methods to create a 2D scanGrid from a given scan.
|
||
|
|
* It sorts out points not relevant for the grid and tries to
|
||
|
|
* reduce noise within the data.
|
||
|
|
* It also marks free spaces between found obstacles.
|
||
|
|
*
|
||
|
|
* @author Sebastian Stock, Uwe Hebbelmann, Andre Schemschat
|
||
|
|
* @date 11.02.2008
|
||
|
|
*/
|
||
|
|
class scanToGrid
|
||
|
|
{
|
||
|
|
private:
|
||
|
|
/** The minimal height of a scan that is relevant */
|
||
|
|
double minRelevantHeight;
|
||
|
|
|
||
|
|
/** The maximal height of a scan that is relevant */
|
||
|
|
double maxRelevantHeight;
|
||
|
|
|
||
|
|
/** The resolution of the grid (centimeters per gridunit) */
|
||
|
|
double resolution;
|
||
|
|
|
||
|
|
/** The maximal distance */
|
||
|
|
int maxDistance;
|
||
|
|
|
||
|
|
/** The distance weighting */
|
||
|
|
double minimalWeighting;
|
||
|
|
|
||
|
|
/** the spot-value of the laser */
|
||
|
|
float spot;
|
||
|
|
|
||
|
|
/** If set, ways between the viewpoint and found point will be created */
|
||
|
|
bool waypoints;
|
||
|
|
|
||
|
|
/** If set, neighbours for each point will be weighted */
|
||
|
|
bool neighbours;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method calculates the weighting of a Point
|
||
|
|
* based on its distance and an internal formula.
|
||
|
|
*
|
||
|
|
* @param distance The distance of the point to the robots position
|
||
|
|
* @return The calculated weighting for the point
|
||
|
|
*/
|
||
|
|
inline float calculateWeighting(long x, long z)
|
||
|
|
{
|
||
|
|
double distance = sqrt(pow((double)x, 2) + pow((double)z, 2));
|
||
|
|
return this->minimalWeighting * distance + 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/** @brief Calculates the normvector for the vector (x|z) */
|
||
|
|
void calculateNormvector(long x, long z, double& xnorm, double &znorm);
|
||
|
|
|
||
|
|
/** @brief Creates a new grid and sets the offsets */
|
||
|
|
scanGrid* createGrid(Scan& scan, const double* transformation);
|
||
|
|
|
||
|
|
/** @brief Creates the free points between the robot and the occupied point */
|
||
|
|
void createWaypoints(scanGrid* grid, long x, long z, float weighting);
|
||
|
|
|
||
|
|
/** @brief Creates the neighbours for the given point */
|
||
|
|
void createNeighbours(scanGrid *grid, long x, long z, float weighting);
|
||
|
|
|
||
|
|
/** @brief Method adds a point (and maybee its neighbours) to the grid */
|
||
|
|
void createPoint(scanGrid* grid, long x, long z, float weighting);
|
||
|
|
|
||
|
|
/** @brief Method checks the grid for values which stand alone and deletes them */
|
||
|
|
void killAlonePoints(scanGrid* grid, int distance, int neighbours);
|
||
|
|
|
||
|
|
/** @brief Checks if the points lies within the relevant area */
|
||
|
|
bool isPointRelevant(const Point& p) const;
|
||
|
|
|
||
|
|
/** @brief Converts the coordinate of a scanpoint to the grid raster */
|
||
|
|
long scaleToGrid(double point);
|
||
|
|
|
||
|
|
public:
|
||
|
|
/** @brief Ctor*/
|
||
|
|
scanToGrid(double resolution, double minRelevantHeight,
|
||
|
|
double maxRelevantHeight, int maxDistance ,
|
||
|
|
int spotRadius, bool createWays,
|
||
|
|
bool neighbours);
|
||
|
|
|
||
|
|
/** @brief Converts the scan to a grid */
|
||
|
|
scanGrid* convert(Scan& scan, const double* transformation);
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif
|