compute_overlap.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2012,
6 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 
37 #include "pointmatcher/IO.h"
38 #include <cassert>
39 #include <iostream>
40 #include <iomanip>
41 #include <fstream>
42 #include <boost/format.hpp>
43 #include <boost/filesystem/path.hpp>
44 #include <boost/filesystem/operations.hpp>
45 #include <boost/lexical_cast.hpp>
46 
47 using namespace std;
48 using namespace PointMatcherSupport;
49 
50 void validateArgs(int argc, char *argv[]);
51 void setupArgs(int argc, char *argv[], unsigned int& startId, unsigned int& endId, string& extension);
52 
57 int main(int argc, char *argv[])
58 {
59  validateArgs(argc, argv);
60 
61  typedef PointMatcher<float> PM;
63  typedef PM::Matrix Matrix;
65  typedef PM::DataPoints DP;
66  typedef PM::Matches Matches;
67 
68  // Process arguments
69  PMIO::FileInfoVector list(argv[1]);
70  bool debugMode = false;
71  if (argc == 4)
72  debugMode = true;
73 
74  if(debugMode)
75  setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
76 
77  // Prepare transformation chain for maps
78  std::shared_ptr<PM::Transformation> rigidTransform;
79  rigidTransform = PM::get().TransformationRegistrar.create("RigidTransformation");
80 
81  PM::Transformations transformations;
82  transformations.push_back(rigidTransform);
83 
84  DP reading, reference;
85  TP Tread = TP::Identity(4,4);
86  DP mapCloud;
87  TP Tref = TP::Identity(4,4);
88 
89  unsigned startingI = 0;
90  unsigned listSizeI = list.size();
91  unsigned listSizeJ = list.size();
92  if(debugMode)
93  {
94  startingI = boost::lexical_cast<unsigned>(argv[2]);
95  listSizeI = startingI + 1;
96  }
97 
98  PM::Matrix overlapResults = PM::Matrix::Ones(listSizeJ, listSizeI);
99 
100  for(unsigned i = startingI; i < listSizeI; i++)
101  {
102  unsigned startingJ = i+1;
103  if(debugMode)
104  {
105  startingJ = boost::lexical_cast<unsigned>(argv[3]);
106  listSizeJ = startingJ + 1;
107  }
108  for(unsigned j = startingJ; j < listSizeJ; j++)
109  {
110  // Load point clouds
111  reading = DP::load(list[i].readingFileName);
112  reference = DP::load(list[j].readingFileName);
113 
114  cout << "Point cloud loaded" << endl;
115 
116  // Load transformation matrices
117  if(list[i].groundTruthTransformation.rows() != 0)
118  {
119  Tread = list[i].groundTruthTransformation;
120  Tref = list[j].groundTruthTransformation;
121  }
122  else
123  {
124  cout << "ERROR: fields gTXX (i.e., ground truth matrix) is required" << endl;
125  abort();
126  }
127 
128  // Move point cloud in global frame
129  transformations.apply(reading, Tread);
130  transformations.apply(reference, Tref);
131 
132  // Preprare filters
133  std::shared_ptr<PM::DataPointsFilter> subSample =
134  PM::get().DataPointsFilterRegistrar.create(
135  "RandomSamplingDataPointsFilter",
136  {{"prob", "0.5"}}
137  );
138 
139  std::shared_ptr<PM::DataPointsFilter> maxDensity =
140  PM::get().DataPointsFilterRegistrar.create(
141  "MaxDensityDataPointsFilter"
142  );
143 
144  /*std::shared_ptr<PM::DataPointsFilter> cutInHalf;
145  cutInHalf = PM::get().DataPointsFilterRegistrar.create(
146  "MinDistDataPointsFilter", PM::Parameters({
147  {"dim", "1"},
148  {"minDist", "0"}
149  }));*/
150 
151  std::shared_ptr<PM::DataPointsFilter> computeDensity =
152  PM::get().DataPointsFilterRegistrar.create(
153  "SurfaceNormalDataPointsFilter",
154  {
155  {"knn", "20"},
156  {"keepDensities", "1"}
157  }
158  );
159 
160  reading = subSample->filter(reading);
161  reading = computeDensity->filter(reading);
162  reading = maxDensity->filter(reading);
163  //reading = cutInHalf->filter(reading);
164  const Matrix inliersRead = Matrix::Zero(1, reading.features.cols());
165  reading.addDescriptor("inliers", inliersRead);
166 
167  reference = subSample->filter(reference);
168  reference = computeDensity->filter(reference);
169  reference = maxDensity->filter(reference);
170  const Matrix inliersRef = Matrix::Zero(1, reference.features.cols());
171  reference.addDescriptor("inliers", inliersRef);
172 
173  //TODO: reverse self and target
174  DP self = reading;
175  DP target = reference;
176 
177  for(int l = 0; l < 2; l++)
178  {
179  const int selfPtsCount = self.features.cols();
180  const int targetPtsCount = target.features.cols();
181 
182  // Build kd-tree
183  int knn = 20;
184  int knnAll = 50;
185  std::shared_ptr<PM::Matcher> matcherSelf =
186  PM::get().MatcherRegistrar.create(
187  "KDTreeMatcher",
188  {{"knn", toParam(knn)}}
189  );
190 
191  std::shared_ptr<PM::Matcher> matcherTarget =
192  PM::get().MatcherRegistrar.create(
193  "KDTreeVarDistMatcher",
194  {
195  {"knn", toParam(knnAll)},
196  {"maxDistField", "maxSearchDist"}
197  }
198  );
199 
200  matcherSelf->init(self);
201  matcherTarget->init(target);
202 
203  Matches selfMatches(knn, selfPtsCount);
204  selfMatches = matcherSelf->findClosests(self);
205 
206  const Matrix maxSearchDist = selfMatches.dists.colwise().maxCoeff().cwiseSqrt();
207  self.addDescriptor("maxSearchDist", maxSearchDist);
208 
209  Matches targetMatches(knnAll, targetPtsCount);
210  targetMatches = matcherTarget->findClosests(self);
211 
212  BOOST_AUTO(inlierSelf, self.getDescriptorViewByName("inliers"));
213  BOOST_AUTO(inlierTarget, target.getDescriptorViewByName("inliers"));
214  for(int i = 0; i < selfPtsCount; i++)
215  {
216  for(int k = 0; k < knnAll; k++)
217  {
218  if (targetMatches.dists(k, i) != Matches::InvalidDist)
219  {
220  inlierSelf(0,i) = 1.0;
221  inlierTarget(0,targetMatches.ids(k, i)) = 1.0;
222  }
223  }
224  }
225 
226  // Swap point clouds
227  PM::swapDataPoints(self, target);
228  }
229 
230  const BOOST_AUTO(finalInlierSelf, self.getDescriptorViewByName("inliers"));
231  const BOOST_AUTO(finalInlierTarget, target.getDescriptorViewByName("inliers"));
232  const double selfRatio = (finalInlierSelf.array() > 0).count()/(double)finalInlierSelf.cols();
233  const double targetRatio = (finalInlierTarget.array() > 0).count()/(double)finalInlierTarget.cols();
234 
235  cout << i << " -> " << j << ": " << selfRatio << endl;
236  cout << j << " -> " << i << ": " << targetRatio << endl;
237 
238  if(debugMode)
239  {
240  self.save("scan_i.vtk");
241  target.save("scan_j.vtk");
242  }
243  else
244  {
245  overlapResults(j,i) = selfRatio;
246  overlapResults(i,j) = targetRatio;
247  }
248  }
249  }
250 
251 
252  // write results in a file
253  std::fstream outFile;
254  outFile.open("overlapResults.csv", fstream::out);
255  for(int x=0; x < overlapResults.rows(); x++)
256  {
257  for(int y=0; y < overlapResults.cols(); y++)
258  {
259  outFile << overlapResults(x, y) << ", ";
260  }
261 
262  outFile << endl;
263  }
264 
265  outFile.close();
266 
267  return 0;
268 }
269 
270 void validateArgs(int argc, char *argv[])
271 {
272  if (!(argc == 2 || argc == 4))
273  {
274  cerr << endl;
275  cerr << " ERROR in command line" << endl << endl;
276  cerr << " Usage: " << argv[0] << " listOfFiles.csv <i j>" << endl;
277  cerr << " Note: 'i' and 'j' are optional arguments. If used, only compute the overlap for those 2 point cloud ids and dump VTK files for visual inspection." << endl;
278  cerr << endl;
279  cerr << " Example: " << endl;
280  cerr << " $ " << argv[0] << " ../example/data/carCloudList.csv" << endl;
281  cerr << endl;
282  abort();
283  }
284 }
285 
286 
287 
PointMatcherIO
IO Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: IO.h:43
PointMatcherSupport::toParam
std::string toParam(const S &value)
Return the a string value using lexical_cast.
Definition: Parametrizable.h:92
PointMatcher::DataPoints::addDescriptor
void addDescriptor(const std::string &name, const Matrix &newDescriptor)
Add a descriptor by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:532
PointMatcher< float >
PointMatcherSupport::setLogger
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
Definition: Logger.cpp:98
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
test.x
x
Definition: test.py:4
kitti-batch-convert.out
string out
Definition: kitti-batch-convert.py:7
PMIO
PointMatcherIO< float > PMIO
Definition: eval_solution.cpp:62
PointMatcher< float >::Matrix
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
validateArgs
void validateArgs(int argc, char *argv[])
Definition: compute_overlap.cpp:270
PointMatcherIO::FileInfoVector
A vector of file info, to be used in batch processing.
Definition: IO.h:245
PointMatcher< float >::swapDataPoints
static void swapDataPoints(DataPoints &a, DataPoints &b)
Exchange in place point clouds a and b, with no data copy.
Definition: pointmatcher/DataPoints.cpp:1022
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
setupArgs
void setupArgs(int argc, char *argv[], unsigned int &startId, unsigned int &endId, string &extension)
std
TP
PM::TransformationParameters TP
Definition: eval_solution.cpp:63
PointMatcherSupport::computeDensity
T computeDensity(const typename PointMatcher< T >::Matrix &NN)
Definition: libpointmatcher/pointmatcher/DataPointsFilters/utils/utils.h:106
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:141
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
DP
PM::DataPoints DP
Definition: convert.cpp:45
PM
PointMatcher< float > PM
Definition: eval_solution.cpp:61
main
int main(int argc, char *argv[])
Definition: compute_overlap.cpp:57
PointMatcher::DataPoints::load
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
Definition: pointmatcher/IO.cpp:375
PointMatcher.h
public interface
IO.h
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:554
PointMatcher::DataPoints::save
void save(const std::string &fileName, bool binary=false) const
Save a point cloud to a file, determine format from extension.
Definition: pointmatcher/IO.cpp:809
PointMatcher< float >::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182


mp2p_icp
Author(s):
autogenerated on Wed Oct 23 2024 02:45:38