| 176 | // TODO(edward) use slerp |
| 177 | template <typename T> |
| 178 | Eigen::Rigid3::Transform<T> AverageTransforms( |
| 179 | const std::vector<Eigen::Rigid3::Transform<T>>& transforms) { |
| 180 | CHECK(!transforms.empty()); |
| 181 | Eigen::Rigid3::Eulers<T> angles(0, 0, 0); |
| 182 | Eigen::Rigid3::Translation<T> translation(0, 0, 0); |
| 183 | for (const Eigen::Rigid3::Transform<T>& transform : transforms) { |
| 184 | translation += transform.block(0, 3, 3, 1); |
| 185 | angles += RotationMatrixToEulerAngles( |
| 186 | Eigen::Rigid3::Rotation<T>(transform.block(0, 0, 3, 3))); |
| 187 | } |
| 188 | translation /= static_cast<T>(transforms.size()); |
| 189 | angles /= static_cast<T>(transforms.size()); |
| 190 | Eigen::Rigid3::Transform<T> result = Eigen::Rigid3::Transform<T>::Identity(); |
| 191 | result.block(0, 0, 3, 3) = common::EulerAnglesToRotationMatrix(angles); |
| 192 | result.block(0, 3, 3, 1) = translation; |
| 193 | |
| 194 | return result; |
| 195 | } |
| 196 | template Eigen::Rigid3::Transform<double> AverageTransforms( |
| 197 | const std::vector<Eigen::Rigid3::Transform<double>>& transforms); |
| 198 | template Eigen::Rigid3::Transform<float> AverageTransforms( |
no test coverage detected