00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "nabo/nabo.h"
00033 #include "helpers.h"
00034
00035 #include <iostream>
00036 #include <fstream>
00037 #include <stdexcept>
00038
00039 using namespace std;
00040 using namespace Nabo;
00041
00042 template<typename T, typename CloudType>
00043 struct Loader
00044 {
00045 void loadMatrix(const char *fileName)
00046 {
00047 data = load<T>(fileName);
00048 }
00049 CloudType getValue() const
00050 {
00051 return data;
00052 }
00053 private:
00054 CloudType data;
00055 };
00056
00057 template<typename T>
00058 struct Loader<T, Eigen::Map<const Eigen::Matrix<T, 3, Eigen::Dynamic>, Eigen::Aligned> >
00059 {
00060 void loadMatrix(const char *fileName)
00061 {
00062 data = load<T>(fileName);
00063 }
00064 Eigen::Map<const Eigen::Matrix<T, 3, Eigen::Dynamic>, Eigen::Aligned> getValue() const
00065 {
00066 return Eigen::Map<const Eigen::Matrix<T, 3, Eigen::Dynamic>, Eigen::Aligned>(data.data(), 3, data.cols());
00067 }
00068
00069 private:
00070 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> data;
00071 };
00072
00073 template<typename T, typename CloudType>
00074 void validate(const char *fileName, const int K, const int dim, const int method, const T maxRadius)
00075 {
00076 typedef Nabo::NearestNeighbourSearch<T, CloudType> NNS;
00077 typedef vector<NNS*> NNSV;
00078 typedef typename NNS::Matrix Matrix;
00079 typedef typename NNS::Vector Vector;
00080 typedef typename NNS::IndexMatrix IndexMatrix;
00081
00082 Loader<T, CloudType> loader;
00083 loader.loadMatrix(fileName);
00084
00085
00086 const CloudType d = loader.getValue();
00087 if (d.rows() != dim)
00088 {
00089 cerr << "Provided data has " << d.rows() << " dimensions, but the requested dimensions were " << dim << endl;
00090 exit(2);
00091 }
00092 if (K >= d.cols())
00093 {
00094 cerr << "Requested more nearest neighbour than points in the data set" << endl;
00095 exit(2);
00096 }
00097
00098
00099 NNSV nnss;
00100 unsigned searchTypeCount(NNS::SEARCH_TYPE_COUNT);
00101 #ifndef HAVE_OPENCL
00102 searchTypeCount -= 3;
00103 #endif // HAVE_OPENCL
00104 for (unsigned i = 0; i < searchTypeCount; ++i)
00105 nnss.push_back(NNS::create(d, d.rows(), typename NNS::SearchType(i)));
00106
00107
00108
00109
00110 const int itCount(method != -1 ? method : d.cols() * 2);
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156 Matrix q(createQuery<T>(d, itCount, method));
00157 IndexMatrix indexes_bf(K, q.cols());
00158 Matrix dists2_bf(K, q.cols());
00159 nnss[0]->knn(q, indexes_bf, dists2_bf, K, 0, NNS::SORT_RESULTS, maxRadius);
00160 assert(indexes_bf.cols() == q.cols());
00161 for (size_t j = 1; j < nnss.size(); ++j)
00162 {
00163 IndexMatrix indexes_kdtree(K, q.cols());
00164 Matrix dists2_kdtree(K, q.cols());
00165 nnss[j]->knn(q, indexes_kdtree, dists2_kdtree, K, 0, NNS::SORT_RESULTS, maxRadius);
00166 if (indexes_bf.rows() != K)
00167 {
00168 cerr << "Different number of points found between brute force and request" << endl;
00169 exit(3);
00170 }
00171 if (indexes_bf.cols() != indexes_kdtree.cols())
00172 {
00173 cerr << "Different number of points found between brute force and NNS type "<< j << endl;
00174 exit(3);
00175 }
00176
00177 for (int i = 0; i < q.cols(); ++i)
00178 {
00179 for (size_t k = 0; k < size_t(K); ++k)
00180 {
00181 if (dists2_bf(k,i) == numeric_limits<float>::infinity())
00182 continue;
00183 const int pbfi(indexes_bf(k,i));
00184 const Vector pbf(d.col(pbfi));
00185 const int pkdt(indexes_kdtree(k,i));
00186 if (pkdt < 0 || pkdt >= d.cols())
00187 {
00188 cerr << "Method " << j << ", query point " << i << ", neighbour " << k << " of " << K << " has invalid index " << pkdt << " out of range [0:" << d.cols() << "[" << endl;
00189 exit(4);
00190 }
00191 const Vector pkdtree(d.col(pkdt));
00192 const Vector pq(q.col(i));
00193 const float distDiff(fabsf((pbf-pq).squaredNorm() - (pkdtree-pq).squaredNorm()));
00194 if (distDiff > numeric_limits<float>::epsilon())
00195 {
00196 cerr << "Method " << j << ", query point " << i << ", neighbour " << k << " of " << K << " is different between bf and kdtree (dist2 " << distDiff << ")\n";
00197 cerr << "* query point:\n";
00198 cerr << pq << "\n";
00199 cerr << "* indexes " << pbfi << " (bf) vs " << pkdt << " (kdtree)\n";
00200 cerr << "* coordinates:\n";
00201 cerr << "bf: (dist " << (pbf-pq).norm() << ")\n";
00202 cerr << pbf << "\n";
00203 cerr << "kdtree (dist " << (pkdtree-pq).norm() << ")\n";
00204 cerr << pkdtree << endl;
00205 cerr << "* bf neighbours:\n";
00206 for (int l = 0; l < K; ++l)
00207 cerr << indexes_bf(l,i) << " (dist " << (d.col(indexes_bf(l,i))-pq).norm() << ")\n";
00208 cerr << "* kdtree neighbours:\n";
00209 for (int l = 0; l < K; ++l)
00210 cerr << indexes_kdtree(l,i) << " (dist " << (d.col(indexes_kdtree(l,i))-pq).norm() << ")\n";
00211 exit(4);
00212 }
00213 }
00214 }
00215 }
00216
00217
00218
00219
00220
00221
00222
00223
00224 for (typename NNSV::iterator it(nnss.begin()); it != nnss.end(); ++it)
00225 delete (*it);
00226 }
00227
00228 int main(int argc, char* argv[])
00229 {
00230 if (argc < 4)
00231 {
00232 cerr << "Usage " << argv[0] << " DATA K DIM METHOD [MAX_RADIUS]" << endl;
00233 return 1;
00234 }
00235
00236 const int K(atoi(argv[2]));
00237 const int dim(atoi(argv[3]));
00238 const int method(atoi(argv[4]));
00239 const float maxRadius(argc >= 6 ? float(atof(argv[5])) : numeric_limits<float>::infinity());
00240
00241 if (dim == 3)
00242 {
00243 validate<float, Eigen::MatrixXf>(argv[1], K, dim, method, maxRadius);
00244 validate<float, Eigen::Matrix3Xf>(argv[1], K, dim, method, maxRadius);
00245 validate<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >(argv[1], K, dim, method, maxRadius);
00246 } else
00247 {
00248 validate<float, Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> >(argv[1], K, dim, method, maxRadius);
00249 }
00250
00251
00252 return 0;
00253 }