#include <uimgpolyproj.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)

	UImgPolyProj ()

	~UImgPolyProj ()

Private Attributes
UCamPar	camPar

UPosition	camPos

URotation	camRot

UProbPoly	floorPoly

double	mapCellSize

UImage *	mapCol

UImage *	mapPath

UPose	robPose

UPose	startPose

Detailed Description

Project an image to a floor allined grid-map.

Author: Christian Andersen

Constructor & Destructor Documentation

UImgPolyProj::UImgPolyProj ( )

Constructor

References camPar, camPos, camRot, UCamPar::invalidate(), mapCellSize, mapCol, mapPath, robPose, UPose::set(), UPosition::set(), URotation::set(), and startPose.

UImgPolyProj::~UImgPolyProj ( )

Destructor

Member Function Documentation

bool UImgPolyProj::clearColMap ( )

Clear Color patch image

References UImage::clear(), mapCol, and UImage::setSize().

Referenced by UFuncImgPoly::initializeFindPath().

bool UImgPolyProj::clearPathMap ( )

Clear Path image

References UImage::clear(), mapPath, UPixel::pixRGB(), and UImage::setSize().

Referenced by doPolygonMap(), and UFuncImgPoly::initializeFindPath().

bool UImgPolyProj::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().

bool UImgPolyProj::doProject	(	UImage *	imColor,
		UImage *	imPathMask
	)

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 UImgPolyProj::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 UFuncImgPoly::findAreaFromSeedRGB(), and setStartPose().

bool UImgPolyProj::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().

URPos UImgPolyProj::getCamToImg ( UPosition camPos )

Get image coordinates from a camera coordinate

UPosition UImgPolyProj::getCamToRob ( UPosition mapCoordinate )

Convert a map 3D position to a 3D position in robot perspective.

References URotation::asMatrix4x4RtoM(), camPos, and camRot.

Referenced by getPixToRobFloor().

UProbPoly* UImgPolyProj::getFloorPoly ( )

inline

Get pointer to result polygon in floor coordinates

Referenced by UFuncImgPoly::findAreaFromSeedRGB().

URPos UImgPolyProj::getFloorToPix ( UPosition posfl )

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 UImgPolyProj::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().

double UImgPolyProj::getMapCellSize ( )

inline

Get gridmap cell size

References mapCellSize.

UImage* UImgPolyProj::getMapColImg ( )

inline

Get (and possibly create) color gridmap image

References mapCol.

Referenced by UFuncImgPoly::initializeFindPath().

UImage* UImgPolyProj::getMapPathImg ( )

inline

Get (and possibly create) path gridmap image

References mapPath.

UPosition UImgPolyProj::getMapToRob ( UPosition mapPos )

Get map position seen from robot

References UPose::getMapToPose(), robPose, and startPose.

Referenced by getFloorToPix().

UPosition UImgPolyProj::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 UImgPolyProj::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(), and getPixToMapFloor().

UPosition UImgPolyProj::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.

UPose UImgPolyProj::getRobPose ( )

inline

Get robot pose

References robPose.

UPosition UImgPolyProj::getRobToCam ( UPosition robPos )

Get robot coordinate position seen from camera

References URotation::asMatrix4x4MtoR(), camPos, and camRot.

Referenced by getFloorToPix().

UPosition UImgPolyProj::getRobToMap ( UPosition robCoordinate )

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().

UPosition UImgPolyProj::getRobToMap	(	UPose	fromPose,
		UPosition	robCoordinate
	)

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 UImgPolyProj::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 UFuncImgPoly::initializeFindPath(), and setStartPose().

bool UImgPolyProj::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 UImgPolyProj::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().

bool UImgPolyProj::paintRobotPath	(	UImage *	img,
		UPose	toPose
	)

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 UImgPolyProj::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().

void UImgPolyProj::setCamPar ( UCamPar * camp )

inline

Set image 3d to image pixel position matrix

References camPar.

Referenced by UFuncImgPoly::findAreaFromSeedRGB().

void UImgPolyProj::setCamPar ( UCamRad * camr )

inline

Set image 3d to image pixel position matrix

References camPar, and UCamRad::getCamPar().

void UImgPolyProj::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 UFuncImgPoly::findAreaFromSeedRGB(), and setStartPose().

void UImgPolyProj::setCamPos	(	UPosition	pos,
		URotation	rot
	)

Set camera position on robot.

References camPos, and camRot.

bool UImgPolyProj::setImages	(	UImage *	mapColorImage,
		UImage *	mapPathImage
	)

Set image buffers to hold grid-map images.

References mapCol, and mapPath.

Referenced by UFuncImgPoly::initializeFindPath().

void UImgPolyProj::setMapCellSize ( double cellSize )

inline

Set cell size in meters for gridmap maps

References mapCellSize.

void UImgPolyProj::setRobPose ( UPose robotPose )

inline

Set Robot pose

References robPose.

void UImgPolyProj::setRobPose	(	double	x,
		double	y,
		double	heading
	)

inline

Set Robot pose

References robPose, and UPose::set().

void UImgPolyProj::setStartPose ( UPose pose )

inline

Set start pose

References startPose.

void UImgPolyProj::setStartPose	(	double	x,
		double	y,
		double	heading
	)

inline

Set start pose

References doPolygonMap(), doProject(), doProjectPolygonToFloorReal(), doProjectPolygonToMapPix(), getImageName(), img, paintGrid(), paintPath(), paintRobot(), paintRobotPath(), paintText(), UPose::set(), setCamPos(), and startPose.

Member Data Documentation

UCamPar UImgPolyProj::camPar

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 UImgPolyProj().