Robotics Software engineer編程筆記（一）

使用Udacity提供的漫游者號模擬器創建環境地圖，尋找樣本。

該項目是根據美國國家航空航天局（NASA）的樣本返回挑戰進行建模的。我使用的windows平臺下的模擬器，也是基于windows平臺下的模擬器進行學習。

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

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

一、圖像二值化

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

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

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（）函數后面加上show（）函數

二、圖像的透視變換

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

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

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

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

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

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

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

3. figure語法及操作

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

(2)例子:

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

相關程序

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)

三、以漫游者號為中心的坐標

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

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

2.坐標轉換

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

# 定義從圖像坐標轉換函數
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# 定義二值化圖像函數
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# 定義圖像映射函數，將攝像頭的圖像映射到平面坐標中去
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# 定義從圖像坐標轉換函數
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)# 函數參數定義部分
# 映射的圖片的一半邊長
dst_size = 5
# 映射的點距離x軸的距離
bottom_offset = 0
# 將圖像二值化
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_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()

其輸出為

四、映射到世界坐標

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

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

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

1.矩陣的旋轉操作

這是轉移矩陣的定義，通過這個矩陣我們可以得到

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

其代碼如下：

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.矩陣的范圍限制

為了將坐標限制在我們200*200的范圍內，我們使用Numpy.clip函數

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

解釋：
clip這個函數將a數組中的元素限制在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為轉向角信息
（4）最終程序

import matplotlib.pyplot as plt
import numpy as np
import cv2 as cv# 定義二值化圖像函數
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# 定義圖像映射函數，將攝像頭的圖像映射到平面坐標中去
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# 定義從圖像坐標轉換函數
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# 定義旋轉操作函數
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# 定義平移操作函數
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# 定義綜合函數，將旋轉和平移函數進行結合，并限制了圖像范圍
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)# 隨機生成漫游者坐標和角度
rover_yaw = np.random.random(1)*360
rover_xpos = np.random.random(1)*160 + 20
rover_ypos = np.random.random(1)*160 + 20# 函數參數定義部分
# 映射的圖片的一半邊長
dst_size = 5
# 映射的點距離x軸的距離
bottom_offset = 0
# 將圖像二值化
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_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()

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。