[学习SLAM]SLAM 中evo的代码详解（三）

目录

associate.py 时间戳匹配

evaluate_ate.py

SVD分解

evaluate_rpe.py

transform44(l):四元数q转旋转矩阵R​

ominus(a,b)计算a和b之间的相对3d变换。

scale(a,scalar):计算尺度

compute_distance/compute_angle

contrib（TUM时间乘某数，并KITTI时间位姿为TUM ）

命令行界面 -asv详解

numpy.linalg.svd函数

---

associate.py 时间戳匹配

associate.py （evo与24讲一样）

"""
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.

For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
"""

    first_keys = first_list.keys()
    second_keys = second_list.keys()
    potential_matches = [(abs(a - (b + offset)), a, b) 
                         for a in first_keys 
                         for b in second_keys 
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
    for diff, a, b in potential_matches:
        if a in first_keys and b in second_keys:
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))
    
    matches.sort()
    return matches

evaluate_ate.py

"""
This script computes the absolute trajectory error from the ground truth
trajectory and the estimated trajectory.
"""

def align(model,data):
    """Align two trajectories using the method of Horn (closed-form).
    
    Input:
    model -- first trajectory (3xn)
    data -- second trajectory (3xn)
    
    Output:
    rot -- rotation matrix (3x3)
    trans -- translation vector (3x1)
    trans_error -- translational error per point (1xn)
    
    """
    numpy.set_printoptions(precision=3,suppress=True)
    model_zerocentered = model - model.mean(1)
    data_zerocentered = data - data.mean(1)
    
    W = numpy.zeros( (3,3) )
    for column in range(model.shape[1]):
        W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
    U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
    S = numpy.matrix(numpy.identity( 3 ))
    if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
        S[2,2] = -1
    rot = U*S*Vh
    trans = data.mean(1) - rot * model.mean(1)
    
    model_aligned = rot * model + trans
    alignment_error = model_aligned - data
    
    trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
        
    return rot,trans,trans_error

SVD分解

model_zerocentered = model - model.mean(1)
data_zerocentered = data - data.mean(1)

W = numpy.zeros( (3,3) )
for column in range(model.shape[1]):
    W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
S = numpy.matrix(numpy.identity( 3 ))
if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
    S[2,2] = -1
rot = U*S*Vh
trans = data.mean(1) - rot * model.mean(1)

model_aligned = rot * model + trans

python中numpy计算数组的行列式numpy.linalg.det()

if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
    S[2,2] = -1
rot = U*S*Vh

evaluate_rpe.py

transform44(l):四元数q转旋转矩阵R

def transform44(l):
    """
    Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion.
    
    Input:
    l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where
         (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion.
         
    Output:
    matrix -- 4x4 homogeneous transformation matrix
    """
    t = l[1:4]
    q = numpy.array(l[4:8], dtype=numpy.float64, copy=True)
    nq = numpy.dot(q, q)
    if nq < _EPS:
        return numpy.array((
        (                1.0,                 0.0,                 0.0, t[0])
        (                0.0,                 1.0,                 0.0, t[1])
        (                0.0,                 0.0,                 1.0, t[2])
        (                0.0,                 0.0,                 0.0, 1.0)
        ), dtype=numpy.float64)
    q *= numpy.sqrt(2.0 / nq)
    q = numpy.outer(q, q)
    return numpy.array((
        (1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3], t[0]),
        (    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3], t[1]),
        (    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], t[2]),
        (                0.0,                 0.0,                 0.0, 1.0)
        ), dtype=numpy.float64)

ominus(a,b)计算a和b之间的相对3d变换。

"""
This script computes the relative pose error from the ground truth trajectory
and the estimated trajectory.
"""

def ominus(a,b):
    """
    Compute the relative 3D transformation between a and b.
    
    Input:
    a -- first pose (homogeneous 4x4 matrix)
    b -- second pose (homogeneous 4x4 matrix)
    
    Output:
    Relative 3D transformation from a to b.
    """
    return numpy.dot(numpy.linalg.inv(a),b)

def ominus(a,b):

计算a和b之间的相对3d变换。
输入：
A——第一姿态（齐次4x4矩阵）
B——第二姿态（齐次4x4矩阵）
输出：
从a到b的相对3d转换。

