Robotics Software engineer編程筆記（一）

使用Udacity提供的漫游者號模擬器創(chuàng)建環(huán)境地圖，尋找樣本。

該項目是根據(jù)美國國家航空航天局（NASA）的樣本返回挑戰(zhàn)進行建模的。我使用的windows平臺下的模擬器，也是基于windows平臺下的模擬器進行學(xué)習(xí)。

所使用的環(huán)境是python3.6， IDE為pycharm 2019。
下面提供其他兩種平臺的模擬器的下載鏈接。
Linux模擬器
Windows模擬器

這是模擬器主界面，分為訓(xùn)練模式和自動模式。關(guān)于漫游者號的訓(xùn)練以及訓(xùn)練參數(shù)的保存可以參考我的上一篇文章使用Keras訓(xùn)練自動駕駛（使用Udacity自動駕駛模擬器），關(guān)于漫游者號模型的保存與之類似，我就不詳細(xì)記錄了。

一、圖像二值化

在這里我使用了cv.cvtColor(image, cv.COLOR_BGR2GRAY)將輸入的image轉(zhuǎn)換成灰度圖像和cv.threshold(image_gray, thresh, 255, cv.THRESH_BINARY)函數(shù)將之前所得到的灰度圖像進行二值化處理。

下面是所定義的二值化函數(shù)，image為輸入的圖片，函數(shù)的默認(rèn)輸入閾值為200。其中cv.threshhold函數(shù)返回兩個值，第一個為閾值，第二個為輸出的圖像。

import cv2 as cv
…
def color_thresh(image, thresh=200):image_gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)ret, image_thresh = cv.threshold(image_gray, thresh, 255，cv.THRESH_BINARY)
return image_thresh
…

這兩個圖分別用opencv和matplotlib輸出.
注意：

1. 在使用opencv時，在程序末尾加上cv.waitKey().
2. 使用matplotlib時，在imshow（）函數(shù)后面加上show（）函數(shù)

二、圖像的透視變換

(Perspective Transformation)是將成像投影到一個新的視平面(Viewing Plane)，也稱作投影映射(Projective Mapping)。

1. M = cv2.getPerspectiveTransform(src, dst)

參數(shù)說明
src：源圖像中待測矩形的四點坐標(biāo)
sdt：目標(biāo)圖像中矩形的四點坐標(biāo) 返回由源圖像中矩形到目標(biāo)圖像矩形變換的矩陣M

2. dst = cv2.warpPerspective(src, M, dsize[, dst[, flags[, borderMode[, borderValue]]]])

參數(shù)說明：
src：輸入圖像
M：變換矩陣
dsize：目標(biāo)圖像shape
flags：插值方式，interpolation方法INTER_LINEAR或INTER_NEAREST
borderMode：邊界補償方式，BORDER_CONSTANT or BORDER_REPLICATE
borderValue：邊界補償大小，常值，默認(rèn)為0

或者 dst = cv2.perspectiveTransform(src, m[, dst])
參數(shù)
src：輸入的2通道或者3通道的圖片
m：變換矩陣
返回的是相同size的圖片

區(qū)別是：warpPerspective適用于圖像，而perspectiveTransform適用于一組點。

3. figure語法及操作

(1) figure(num=None, figsize=None, dpi=None, facecolor=None, edgecolor=None, frameon=True)
參數(shù)說明：
num:圖像編號或名稱，數(shù)字為編號 ，字符串為名稱
figsize:指定figure的寬和高，單位為英寸；
dpi參數(shù)指定繪圖對象的分辨率，即每英寸多少個像素，缺省值為80 1英寸等于2.5cm,A4紙是 21*30cm的紙張
facecolor:背景顏色
edgecolor:邊框顏色
frameon:是否顯示邊框

(2)例子:

import matplotlib.pyplot as plt
# 創(chuàng)建自定義圖像
fig=plt.figure(figsize=(4,3),facecolor='blue')
plt.show()

相關(guān)程序

def perspect_transform(img, src, dst):M = cv.getPerspectiveTransform(src, dst)img_perspect = cv.warpPerspective(img, M, (img.shape[1], img.shape[0]))return img_perspect
…
dst_size = 5
bottom_offset = 6
image_thresh = color_thresh(image)
src = np.float32([[14, 140], [301 ,140],[200, 96], [118, 96]])
dst = np.float32([[image.shape[1]/2 - dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - 2*dst_size - bottom_offset],[image.shape[1]/2 - dst_size, image.shape[0] - 2*dst_size - bottom_offset],])
image_prespect = perspect_transform(image, src, dst)
cv.imshow('image_prespect', image_prespect)

三、以漫游者號為中心的坐標(biāo)

1.獲取圖像中非零像素點的位置，返回x，y坐標(biāo)

ypos, xpos = colorsel.nonzero()
plt.plot(xpos, ypos, '.')
plt.xlim(0, 320)
plt.ylim(0, 160)
plt.show()

