Project to develop a software package to derive a 6 DoF pose of the HTC Vive Tracker.
Final GitHub Repo: vive_tracker_loc
ROS Package Version: vive_tracker_loc ‘cmake_convert’ branch
Summary
The goal of this project was to develop a software package to derive the pose of the HTC Vive Tracker. This software package needed to be capable of running on a Raspberry Pi 2 and also only require the Vive Tracker and a Lighthouse to work. This project was proposed by Professor Michael Rubenstein and is intended to be an add-on to his course focused on the creation of a quadrotor from start to finish. He wants to be able to use the pose estimate derived from the Vive Tracker to perform control of the quadrotor.
Background
The HTC Vive system consists of a main headset unit, two lighthouses, and two controllers. The lighthouses are the core element of this system because they track the other components. Each lighthouse sends out sweeps and pulses of infrared light. The vive devices (e.g. the headset, controllers, and tracker) are all equipped with IR sensors which detect when they received each sweep of light, the duration, and the timestamp associated with the sweep and from that information can derive the horizontal and vertical angles of each sensor relative to the lighthouse (more info here). HTC also released a set of sensors in a smaller package known as the Vive Tracker. The Vive Tracker is what I used for my project.
Approach
Initial Efforts
My initial approach involved using SteamVR and OpenVR to get the pose of the Vive Tracker. However, after Professor Rubenstein made it clear he needed this to work on only a Raspberry Pi, I shifted my efforts toward a more custom version.
Libsurvive
After conducting some research to try and find a software package which could interface with the HTC Vive, I found libsurvive. Libsurvive is a project which aims to reverse engineer the protocol behind the HTC Vive system in order to create a custom VR system built around a lightweight C library. I talked to some of the developers on Discord about the state of the project and they helped me get the software set up. They told me they were working on localization but had not gotten anything working well yet. However, though they had not gotten localization working, they had a way to extract the angle of each sensor on any of the Vive devices to the lighthouses, which is all I needed to determine the pose.
Perspective n-Point Algorithm
Now that I had a working piece of software to determine the angle from the lighthouse to each sensor on the tracker, I needed a way to determine the pose from this data. I conducted some research and found out that HTC uses an algorithm called Perspective n-Point (PnP) to solve this problem. I also found out that OpenCV has an implementation of this algorithm which is used for extrinsic camera calibration. With the help of one of the directors of the program, Jarvis Schultz, I was able to get the OpenCV version working with the data I had. The main challenge during this portion of the project was getting C++ code needed for OpenCV to work with the existing C code used in Libsurvive. After rewriting portions of the Makefile and modifying some of the struct definitions, I was able to pass in a set of angles to the OpenCV code (opencv_pose_calc.cpp) and return back an SE3 tranformation matrix from the lighthouse to the tracker.
Executable
After determining the pose using the PnP algorithm, I went back and rewrote the executable to return the current position and quaternion of the tracker relative to the lighthouse. The final executable is on the master branch of the repository and is called runposer.c.
Extensions
ROS Package
Once I had a working version of my software package for pose estimation using only a lighthouse, a tracker, and a Raspberry Pi, I wanted to get a version of the software working with the Robot Operating System (ROS). This involved only a few modifications including adding the ability to use cmake for compilation, putting the package into a ROS workspace, and writing a node to publish pose data from the tracker. This edition of the software currently resides on the cmake_convert branch of the GitHub repo. Eventually, I would like to create a standalone ROS package that could be distributed for others to use.
Results
To test the accuracy of the pose estimation, I mounted the Vive Tracker to the end effector of the Sawyer robot we have in the MSR lab. Using the ROS package, I was able to perform a brief calibration to determine the location of the lighthouse relative to the base of Sawyer and then visualize the pose estimation of the tracker in Rviz. I then recorded four different movements of Sawyer’s arm and played them back to create four test runs. The video below shows both the Rviz visualization and the actual movement of Sawyer and the tracker. In the Rviz portion of the video, the two frames at the end of Sawyer’s arm represent the position and orientation of the tracker and Sawyer’s gripper (with the tracker offset slightly behind and above the gripper). The frame at the top of the Rviz portion of the video represents the lighthouse.
Figure 1: Sawyer setup with the Vive Tracker attached to its end effector
Figure 2: Close up image of the Vive Tracker mounted to Sawyer's end effector
Figure 3: Lighthouse setup, above Sawyer
Future Work
Below are a number of extensions to this project which I would like to implement in the future:
- Eliminate the need for OpenCV as a dependency. This would require a different implementation of the PnP algorithm.
- Contribute this pose estimation work back to libsurvive. I would likke to help them improve their software package if I can eliminate the need for OpenCV and make my code entirely C.
- Get pose estimation working with more than one lighthouse.
- Get pose estimation working with more than one tracker.
- Implement position control on a quadrotor using the tracker for position estimation.
- Build a custom tracker with a smaller footprint for use on vehicles such as a quadrotor.