AURobotServers
4
|
#include <uimgproj.h>
Public Member Functions | |
bool | clearColMap () |
bool | clearPathMap () |
bool | doPolygonMap (UImage *imColor, UImage *imPathMask, CvPoint *polygon, int polygonCnt, bool *isObst) |
bool | doProject (UImage *imColor, UImage *imPathMask) |
bool | doProjectPolygonToFloorReal (CvPoint *polygon, bool *isObst, int polygonCnt, UProbPoly *destPoly) |
bool | doProjectPolygonToMapPix (CvPoint *polygon, int *polygonCnt, bool *isObst) |
URPos | getCamToImg (UPosition camPos) |
UPosition | getCamToRob (UPosition mapCoordinate) |
UProbPoly * | getFloorPoly () |
URPos | getFloorToPix (UPosition posfl) |
bool | getImageName (const char *logImg, const char *logPos, const char *subDir, int imgNum, char *bufferImgName, const int bufferSize, UTime *imgTime, int hdrMode, int device) |
double | getMapCellSize () |
UImage * | getMapColImg () |
UImage * | getMapPathImg () |
UPosition | getMapToRob (UPosition mapPos) |
UPosition | getPixToMapFloor (int w, int h, bool *lookDown=NULL) |
UPosition | getPixToRobFloor (int w, int h, bool *lookDown=NULL) |
UPosition | getPixToRobFloor (float w, float h, bool *lookDown=NULL) |
UPose | getRobPose () |
UPosition | getRobToCam (UPosition robPos) |
UPosition | getRobToMap (UPosition robCoordinate) |
UPosition | getRobToMap (UPose fromPose, UPosition robCoordinate) |
void | paintGrid (UImage *img) |
bool | paintPath (const char *filename) |
bool | paintRobot (UImage *img, int imageNumber) |
bool | paintRobotPath (UImage *img, UPose toPose) |
void | paintText (UImage *img, int x, int y, const char *text) |
void | setCamPar (UCamPar *camp) |
void | setCamPar (UCamRad *camr) |
void | setCamPos (double x, double y, double z, double omega, double phi, double kappa) |
void | setCamPos (UPosition pos, URotation rot) |
bool | setImages (UImage *mapColorImage, UImage *mapPathImage) |
void | setMapCellSize (double cellSize) |
void | setRobPose (UPose robotPose) |
void | setRobPose (double x, double y, double heading) |
void | setStartPose (UPose pose) |
void | setStartPose (double x, double y, double heading) |
UImgProj () | |
~UImgProj () | |
Private Attributes | |
UCamPar | camPar |
UPosition | camPos |
URotation | camRot |
UProbPoly | floorPoly |
double | mapCellSize |
UImage * | mapCol |
UImage * | mapPath |
UPose | robPose |
UPose | startPose |
Project an image to a floor allined grid-map.
UImgProj::UImgProj | ( | ) |
Constructor
References camPar, camPos, camRot, UCamPar::invalidate(), mapCellSize, mapCol, mapPath, robPose, UPose::set(), UPosition::set(), URotation::set(), and startPose.
UImgProj::~UImgProj | ( | ) |
Destructor
bool UImgProj::clearColMap | ( | ) |
Clear Color patch image
References UImage::clear(), mapCol, and UImage::setSize().
Referenced by UFunctionCamPath::initializeFindPath().
bool UImgProj::clearPathMap | ( | ) |
Clear Path image
References UImage::clear(), mapPath, UPixel::pixRGB(), and UImage::setSize().
Referenced by doPolygonMap(), and UFunctionCamPath::initializeFindPath().
bool UImgProj::doPolygonMap | ( | UImage * | imColor, |
UImage * | imPathMask, | ||
CvPoint * | polygon, | ||
int | polygonCnt, | ||
bool * | isObst | ||
) |
Make map based on polygon of passable floor.
References clearPathMap(), UImage::cvArr(), getFloorToPix(), UImage::getPix(), UImage::getPixRef(), UImage::height(), imagePath, mapCellSize, mapCol, mapPath, UImage::name, UPixel::p2, roundi(), UImage::saveBMP(), UPosition::set(), UPixel::swapRB(), UImage::width(), URPos::x, and URPos::y.
Referenced by setStartPose().
Provect an imageset for present robot and camera position if mask value is > 0, then thecolor will be projected to the color map, and the masked position will be marked as passable in the path image. Returns true if projection is possible.
References camPar, getFloorToPix(), UImage::getPix(), UImage::getPixRef(), getPixToMapFloor(), UImage::height(), UCamPar::isValid(), mapCellSize, mapCol, mapPath, maxi(), mind(), UPixel::p3, roundi(), UPosition::set(), UPosition::show(), UPixel::tone(), UImage::width(), URPos::x, UPosition::x, and URPos::y.
Referenced by setStartPose().
bool UImgProj::doProjectPolygonToFloorReal | ( | CvPoint * | polygon, |
bool * | isObst, | ||
int | polygonCnt, | ||
UProbPoly * | destPoly | ||
) |
Convert image polygon to corresponding flat floor polygon in robot perspective, from known camera and canera position. Any points above the horizon are just removed (no interpolation), but 'isObst'-flag is set to false if the point goet to or from an above horizon removed point. This function will not modify original polygon data.
Returns true if pointers are non NULL Returns points in UPosition array of maximum size polyRealCnt The number of points are returned into polyRealCnt.
References UProbPoly::addPos(), camPar, getFloorToPix(), getPixToRobFloor(), UCamPar::getRadialD2U(), maxi(), UPosition::set(), and URPos::y.
Referenced by UFunctionCamPath::findPath(), and setStartPose().
bool UImgProj::doProjectPolygonToMapPix | ( | CvPoint * | polygon, |
int * | polygonCnt, | ||
bool * | isObst | ||
) |
Make map based on polygon of passable floor. The result coordinates are in image pixels. The polygin is converted to robot floor perspective using camera position on the robot. Any points above the horizon are just removed (no interpolation), but 'isObst'-flag is set to false if the point goet to or from an above horizon removed point. This function modifies source data to the new projection.
Returns true if pointers are non NULL
References getPixToMapFloor(), UImage::height(), mapCellSize, mapCol, roundi(), UPosition::x, and UPosition::y.
Referenced by setStartPose().
Convert a map 3D position to a 3D position in robot perspective.
References URotation::asMatrix4x4RtoM(), camPos, and camRot.
Referenced by getPixToRobFloor().
|
inline |
Get pointer to result polygon in floor coordinates
Referenced by UFunctionCamPath::findPath().
Get pixel position in source image 'img' for this (floor) position
References camPar, UCamPar::getCtoPRob(), getMapToRob(), and getRobToCam().
Referenced by doPolygonMap(), doProject(), and doProjectPolygonToFloorReal().
bool UImgProj::getImageName | ( | const char * | logImg, |
const char * | logPos, | ||
const char * | subDir, | ||
int | imgNum, | ||
char * | bufferImgName, | ||
const int | bufferSize, | ||
UTime * | imgTime, | ||
int | hdrMode, | ||
int | device | ||
) |
Get image name, time and pose. Returns true if found. Returns name in bufferImgName, and image time in imgTime.
References imagePath, robPose, UPose::set(), and UTime::setTime().
Referenced by setStartPose().
|
inline |
Get gridmap cell size
References mapCellSize.
|
inline |
Get (and possibly create) color gridmap image
References mapCol.
Referenced by UFunctionCamPath::initializeFindPath().
|
inline |
Get (and possibly create) path gridmap image
References mapPath.
Get map position seen from robot
References UPose::getMapToPose(), robPose, and startPose.
Referenced by getFloorToPix().
UPosition UImgProj::getPixToMapFloor | ( | int | w, |
int | h, | ||
bool * | lookDown = NULL |
||
) |
Convert a camera image pixel position to a position on the map. Requires valid camera position and pobot position (and camera intrincic inf too). Returns position, and flags if camera is looking down, i.e. below the horizon.
References getPixToRobFloor(), and getRobToMap().
Referenced by doProject(), and doProjectPolygonToMapPix().
UPosition UImgProj::getPixToRobFloor | ( | int | w, |
int | h, | ||
bool * | lookDown = NULL |
||
) |
Convert a pixel position to a 3d position in the floor plane. The optional 'lookDown' parameter is false if image line is parallel to floor plane or points above the horizon. The restriction that the ray must be 2 cm below camera at 1 meter distance - e.g. max range is 50 m with a camera height of 1m.
References camPar, camPos, getCamToRob(), ULine::getPlaneLineCrossing(), UCamPar::getPtoCRob(), ULine::setFromPoints(), and UPosition::z.
Referenced by doProjectPolygonToFloorReal(), getPixToMapFloor(), and UFuncLinefinder::houghTest().
UPosition UImgProj::getPixToRobFloor | ( | float | w, |
float | h, | ||
bool * | lookDown = NULL |
||
) |
Convert a pixel position to a 3d position in the floor plane. The optional 'lookDown' parameter is false if image line is parallel to floor plane or points above the horizon. The restriction that the ray must be 2 cm below camera at 1 meter distance - e.g. max range is 50 m with a camera height of 1m.
References camPar, camPos, getCamToRob(), ULine::getPlaneLineCrossing(), UCamPar::getPtoCRob(), ULine::setFromPoints(), and UPosition::z.
Get robot coordinate position seen from camera
References URotation::asMatrix4x4MtoR(), camPos, and camRot.
Referenced by getFloorToPix().
Convert a position (3d) as seen by robot and convert is to map (world) coordinates (3d). The rob pose is relative to the robot start pose.
References UPose::getPoseToMap(), robPose, and startPose.
Referenced by getPixToMapFloor(), paintRobot(), and paintRobotPath().
Convert a position (3d) as seen by robot at position 'fromPose' and convert is to map (world) coordinates (3d). The 'fromPose' is relative to the robot start pose.
References UPose::getPoseToMap(), and startPose.
void UImgProj::paintGrid | ( | UImage * | img | ) |
Paint 1m grid in image
References UImage::cvArr(), UImage::height(), mapCellSize, mapCol, roundi(), startPose, UImage::width(), UPose::x, and UPose::y.
Referenced by UFunctionCamPath::initializeFindPath(), and setStartPose().
bool UImgProj::paintPath | ( | const char * | filename | ) |
Paint the robot path from this odometer file
References absd(), UPose::h, imagePath, limitToPi(), mapCol, paintRobotPath(), robPose, UPose::set(), UTime::setTime(), sqr(), UPose::x, and UPose::y.
Referenced by setStartPose().
bool UImgProj::paintRobot | ( | UImage * | img, |
int | imageNumber | ||
) |
Paint an arrow (wheel triangle) in red on mapCol.Conv3DToPixel(Returns true if map image exist) Paints the provided image number (at times). If image number is 0, then no number.
References UImage::cvArr(), getRobToMap(), UImage::height(), mapCellSize, paintText(), UPosition::position(), roundi(), UPosition::x, and UPosition::y.
Referenced by setStartPose().
Paint green line from present position to this position
References UImage::cvArr(), getRobToMap(), UImage::height(), mapCellSize, UPosition::position(), roundi(), UPosition::x, and UPosition::y.
Referenced by paintPath(), and setStartPose().
void UImgProj::paintText | ( | UImage * | img, |
int | x, | ||
int | y, | ||
const char * | text | ||
) |
Paint text in this image (black - large font)
References UImage::cvArr().
Referenced by paintRobot(), and setStartPose().
|
inline |
Set image 3d to image pixel position matrix
References camPar.
Referenced by UFunctionCamPath::findPath(), and UFuncLinefinder::houghTest().
|
inline |
Set image 3d to image pixel position matrix
References camPar, and UCamRad::getCamPar().
void UImgProj::setCamPos | ( | double | x, |
double | y, | ||
double | z, | ||
double | omega, | ||
double | phi, | ||
double | kappa | ||
) |
Set camera position on robot.
References camPos, camRot, URotation::Kappa, URotation::Omega, URotation::Phi, UPosition::x, UPosition::y, and UPosition::z.
Referenced by UFunctionCamPath::findPath(), UFuncLinefinder::houghTest(), and setStartPose().
Set image buffers to hold grid-map images.
References mapCol, and mapPath.
Referenced by UFunctionCamPath::initializeFindPath().
|
inline |
Set cell size in meters for gridmap maps
References mapCellSize.
|
inline |
Set Robot pose
References robPose, and UPose::set().
|
inline |
Set start pose
References doPolygonMap(), doProject(), doProjectPolygonToFloorReal(), doProjectPolygonToMapPix(), getImageName(), img, paintGrid(), paintPath(), paintRobot(), paintRobotPath(), paintText(), UPose::set(), setCamPos(), and startPose.
|
private |
Conversion from camera 3d position to image coordinate and reverse needs knowledge of camera parameters, so these must be loaded (copied) to this position.
Referenced by doProject(), doProjectPolygonToFloorReal(), getFloorToPix(), getPixToRobFloor(), setCamPar(), and UImgProj().
|
private |
Camera position on robot
Referenced by getCamToRob(), getPixToRobFloor(), getRobToCam(), setCamPos(), and UImgProj().
|
private |
Camera rotation on robot
Referenced by getCamToRob(), getRobToCam(), setCamPos(), and UImgProj().
|
private |
The found polygon in the newest image
|
private |
Pixel size in meter for map images.
Referenced by doPolygonMap(), doProject(), doProjectPolygonToMapPix(), getMapCellSize(), paintGrid(), paintRobot(), paintRobotPath(), setMapCellSize(), and UImgProj().
|
private |
Gridmap image with projected color
Referenced by clearColMap(), doPolygonMap(), doProject(), doProjectPolygonToMapPix(), getMapColImg(), paintGrid(), paintPath(), setImages(), and UImgProj().
|
private |
Image with path-probability
Referenced by clearPathMap(), doPolygonMap(), doProject(), getMapPathImg(), setImages(), and UImgProj().
|
private |
Robot position and orientation on map
Referenced by getImageName(), getMapToRob(), getRobPose(), getRobToMap(), paintPath(), setRobPose(), and UImgProj().
|
private |
Where to start at the map
Referenced by getMapToRob(), getRobToMap(), paintGrid(), setStartPose(), and UImgProj().