student Namespace Reference

Functions
void	mouseCallback (int event, int x, int y, int, void *p)
	Function called after every mouse click. More...
std::vector< cv::Point2f >	pickNPoints (int n0, const cv::Mat &img)
	Function to pick points from image. More...
bool	extrinsicCalib (const cv::Mat &img_in, std::vector< cv::Point3f > object_points, const cv::Mat &camera_matrix, cv::Mat &rvec, cv::Mat &tvec, const std::string &config_folder)
	Function for extrinsic calibration. More...
void	imageUndistort (const cv::Mat &img_in, cv::Mat &img_out, const cv::Mat &cam_matrix, const cv::Mat &dist_coeffs, const std::string &config_folder)
	Function undistort the given image. More...
void	findPlaneTransform (const cv::Mat &cam_matrix, const cv::Mat &rvec, const cv::Mat &tvec, const std::vector< cv::Point3f > &object_points_plane, const std::vector< cv::Point2f > &dest_image_points_plane, cv::Mat &plane_transf, const std::string &config_folder)
	Perspective projection. More...
void	unwarp (const cv::Mat &img_in, cv::Mat &img_out, const cv::Mat &transf, const std::string &config_folder)
	Image unwarping. More...
bool	findRobot (const cv::Mat &img_in, const double scale, Polygon &triangle, double &x, double &y, double &theta, const std::string &config_folder)
	find Robot function in student interface More...
void	loadImage (cv::Mat &img_out, const std::string &config_folder)
	load Image function in student interface More...
bool	getCurvature (int step, Path &path)
bool	sort_pair_mission2 (const std::pair< int, Polygon > &a, const std::pair< int, Polygon > &b)
std::vector< Polygon >	obstacleOffsetting (const std::vector< Polygon > ob, int offset_radius)
	Expand obstacles region to avoid collision. More...
Polygon	resizeBorders (const Polygon &borders, int resize)
	Resize the border for avoiding collision with border. More...
Polygon	sample_borders (Polygon &borders)
	Sample the borders with multiple points. More...
std::pair< double, double >	get_center (const Polygon &poly)
	get centroid of any polygon More...
bool	pointInsidePolygon (Polygon poly, Point pt)
	function to check if a given point is inside polygon More...
double	compute_angle_gate (Polygon borders, double gateX, double gateY)
	function to compute the gate angle More...
double	internal_angle (double angle1, double angle2)
double	get_angle (Pose first, Pose second, Pose third)
	function to compute the approach angle between two nodes More...
bool	check_collison_with_borders_and_obstacles (Path path, Polygon borders, Polygon sampled_borders, std::vector< Polygon > obstacle_list, std::vector< double > obs_radius, std::vector< Point > obs_center)
	function to check if the generated path is colliding with borders and obstacles More...
void	RRT_Star (const float x, const float y, const float theta, Path &path, std::vector< Point > &localGoals, const Polygon &borders, Polygon &sampled_borders, const std::vector< Polygon > &obstacle_list, std::vector< double > obs_radius, std::vector< Point > obs_center, config_Params_planPath config_params)
	implmentation of RRT Star function More...
void	RRT_Star_ompl (const float x, const float y, const float theta, Path &path, std::vector< Point > &localGoals, const Polygon &borders, Polygon &sampled_borders, const std::vector< Polygon > &obstacle_list, std::vector< double > obs_radius, std::vector< Point > obs_center, config_Params_planPath config_params)
	implmentation of RRT Star function using OMPL library More...
std::vector< Point >	compute_vicitim_mission2 (const float x, const float y, const float theta, const Polygon &borders, const std::vector< Polygon > &obstacle_list, std::pair< double, double > gateCenter, std::vector< std::pair< int, Polygon >> victim_list, config_Params_planPath config_params)
	implmentation of Mission targets for mision 2 More...
bool	planPath (const Polygon &borders, const std::vector< Polygon > &obstacle_list, const std::vector< std::pair< int, Polygon >> &victim_list, const Polygon &gate, const float x, const float y, const float theta, Path &path, const std::string &config_folder)
	Plan a safe and fast path in the arena. More...
