vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

More functions added

Browse Source
This commit is contained in:
michaelpivato
2020-06-10 12:24:56 +09:30
parent 07f39dbe3d
commit 3710255703
1 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
pycar/src/car/tracking/icp.py
View File

@@ -42,11 +42,19 @@ def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray:
"""
return (data.dot(model.T).sum(0) / data.shape[0]) - calc_mass_centre(data).dot(calc_mass_centre(model).T)
def calc_quaternion(cross_cov):
def calc_delta(cross_cov):
A = (cross_cov - cross_cov.T)
return np.ndarray([[A[2][3]], [A[3][1]], [A[1][2]]])
def calc_quaternion(cross_cov, delta):
"""
Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3)
"""
pass
Q = np.array([[[cross_cov.trace()], [delta.T]],
[[delta], [cross_cov + cross_cov.T - cross_cov.trace().dot(np.identity(3))]]])
eigen_values, eigen_vectors = np.linalg.eig(Q)
optimal_eigen_index = eigen_values[eigen_values == eigen_values.max()][0]
return eigen_vectors[:,optimal_eigen_index]
def calc_translation(optimal_rotation, mass_centre_data, mass_centre_model):
"""
@@ -60,7 +68,8 @@ def calc_rotation_matrix(q):
Returns the rotation matrix for the given unit quaternion.
"""
return np.array([[[q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2], [2 * (q[1] * q[2] - q[0] * q[3])], [2 * (q[1] * q[3] - q[0] * q[2])]],
[]])
[[2 * (q[1] * q[2] + q[0] * q[3])], [q[0] ** 2 + q[2] ** 2 - q[1] ** 2 - q[3] **2], [2 * (q[2] * q[3] - q[0] * q[1])]],
[[2 * (q[1] * q[3] - q[0] * q[2])], [2 * (q[2] * q[3] + q[0] * q[1])], [2 * (q[2] * q[3] + q[0] * q[1])], [q[0] ** 2 + q[3] ** 2- q[1] ** 2 - q[2] ** 2]]])
def calc_mass_centre(points: np.ndarray) -> np.ndarray:
"""