PointCloudFilter.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::ArmarXObjects::PointCloudFilter
19  * @author Markus Grotz ( markus dot grotz at kit dot edu )
20  * @date 2016
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #pragma once
26 
27 #include <pcl/point_types.h>
28 
29 #include <pcl/common/transforms.h>
30 
31 #include <pcl/filters/filter.h>
32 #include <pcl/filters/passthrough.h>
33 #include <pcl/filters/approximate_voxel_grid.h>
34 #include <pcl/filters/crop_hull.h>
35 
36 #include <Eigen/Core>
37 
40 
41 #include <ArmarXGui/interface/RemoteGuiInterface.h>
43 
44 #include <RobotAPI/interface/core/PoseBase.h>
45 #include <RobotAPI/interface/core/RobotState.h>
48 
49 #include <VisionX/interface/components/PointCloudFilter.h>
51 
52 #include <mutex>
53 
54 
55 namespace visionx
56 {
57  class CoordinateFrame;
58  template <class PointT>
60 
61 
62  /// @class PointCloudFilterPropertyDefinitions
65  {
66  public:
68  };
69 
70 
71  /**
72  * @defgroup Component-PointCloudFilter PointCloudFilter
73  * @ingroup VisionX-Components
74  * A description of the component PointCloudFilter.
75  *
76  * @class PointCloudFilter
77  * @ingroup Component-PointCloudFilter
78  * @brief Brief description of class PointCloudFilter.
79  *
80  * Detailed description of class PointCloudFilter.
81  */
83  virtual public armarx::PointCloudFilterInterface,
84  virtual public visionx::PointCloudProcessor
85  {
86  public:
87 
88  /// @see armarx::ManagedIceObject::getDefaultName()
89  virtual std::string getDefaultName() const override;
90 
91  virtual std::string getReferenceFrame(const Ice::Current& = Ice::emptyCurrent) override;
92 
93  virtual void setCroppingParameters(const armarx::Vector3BasePtr& min, const armarx::Vector3BasePtr& max,
94  const std::string& frame, const Ice::Current& = Ice::emptyCurrent) override;
95 
96  protected:
97 
98  /// @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
99  virtual void onInitPointCloudProcessor() override;
100 
101  /// @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
102  virtual void onConnectPointCloudProcessor() override;
103 
104  /// @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
105  virtual void onDisconnectPointCloudProcessor() override;
106 
107  /// @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
108  virtual void onExitPointCloudProcessor() override;
109 
110  /// @see visionx::PointCloudProcessor::process()
111  virtual void process() override;
112 
113  template <typename PointType> void processPointCloud();
114 
115  /// @see PropertyUser::createPropertyDefinitions()
117 
118 
119  private:
120 
121  /// Draw the given frame with given role.
122  void drawFrame(const armarx::FramedPose& frame, const std::string& role);
123 
124  /// Change the point cloud to given frame.
125  /// Also logs transformation and draws target frame.
126  template <typename PointT>
127  void changePointCloudFrame(FramedPointCloud<PointT>& pointCloud,
128  const std::string& role, const std::string& frame);
129 
130  /// Create the remote gui. May only be called if `remoteGui` is not nullptr.
131  void remoteGuiCreate();
132  /// Create the remote gui. May only be called if `remoteGui` is not nullptr.
133  void remoteGuiRun();
134 
135  void remoteGuiUpdate(armarx::RemoteGui::TabProxy& tab);
136 
137 
138 
139  private:
140 
141  // Robot
142  armarx::RobotStateComponentInterfacePrx robotStateComponent;
143  VirtualRobot::RobotPtr localRobot;
144 
145  std::string pointCloudFormat;
146  std::string providerSourceFrameName; ///< Point cloud frame specified by provider.
147 
148  struct Parameters
149  {
150  // Frames
151  std::string sourceFrameName;
152  std::string targetFrameName;
153 
154  // Cropping
155  bool croppingEnabled;
156  Eigen::Vector3f croppingMinPoint;
157  Eigen::Vector3f croppingMaxPoint;
158  Eigen::Vector3f croppingRPY;
159  std::string croppingFrameName;
160 
161  //SOR
163  {
164  bool enabled = false;
165  float stddevMulThresh = 1;
166  int meanK = 50;
167  };
168  StatisticalOutlierRemoval statisticalOutlierRemoval;
169 
170  // Downsampling
171  bool downsamplingEnabled;
172  float gridLeafSize;
173 
174  // Collision model filter
175  bool applyCollisionModelFilter;
176 
177  bool reportCloudOutsideOfCroppingArea = false;
178 
179  Eigen::Matrix4f providerFrameTransformation = Eigen::Matrix4f::Identity();
180  };
181 
182  std::mutex parametersMutex;
183  Parameters parameters;
184 
185 
186  // Remote GUI
187  /// The remote GUI proxy. Only fetched when property `RemoteGuiEnabled` is true.
188  armarx::RemoteGuiInterfacePrx remoteGui;
189  /// The remote GUI tab.
190  armarx::RemoteGui::TabProxy remoteGuiTab;
191  /// The remote GUI main thread.
193 
194 
195  // Debug Drawer
196  armarx::DebugDrawerTopic debugDrawer { getDefaultName() };
197 
198  };
199 }
200 
visionx::PointCloudFilter::onInitPointCloudProcessor
virtual void onInitPointCloudProcessor() override
Definition: PointCloudFilter.cpp:122
armarx::DebugDrawerTopic
The DebugDrawerTopic wraps a DebugDrawerInterfacePrx and provides a more useful interface than the Ic...
Definition: DebugDrawerTopic.h:152
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
visionx::PointCloudFilter::setCroppingParameters
virtual void setCroppingParameters(const armarx::Vector3BasePtr &min, const armarx::Vector3BasePtr &max, const std::string &frame, const Ice::Current &=Ice::emptyCurrent) override
Definition: PointCloudFilter.cpp:111
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
visionx::PointCloudFilter::processPointCloud
void processPointCloud()
Definition: PointCloudFilter.cpp:281
visionx::PointCloudFilter::getReferenceFrame
virtual std::string getReferenceFrame(const Ice::Current &=Ice::emptyCurrent) override
Definition: PointCloudFilter.cpp:105
DebugDrawerTopic.h
armarx::RemoteGui::TabProxy
Definition: WidgetProxy.h:17
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
visionx::PointCloudProcessor
The PointCloudProcessor class provides an interface for access to PointCloudProviders via Ice and sha...
Definition: PointCloudProcessor.h:186
FramedPose.h
visionx::PointCloudFilter::Parameters::StatisticalOutlierRemoval
Definition: PointCloudFilter.h:162
visionx::PointCloudFilter::process
virtual void process() override
Definition: PointCloudFilter.cpp:238
max
T max(T t1, T t2)
Definition: gdiam.h:48
visionx::PointCloudFilter::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PointCloudFilter.cpp:519
WidgetProxy.h
visionx::PointCloudProcessorPropertyDefinitions
Properties of PointCloudProcessor.
Definition: PointCloudProcessor.h:173
PointCloudProcessor.h
TaskUtil.h
visionx::PointCloudFilter::onExitPointCloudProcessor
virtual void onExitPointCloudProcessor() override
Definition: PointCloudFilter.cpp:233
Component.h
visionx::PointCloudFilter
Brief description of class PointCloudFilter.
Definition: PointCloudFilter.h:82
visionx::FramedPointCloud
A point cloud which keeps track of its reference coordinate frame and allows changing frames using ar...
Definition: PointCloudFilter.h:59
visionx::PointCloudFilter::onConnectPointCloudProcessor
virtual void onConnectPointCloudProcessor() override
Definition: PointCloudFilter.cpp:173
visionx::PointCloudFilter::onDisconnectPointCloudProcessor
virtual void onDisconnectPointCloudProcessor() override
Definition: PointCloudFilter.cpp:259
visionx::PointCloudFilter::Parameters::StatisticalOutlierRemoval::stddevMulThresh
float stddevMulThresh
Definition: PointCloudFilter.h:165
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::PointCloudFilter::getDefaultName
virtual std::string getDefaultName() const override
Definition: PointCloudFilter.cpp:100
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
visionx::PointCloudFilter::Parameters::StatisticalOutlierRemoval::enabled
bool enabled
Definition: PointCloudFilter.h:164
min
T min(T t1, T t2)
Definition: gdiam.h:42
visionx::PointCloudFilterPropertyDefinitions
Definition: PointCloudFilter.h:63
visionx::PointCloudFilter::Parameters::StatisticalOutlierRemoval::meanK
int meanK
Definition: PointCloudFilter.h:166
visionx::PointCloudFilterPropertyDefinitions::PointCloudFilterPropertyDefinitions
PointCloudFilterPropertyDefinitions(std::string prefix)
Definition: PointCloudFilter.cpp:54
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18