bool	sort_pair (const std::pair< int, Polygon > &a, const std::pair< int, Polygon > &b)
bool	processObstacles (const cv::Mat &hsv_img, const double scale, std::vector< Polygon > &obstacle_list, config_Params_ProcessMap config_params)
	Obstacle detection function. More...
bool	processGate (const cv::Mat &hsv_img, const double scale, Polygon &gate, config_Params_ProcessMap config_params)
	gate detection function More...
int	get_victim_id (cv::Rect boundingRect, cv::Mat img, config_Params_ProcessMap config_params)
	Function to get victim ID. More...
bool	processVictims (const cv::Mat &hsv_img, const double scale, std::vector< std::pair< int, Polygon >> &victim_list, config_Params_ProcessMap config_params)
	Victim detection function along with digit recognition call. More...
bool	processMap (const cv::Mat &img_in, const double scale, std::vector< Polygon > &obstacle_list, std::vector< std::pair< int, Polygon >> &victim_list, Polygon &gate, const std::string &config_folder)
	Main function to call processObstacles, processGate and processVictims. More...
void	genericImageListener (const cv::Mat &img_in, std::string topic, const std::string &config_folder)
	Function to save images for intrinsic calibration. More...

Variables
cv::Mat	dist_coeffs_for_ex
cv::Mat	debug_image

Function Documentation

bool student::check_collison_with_borders_and_obstacles	(	Path	path,
		Polygon	borders,
		Polygon	sampled_borders,
		std::vector< Polygon >	obstacle_list,
		std::vector< double >	obs_radius,
		std::vector< Point >	obs_center
	)

function to check if the generated path is colliding with borders and obstacles

This is an additional function which checcks if the path generated by Clothoids/Dubins is colliding with the obstacles and borders. Here sampled borders are used to check the distance between path and borders

Parameters

path	The path generated by Clothoids/Dubins
sampled_borders	the borders points which are sampled
obstacle_list	The list of obstacle and their points
obs_radius	Radius of each obstacle
obs_center	Center of each obstacle

Returns

true/false- Colliding/Not Colliding

double student::compute_angle_gate	(	Polygon	borders,
		double	gateX,
		double	gateY
	)

function to compute the gate angle

gate angle is very important and cannot be same as other cases. Because the recatngle of gate can be different locations(Left,Right,Bottom,Top) and oriented(Horizontal, Vertical) in different way. A combination of the locations and orientation are possible.

Parameters

borders	the border locations
gateX	gate center X
gateY	gate center Y

Returns

the approach angle of the final gate

std::vector<Point> student::compute_vicitim_mission2	(	const float	x,
		const float	y,
		const float	theta,
		const Polygon &	borders,
		const std::vector< Polygon > &	obstacle_list,
		std::pair< double, double >	gateCenter,
		std::vector< std::pair< int, Polygon >>	victim_list,
		config_Params_planPath	config_params
	)

implmentation of Mission targets for mision 2

This is very simple logic, where RRT* path is generated from source and gate . And a threshold value is used to check the distance of victims from path. If the distance is high the cost is computed accordingly and that vicitm is rejected to be saved. Also the victim is then sorted based on their occurance in path direction. This helps save time for robot.

Parameters

x	The robot location x
y	The robot location y
theta	The robot orientation theta
path	the output path variable
localGoals	the local goals which includes the victims and gate
borders	original borders
sampled_borders	the borders points which are sampled
obstacle_list	The list of obstacle and their points
obs_radius	Radius of each obstacle
obs_center	Center of each obstacle
config_params	Configuration parameters related to mission2 victim computation

bool student::extrinsicCalib	(	const cv::Mat &	img_in,
		std::vector< cv::Point3f >	object_points,
		const cv::Mat &	camera_matrix,
		cv::Mat &	rvec,
		cv::Mat &	tvec,
		const std::string &	config_folder
	)

Function for extrinsic calibration.

Extrinsic calibration to determine the Rotational and translational matrix. Four points will be chosen in the image plane and then these 4 points will be solved using the solvePnP interface from opencv to solve the extrinsic problem.

