Discover Euler angle vector from 3x3 matrix Uses the conventions above. Parameters ---------- M : array-like, shape (3,3) cy_th

filter points based on h,v FOV

Return quaternion corresponding to these Euler angles Uses the z, then y, then x convention above Parameters ---------- z : scalar

Return quaternion corresponding to these Euler angles Uses the z, then y, then x convention above Parameters ---------- z : scalar

extract horizontal in-range points

extract vertical in-range points

Discover Euler angle vector from 3x3 matrix Uses the conventions above. Parameters ---------- M : array-like, shape (3,3) cy_th

Reads a trajectory from a text file. File format: The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be mat

Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. Input: l -- tuple consisting of (stamp,tx,ty,tz,qx

Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. Input: l -- tuple consisting of (stamp,tx,ty,tz,qx

Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim to find the closest match for every input tuple.

Return matrix for rotations around z, y and x axes Uses the z, then y, then x convention above Parameters ---------- z : scalar

print 'nomalized depth value' nomalize values to 0-255 & close distance value has high value. (similar to stereo vision's disparity map)

print 'nomalized depth value' nomalize values to 0-255 & close distance value has low value.

Convert angle, axis pair to Euler angles Parameters ---------- theta : scalar angle of rotation vector : 3 element sequence

Convert angle, axis pair to Euler angles Parameters ---------- theta : scalar angle of rotation vector : 3 element sequence

Return angle, axis corresponding to these Euler angles Uses the z, then y, then x convention above Parameters ---------- z : scalar

Return angle, axis corresponding to these Euler angles Uses the z, then y, then x convention above Parameters ---------- z : scalar

Return matrix for rotations around z, y and x axes Uses the z, then y, then x convention above Parameters ---------- z : scalar

iround(number) -> integer Round a number to the nearest integer.

Return Euler angles corresponding to quaternion `q` Parameters ---------- q : 4 element sequence w, x, y, z of quaternion Re

Return Euler angles corresponding to quaternion `q` Parameters ---------- q : 4 element sequence w, x, y, z of quaternion Re