2.坐標(biāo)轉(zhuǎn)換

定義一個函數(shù)來接收二進制圖像，提取圖像坐標(biāo)中的x和y位置，然后在漫游者號坐標(biāo)中返回x和y。

# 定義從圖像坐標(biāo)轉(zhuǎn)換函數(shù)
def rover_coords(binary_img):ypos, xpos = binary_img.nonzero()x_pixel = -(ypos - binary_img.shape[0]).astype(np.float)y_pixel = -(xpos - binary_img.shape[1]/2 ).astype(np.float)return x_pixel, y_pixel

3.綜合程序

import matplotlib.pyplot as plt
import numpy as np
import cv2 as cv# 定義二值化圖像函數(shù)
def color_thresh(img, thresh=200):img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)ret, img_thresh = cv.threshold(img_gray, thresh, 255, cv.THRESH_BINARY)return img_thresh# 定義圖像映射函數(shù)，將攝像頭的圖像映射到平面坐標(biāo)中去
def perspect_transform(img, src, dst):M = cv.getPerspectiveTransform(src, dst)  # 定義變換矩陣img_perspect = cv.warpPerspective(img, M, (img.shape[1], img.shape[0]))return img_perspect# 定義從圖像坐標(biāo)轉(zhuǎn)換函數(shù)
def rover_coords(binary_img):ypos, xpos = binary_img.nonzero()x_pixel = -(ypos - binary_img.shape[0]).astype(np.float)y_pixel = -(xpos - binary_img.shape[1]/2 ).astype(np.float)return x_pixel, y_pixel# Define the filename, read and plot the image
filename = '.../sample.jpg'
image = cv.imread(filename)# 函數(shù)參數(shù)定義部分
# 映射的圖片的一半邊長
dst_size = 5
# 映射的點距離x軸的距離
bottom_offset = 0
# 將圖像二值化
image_thresh = color_thresh(image)
# 定義原圖像空間坐標(biāo)和映射圖像空間坐標(biāo)
src = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]])
dst = np.float32([[image.shape[1]/2 - dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - 2*dst_size - bottom_offset],[image.shape[1]/2 - dst_size, image.shape[0] - 2*dst_size - bottom_offset],])
# 映射圖像
image_prespect = perspect_transform(image_thresh, src, dst)
# 在二值化圖像尋找非零點
xpix, ypix = rover_coords(image_prespect)
fig = plt.figure(figsize=(5, 7.5))
plt.plot(xpix, ypix, '.')
plt.ylim(-160, 160)
plt.xlim(0, 160)# 輸出部分
plt.title('Rover-Centric Map', fontsize=20)
plt.show()
cv.imshow('image_prespect', image_prespect)
cv.imshow('thresh', image_thresh)
cv.waitKey()

其輸出為

四、映射到世界坐標(biāo)

我們使用的漫游者號地圖尺寸大概為200m×200m，我們將要建立一個分辨率為200×200像素的俯視圖，其中一個像素代表了一平方米。

現(xiàn)在已經(jīng)生成了以了漫游者號為中心的可導(dǎo)航地形的地圖，下一步是將這些點映射到世界坐標(biāo)。我們需要將矩陣旋轉(zhuǎn)后跟平移。旋轉(zhuǎn)是這樣理解的：當(dāng)攝像機拍照時，漫游車可以指向任意方向，由它的偏航角給出。平移是為了說明漫游者在拍照時可能位于世界的任何位置。所以，這是一個兩步過程：

1. 旋轉(zhuǎn)以漫游者號為中心的坐標(biāo)，使x軸和y軸平行于世界空間中的軸。
2. 通過世界中漫游者號的位置（位置矢量）給出的x和y位置值來轉(zhuǎn)換旋轉(zhuǎn)的位置。從正x方向逆時針測量偏航角，如下所示：

1.矩陣的旋轉(zhuǎn)操作

這是轉(zhuǎn)移矩陣的定義，通過這個矩陣我們可以得到

通過這個公式我們可以得到漫游者號在實際中的坐標(biāo)位置

其代碼如下：

def rotate_pix(xpix, ypix, yaw):yaw_rad = yaw * np.pi / 180xpix_rotated = (xpix * np.cos(yaw_rad)) - (ypix * np.sin(yaw_rad))ypix_rotated = (xpix * np.sin(yaw_rad)) + (ypix * np.cos(yaw_rad))return xpix_rotated, ypix_rotated

2.矩陣的平移操作

def translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale):xpix_translated = (xpix_rot / scale) + xposypix_translated = (ypix_rot / scale) + yposreturn xpix_translated, ypix_translated

3.矩陣的范圍限制

為了將坐標(biāo)限制在我們200*200的范圍內(nèi)，我們使用Numpy.clip函數(shù)

numpy.clip(a, a_min, a_max, out=None)

解釋：
clip這個函數(shù)將a數(shù)組中的元素限制在a_min, a_max之間，大于a_max的就使得它等于 a_max，小于a_min,的就使得它等于a_min。