Parameters

img_in	input image
oject_points	4 points that are chosen in image
camera_matrix	The obtained camera matrix from intrinsic calibration
rvec	Output rotational vector
tvec	Output translational vector
config_folder	Output folder (if file existing then function justs reads the file to get rvec and tvec)

void student::findPlaneTransform	(	const cv::Mat &	cam_matrix,
		const cv::Mat &	rvec,
		const cv::Mat &	tvec,
		const std::vector< cv::Point3f > &	object_points_plane,
		const std::vector< cv::Point2f > &	dest_image_points_plane,
		cv::Mat &	plane_transf,
		const std::string &	config_folder
	)

Perspective projection.

Now to have a bird’s eye view of the image, where we need to project the 3D objects in a image plane, carry out Perspective Projection initially. This is again carried out with the opencv interfaces, projectPoints() and getPerspectiveTransform().

Parameters

cam_matrix	camera_matrix
rvec	Output rotational vector
tvec	Output translational vector
object_points_plane	Object points
dest_image_points_plane	destination image points plane
config_folder	Output folder (if file existing then function justs reads the file to get rvec and tvec)

bool student::findRobot	(	const cv::Mat &	img_in,
		const double	scale,
		Polygon &	triangle,
		double &	x,
		double &	y,
		double &	theta,
		const std::string &	config_folder
	)

find Robot function in student interface

Directly utilized the function provided by the teaching assistant as I found that implementation was already in the best shape. RGB→HSV→blue mask→Contours→Approximate polynomial→find 3 points of polynomial→Find center of triangle→Find angle between top vertex and center(Orientation)→return state(x,y,ψ)

Parameters

img_in	Input Image
scale	scaling factor
triangle	The outpu triangle of robot
x	robot pose (center) x
y	robot pose (center) y
theta	robot pose (initial) theta
config_folder	config folder if any configuration params to be loaded

Returns

true/false robot found or not

void student::genericImageListener	(	const cv::Mat &	img_in,
		std::string	topic,
		const std::string &	config_folder
	)

Function to save images for intrinsic calibration.

Through this the images are saved from the simulator which contains checkerboard.

Parameters

img_in	Input Image
topic	topic in which the image arrives
config_folder	config folder to store the saved images

Returns

true/false robot found or not

double student::get_angle	(	Pose	first,
		Pose	second,
		Pose	third
	)

function to compute the approach angle between two nodes

The motion planning generates points betwwen source, victims and to gate. This points are then given to local planner(Clothoids/Dubins). This function will compute the approach angle between two points. Also a special logic to get the approach angle for victims. The logic is to use the line segment angle difference. Three points are taken, the angle between the first line segment(first and second points) and next line segments(second and third points) are used to compute thh approach angle of second point.

Parameters

first	First point
second	Second point
third	Third point

Returns

the approach angle of the second point

std::pair<double, double> student::get_center

(

const Polygon &

poly

)

get centroid of any polygon

sum_of_all_vertices/ size_of_vertices

Parameters

poly	Inpput polygon

Returns

center as a std::pair

int student::get_victim_id	(	cv::Rect	boundingRect,
		cv::Mat	img,
		config_Params_ProcessMap	config_params
	)

Function to get victim ID.

This is mainly done to detect the number of Victims, so that the priority to save the victims can be known to the planning algorithm. This involves template matching majorly and the templates were provided in lecture. I have used majorly the opencv template matching methods. But I also implemented the tesseract-ocr method,although the results were not satisfying. Template rotation was performed ,Each template will be rotated by 5 degrees and then they are set to calculate the score and the maximum of all of that will be be finalized as digits. Steps followed: Detect Victims→get bounding rect→mask and filter image→change background→read templates→Resize and filter the ROI→change different orientation of ROI→match template→return Digit

Parameters

boundingRect	The bounding rectangle of the victim location
img	hsv image of the input
config_params	configuration parameters realted to obstacle detection, includes template location

Returns

vicitm id

bool student::getCurvature	(	int	step,
		Path &	path
	)

