t265_wheel_odometry.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 ## License: Apache 2.0. See LICENSE file in root directory.
4 ## Copyright(c) 2019 Intel Corporation. All Rights Reserved.
5 from __future__ import print_function
6 
7 """
8 This example shows how to fuse wheel odometry measurements (in the form of 3D translational velocity measurements) on the T265 tracking camera to use them together with the (internal) visual and intertial measurements.
9 This functionality makes use of two API calls:
10 1. Configuring the wheel odometry by providing a json calibration file (in the format of the accompanying calibration file)
11 Please refer to the description of the calibration file format here: https://github.com/IntelRealSense/librealsense/blob/master/doc/t265.md#wheel-odometry-calibration-file-format.
12 2. Sending wheel odometry measurements (for every measurement) to the camera
13 
14 Expected output:
15 For a static camera, the pose output is expected to move in the direction of the (artificial) wheel odometry measurements (taking into account the extrinsics in the calibration file).
16 The measurements are given a high weight/confidence, i.e. low measurement noise covariance, in the calibration file to make the effect visible.
17 If the camera is partially occluded the effect will be even more visible (also for a smaller wheel odometry confidence / higher measurement noise covariance) because of the lack of visual feedback. Please note that if the camera is *fully* occluded the pose estimation will switch to 3DOF, estimate only orientation, and prevent any changes in the position.
18 """
19 
20 import pyrealsense2 as rs
21 
22 # load wheel odometry config before pipe.start(...)
23 # get profile/device/ wheel odometry sensor by profile = cfg.resolve(pipe)
24 pipe = rs.pipeline()
25 cfg = rs.config()
26 profile = cfg.resolve(pipe)
27 dev = profile.get_device()
28 tm2 = dev.as_tm2()
29 
30 if(tm2):
31  # tm2.first_wheel_odometer()?
32  pose_sensor = tm2.first_pose_sensor()
33  wheel_odometer = pose_sensor.as_wheel_odometer()
34 
35  # calibration to list of uint8
36  f = open("calibration_odometry.json")
37  chars = []
38  for line in f:
39  for c in line:
40  chars.append(ord(c)) # char to uint8
41 
42  # load/configure wheel odometer
43  wheel_odometer.load_wheel_odometery_config(chars)
44 
45 
46  pipe.start()
47  try:
48  for _ in range(100):
49  frames = pipe.wait_for_frames()
50  pose = frames.get_pose_frame()
51  if pose:
52  data = pose.get_pose_data()
53  print("Frame #{}".format(pose.frame_number))
54  print("Position: {}".format(data.translation))
55 
56  # provide wheel odometry as vecocity measurement
57  wo_sensor_id = 0 # indexed from 0, match to order in calibration file
58  frame_num = 0 # not used
59  v = rs.vector()
60  v.x = 0.1 # m/s
61  wheel_odometer.send_wheel_odometry(wo_sensor_id, frame_num, v)
62  finally:
63  pipe.stop()
static std::string print(const transformation &tf)


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:50:11