SE3 Pose Interp - Interpolate camera pose or trajectory in SE3, pose interpolation, trajectory interpolation

Related tags

Deep Learning SE3_Pose_Interp
Overview

SE3 Pose Interpolation

Pose estimated from SLAM system are always discrete, and often not equal to the original sequence frame size.

This repo helps to remedy it and interpolate the pose for any interval timestamp you want.

p_interp_demo

Dependencies & Environment

The repo has minimal requirement:

python==3.7
numpy==1.19
transformations==2021.6.6
evo==v1.13.5

How to Run

The script takes two files as input data, keyframe pose and lookup timestamps, the lookup timestamps contains much more timestamps data than keyframe sequences.

To run this script simply try:

python pose_interp.py --kf_pose ./data/kf_pose_result_tum.txt \
                      --timestamps ./data/timestamps.txt

The output file will be saved at the same directory with extra suffix _interp.txt

File format

Please make sure the estimated key-frame pose file (e.g.: ./data/kf_pose_result_tum.txt) is in TUM format:

timestamp t_x t_y t_z q_x q_y q_z q_w

The timestamps file for all frames (e.g.: ./data/timestamps.txt) is saved as following:

sequence_id timestamp

The output interpolated pose file which contains pose for each timestamp of every frame in the original sequence (e.g.: ./data/kf_pose_result_tum_interp.txt) is also in TUM format:

timestamp t_x t_y t_z q_x q_y q_z q_w

Visualization

We use evo to visualize the pose file, simply run the following code to get the plots

pose_interp

To run the visualization code, please try:

python pose_vis.py --kf_pose ./data/kf_pose_result_tum_vis.txt --full_pose ./data/kf_pose_result_tum_interp.txt

Please note that file kf_pose_result_tum_vis.txt is downsampled from original keyframe sequence kf_pose_result_tum_vis.txt for better visualization effect.

Disclaimer

This repo is adapted from https://github.com/ethz-asl/robotcar_tools/blob/master/python/interpolate_poses.py

The modification includes:

  • fixed axis align mis-match bug
  • add visualization for sanity check
  • consistent data format with clear comments
  • loop up any given interval timestamp

If you use part of this code please cite:

