持續創作，加速成長！這是我參與「掘金日新計劃 · 6 月更文挑戰」的第14天，點擊查看活動詳情

 

本次內容主要是上週內容的延續，主要畫出kitti車的行駛的軌跡

同樣的，我們先來看看最終實現的效果：

視頻

  接下來就進入一步步的編碼環節。。。

       

1、利用IMU、GPS計算汽車移動距離和旋轉角度

• 計算移動距離

• 通過GPS計算

  ```python

  定義計算GPS距離方法

  def computer_great_circle_distance(lat1,lon1,lat2,lon2): delta_sigma = float(np.sin(lat1np.pi/180)np.sin(lat2np.pi/180)+\ np.cos(lat1np.pi/180)np.cos(lat2np.pi/180)np.cos(lon1np.pi/180-lon2np.pi/180)) return 6371000.0np.arccos(np.clip(delta_sigma,-1,1))

  使用GPS計算距離

  gps_distance += [computer_great_circle_distance(imu_data.lat,imu_data.lon,prev_imu_data.lat,prev_imu_data.lon)]

  ```

• 通過IMU計算

  ```python IMU_COLUMN_NAMES = ['lat','lon','alt','roll','pitch','yaw','vn','ve','vf','vl','vu','ax','ay','az','af', 'al','au','wx','wy','wz','wf','wl','wu','posacc','velacc','navstat','numsats','posmode', 'velmode','orimode']

  獲取IMU數據

  imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)

  使用IMU計算距離

  imu_distance += [0.1*np.linalg.norm(imu_data[['vf','vl']])] ```

• 比較兩種方式計算出的距離（GPS/IMU）

  python import matplotlib.pyplot as plt plt.figure(figsize=(20,10)) plt.plot(gps_distance, label='gps_distance') plt.plot(imu_distance, label='imu_distance') plt.legend() plt.show()

顯然，IMU計算的距離較為平滑。

   

• 計算旋轉角度 旋轉角度的計算較為簡單，我們只需要根據IMU獲取到的yaw值就可以計算（前後兩幀圖像的yaw值相減）

   

2、畫出kitti車的行駛軌跡

```python prev_imu_data = None locations = []

for frame in range(150): imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)

if prev_imu_data is not None:
    displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])
    yaw_change = float(imu_data.yaw-prev_imu_data.yaw)
    for i in range(len(locations)):
        x0, y0 = locations[i]
        x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
        y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
        locations[i] = np.array([x1,y1])

locations += [np.array([0,0])]           
prev_imu_data =imu_data

plt.figure(figsize=(20,10)) plt.plot(np.array(locations)[:, 0],np.array(locations)[:, 1]) ```

   

3、畫出所有車輛的軌跡

```python class Object(): def init(self, center): self.locations = deque(maxlen=20) self.locations.appendleft(center)

def update(self, center, displacement, yaw):
    for i in range(len(self.locations)):
        x0, y0 = self.locations[i]
        x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
        y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
        self.locations[i] = np.array([x1,y1])

    if center is not None:    
        self.locations.appendleft(center)


def reset(self):
    self.locations = deque(maxlen=20)

創建發佈者

loc_pub = rospy.Publisher('kitti_loc', MarkerArray, queue_size=10)

#獲取距離和旋轉角度 imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)

    if prev_imu_data is None:
        for track_id in centers:
            tracker[track_id] = Object(centers[track_id])
    else:
        displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])
        yaw_change = float(imu_data.yaw - prev_imu_data.yaw)

        for track_id in centers: # for one frame id 
            if track_id in tracker:
                tracker[track_id].update(centers[track_id], displacement, yaw_change)
            else:
                tracker[track_id] = Object(centers[track_id])
        for track_id in tracker:# for whole ids tracked by prev frame,but current frame did not
            if track_id not in centers: # dont know its center pos
                tracker[track_id].update(None, displacement, yaw_change)


    prev_imu_data = imu_data

def publish_loc(loc_pub, tracker, centers): marker_array = MarkerArray()

for track_id in centers:
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()


    marker.action = marker.ADD
    marker.lifetime = rospy.Duration(LIFETIME)
    marker.type = Marker.LINE_STRIP
    marker.id = track_id

    marker.color.r = 1.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 1.0
    marker.scale.x = 0.2


    marker.points = []
    for p in tracker[track_id].locations:
        marker.points.append(Point(p[0], p[1], 0))

    marker_array.markers.append(marker)

loc_pub.publish(marker_array)

```

       

咻咻咻咻~~duang\~~點個讚唄

上一篇：使用kitti數據集實現自動駕駛——發佈照片、點雲、IMU、GPS、顯示2D和3D偵測框