void student::imageUndistort	(	const cv::Mat &	img_in,
		cv::Mat &	img_out,
		const cv::Mat &	cam_matrix,
		const cv::Mat &	dist_coeffs,
		const std::string &	config_folder
	)

Function undistort the given image.

Using the distortion coefficients obtained in the previous steps, remove the distorted effect on the image. This is done using the opencv undistort function

Parameters

img_in	input image
img_out	output image
camera_matrix	The obtained camera matrix from intrinsic calibration
dist_coeffs	distortion coefficients
config_folder	Output folder (if file existing then function justs reads the file to get rvec and tvec)

double student::internal_angle	(	double	angle1,
		double	angle2
	)

void student::loadImage	(	cv::Mat &	img_out,
		const std::string &	config_folder
	)

load Image function in student interface

Function used directly given by Teaching assistant

Parameters

img_out	Output Image
config_folder	location where load image is stored

void student::mouseCallback	(	int	event,
		int	x,
		int	y,
		int	,
		void *	p
	)

Function called after every mouse click.

Function obatined from Professor interface

Parameters

event	mouse event occured
x	x-position of mouse event
y	y-position of mouse event

std::vector<Polygon> student::obstacleOffsetting	(	const std::vector< Polygon >	ob,
		int	offset_radius
	)

Expand obstacles region to avoid collision.

Using clipper libray , the execution of enlarging the obstacles is easier with AddPath() and Execute () API's

Parameters

ob	All Obstacles polygon to be expanded
offet_radius	The radius by which the obstacle is to be expanded

Returns

Exapanded obstacles with the given radius

std::vector<cv::Point2f> student::pickNPoints	(	int	n0,
		const cv::Mat &	img
	)

Function to pick points from image.

Function obatined from Professor interface

Parameters

n0	number of points to be picked
img	the image from which the points are to be chosen

bool student::planPath	(	const Polygon &	borders,
		const std::vector< Polygon > &	obstacle_list,
		const std::vector< std::pair< int, Polygon >> &	victim_list,
		const Polygon &	gate,
		const float	x,
		const float	y,
		const float	theta,
		Path &	path,
		const std::string &	config_folder
	)

Plan a safe and fast path in the arena.

Parameters

borders	border of the arena [m]
obstacle_list	list of obstacle polygon [m]
victim_list	list of pair victim_id and polygon [m]
gate	polygon representing the gate [m]
x	x position of the robot in the arena reference system
y	y position of the robot in the arena reference system
theta	yaw of the robot in the arena reference system
config_folder	A custom string from config file.

bool student::pointInsidePolygon	(	Polygon	poly,
		Point	pt
	)

function to check if a given point is inside polygon

using boost::within library

Parameters

poly	input polygon
pt	input point to check if this is inside the poly

Returns

true/false - insidePolygon/NotInsidePolygon

bool student::processGate	(	const cv::Mat &	hsv_img,
		const double	scale,
		Polygon &	gate,
		config_Params_ProcessMap	config_params
	)

gate detection function

Steps followed: RGB→HSV→Apply Green mask→Contours→approximate Polynomial→detect 4 points→Return Gate

Parameters

hsv_img	HSV Image input
scale	scaling factor
gate	Output gate polygon
config_params	configuration parameters realted to gate detection

Returns

true/false Execution successful

bool student::processMap	(	const cv::Mat &	img_in,
		const double	scale,
		std::vector< Polygon > &	obstacle_list,
		std::vector< std::pair< int, Polygon >> &	victim_list,
		Polygon &	gate,
		const std::string &	config_folder
	)

Main function to call processObstacles, processGate and processVictims.

Parameters

boundingRect	The bounding rectangle of the victim location
img_in	rgb image of the input
scale	scaling factor
obstacle_list	Output obstacle list
victim_list	Output victim list pair
gate	Output gate polygon
config_folder	config folder

Returns

true/false Execution successful

bool student::processObstacles	(	const cv::Mat &	hsv_img,
		const double	scale,
		std::vector< Polygon > &	obstacle_list,
		config_Params_ProcessMap	config_params
	)

Obstacle detection function.

