def render(self):
"""
Returns
-------
img
Description
-----------
This method returns an image of map canvas with car image plotted over it. This image
can be used to visualize car motion on the map canvas
"""
# Fetch the car image rotated by current car inclination angle
carImg = self.car.getCarImg()
# Get image's dimensions
carHeight,carWidth = carImg.shape[:2]
# get car coordinates in image reference frame
centerX,centerY = self.getCarCoordinates()
# Estimate location of top left corner of car image in map canvas
plotX = int(centerX - carWidth/2)
plotY = int(centerY - carHeight/2)
#print('plotX',plotX,'plotY',plotY)
# Make a copy of original map image so that changes arre not permanently
# written
mapImg2 = copy.deepcopy(self.mapImg)
# The car could be at the border so we add padding first, plot car and then crop out the border
# max width and height that 10,20 car take is 22,22 so
# we must have border of 11. but we set border as 15 as safe margin
border = 15
mapImg2 = cv2.copyMakeBorder(mapImg2, border, border, border, border, cv2.BORDER_CONSTANT,value=0 )
# take care of border for car image top left corner
plotX += border
plotY += border
# Blend car image with portion of map image at the location calculated above
# mergeImages combines two images in proportion alpha and beta (output = alpha*img1 + beta*img2)
blendedPart = mergeImages(carImg,1.0,mapImg2[plotY:plotY+ carHeight,plotX:plotX+carWidth ],0.0)
# paste the blended part onto map image
mapImg2[plotY:plotY+ carHeight,plotX:plotX+carWidth ] = blendedPart
# Remove the borders
mapImg2 = mapImg2[border:border+self.mapHeight,border:border+self.mapWidth]
#plot goal circles
#Inner solid circle
mapImg2 = cv2.circle(mapImg2,(self.goalX,self.mapHeight-1-self.goalY),8,(255,0,0),-1)
# Outer ring representing area of radius 30 around goal
mapImg2 = cv2.circle(mapImg2,(self.goalX,self.mapHeight-1-self.goalY),30,(0,0,255),2)
return mapImg2
def renderV2(self):
"""
Returns
-------
img
Description
-----------
This is another version of render() method.
This method returns an image of map canvas with car image plotted over it. This image
can be used to visualize car motion on the map canvas. It also plots current state image,
step reward and next goal location in the bottom right corner of map image
"""
# Fetch the car image rotated by current car inclination angle
carImg = self.car.getCarImg()
# Get image's dimensions
carHeight,carWidth = carImg.shape[:2]
# get car coordinates in image reference frame
centerX,centerY = self.getCarCoordinates()
# Estimate location of top left corner of car image in map canvas
plotX = int(centerX - carWidth/2)
plotY = int(centerY - carHeight/2)
# Make a copy of original map image so that changes arre not permanently
# written
mapImg2 = copy.deepcopy(self.mapImg)
# The car could be at the border so we add padding first, plot car and then crop out the border
# max width and height that 10,20 car take is 22,22 so
# we must have border of 11. but we set border as 15 as safe margin
border = 15
mapImg2 = cv2.copyMakeBorder(mapImg2, border, border, border, border, cv2.BORDER_CONSTANT,value=0 )
# take care of border for car image top left corner
plotX += border
plotY += border
# Blend car image with portion of map image at the location calculated above
# mergeImages combines two images in proportion alpha and beta (output = alpha*img1 + beta*img2)
blendedPart = mergeImages(carImg,1.0,mapImg2[plotY:plotY+ carHeight,plotX:plotX+carWidth ],0.0)
# paste the blended part onto map image
mapImg2[plotY:plotY+ carHeight,plotX:plotX+carWidth ] = blendedPart
# Remove the borders
mapImg2 = mapImg2[border:border+self.mapHeight,border:border+self.mapWidth]
#plot goal circles
#Inner solid circle
mapImg2 = cv2.circle(mapImg2,(self.goalX,self.mapHeight-1-self.goalY),8,(255,0,0),-1)
# Outer ring representing area of radius 30 around goal
mapImg2 = cv2.circle(mapImg2,(self.goalX,self.mapHeight-1-self.goalY),30,(0,0,255),2)
# get Current state image
state = self.estimateStateV2()
# Make a copy of the state image
stateImg = copy.deepcopy(state[0].squeeze()) # squeeze out extra dimesion of state image
stateHeight,stateWidth = stateImg.shape
# State image is in normalized form [0.0,1.0]. So multiply by 255.0 to bring it to [0,255]
stateImg = np.array(stateImg*255.0,dtype = np.uint8)
# Convert grayscale image to RGB because map image where we want to paste it is RGB
stateImg = cv2.cvtColor(stateImg,cv2.COLOR_GRAY2RGB)
# Paste state image onto map image
mapImg2[500:500+stateHeight,1200:1200+stateWidth,:] = stateImg[:,:,:]
# Set text color to be blue
textColor = (255,0,0) # in openCV order of color channels is BGR
# Indicate 'state image'
cv2.putText(img=mapImg2,text='State Image',org=(1170,480),fontFace=cv2.FONT_HERSHEY_SIMPLEX,\
fontScale=0.5,color=textColor,thickness=2)
# Prepare text for showing step reward
stepRewardMsg = 'Step Reward: %f'%self.reward
# Write step reward onto map image
cv2.putText(img=mapImg2,text=stepRewardMsg,org=(1115,570),fontFace=cv2.FONT_HERSHEY_SIMPLEX,\
fontScale=0.5,color=textColor,thickness=2)
# Prepare text for showing next goal
goalMsg = 'Next Goal: (%d,%d)'%(self.goalX,self.goalY)
# Write next goal location onto map image
cv2.putText(img=mapImg2,text=goalMsg,org=(1125,600),fontFace=cv2.FONT_HERSHEY_SIMPLEX,\
fontScale=0.5,color=textColor,thickness=2)
return mapImg2
def estimateStateV2(self):
"""
Returns
-------
state : list
Description
-----------
Estimate current state of the environment.
State consists of 4 elements
# 0: Crop of mask image centered at car's location rotated by (90 - Car.angle) and
arrow image pasted on it.
# 1: Distance to Goal scaled by maxGoalDistance
# 2: Orientation of Goal wrt car
# 3: Negative orientation of Goal wrt car
"""
################ State Element 0 #################################
# Fetch the crop of Mask image of size 'crop_size' centered at car's location and rotated
# by (90 - car.angle). This represents car's front view
cropWithCar = self.getCurrentMaskCropV2()
w,h =cropWithCar.shape[:2]
# Rotate the image of an Arrow by 90 because car will always be facing up in
# its front view
arrowImgRotated = imgRotate(self.arrowImg,90.0) # arrow always facing up
# Check the size of rotated image
arrowH,arrowW = arrowImgRotated.shape[:2]
# Calculate where in the cropped image , arrow image's top left corner should be
plotX = int(w/2 - arrowW/2)
plotY = int(h/2 - arrowH/2)
# Blend Arrow image with portion of cropped image at the location calculated above
# mergeImages combines two images in proportion alpha and beta (output = alpha*img1 + beta*img2)
blendedPart = mergeImages(img1=arrowImgRotated,alpha=1.0,img2=cropWithCar[plotY:plotY+ arrowH,plotX:plotX+arrowW ],beta=0.0)
# paste the blended part onto the cropped image at location calculated earlier
cropWithCar[plotY:plotY+ arrowH,plotX:plotX+arrowW ] = blendedPart
# Normalize the crop image to [0.0,1.0] pixel values. This image will be used as one of the states
# to be fed into the CNN. So it is better to provide normalized image rather than [0,255] image
cropWithCar = (cropWithCar)/(255.0)
# output dimension should be 1xHxW so expand dimension
cropWithCar = np.expand_dims(cropWithCar,0)
####################### State element 1 ###############################
# calculate distance to goal
goalDistance = np.sqrt((self.car.posX - self.goalX)**2 + (self.car.posY - self.goalY)**2)
# normalize goalDistance by maxGoalDistance to lie between [0.0,1.0]
goalDistance = (goalDistance)/(self.maxGoalDistance)
####################### State element 2 and 3 ###############################
# Estimate vector joining car position and goal position
xx = self.goalX - self.car.posX
yy = self.goalY - self.car.posY
angle = math.radians(self.car.angle)
# code taken from kivy vector.rotate (https://github.com/kivy/kivy/blob/420c8e2b8432d5363ed0662be7a649c2a8b86274/kivy/vector.py#L289)
# Estimate vector representing Car's pointing direction
carVecX = (self.car.velocityX * math.cos(angle)) - (self.car.velocityY * math.sin(angle))
carVecY = (self.car.velocityY * math.cos(angle)) + (self.car.velocityX * math.sin(angle))
# code taken from kivy vector.angle (https://github.com/kivy/kivy/blob/420c8e2b8432d5363ed0662be7a649c2a8b86274/kivy/vector.py#L289)
# Estimate angle between vectors calculated above
orientation = -(180 / math.pi) * math.atan2(
carVecX * yy - carVecY * xx,
carVecX * xx + carVecY * yy)
# normalize orientation to range [-1.0,1.0]
orientation /= 180
# Assemble the state list
state = [cropWithCar,goalDistance,-orientation,orientation]
return state