42 #include <boost/format.hpp>
43 #include <boost/filesystem/path.hpp>
44 #include <boost/filesystem/operations.hpp>
45 #include <boost/lexical_cast.hpp>
51 void setupArgs(
int argc,
char *argv[],
unsigned int& startId,
unsigned int& endId,
string& extension);
57 int main(
int argc,
char *argv[])
65 typedef PM::DataPoints
DP;
66 typedef PM::Matches Matches;
70 bool debugMode =
false;
78 std::shared_ptr<PM::Transformation> rigidTransform;
79 rigidTransform =
PM::get().TransformationRegistrar.create(
"RigidTransformation");
81 PM::Transformations transformations;
82 transformations.push_back(rigidTransform);
84 DP reading, reference;
85 TP Tread = TP::Identity(4,4);
87 TP Tref = TP::Identity(4,4);
89 unsigned startingI = 0;
90 unsigned listSizeI = list.size();
91 unsigned listSizeJ = list.size();
94 startingI = boost::lexical_cast<unsigned>(argv[2]);
95 listSizeI = startingI + 1;
98 PM::Matrix overlapResults = PM::Matrix::Ones(listSizeJ, listSizeI);
100 for(
unsigned i = startingI; i < listSizeI; i++)
102 unsigned startingJ = i+1;
105 startingJ = boost::lexical_cast<unsigned>(argv[3]);
106 listSizeJ = startingJ + 1;
108 for(
unsigned j = startingJ; j < listSizeJ; j++)
111 reading =
DP::load(list[i].readingFileName);
112 reference =
DP::load(list[j].readingFileName);
114 cout <<
"Point cloud loaded" << endl;
117 if(list[i].groundTruthTransformation.rows() != 0)
119 Tread = list[i].groundTruthTransformation;
120 Tref = list[j].groundTruthTransformation;
124 cout <<
"ERROR: fields gTXX (i.e., ground truth matrix) is required" << endl;
129 transformations.apply(reading, Tread);
130 transformations.apply(reference, Tref);
133 std::shared_ptr<PM::DataPointsFilter> subSample =
134 PM::get().DataPointsFilterRegistrar.create(
135 "RandomSamplingDataPointsFilter",
139 std::shared_ptr<PM::DataPointsFilter> maxDensity =
140 PM::get().DataPointsFilterRegistrar.create(
141 "MaxDensityDataPointsFilter"
152 PM::get().DataPointsFilterRegistrar.create(
153 "SurfaceNormalDataPointsFilter",
156 {
"keepDensities",
"1"}
160 reading = subSample->filter(reading);
162 reading = maxDensity->filter(reading);
164 const Matrix inliersRead = Matrix::Zero(1, reading.
features.cols());
167 reference = subSample->filter(reference);
169 reference = maxDensity->filter(reference);
170 const Matrix inliersRef = Matrix::Zero(1, reference.
features.cols());
175 DP target = reference;
177 for(
int l = 0; l < 2; l++)
179 const int selfPtsCount =
self.
features.cols();
180 const int targetPtsCount = target.
features.cols();
185 std::shared_ptr<PM::Matcher> matcherSelf =
186 PM::get().MatcherRegistrar.create(
191 std::shared_ptr<PM::Matcher> matcherTarget =
192 PM::get().MatcherRegistrar.create(
193 "KDTreeVarDistMatcher",
196 {
"maxDistField",
"maxSearchDist"}
200 matcherSelf->init(
self);
201 matcherTarget->init(target);
203 Matches selfMatches(knn, selfPtsCount);
204 selfMatches = matcherSelf->findClosests(
self);
206 const Matrix maxSearchDist = selfMatches.dists.colwise().maxCoeff().cwiseSqrt();
207 self.addDescriptor(
"maxSearchDist", maxSearchDist);
209 Matches targetMatches(knnAll, targetPtsCount);
210 targetMatches = matcherTarget->findClosests(
self);
212 BOOST_AUTO(inlierSelf,
self.getDescriptorViewByName(
"inliers"));
214 for(
int i = 0; i < selfPtsCount; i++)
216 for(
int k = 0; k < knnAll; k++)
218 if (targetMatches.dists(k, i) != Matches::InvalidDist)
220 inlierSelf(0,i) = 1.0;
221 inlierTarget(0,targetMatches.ids(k, i)) = 1.0;
230 const BOOST_AUTO(finalInlierSelf,
self.getDescriptorViewByName(
"inliers"));
232 const double selfRatio = (finalInlierSelf.array() > 0).count()/(double)finalInlierSelf.cols();
233 const double targetRatio = (finalInlierTarget.array() > 0).count()/(double)finalInlierTarget.cols();
235 cout << i <<
" -> " << j <<
": " << selfRatio << endl;
236 cout << j <<
" -> " << i <<
": " << targetRatio << endl;
240 self.save(
"scan_i.vtk");
241 target.
save(
"scan_j.vtk");
245 overlapResults(j,i) = selfRatio;
246 overlapResults(i,j) = targetRatio;
253 std::fstream outFile;
255 for(
int x=0;
x < overlapResults.rows();
x++)
257 for(
int y=0; y < overlapResults.cols(); y++)
259 outFile << overlapResults(
x, y) <<
", ";
272 if (!(argc == 2 || argc == 4))
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;
279 cerr <<
" Example: " << endl;
280 cerr <<
" $ " << argv[0] <<
" ../example/data/carCloudList.csv" << endl;