Steps followed RGB→HSV→Apply red mask→Contours→approximate Polynomial→Return Obstacles

Parameters

hsv_img	HSV Image input
scale	Scaling factor
obstacle_list	Output obstacle list
config_params	configuration parameters realted to obstacle detection

Returns

true/false Execution successful

Steps involved in process Obstacles one by one

First range of red region in HSV

Second range of red region in HSV

add the regions of red space in HSV format

find all contours in the image

approximate the contours

create a obstacle list

debug plot in seperate debug image

bool student::processVictims	(	const cv::Mat &	hsv_img,
		const double	scale,
		std::vector< std::pair< int, Polygon >> &	victim_list,
		config_Params_ProcessMap	config_params
	)

Victim detection function along with digit recognition call.

Steps followed: RGB→HSV→Apply Green mask→Contours→approximate Polynomial→detect more than 6 points(check circle)→ get id of victim → return Victims

Parameters

hsv_img	HSV Image input
scale	scaling factor
victim_list	Output victim list pair
config_params	configuration parameters realted to gate detection

Returns

true/false Execution successful

Polygon student::resizeBorders	(	const Polygon &	borders,
		int	resize
	)

Resize the border for avoiding collision with border.

manual computation of resizing borders

Parameters

borders	border which is given as polygon
resize	the resizing factor

Returns

resized border

void student::RRT_Star	(	const float	x,
		const float	y,
		const float	theta,
		Path &	path,
		std::vector< Point > &	localGoals,
		const Polygon &	borders,
		Polygon &	sampled_borders,
		const std::vector< Polygon > &	obstacle_list,
		std::vector< double >	obs_radius,
		std::vector< Point >	obs_center,
		config_Params_planPath	config_params
	)

implmentation of RRT Star function

This is an own implementation of RRT Star with few references.

Parameters

x	The robot location x
y	The robot location y
theta	The robot orientation theta
path	the output path variable
localGoals	the local goals which includes the victims and gate
borders	original borders
sampled_borders	the borders points which are sampled
obstacle_list	The list of obstacle and their points
obs_radius	Radius of each obstacle
obs_center	Center of each obstacle
config_params	Configuration parameters related to mission2 victim computation

void student::RRT_Star_ompl	(	const float	x,
		const float	y,
		const float	theta,
		Path &	path,
		std::vector< Point > &	localGoals,
		const Polygon &	borders,
		Polygon &	sampled_borders,
		const std::vector< Polygon > &	obstacle_list,
		std::vector< double >	obs_radius,
		std::vector< Point >	obs_center,
		config_Params_planPath	config_params
	)

implmentation of RRT Star function using OMPL library

This is an RRT Star using OMPL API's.

Parameters

x	The robot location x
y	The robot location y
theta	The robot orientation theta
path	the output path variable
localGoals	the local goals which includes the victims and gate
borders	original borders
sampled_borders	the borders points which are sampled
obstacle_list	The list of obstacle and their points
obs_radius	Radius of each obstacle
obs_center	Center of each obstacle
config_params	Configuration parameters related to mission2 victim computation

Polygon student::sample_borders

(

Polygon &

borders

)

Sample the borders with multiple points.

interpolate the border points, to check if generated path is closer to sampled borders

Parameters

borders

border which is given as polygon

Returns

interpolated border points

bool student::sort_pair	(	const std::pair< int, Polygon > &	a,
		const std::pair< int, Polygon > &	b
	)

bool student::sort_pair_mission2	(	const std::pair< int, Polygon > &	a,
		const std::pair< int, Polygon > &	b
	)

void student::unwarp	(	const cv::Mat &	img_in,
		cv::Mat &	img_out,
		const cv::Mat &	transf,
		const std::string &	config_folder
	)

Image unwarping.

Image unwarping using opencv API warpPerspective()

Parameters

img_in	input image
img_out	output image
transf	tranformation matrix
config_folder	Output folder (if file existing then function justs reads the file to get rvec and tvec)

Variable Documentation

cv::Mat student::debug_image

cv::Mat student::dist_coeffs_for_ex