@software{cheng2022poseinterp,
  author = {Lisa, Mona and Bot, Hew},
  doi = {10.5281/zenodo.1234},
  month = {12},
  title = {{SE3 Pose Interpolation Toolbox}},
  url = {https://github.com/rancheng/se3_pose_interp},
  version = {1.0.0},
  year = {2022}
}

and

@article{RobotCarDatasetIJRR,
  Author = {Will Maddern and Geoff Pascoe and Chris Linegar and Paul Newman},
  Title = {{1 Year, 1000km: The Oxford RobotCar Dataset}},
  Journal = {The International Journal of Robotics Research (IJRR)},
  Volume = {36},
  Number = {1},
  Pages = {3-15},
  Year = {2017},
  doi = {10.1177/0278364916679498},
  URL =
{http://dx.doi.org/10.1177/0278364916679498},
  eprint =
{http://ijr.sagepub.com/content/early/2016/11/28/0278364916679498.full.pdf+html},
  Pdf = {http://robotcar-dataset.robots.ox.ac.uk/images/robotcar_ijrr.pdf}}

License

SE3_Pose_Interp is released under a MIT license (see LICENSE.txt)

If you use SE3_Pose_Interp in an academic work, please cite the most relevant publication associated by visiting: https://rancheng.github.io

You might also like...
Visualize Camera's Pose Using Extrinsic Parameter by Plotting Pyramid Model on 3D Space
Visualize Camera's Pose Using Extrinsic Parameter by Plotting Pyramid Model on 3D Space

extrinsic2pyramid Visualize Camera's Pose Using Extrinsic Parameter by Plotting Pyramid Model on 3D Space Intro A very simple and straightforward modu

JEONG HYEONJIN 106 Dec 28, 2022
Official PyTorch implementation of
Official PyTorch implementation of "Camera Distance-aware Top-down Approach for 3D Multi-person Pose Estimation from a Single RGB Image", ICCV 2019

PoseNet of "Camera Distance-aware Top-down Approach for 3D Multi-person Pose Estimation from a Single RGB Image" Introduction This repo is official Py

Gyeongsik Moon 677 Dec 25, 2022
PoseViz – Multi-person, multi-camera 3D human pose visualization tool built using Mayavi.
PoseViz – Multi-person, multi-camera 3D human pose visualization tool built using Mayavi.

PoseViz – 3D Human Pose Visualizer Multi-person, multi-camera 3D human pose visualization tool built using Mayavi. As used in MeTRAbs visualizations.

István Sárándi 79 Dec 30, 2022
Towards Multi-Camera 3D Human Pose Estimation in Wild Environment
Towards Multi-Camera 3D Human Pose Estimation in Wild Environment

PanopticStudio Toolbox This repository has a toolbox to download, process, and visualize the Panoptic Studio (Panoptic) data. Note: Sep-21-2020: Curre

null 335 Jan 9, 2023
Create animations for the optimization trajectory of neural nets
Create animations for the optimization trajectory of neural nets

Animating the Optimization Trajectory of Neural Nets loss-landscape-anim lets you create animated optimization path in a 2D slice of the loss landscap

Logan Yang 81 Dec 25, 2022
Code + pre-trained models for the paper Keeping Your Eye on the Ball Trajectory Attention in Video Transformers

Motionformer This is an official pytorch implementation of paper Keeping Your Eye on the Ball: Trajectory Attention in Video Transformers. In this rep

Facebook Research 192 Dec 23, 2022
Learning trajectory representations using self-supervision and programmatic supervision.
Learning trajectory representations using self-supervision and programmatic supervision.

Trajectory Embedding for Behavior Analysis (TREBA) Implementation from the paper: Jennifer J. Sun, Ann Kennedy, Eric Zhan, David J. Anderson, Yisong Y

null 58 Jan 6, 2023
This is the codebase for the ICLR 2021 paper Trajectory Prediction using Equivariant Continuous Convolution
This is the codebase for the ICLR 2021 paper Trajectory Prediction using Equivariant Continuous Convolution

Trajectory Prediction using Equivariant Continuous Convolution (ECCO) This is the codebase for the ICLR 2021 paper Trajectory Prediction using Equivar

Spatiotemporal Machine Learning 45 Jul 22, 2022
This is the official code of our paper
This is the official code of our paper "Diversity-based Trajectory and Goal Selection with Hindsight Experience Relay" (PRICAI 2021)

Diversity-based Trajectory and Goal Selection with Hindsight Experience Replay This is the official implementation of our paper "Diversity-based Traje

Tianhong Dai 6 Jul 18, 2022
Comments
  • a bug in pose interpolation

    a bug in pose interpolation

    Thank you for these scripts. I use these to interpolate pose as data processing before evo. Here is my modifications.

    diff --git a/pose_interp.py b/pose_interp.py
index 5433111..51c978f 100644
--- a/pose_interp.py
+++ b/pose_interp.py
@@ -47,9 +47,7 @@ def interpolate_poses(pose_timestamps, abs_poses, requested_timestamps, origin_t
     for i, pose in enumerate(abs_poses):
         if i > 0 and pose_timestamps[i - 1] >= pose_timestamps[i]:
             raise ValueError('Pose timestamps must be in ascending order')
-
-        abs_quaternions[:, i] = pose[
-                                3:]  # np.roll(pose[3:], -1) uncomment this if the quaternion is saved as [w, x, y, z]
+        abs_quaternions[:, i] = np.roll(pose[3:], 1)  # convert [x y z w] to [w x y z]
         abs_positions[:, i] = pose[:3]
 
     upper_indices = [bisect.bisect(pose_timestamps, pt) for pt in requested_timestamps]
@@ -58,7 +56,7 @@ def interpolate_poses(pose_timestamps, abs_poses, requested_timestamps, origin_t
     if max(upper_indices) >= len(pose_timestamps):
         upper_indices = [min(i, len(pose_timestamps) - 1) for i in upper_indices]
 
-    fractions = (requested_timestamps - pose_timestamps[lower_indices]) // \
+    fractions = (requested_timestamps - pose_timestamps[lower_indices]) / \
                 (pose_timestamps[upper_indices] - pose_timestamps[lower_indices])
 
     quaternions_lower = abs_quaternions[:, lower_indices]
@@ -120,19 +118,15 @@ def interpolate_poses(pose_timestamps, abs_poses, requested_timestamps, origin_t
     poses_mat[0:3, 3::4] = positions_interp
     poses_mat[3, 3::4] = 1
 
-    poses_mat = np.linalg.solve(poses_mat[0:4, 0:4], poses_mat)
+    # poses_mat = np.linalg.solve(poses_mat[0:4, 0:4], poses_mat)
 
     poses_out = [0] * (len(requested_timestamps) - 1)
     for i in range(1, len(requested_timestamps)):
         pose_mat = poses_mat[0:4, i * 4:(i + 1) * 4]
-        pose_rot = pose_mat.copy()
-        pose_rot[:3, -1] = 0
-        pose_rot[-1, :3] = 0
         pose_position = pose_mat[:3, -1]
-        pose_quaternion = tfs.quaternion_from_matrix(pose_rot, isprecise=True)  # [w x y z]
-        poses_out[i - 1] = [requested_timestamps[i] / 1e6, -pose_position[0], -pose_position[1], pose_position[2],
-                            -pose_quaternion[3], -pose_quaternion[2], pose_quaternion[1], pose_quaternion[0]]
-        # poses_out[i - 1] = poses_mat[0:4, i * 4:(i + 1) * 4]
+        pose_quaternion = tfs.quaternion_from_matrix(pose_mat, isprecise=True)  # [w x y z]
+        poses_out[i - 1] = [requested_timestamps[i] / 1e6, pose_position[0], pose_position[1], pose_position[2],
+                            pose_quaternion[1], pose_quaternion[2], pose_quaternion[3], pose_quaternion[0]]
 
     return poses_out
    opened by BBin1103 0
Owner
Ran Cheng
Robotics, Vision, Learning
Ran Cheng
GitHub
A Planar RGB-D SLAM which utilizes Manhattan World structure to provide optimal camera pose trajectory while also providing a sparse reconstruction containing points, lines and planes, and a dense surfel-based reconstruction.

ManhattanSLAM Authors: Raza Yunus, Yanyan Li and Federico Tombari ManhattanSLAM is a real-time SLAM library for RGB-D cameras that computes the camera

null 117 Dec 28, 2022
Implementation of SE3-Transformers for Equivariant Self-Attention, in Pytorch.

SE3 Transformer - Pytorch Implementation of SE3-Transformers for Equivariant Self-Attention, in Pytorch. May be needed for replicating Alphafold2 resu

Phil Wang 207 Dec 23, 2022
Pip-package for trajectory benchmarking from "Be your own Benchmark: No-Reference Trajectory Metric on Registered Point Clouds", ECMR'21

Map Metrics for Trajectory Quality Map metrics toolkit provides a set of metrics to quantitatively evaluate trajectory quality via estimating consiste

Mobile Robotics Lab. at Skoltech 31 Oct 28, 2022
Make a Turtlebot3 follow a figure 8 trajectory and create a robot arm and make it follow a trajectory

HW2 - ME 495 Overview Part 1: Makes the robot move in a figure 8 shape. The robot starts moving when launched on a real turtlebot3 and can be paused a

Devesh Bhura 0 Oct 21, 2022
Trajectory Extraction of road users via Traffic Camera

Traffic Monitoring Citation The associated paper for this project will be published here as soon as possible. When using this software, please cite th

Julian Strosahl 14 Dec 17, 2022
Camera-caps - Examine the camera capabilities for V4l2 cameras

camera-caps This is a graphical user interface over the v4l2-ctl command line to

Jetsonhacks 25 Dec 26, 2022
A selection of State Of The Art research papers (and code) on human locomotion (pose + trajectory) prediction (forecasting)

A selection of State Of The Art research papers (and code) on human trajectory prediction (forecasting). Papers marked with [W] are workshop papers.

Karttikeya Manglam 40 Nov 18, 2022
Back to the Feature: Learning Robust Camera Localization from Pixels to Pose (CVPR 2021)

Back to the Feature with PixLoc We introduce PixLoc, a neural network for end-to-end learning of camera localization from an image and a 3D model via

Computer Vision and Geometry Lab 610 Jan 5, 2023
Neural Reprojection Error: Merging Feature Learning and Camera Pose Estimation

Neural Reprojection Error: Merging Feature Learning and Camera Pose Estimation This is the official repository for our paper Neural Reprojection Error

Hugo Germain 78 Dec 1, 2022
Camera calibration & 3D pose estimation tools for AcinoSet

AcinoSet: A 3D Pose Estimation Dataset and Baseline Models for Cheetahs in the Wild Daniel Joska, Liam Clark, Naoya Muramatsu, Ricardo Jericevich, Fre

African Robotics Unit 42 Nov 16, 2022