13 std::vector<Eigen::Vector3f>
14 to3D(
const std::vector<Eigen::Vector2f>&
v)
17 std::vector<Eigen::Vector3f> v3;
22 std::back_inserter(v3),
23 static_cast<Eigen::Vector3f (*)(
const Eigen::Vector2f&)
>(&
to3D));
28 std::vector<Eigen::Vector2f>
29 to2D(
const std::vector<Eigen::Vector3f>&
v)
32 std::vector<Eigen::Vector2f> v2;
37 std::back_inserter(v2),
38 static_cast<Eigen::Vector2f (*)(
const Eigen::Vector3f&)
>(&
to2D));
47 p2.translation() = p3.translation().head<2>();
49 const auto rotatedVec = p3.rotation() * Eigen::Vector3f::UnitX();
50 const float yaw = std::atan2(rotatedVec.y(), rotatedVec.x());
52 p2.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();