def pix_to_world(xpix, ypix, xpos, ypos, yaw, world_size, scale):xpix_rot, ypix_rot = rotate_pix(xpix, ypix, yaw)xpix_tran, ypix_tran = translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale)x_pix_world = np.clip(np.int_(xpix_tran), 0, world_size - 1)y_pix_world = np.clip(np.int_(ypix_tran), 0, world_size - 1)return x_pix_world, y_pix_world

其中xpix，ypix為輸入的漫游者號的位置信息，yaw為轉(zhuǎn)向角信息
（4）最終程序

import matplotlib.pyplot as plt
import numpy as np
import cv2 as cv# 定義二值化圖像函數(shù)
def color_thresh(img, thresh=200):img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)ret, img_thresh = cv.threshold(img_gray, thresh, 255, cv.THRESH_BINARY)return img_thresh# 定義圖像映射函數(shù)，將攝像頭的圖像映射到平面坐標(biāo)中去
def perspect_transform(img, src, dst):M = cv.getPerspectiveTransform(src, dst)  # 定義變換矩陣img_perspect = cv.warpPerspective(img, M, (img.shape[1], img.shape[0]))return img_perspect# 定義從圖像坐標(biāo)轉(zhuǎn)換函數(shù)
def rover_coords(binary_img):ypos, xpos = binary_img.nonzero()x_pixel = -(ypos - binary_img.shape[0]).astype(np.float)y_pixel = -(xpos - binary_img.shape[1]/2 ).astype(np.float)return x_pixel, y_pixel# 定義旋轉(zhuǎn)操作函數(shù)
def rotate_pix(xpix, ypix, yaw):yaw_rad = yaw * np.pi / 180xpix_rotated = (xpix * np.cos(yaw_rad)) - (ypix * np.sin(yaw_rad))ypix_rotated = (xpix * np.sin(yaw_rad)) + (ypix * np.cos(yaw_rad))return xpix_rotated, ypix_rotated# 定義平移操作函數(shù)
def translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale):xpix_translated = (xpix_rot / scale) + xposypix_translated = (ypix_rot / scale) + yposreturn xpix_translated, ypix_translated# 定義綜合函數(shù)，將旋轉(zhuǎn)和平移函數(shù)進行結(jié)合，并限制了圖像范圍
def pix_to_world(xpix, ypix, xpos, ypos, yaw, world_size, scale):xpix_rot, ypix_rot = rotate_pix(xpix, ypix, yaw)xpix_tran, ypix_tran = translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale)x_pix_world = np.clip(np.int_(xpix_tran), 0, world_size - 1)y_pix_world = np.clip(np.int_(ypix_tran), 0, world_size - 1)return x_pix_world, y_pix_world# Define the filename, read and plot the image
filename = '.../sample1.jpg'
image = cv.imread(filename)# 隨機生成漫游者坐標(biāo)和角度
rover_yaw = np.random.random(1)*360
rover_xpos = np.random.random(1)*160 + 20
rover_ypos = np.random.random(1)*160 + 20# 函數(shù)參數(shù)定義部分
# 映射的圖片的一半邊長
dst_size = 5
# 映射的點距離x軸的距離
bottom_offset = 0
# 將圖像二值化
image_thresh = color_thresh(image)
# 定義原圖像空間坐標(biāo)和映射圖像空間坐標(biāo)
src = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]])
dst = np.float32([[image.shape[1]/2 - dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - 2*dst_size - bottom_offset],[image.shape[1]/2 - dst_size, image.shape[0] - 2*dst_size - bottom_offset],])
# 映射圖像
image_prespect = perspect_transform(image_thresh, src, dst)
# 在二值化圖像尋找非零點
xpix, ypix = rover_coords(image_prespect)# 建立地圖
worldmap = np.zeros((200, 200))
# 我們之前的圖像是以10個像素為一個平方米，為了映射到世界地圖上，我們需要定義scale
scale = 10
x_world, y_world = pix_to_world(xpix, ypix, rover_xpos, rover_ypos, rover_yaw, worldmap.shape[0], scale)
worldmap[y_world, x_world] += 1fig = plt.figure(figsize=(5, 7.5))
plt.plot(x_world, y_world, 'w.')
plt.ylim(0, 200)
plt.xlim(0, 200)
plt.imshow(worldmap, cmap='gray')# 輸出部分
plt.title('World Space', fontsize=20)
plt.show()
cv.imshow('image_prespect', image_prespect)
cv.imshow('thresh', image_thresh)
cv.waitKey()

因為我們沒有讀取數(shù)據(jù)中的漫游者位置和轉(zhuǎn)向角，因此在這個程序中位置和轉(zhuǎn)向角是隨機生成的。所以每次生成的圖像都不會一樣。

總結(jié)

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（一）-样本搜索和找回-基于漫游者号模拟器的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。