Return the log(R) where R is a rotation matrix. So it returns a skew symmetric matrix.
(R: np.ndarray)
| 65 | |
| 66 | @staticmethod |
| 67 | def log_rotation(R: np.ndarray): |
| 68 | """Return the log(R) where R is a rotation matrix. So it returns a skew symmetric matrix.""" |
| 69 | arg = 0.5 * (R[0, 0] + R[1, 1] + R[2, 2] - 1) # in [-1, 1] |
| 70 | if arg > -1: |
| 71 | if arg < 1: |
| 72 | # 0 < angle < pi |
| 73 | angle = np.arccos(arg) |
| 74 | sinAngle = np.sin(angle) |
| 75 | c = 0.5 * angle / sinAngle |
| 76 | S = c * (R - R.T) |
| 77 | else: |
| 78 | # arg == 1, angle == 0 |
| 79 | # R is the identity matrix and S is the zero matrix |
| 80 | S = np.zeros([3, 3]) |
| 81 | else: # arg == -1, angle == pi |
| 82 | # R + I is symmetric. To avoid bias, we use (R[i,j]+R[j,i]) / 2 for off-diagonal entries rather than R[i,j] |
| 83 | s = np.zeros((3, 1)) |
| 84 | if R[0, 0] >= R[1, 1]: |
| 85 | if R[0, 0] >= R[2, 2]: |
| 86 | # R[0,0] is the maximum diagonal term |
| 87 | s[0] = R[0, 0] + 1 |
| 88 | s[1] = 0.5 * (R[0, 1] + R[1, 0]) |
| 89 | s[2] = 0.5 * (R[0, 2] + R[2, 0]) |
| 90 | else: |
| 91 | # R[2,2] is the maximum diagonal term |
| 92 | s[0] = 0.5 * (R[2, 0] + R[0, 2]) |
| 93 | s[1] = 0.5 * (R[2, 1] + R[1, 2]) |
| 94 | s[2] = R[2, 2] + 1 |
| 95 | else: |
| 96 | if R[1, 1] >= R[2, 2]: |
| 97 | # R[1,1] is the maximum diagonal term |
| 98 | s[0] = 0.5 * (R[1, 0] + R[0, 1]) |
| 99 | s[1] = R[1, 1] + 1 |
| 100 | s[2] = 0.5 * (R[1, 2] + R[2, 1]) |
| 101 | else: |
| 102 | # R[2,2] is the maximum diagonal term |
| 103 | s[0] = 0.5 * (R[2, 0] + R[0, 2]) |
| 104 | s[1] = 0.5 * (R[2, 1] + R[1, 2]) |
| 105 | s[2] = R[2, 2] + 1 |
| 106 | length = np.sqrt(np.sum(s ** 2.0)) |
| 107 | if length > 0: |
| 108 | adjust = np.pi * np.sqrt(0.5) / length |
| 109 | s = s * adjust |
| 110 | else: |
| 111 | s = s * 0 |
| 112 | |
| 113 | S = get_cross_product_matrix(s) |
| 114 | # S = np.array([ |
| 115 | # [0, -s[2], s[1]], |
| 116 | # [s[2], 0, -s[0]], |
| 117 | # [-s[1], s[0], 0], |
| 118 | # ]) |
| 119 | |
| 120 | return S |
| 121 | |
| 122 | def rotate_translate(self, R: np.ndarray, t: np.ndarray): |
| 123 | self.R = R @ self.R # 3*3 rotation matrix |
no test coverage detected