| 171 | } |
| 172 | |
| 173 | colvec pose_inverse(const colvec& X) { |
| 174 | mat R = ypr_to_R(X.rows(3, 5)); |
| 175 | colvec n = R.col(0); |
| 176 | colvec o = R.col(1); |
| 177 | colvec a = R.col(2); |
| 178 | |
| 179 | colvec XIxyz = -trans(R) * (X.rows(0, 2)); |
| 180 | colvec XIypr(3); |
| 181 | double XIy = atan2(o(0), n(0)); |
| 182 | double XIp = atan2(-a(0), n(0) * cos(XIy) + o(0) * sin(XIy)); |
| 183 | double XIr = atan2(n(2) * sin(XIy) - o(2) * cos(XIy), -n(1) * sin(XIy) + o(1) * cos(XIy)); |
| 184 | XIypr(0) = XIy; |
| 185 | XIypr(1) = XIp; |
| 186 | XIypr(2) = XIr; |
| 187 | |
| 188 | colvec XI = join_cols(XIxyz, XIypr); |
| 189 | return XI; |
| 190 | } |
| 191 | |
| 192 | colvec pose_update_2d(const colvec& X1, const colvec& X2) { |
| 193 | mat R = yaw_to_R(X1(2)); |
no test coverage detected