FitKnownRectangleRotationMaxPoints.h
Go to the documentation of this file.
1 #pragma once
2
3 #include <pcl/point_cloud.h>
4 #include <pcl/point_types.h>
5 #include <pcl/common/transforms.h>
6
7 #include <Eigen/Geometry>
8
9
10 namespace Eigen
11 {
12  using Hyperplane3f = Hyperplane<float, 3>;
13 }
14
15
16 namespace visionx
17 {
18
19  /**
20  * @brief Finds the rotation of a rectangle of known size at a given position
21  * that contains the most points from a point cloud.
22  */
24  {
25  public:
26  using PointT = pcl::PointXYZRGBA;
27
28
29  public:
30
32
33
34  // Input
35
36  /// Set the rectangle size.
37  void setRectangleSize(const Eigen::Vector2f& value);
38  Eigen::Vector2f getRectangleSize() const;
39
40  /// Set the number of solution steps, i.e. the resolution of the solution space.
41  void setNumSteps(const size_t& value);
42  size_t getNumSteps() const;
43
44
45  /**
46  * @brief Set the input cloud and the rectangle's position and normal.
47  * @param cloud The input cloud.
48  * @param normal The rectangle normal.
49  * @param center The rectangle center.
50  */
51  void setInputCloud(const pcl::PointCloud<PointT>& cloud,
52  Eigen::Vector3f normal, Eigen::Vector3f center);
53
54
55  // Run
56
57  /**
58  * @brief Count the number of inliers per rotation step, and return
59  * the global pose corresponding to the rotation angle with the highest
60  * number of inliers.
61  *
62  * The best rotation angle can be retrieved via `getAngle()`.
63  * The number of inliers per step retrieved via `getInlierCounts()`.
64  *
65  * @return The global pose of the fitted rectangle.
66  */
68  /// @see `setInputCloud()`, `fit()`
69  Eigen::Matrix4f fit(const pcl::PointCloud<PointT>& cloud,
70  Eigen::Vector3f normal, Eigen::Vector3f center);
71
72
73  /// Count the inliers for a given local rotation angle.
74  size_t countInliersOfAngle(float angle) const;
75  /// Get the inliers for a given local rotation angle.
76  pcl::PointIndicesPtr findInliersOfAngle(float angle) const;
77
78
79  // Results
80
82  float getBestAngle() const;
84  size_t getNumInliers() const
85  {
86  return inlierCounts.at(bestStep);
87  }
88  /// Get the inliers of the best local rotation angle.
89  pcl::PointIndicesPtr getInliers() const
90  {
92  }
93
94
95  /// Get the inlier counts per step.
96  std::vector<size_t> getInlierCounts() const;
97
98
99  // Helpers
100
101  /// Get the local rotation angle of a solution step.
102  template <typename Int>
103  float stepToAngle(Int step) const
104  {
105  return (step / float(numSteps)) * maxAngle;
106  }
107  /// Get the global pose corresponding to a local rotation angle.
109
110
111  private:
112
113  /// Initialize `aligned` and `alignedTransform`.
114  void initAlignedPointCloud(const pcl::PointCloud<PointT>& cloud, Eigen::Vector3f normal, Eigen::Vector3f center);
115  pcl::PointCloud<PointT>::ConstPtr getAlignedPointCloud() const;
116
117  /// Set `rotated` by rotating `aligned` by the given angle.
118  void setRotated(float angle) const;
119  /// Checks whether a rotated point is an inlier.
120  bool isInlier(const PointT& p) const;
121
122
123  public:
124
125  // Settings.
126
127  /// The rectangle size.
128  Eigen::Vector2f rectangleSize = Eigen::Vector2f::Ones();
129
130  /// The number of steps (i.e. resolution of solution space).
131  size_t numSteps = 180;
132  const float maxAngle = float(M_PI); // Need to cover 180 degrees.
133
134
135  // Runtime.
136
137  /// Transformed so center is at 0 and z = normal.
138  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr aligned {new pcl::PointCloud<pcl::PointXYZRGBA>};
139  /// Transformation applied to get `aligned`.
141
142  /// Buffer for point clouds rotated for inlier counting.
143  mutable pcl::PointCloud<pcl::PointXYZRGBA>::Ptr rotated {new pcl::PointCloud<pcl::PointXYZRGBA>};
144
145
146  // Results
147
148  size_t bestStep = 0;
149  /// The best angle.
150  float bestAngle = 0;
151  /// The inlier counts per step.
152  std::vector<size_t> inlierCounts;
153
154
155  };
156
157 }
visionx::FitKnownRectangleRotationMaxPoints::setRectangleSize
void setRectangleSize(const Eigen::Vector2f &value)
Set the rectangle size.
Definition: FitKnownRectangleRotationMaxPoints.cpp:19
visionx::FitKnownRectangleRotationMaxPoints::alignedTransform
Eigen::Matrix4f alignedTransform
Transformation applied to get aligned.
Definition: FitKnownRectangleRotationMaxPoints.h:140
Eigen
Definition: Elements.h:36
visionx::FitKnownRectangleRotationMaxPoints::getRectangleSize
Eigen::Vector2f getRectangleSize() const
Definition: FitKnownRectangleRotationMaxPoints.cpp:15
visionx::FitKnownRectangleRotationMaxPoints::aligned
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr aligned
Transformed so center is at 0 and z = normal.
Definition: FitKnownRectangleRotationMaxPoints.h:138
visionx::FitKnownRectangleRotationMaxPoints::fit
Eigen::Matrix4f fit()
Count the number of inliers per rotation step, and return the global pose corresponding to the rotati...
Definition: FitKnownRectangleRotationMaxPoints.cpp:42
visionx::FitKnownRectangleRotationMaxPoints::numSteps
size_t numSteps
The number of steps (i.e. resolution of solution space).
Definition: FitKnownRectangleRotationMaxPoints.h:131
visionx
Definition: OpenPoseStressTest.h:38
visionx::FitKnownRectangleRotationMaxPoints::rectangleSize
Eigen::Vector2f rectangleSize
The rectangle size.
Definition: FitKnownRectangleRotationMaxPoints.h:128
visionx::FitKnownRectangleRotationMaxPoints::angleToPose
Eigen::Matrix4f angleToPose(float angle)
Get the global pose corresponding to a local rotation angle.
Definition: FitKnownRectangleRotationMaxPoints.cpp:133
visionx::FitKnownRectangleRotationMaxPoints::getInlierCounts
std::vector< size_t > getInlierCounts() const
Get the inlier counts per step.
Definition: FitKnownRectangleRotationMaxPoints.cpp:140
visionx::FitKnownRectangleRotationMaxPoints::bestStep
size_t bestStep
Definition: FitKnownRectangleRotationMaxPoints.h:148
visionx::FitKnownRectangleRotationMaxPoints::countInliersOfAngle
size_t countInliersOfAngle(float angle) const
Count the inliers for a given local rotation angle.
Definition: FitKnownRectangleRotationMaxPoints.cpp:103
visionx::FitKnownRectangleRotationMaxPoints
Finds the rotation of a rectangle of known size at a given position that contains the most points fro...
Definition: FitKnownRectangleRotationMaxPoints.h:23
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
visionx::FitKnownRectangleRotationMaxPoints::getInliers
pcl::PointIndicesPtr getInliers() const
Get the inliers of the best local rotation angle.
Definition: FitKnownRectangleRotationMaxPoints.h:89
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
visionx::FitKnownRectangleRotationMaxPoints::maxAngle
const float maxAngle
Definition: FitKnownRectangleRotationMaxPoints.h:132
visionx::FitKnownRectangleRotationMaxPoints::inlierCounts
std::vector< size_t > inlierCounts
The inlier counts per step.
Definition: FitKnownRectangleRotationMaxPoints.h:152
M_PI
#define M_PI
Definition: MathTools.h:17
Eigen::Hyperplane3f
Hyperplane< float, 3 > Hyperplane3f
Definition: Elements.h:38
visionx::FitKnownRectangleRotationMaxPoints::getNumSteps
size_t getNumSteps() const
Definition: FitKnownRectangleRotationMaxPoints.cpp:25
visionx::FitKnownRectangleRotationMaxPoints::setInputCloud
void setInputCloud(const pcl::PointCloud< PointT > &cloud, Eigen::Vector3f normal, Eigen::Vector3f center)
Set the input cloud and the rectangle's position and normal.
Definition: FitKnownRectangleRotationMaxPoints.cpp:35
visionx::FitKnownRectangleRotationMaxPoints::setNumSteps
void setNumSteps(const size_t &value)
Set the number of solution steps, i.e. the resolution of the solution space.
Definition: FitKnownRectangleRotationMaxPoints.cpp:29
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
visionx::FitKnownRectangleRotationMaxPoints::stepToAngle
float stepToAngle(Int step) const
Get the local rotation angle of a solution step.
Definition: FitKnownRectangleRotationMaxPoints.h:103
float
#define float
Definition: 16_Level.h:22
visionx::FitKnownRectangleRotationMaxPoints::FitKnownRectangleRotationMaxPoints
FitKnownRectangleRotationMaxPoints()
Definition: FitKnownRectangleRotationMaxPoints.cpp:10
visionx::FitKnownRectangleRotationMaxPoints::bestAngle
float bestAngle
The best angle.
Definition: FitKnownRectangleRotationMaxPoints.h:150
visionx::FitKnownRectangleRotationMaxPoints::PointT
pcl::PointXYZRGBA PointT
Definition: FitKnownRectangleRotationMaxPoints.h:26
visionx::FitKnownRectangleRotationMaxPoints::getNumInliers
size_t getNumInliers() const
Definition: FitKnownRectangleRotationMaxPoints.h:84
armarx::VariantType::Int
const VariantTypeId Int
Definition: Variant.h:916
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
visionx::FitKnownRectangleRotationMaxPoints::getBestAngle
float getBestAngle() const