PointCloudUtility.cpp
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::PointCloudUtility
19  * @author Tim Niklas Freudenstein ( uidnv at student dot kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "PointCloudUtility.h"
26 
27 
28 using namespace armarx;
29 
30 void
31 PointCloudUtility::fusePointclouds(std::string file1, std::string file2, std::string out)
32 {
33  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc1 = loadPointCloud(file1);
34  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc2 = loadPointCloud(file2);
35  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc1Aligned =
36  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
37  pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> filter;
38 
39  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp1 =
40  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
41  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp2 =
42  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
43 
44  // filter.setLeafSize(0.01, 0.01, 0.01);
45  // filter.setInputCloud(pc1);
46  // filter.filter(*temp1);
47  // filter.setInputCloud(pc2);
48  // filter.filter(*temp2);
49 
50 
51  pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> aligner;
52  aligner.setEuclideanFitnessEpsilon(1e-20);
53  aligner.setTransformationEpsilon(1e-20);
54  aligner.setMaximumIterations(10000000);
55  aligner.setUseReciprocalCorrespondences(true);
56  aligner.setInputSource(pc1);
57  aligner.setInputTarget(pc2);
58  aligner.align(*pc1Aligned);
59  pcl::PointCloud<pcl::PointXYZRGBA> pcout = *pc2 + *pc1Aligned;
60  ARMARX_VERBOSE << aligner.getFitnessScore();
61  pcl::io::savePCDFileASCII(out, pcout);
62 }
63 
64 void
65 PointCloudUtility::moveOrigin(std::string file, std::string out, Eigen::Vector3f translation)
66 {
67  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
68  transform.block<3, 1>(0, 3) = translation;
69  this->transformPointcloud(file, out, transform);
70 }
71 
72 void
73 PointCloudUtility::rotatePointCloud(std::string file, std::string out, Eigen::Matrix3f rotation)
74 {
75  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
76  transform.block<3, 3>(0, 0) = rotation;
77  this->transformPointcloud(file, out, transform);
78 }
79 
80 void
81 PointCloudUtility::transformPointcloud(std::string file, std::string out, Eigen::Matrix4f pose)
82 {
83  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file);
84  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud(
85  new pcl::PointCloud<pcl::PointXYZRGBA>());
86  pcl::transformPointCloud<pcl::PointXYZRGBA>(*pc, *transformed_cloud, pose);
87 
88  pcl::io::savePCDFileASCII(out, *transformed_cloud);
89 }
90 
91 void
92 PointCloudUtility::showResult(std::string file)
93 {
94  pcl::visualization::PCLVisualizer viewer;
95  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file);
96  viewer.addPointCloud(pc);
97  viewer.addCoordinateSystem(1.0);
98  viewer.spin();
99  while (!viewer.wasStopped())
100  {
101  }
102 }
103 
104 void
105 PointCloudUtility::process()
106 {
107  std::string file1 = getProperty<std::string>("PointCloud1").getValue();
108  std::string file2 = getProperty<std::string>("PointCloud2").getValue();
109  std::string out = getProperty<std::string>("ResultFileName").getValue();
110  bool merge = getProperty<bool>("Merge").getValue();
111  bool transform = getProperty<bool>("Transform").getValue();
112  bool view = getProperty<bool>("Show").getValue();
113  std::string temp = "temp.pcd";
114  ARMARX_VERBOSE << "Starting";
115  if (merge)
116  {
117 
118  fusePointclouds(file1, file2, temp);
119  ARMARX_VERBOSE << "Merged";
120  }
121  else
122  {
123  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file1);
124  pcl::io::savePCDFileASCII(temp, *pc);
125  }
126  if (transform)
127  {
128  Eigen::Vector3f tVector;
129  tVector << getProperty<float>("X").getValue(), getProperty<float>("Y").getValue(),
130  getProperty<float>("Z").getValue();
131  Eigen::Vector3f rot;
132  rot << getProperty<float>("Roll").getValue(), getProperty<float>("Pitch").getValue(),
133  getProperty<float>("Yaw").getValue();
134  Eigen::Matrix4f transpose;
135  VirtualRobot::MathTools::posrpy2eigen4f(tVector, rot, transpose);
136  transformPointcloud(temp, out, transpose);
137  ARMARX_VERBOSE << "Transformed.";
138  }
139  else
140  {
141  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(temp);
142  pcl::io::savePCDFileASCII(out, *pc);
143  }
144  if (view)
145  {
146  showResult(out);
147  }
148  //auto manager = getArmarXManager();
149  //auto name = getName();
150  //std::thread {[manager, name]{manager->removeObjectBlocking(name);}}.detach();
151 }
152 
153 void
154 PointCloudUtility::onInitComponent()
155 {
156  processorTask = new RunningTask<PointCloudUtility>(this, &PointCloudUtility::process);
157 }
158 
159 /**
160  * @brief PointCloudHandLocalizer::loadPointCloud loads a point cloud file from
161  * the ArmarXDataPath. Returns nullptr if failed.
162  * @param filename the name of the pointcloud file
163  * @return a pointer to said pointcload
164  */
165 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
166 PointCloudUtility::loadPointCloud(std::string filename)
167 {
168 
169  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model(new pcl::PointCloud<pcl::PointXYZRGBA>());
170  if (!ArmarXDataPath::getAbsolutePath(filename, filename))
171  {
172  ARMARX_ERROR << "Could not find point cloud file in ArmarXDataPath: " << filename;
173  return nullptr;
174  }
175  else if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *model) == -1)
176  {
177  ARMARX_WARNING << "unable to load point cloud from file " << filename;
178  return nullptr;
179  }
180  else
181  {
182  return model;
183  }
184 }
185 
186 void
187 PointCloudUtility::onConnectComponent()
188 {
189 }
190 
191 void
192 PointCloudUtility::onDisconnectComponent()
193 {
194 }
195 
196 void
197 PointCloudUtility::onExitComponent()
198 {
199 }
200 
201 armarx::PropertyDefinitionsPtr
202 PointCloudUtility::createPropertyDefinitions()
203 {
204  return armarx::PropertyDefinitionsPtr(
205  new PointCloudUtilityPropertyDefinitions(getConfigIdentifier()));
206 }
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
PointCloudUtility.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::PointCloudUtility::onConnectComponent
void onConnectComponent() override
Definition: PointCloudUtility.cpp:187
armarx::PointCloudUtility::onExitComponent
void onExitComponent() override
Definition: PointCloudUtility.cpp:197
armarx::RunningTask
Definition: ArmarXMultipleObjectsScheduler.h:36
merge
Use of this software is granted under one of the following two to be chosen freely by the user Boost Software License Version Marcin Kalicinski Permission is hereby free of to any person or organization obtaining a copy of the software and accompanying documentation covered by this and transmit the and to prepare derivative works of the and to permit third parties to whom the Software is furnished to do all subject to the including the above license this restriction and the following must be included in all copies of the in whole or in and all derivative works of the unless such copies or derivative works are solely in the form of machine executable object code generated by a source language processor THE SOFTWARE IS PROVIDED AS WITHOUT WARRANTY OF ANY EXPRESS OR INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF FITNESS FOR A PARTICULAR TITLE AND NON INFRINGEMENT IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER WHETHER IN TORT OR ARISING OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE The MIT Marcin Kalicinski Permission is hereby free of to any person obtaining a copy of this software and associated documentation to deal in the Software without including without limitation the rights to merge
Definition: license.txt:39
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::PointCloudUtility::onInitComponent
void onInitComponent() override
Definition: PointCloudUtility.cpp:154
filename
std::string filename
Definition: VisualizationRobot.cpp:86
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
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::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
armarx::PointCloudUtilityPropertyDefinitions
Definition: PointCloudUtility.h:46
armarx::PointCloudUtility::onDisconnectComponent
void onDisconnectComponent() override
Definition: PointCloudUtility.cpp:192
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:109
pc
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics Blackwell June If you use this software you should cite the aforementioned paper in any resulting publication Please send comments or bug reports to Ruwen Roland BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE Example usage This section shows how to use the library to detect the shapes in a point cloud PointCloud pc
Definition: ReadMe.txt:68
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:772
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:35
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::PointCloudUtility::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PointCloudUtility.cpp:202