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 
31 void 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 = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
36  pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> filter;
37 
38  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp1 = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
39  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp2 = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
40 
41  // filter.setLeafSize(0.01, 0.01, 0.01);
42  // filter.setInputCloud(pc1);
43  // filter.filter(*temp1);
44  // filter.setInputCloud(pc2);
45  // filter.filter(*temp2);
46 
47 
48 
49  pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> aligner;
50  aligner.setEuclideanFitnessEpsilon(1e-20);
51  aligner.setTransformationEpsilon(1e-20);
52  aligner.setMaximumIterations(10000000);
53  aligner.setUseReciprocalCorrespondences(true);
54  aligner.setInputSource(pc1);
55  aligner.setInputTarget(pc2);
56  aligner.align(*pc1Aligned);
57  pcl::PointCloud<pcl::PointXYZRGBA> pcout = *pc2 + *pc1Aligned;
58  ARMARX_VERBOSE << aligner.getFitnessScore();
59  pcl::io::savePCDFileASCII(out, pcout);
60 
61 }
62 void PointCloudUtility::moveOrigin(std::string file, std::string out, Eigen::Vector3f translation)
63 {
65  transform.block<3, 1>(0, 3) = translation;
66  this->transformPointcloud(file, out, transform);
67 
68 }
69 void PointCloudUtility::rotatePointCloud(std::string file, std::string out, Eigen::Matrix3f rotation)
70 {
72  transform.block<3, 3>(0, 0) = rotation;
73  this->transformPointcloud(file, out, transform);
74 }
75 
76 void PointCloudUtility::transformPointcloud(std::string file, std::string out, Eigen::Matrix4f pose)
77 {
78  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file);
79  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
80  pcl::transformPointCloud<pcl::PointXYZRGBA>(*pc, *transformed_cloud, pose);
81 
82  pcl::io::savePCDFileASCII(out, *transformed_cloud);
83 }
84 void PointCloudUtility::showResult(std::string file)
85 {
86  pcl::visualization::PCLVisualizer viewer;
87  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file);
88  viewer.addPointCloud(pc);
89  viewer.addCoordinateSystem(1.0);
90  viewer.spin();
91  while (!viewer.wasStopped())
92  {
93 
94  }
95 }
96 
97 void PointCloudUtility::process()
98 {
99  std::string file1 = getProperty<std::string>("PointCloud1").getValue();
100  std::string file2 = getProperty<std::string>("PointCloud2").getValue();
101  std::string out = getProperty<std::string>("ResultFileName").getValue();
102  bool merge = getProperty<bool>("Merge").getValue();
103  bool transform = getProperty<bool>("Transform").getValue();
104  bool view = getProperty<bool>("Show").getValue();
105  std::string temp = "temp.pcd";
106  ARMARX_VERBOSE << "Starting";
107  if (merge)
108  {
109 
110  fusePointclouds(file1, file2, temp);
111  ARMARX_VERBOSE << "Merged";
112  }
113  else
114  {
115  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file1);
116  pcl::io::savePCDFileASCII(temp, *pc);
117  }
118  if (transform)
119  {
120  Eigen::Vector3f tVector;
121  tVector << getProperty<float>("X").getValue(),
122  getProperty<float>("Y").getValue(),
123  getProperty<float>("Z").getValue();
124  Eigen::Vector3f rot;
125  rot << getProperty<float>("Roll").getValue(),
126  getProperty<float>("Pitch").getValue(),
127  getProperty<float>("Yaw").getValue();
129  VirtualRobot::MathTools::posrpy2eigen4f(tVector, rot, transpose);
130  transformPointcloud(temp, out, transpose);
131  ARMARX_VERBOSE << "Transformed.";
132  }
133  else
134  {
135  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(temp);
136  pcl::io::savePCDFileASCII(out, *pc);
137  }
138  if (view)
139  {
140  showResult(out);
141  }
142  //auto manager = getArmarXManager();
143  //auto name = getName();
144  //std::thread {[manager, name]{manager->removeObjectBlocking(name);}}.detach();
145 }
147 {
148  processorTask = new RunningTask<PointCloudUtility>(this, &PointCloudUtility::process);
149 
150 }
151 
152 /**
153  * @brief PointCloudHandLocalizer::loadPointCloud loads a point cloud file from
154  * the ArmarXDataPath. Returns nullptr if failed.
155  * @param filename the name of the pointcloud file
156  * @return a pointer to said pointcload
157  */
158 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr PointCloudUtility::loadPointCloud(std::string filename)
159 {
160 
161  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model(new pcl::PointCloud<pcl::PointXYZRGBA>());
163  {
164  ARMARX_ERROR << "Could not find point cloud file in ArmarXDataPath: " << filename;
165  return nullptr;
166  }
167  else if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *model) == -1)
168  {
169  ARMARX_WARNING << "unable to load point cloud from file " << filename;
170  return nullptr;
171  }
172  else
173  {
174  return model;
175  }
176 }
177 
178 
180 {
181 
182 }
183 
184 
186 {
187 
188 
189 }
190 
191 
193 {
194 
195 }
196 
198 {
201 }
202 
203 
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
PointCloudUtility.h
armarx::PointCloudUtility::onConnectComponent
void onConnectComponent() override
Definition: PointCloudUtility.cpp:179
armarx::PointCloudUtility::onExitComponent
void onExitComponent() override
Definition: PointCloudUtility.cpp:192
armarx::RunningTask
Definition: ArmarXMultipleObjectsScheduler.h:35
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:523
armarx::PointCloudUtility::onInitComponent
void onInitComponent() override
Definition: PointCloudUtility.cpp:146
filename
std::string filename
Definition: VisualizationRobot.cpp:83
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::PointCloudUtilityPropertyDefinitions
Definition: PointCloudUtility.h:43
armarx::PointCloudUtility::onDisconnectComponent
void onDisconnectComponent() override
Definition: PointCloudUtility.cpp:185
IceUtil::Handle< class PropertyDefinitionContainer >
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
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:111
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
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::PointCloudUtility::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PointCloudUtility.cpp:197