| 167 | } |
| 168 | |
| 169 | Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, |
| 170 | const double ry, |
| 171 | const double rz) { |
| 172 | const Eigen::Matrix3d Rx = |
| 173 | Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix(); |
| 174 | const Eigen::Matrix3d Ry = |
| 175 | Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix(); |
| 176 | const Eigen::Matrix3d Rz = |
| 177 | Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix(); |
| 178 | return Rz * Ry * Rx; |
| 179 | } |
| 180 | |
| 181 | Eigen::Quaterniond AverageQuaternions( |
| 182 | const std::vector<Eigen::Quaterniond>& quats, |