laser_scanner_conversion.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2021
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <algorithm>
25 #include <cmath>
26 
27 #include <Eigen/Geometry>
28 
29 #include <RobotAPI/interface/units/LaserScannerUnit.h>
30 
32 {
33 
34  template <typename EigenVectorT>
35  EigenVectorT
36  toCartesian(const LaserScanStep& laserScanStep)
37  {
38  EigenVectorT point = EigenVectorT::Identity();
39 
40  point.x() = laserScanStep.distance * std::cos(laserScanStep.angle);
41  point.y() = laserScanStep.distance * std::sin(laserScanStep.angle);
42 
43  return point;
44  }
45 
46  template <typename EigenVectorT>
47  std::vector<EigenVectorT>
48  toCartesian(const armarx::LaserScan& laserScan)
49  {
50  std::vector<EigenVectorT> points;
51  points.reserve(laserScan.size());
52 
53  std::transform(laserScan.begin(),
54  laserScan.end(),
55  std::back_inserter(points),
56  [](const LaserScanStep& pt) { return toCartesian<EigenVectorT>(pt); });
57 
58  return points;
59  }
60 
61 } // namespace armarx::armem::laser_scans::util
armarx::armem::laser_scans::util
Definition: laser_scanner_conversion.h:31
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::armem::laser_scans::util::toCartesian
EigenVectorT toCartesian(const LaserScanStep &laserScanStep)
Definition: laser_scanner_conversion.h:36
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:315