examples/test_kdtree.cpp

   1 #define KDTREE_DEFINE_OSTREAM_OPERATORS
   2
   3 // Make SURE all our asserts() are checked
   4 #undef NDEBUG
   5
   6 #include <kdtree++/kdtree.hpp>
   7
   8 #include <deque>
   9 #include <iostream>
  10 #include <vector>
  11 #include <limits>
  12 #include <functional>
  13 #include <set>
  14
  15 // used to ensure all triplets that are accessed via the operator<< are initialised.
  16 std::set<const void*> registered;
  17
  18 struct triplet
  19 {
  20   typedef int value_type;
  21
  22   triplet(value_type a, value_type b, value_type c)
  23   {
  24     d[0] = a;
  25     d[1] = b;
  26     d[2] = c;
  27     bool reg_ok = (registered.find(this) == registered.end());
  28     assert(reg_ok);
  29     registered.insert(this).second;
  30   }
  31
  32   triplet(const triplet & x)
  33   {
  34     d[0] = x.d[0];
  35     d[1] = x.d[1];
  36     d[2] = x.d[2];
  37     bool reg_ok = (registered.find(this) == registered.end());
  38     assert(reg_ok);
  39     registered.insert(this).second;
  40   }
  41
  42   ~triplet()
  43   {
  44     bool unreg_ok = (registered.find(this) != registered.end());
  45     assert(unreg_ok);
  46     registered.erase(this);
  47   }
  48
  49   double distance_to(triplet const& x) const
  50   {
  51      double dist = 0;
  52      for (int i = 0; i != 3; ++i)
  53         dist += (d[i]-x.d[i])*(d[i]-x.d[i]);
  54      return std::sqrt(dist);
  55   }
  56
  57   inline value_type operator[](size_t const N) const { return d[N]; }
  58
  59   value_type d[3];
  60 };
  61
  62
  63
  64 // same as triplet, except with the values reversed.
  65 struct alternate_triplet
  66 {
  67   typedef int value_type;
  68
  69   alternate_triplet(const triplet & x)
  70   {
  71     d[0] = x.d[2];
  72     d[1] = x.d[1];
  73     d[2] = x.d[0];
  74   }
  75
  76   inline value_type operator[](size_t const N) const { return d[2-N]; }
  77
  78   value_type d[3];
  79 };
  80
  81 inline bool operator==(triplet const& A, triplet const& B) {
  82   return A.d[0] == B.d[0] && A.d[1] == B.d[1] && A.d[2] == B.d[2];
  83 }
  84
  85 std::ostream& operator<<(std::ostream& out, triplet const& T)
  86 {
  87   assert(registered.find(&T) != registered.end());
  88   return out << '(' << T.d[0] << ',' << T.d[1] << ',' << T.d[2] << ')';
  89 }
  90
  91 inline double tac( triplet t, size_t k ) { return t[k]; }
  92
  93 // use tac as a class instead of a function,
  94 // can access more than one type with just 1 definition.
  95 struct alternate_tac
  96 {
  97    typedef double result_type;
  98    double operator()( triplet const& t, size_t k ) const { return t[k]; }
  99    double operator()( alternate_triplet const& t, size_t k ) const { return t[k]; }
 100 };
 101
 102
 103 typedef KDTree::KDTree<3, triplet, std::pointer_to_binary_function<triplet,size_t,double> > tree_type;
 104
 105 struct Predicate
 106 {
 107    bool operator()( triplet const& t ) const
 108    {
 109       return t[0] > 3;  // anything, we are currently testing that it compiles.
 110    }
 111 };
 112
 113 // never finds anything
 114 struct FalsePredicate
 115 {
 116    bool operator()( triplet const& t ) const { return false; }
 117 };
 118
 119 int main()
 120 {
 121    // check that it'll find nodes exactly MAX away
 122    {
 123       tree_type exact_dist(std::ptr_fun(tac));
 124         triplet c0(5, 4, 0);
 125         exact_dist.insert(c0);
 126         triplet target(7,4,0);
 127
 128       std::pair<tree_type::const_iterator,double> found = exact_dist.find_nearest(target,2);
 129       assert(found.first != exact_dist.end());
 130       assert(found.second == 2);
 131       std::cout << "Test find_nearest(), found at exact distance away from " << target << ", found " << *found.first << std::endl;
 132    }
 133
 134    // do the same test, except use alternate_triplet as the search key
 135    {
 136       // NOTE: stores triplet, but we search with alternate_triplet
 137       typedef KDTree::KDTree<3, triplet, alternate_tac> alt_tree;
 138
 139       triplet actual_target(7,0,0);
 140
 141       alt_tree tree;
 142       tree.insert( triplet(0, 0, 7) );
 143       tree.insert( triplet(0, 0, 7) );
 144       tree.insert( triplet(0, 0, 7) );
 145       tree.insert( triplet(3, 0, 0) );
 146       tree.insert( actual_target );
 147       tree.optimise();
 148
 149       alternate_triplet target( actual_target );
 150
 151       std::pair<alt_tree::const_iterator,double> found = tree.find_nearest(target);
 152       assert(found.first != tree.end());
 153       std::cout << "Test with alternate search type, found: " << *found.first << ", wanted " << actual_target << std::endl;
 154       assert(found.second == 0);
 155       assert(*found.first == actual_target);
 156    }
 157
 158
 159    {
 160       tree_type exact_dist(std::ptr_fun(tac));
 161         triplet c0(5, 2, 0);
 162         exact_dist.insert(c0);
 163         triplet target(7,4,0);
 164
 165         // call find_nearest without a range value - it found a compile error earlier.
 166       std::pair<tree_type::const_iterator,double> found = exact_dist.find_nearest(target);
 167       assert(found.first != exact_dist.end());
 168       std::cout << "Test find_nearest(), found at exact distance away from " << target << ", found " << *found.first << " @ " << found.second << " should be " << std::sqrt(8) << std::endl;
 169       assert(found.second == std::sqrt(8));
 170    }
 171
 172    {
 173       tree_type exact_dist(std::ptr_fun(tac));
 174         triplet c0(5, 2, 0);
 175         exact_dist.insert(c0);
 176         triplet target(7,4,0);
 177
 178       std::pair<tree_type::const_iterator,double> found = exact_dist.find_nearest(target,std::sqrt(8));
 179       assert(found.first != exact_dist.end());
 180       std::cout << "Test find_nearest(), found at exact distance away from " << target << ", found " << *found.first << " @ " << found.second << " should be " << std::sqrt(8) << std::endl;
 181       assert(found.second == std::sqrt(8));
 182    }
 183
 184   tree_type src(std::ptr_fun(tac));
 185
 186   triplet c0(5, 4, 0); src.insert(c0);
 187   triplet c1(4, 2, 1); src.insert(c1);
 188   triplet c2(7, 6, 9); src.insert(c2);
 189   triplet c3(2, 2, 1); src.insert(c3);
 190   triplet c4(8, 0, 5); src.insert(c4);
 191   triplet c5(5, 7, 0); src.insert(c5);
 192   triplet c6(3, 3, 8); src.insert(c6);
 193   triplet c7(9, 7, 3); src.insert(c7);
 194   triplet c8(2, 2, 6); src.insert(c8);
 195   triplet c9(2, 0, 6); src.insert(c9);
 196
 197   std::cout << src << std::endl;
 198
 199   src.erase(c0);
 200   src.erase(c1);
 201   src.erase(c3);
 202   src.erase(c5);
 203
 204   src.optimise();
 205
 206
 207   // test the efficient_replace_and_optimise()
 208   tree_type eff_repl = src;
 209   {
 210      std::vector<triplet> vec;
 211      // erased above as part of test vec.push_back(triplet(5, 4, 0));
 212      // erased above as part of test vec.push_back(triplet(4, 2, 1));
 213      vec.push_back(triplet(7, 6, 9));
 214      // erased above as part of test vec.push_back(triplet(2, 2, 1));
 215      vec.push_back(triplet(8, 0, 5));
 216      // erased above as part of test vec.push_back(triplet(5, 7, 0));
 217      vec.push_back(triplet(3, 3, 8));
 218      vec.push_back(triplet(9, 7, 3));
 219      vec.push_back(triplet(2, 2, 6));
 220      vec.push_back(triplet(2, 0, 6));
 221
 222      eff_repl.clear();
 223      eff_repl.efficient_replace_and_optimise(vec);
 224   }
 225
 226
 227   std::cout << std::endl << src << std::endl;
 228
 229   tree_type copied(src);
 230   std::cout << copied << std::endl;
 231   tree_type assigned;
 232   assigned = src;
 233   std::cout << assigned << std::endl;
 234
 235   for (int loop = 0; loop != 4; ++loop)
 236     {
 237       tree_type * target;
 238       switch (loop)
 239         {
 240         case 0: std::cout << "Testing plain construction" << std::endl;
 241           target = &src;
 242           break;
 243
 244         case 1: std::cout << "Testing copy-construction" << std::endl;
 245           target = &copied;
 246           break;
 247
 248         case 2: std::cout << "Testing assign-construction" << std::endl;
 249           target = &assigned;
 250           break;
 251
 252    default:
 253         case 4: std::cout << "Testing efficient-replace-and-optimise" << std::endl;
 254           target = &eff_repl;
 255           break;
 256         }
 257       tree_type & t = *target;
 258
 259       int i=0;
 260       for (tree_type::const_iterator iter=t.begin(); iter!=t.end(); ++iter, ++i);
 261       std::cout << "iterator walked through " << i << " nodes in total" << std::endl;
 262       if (i!=6)
 263         {
 264           std::cerr << "Error: does not tally with the expected number of nodes (6)" << std::endl;
 265           return 1;
 266         }
 267       i=0;
 268       for (tree_type::const_reverse_iterator iter=t.rbegin(); iter!=t.rend(); ++iter, ++i);
 269       std::cout << "reverse_iterator walked through " << i << " nodes in total" << std::endl;
 270       if (i!=6)
 271         {
 272           std::cerr << "Error: does not tally with the expected number of nodes (6)" << std::endl;
 273           return 1;
 274         }
 275
 276       triplet s(5, 4, 3);
 277       std::vector<triplet> v;
 278       unsigned int const RANGE = 3;
 279
 280       size_t count = t.count_within_range(s, RANGE);
 281       std::cout << "counted " << count
 282                 << " nodes within range " << RANGE << " of " << s << ".\n";
 283       t.find_within_range(s, RANGE, std::back_inserter(v));
 284
 285       std::cout << "found   " << v.size() << " nodes within range " << RANGE
 286                 << " of " << s << ":\n";
 287       std::vector<triplet>::const_iterator ci = v.begin();
 288       for (; ci != v.end(); ++ci)
 289         std::cout << *ci << " ";
 290       std::cout << "\n" << std::endl;
 291
 292       std::cout << std::endl << t << std::endl;
 293
 294       // search for all the nodes at exactly 0 dist away
 295       for (tree_type::const_iterator target = t.begin(); target != t.end(); ++target)
 296       {
 297          std::pair<tree_type::const_iterator,double> found = t.find_nearest(*target,0);
 298          assert(found.first != t.end());
 299          assert(*found.first == *target);
 300          std::cout << "Test find_nearest(), found at exact distance away from " << *target << ", found " << *found.first << std::endl;
 301       }
 302
 303       {
 304          const double small_dist = 0.0001;
 305          std::pair<tree_type::const_iterator,double> notfound = t.find_nearest(s,small_dist);
 306          std::cout << "Test find_nearest(), nearest to " << s << " within " << small_dist << " should not be found" << std::endl;
 307
 308          if (notfound.first != t.end())
 309          {
 310             std::cout << "ERROR found a node at dist " << notfound.second << " : " << *notfound.first << std::endl;
 311             std::cout << "Actual distance = " << s.distance_to(*notfound.first) << std::endl;
 312          }
 313
 314          assert(notfound.first == t.end());
 315       }
 316
 317       {
 318          std::pair<tree_type::const_iterator,double> nif = t.find_nearest_if(s,std::numeric_limits<double>::max(),Predicate());
 319          std::cout << "Test find_nearest_if(), nearest to " << s << " @ " << nif.second << ": " << *nif.first << std::endl;
 320
 321          std::pair<tree_type::const_iterator,double> cantfind = t.find_nearest_if(s,std::numeric_limits<double>::max(),FalsePredicate());
 322          std::cout << "Test find_nearest_if(), nearest to " << s << " should never be found (predicate too strong)" << std::endl;
 323          assert(cantfind.first == t.end());
 324       }
 325
 326
 327
 328
 329       {
 330       std::pair<tree_type::const_iterator,double> found = t.find_nearest(s,std::numeric_limits<double>::max());
 331       std::cout << "Nearest to " << s << " @ " << found.second << " " << *found.first << std::endl;
 332       std::cout << "Should be " << found.first->distance_to(s) << std::endl;
 333       // NOTE: the assert does not check for an exact match, as it is not exact when -O2 or -O3 is
 334       // switched on.  Some sort of optimisation makes the math inexact.
 335       assert( fabs(found.second - found.first->distance_to(s)) < std::numeric_limits<double>::epsilon() );
 336       }
 337
 338       {
 339       triplet s2(10, 10, 2);
 340       std::pair<tree_type::const_iterator,double> found = t.find_nearest(s2,std::numeric_limits<double>::max());
 341       std::cout << "Nearest to " << s2 << " @ " << found.second << " " << *found.first << std::endl;
 342       std::cout << "Should be " << found.first->distance_to(s2) << std::endl;
 343       // NOTE: the assert does not check for an exact match, as it is not exact when -O2 or -O3 is
 344       // switched on.  Some sort of optimisation makes the math inexact.
 345       assert( fabs(found.second - found.first->distance_to(s2)) < std::numeric_limits<double>::epsilon() );
 346       }
 347
 348       std::cout << std::endl;
 349
 350       std::cout << t << std::endl;
 351
 352       // Testing iterators
 353       {
 354         std::cout << "Testing iterators" << std::endl;
 355
 356         t.erase(c2);
 357         t.erase(c4);
 358         t.erase(c6);
 359         t.erase(c7);
 360         t.erase(c8);
 361         //    t.erase(c9);
 362
 363         std::cout << std::endl << t << std::endl;
 364
 365         std::cout << "Forward iterator test..." << std::endl;
 366         std::vector<triplet> forwards;
 367         for (tree_type::iterator i = t.begin(); i != t.end(); ++i)
 368           { std::cout << *i << " " << std::flush; forwards.push_back(*i); }
 369         std::cout << std::endl;
 370         std::cout << "Reverse iterator test..." << std::endl;
 371         std::vector<triplet> backwards;
 372         for (tree_type::reverse_iterator i = t.rbegin(); i != t.rend(); ++i)
 373           { std::cout << *i << " " << std::flush; backwards.push_back(*i); }
 374         std::cout << std::endl;
 375         std::reverse(backwards.begin(),backwards.end());
 376         assert(backwards == forwards);
 377       }
 378     }
 379
 380
 381   // Walter reported that the find_within_range() wasn't giving results that were within
 382   // the specified range... this is the test.
 383   {
 384      tree_type tree(std::ptr_fun(tac));
 385      tree.insert( triplet(28.771200,16.921600,-2.665970) );
 386      tree.insert( triplet(28.553101,18.649700,-2.155560) );
 387      tree.insert( triplet(28.107500,20.341400,-1.188940) );
 388      tree.optimise();
 389
 390      std::deque< triplet > vectors;
 391      triplet sv(18.892500,20.341400,-1.188940);
 392      tree.find_within_range(sv, 10.0f, std::back_inserter(vectors));
 393
 394      std::cout << std::endl << "Test find_with_range( " << sv << ", 10.0f) found " << vectors.size() << " candidates." << std::endl;
 395
 396      // double-check the ranges
 397      for (std::deque<triplet>::iterator v = vectors.begin(); v != vectors.end(); ++v)
 398      {
 399         double dist = sv.distance_to(*v);
 400         std::cout << "  " << *v << " dist=" << dist << std::endl;
 401         if (dist > 10.0f)
 402            std::cout << "    This point is too far! But that is by design, its within a 'box' with a 'radius' of 10, not a sphere with a radius of 10" << std::endl;
 403         // Not a valid test, it can be greater than 10 if the point is in the corners of the box.
 404         // assert(dist <= 10.0f);
 405      }
 406   }
 407
 408
 409   return 0;
 410 }
 411
 412 /* COPYRIGHT --
 413  *
 414  * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 415  * libkdtree++ is (c) 2004-2007 Martin F. Krafft <libkdtree@pobox.madduck.net>
 416  * and Sylvain Bougerel <sylvain.bougerel.devel@gmail.com> distributed under the
 417  * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 418  * root for more information.
 419  *
 420  * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 421  * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 422  * OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 423  */