eigen.cpp
Go to the documentation of this file.
1 #include "eigen.h"
2 
3 #include <algorithm>
4 #include <cmath>
5 #include <iterator>
6 #include <vector>
7 
9 
11 {
12 
13  std::vector<Eigen::Vector3f>
14  to3D(const std::vector<Eigen::Vector2f>& v)
15 
16  {
17  std::vector<Eigen::Vector3f> v3;
18  v3.reserve(v.size());
19 
20  std::transform(v.begin(),
21  v.end(),
22  std::back_inserter(v3),
23  static_cast<Eigen::Vector3f (*)(const Eigen::Vector2f&)>(&to3D));
24 
25  return v3;
26  }
27 
28  std::vector<Eigen::Vector2f>
29  to2D(const std::vector<Eigen::Vector3f>& v)
30 
31  {
32  std::vector<Eigen::Vector2f> v2;
33  v2.reserve(v.size());
34 
35  std::transform(v.begin(),
36  v.end(),
37  std::back_inserter(v2),
38  static_cast<Eigen::Vector2f (*)(const Eigen::Vector3f&)>(&to2D));
39 
40  return v2;
41  }
42 
44  to2D(const core::Pose& p3)
45  {
47  p2.translation() = p3.translation().head<2>();
48 
49  const auto rotatedVec = p3.rotation() * Eigen::Vector3f::UnitX();
50  const float yaw = std::atan2(rotatedVec.y(), rotatedVec.x());
51 
52  p2.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
53 
54  return p2;
55  }
56 
57 
58 } // namespace armarx::navigation::conv
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
basic_types.h
armarx::navigation::core::Pose2D
Eigen::Isometry2f Pose2D
Definition: basic_types.h:34
armarx::navigation::conv
This file is part of ArmarX.
Definition: eigen.cpp:10
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
armarx::navigation::conv::to2D
std::vector< Eigen::Vector2f > to2D(const std::vector< Eigen::Vector3f > &v)
Definition: eigen.cpp:29
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::navigation::conv::to3D
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:14
eigen.h