scale(a,scalar):计算尺度

def scale(a,scalar):
    """
    Scale the translational components of a 4x4 homogeneous matrix by a scale factor.
    """
    return numpy.array(
        [[a[0,0], a[0,1], a[0,2], a[0,3]*scalar],
         [a[1,0], a[1,1], a[1,2], a[1,3]*scalar],
         [a[2,0], a[2,1], a[2,2], a[2,3]*scalar],
         [a[3,0], a[3,1], a[3,2], a[3,3]]]

compute_distance/compute_angle

def compute_distance(transform):
    """
    Compute the distance of the translational component of a 4x4 homogeneous matrix.
    """
    return numpy.linalg.norm(transform[0:3,3])

def compute_angle(transform):
    """
    Compute the rotation angle from a 4x4 homogeneous matrix.
    """
    # an invitation to 3-d vision, p 27
    return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) ))

# an invitation to 3-d vision, p 27
return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) )) 
#numpy.trace是求shape的对角线上的元素的和

#min /max 防止大于180，小于-180

contrib（TUM时间乘某数，并KITTI时间位姿为TUM ）

DESC = """rename the 'est_name' field in a result file"""

DESC = """Record a tf frame's trajectory to a geometry_msgs/PoseStamped bag."""

DESC = """multiply the timestamps of a TUM trajectory file by a factor"""

DESC = "Combine KITTI poses and timestamps files to a TUM trajectory file"

 

命令行界面 -asv详解

usability options:

-v, --verbose verbose output

algorithm options:

-a, --align alignment with Umeyama's method (no scale) - requires --ref

-s, --correct_scale scale correction with Umeyama's method - requires --re

 

补充：

numpy.linalg.svd函数

转载自：python之SVD函数介绍
函数：np.linalg.svd(a,full_matrices=1,compute_uv=1)
参数：

• a是一个形如(?,?)的矩阵

• full_matrices的取值为0或者1，默认值为1，这时u的大小为(?,?)返回值：总共有三个返回值u,s,v
• u大小为(?,?)，v的大小为(?,?) 。否则u的大小为(?,?)，v的大小为(?,?) ，?=???(?,?) ，s大小为(?,?)，v大小为(?,?)
• compute_uv的取值是为0或者1，默认值为1，表示计算u,s,v。为0的时候只计算s。
• ?=?∗?∗?其中s是对矩阵a的奇异值分解。s除了对角元素不为0，其他元素都为0，并且对角元素从大到小排列。s中有n个奇异值，一般排在后面的比较接近0，所以仅保留比较大的r个奇异值。

举例：

from numpy import *
data = mat([[1,2,3],[4,5,6]])
U,sigma,VT = np.linalg.svd(data)
print U
[[-0.3863177  -0.92236578]
 [-0.92236578  0.3863177 ]]
print sigma
[9.508032   0.77286964]
print VT
[[-0.42866713 -0.56630692 -0.7039467 ]
 [ 0.80596391  0.11238241 -0.58119908]
 [ 0.40824829 -0.81649658  0.40824829]]

因为sigma是除了对角元素不为0，其他元素都为0。所以返回的时候，作为一维矩阵返回。本来sigma应该是由3个值的，但是因为最后一个值为0，所以直接省略了。

关于奇异值：

• 对于方阵而言，?=??−1，其中?
• 为特征向量。但不是方阵的矩阵没有特征向量。
• 非方阵矩阵可以用奇异值分解描述矩阵。?=????，其中U叫做左奇异值，S叫做奇异值，V叫做右奇异值。因为?只有对角线的数不为0，并且数值是从大到小排列，所以一般只取r个。r的值越接近?的列数，那么三个矩阵的乘法得到的矩阵越接近?。
• 因为三个矩阵的面积之和远远小于原矩阵?，所以当?是很大的矩阵，我们向压缩空间表达?的时候，可以使用这三个矩阵。当?不是矩阵时，把?转置成??。且(???)?=??，其中?是右奇异值，∂?=?‾‾√，这里的∂就是上述的奇异值。?=??∂，?就是上面的左奇异值。