BVH_Example.cpp
Go to the documentation of this file.
1 #include <Eigen/StdVector>
2 #include <unsupported/Eigen/BVH>
3 #include <iostream>
4 
5 using namespace Eigen;
7 
8 namespace Eigen {
9  Box2d bounding_box(const Vector2d &v) { return Box2d(v, v); } //compute the bounding box of a single point
10 }
11 
12 struct PointPointMinimizer //how to compute squared distances between points and rectangles
13 {
14  PointPointMinimizer() : calls(0) {}
15  typedef double Scalar;
16 
17  double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
18  double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); }
19  double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); }
20  double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); }
21 
22  int calls;
23 };
24 
25 int main()
26 {
27  typedef std::vector<Vector2d, aligned_allocator<Vector2d> > StdVectorOfVector2d;
28  StdVectorOfVector2d redPoints, bluePoints;
29  for(int i = 0; i < 100; ++i) { //initialize random set of red points and blue points
30  redPoints.push_back(Vector2d::Random());
31  bluePoints.push_back(Vector2d::Random());
32  }
33 
34  PointPointMinimizer minimizer;
35  double minDistSq = std::numeric_limits<double>::max();
36 
37  //brute force to find closest red-blue pair
38  for(int i = 0; i < (int)redPoints.size(); ++i)
39  for(int j = 0; j < (int)bluePoints.size(); ++j)
40  minDistSq = std::min(minDistSq, minimizer.minimumOnObjectObject(redPoints[i], bluePoints[j]));
41  std::cout << "Brute force distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;
42 
43  //using BVH to find closest red-blue pair
44  minimizer.calls = 0;
45  KdBVH<double, 2, Vector2d> redTree(redPoints.begin(), redPoints.end()), blueTree(bluePoints.begin(), bluePoints.end()); //construct the trees
46  minDistSq = BVMinimize(redTree, blueTree, minimizer); //actual BVH minimization call
47  std::cout << "BVH distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;
48 
49  return 0;
50 }
#define max(a, b)
Definition: datatypes.h:20
Vector v2
double minimumOnVolumeObject(const Box2d &r, const Vector2d &v)
Definition: BVH_Example.cpp:18
Vector v1
Box2d bounding_box(const Vector2d &v)
Definition: BVH_Example.cpp:9
#define min(a, b)
Definition: datatypes.h:19
Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
Definition: BVAlgorithms.h:219
int main()
Definition: BVH_Example.cpp:25
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
An axis aligned box.
double minimumOnObjectVolume(const Vector2d &v, const Box2d &r)
Definition: BVH_Example.cpp:19
Array< int, Dynamic, 1 > v
static const double r2
EIGEN_DEVICE_FUNC Scalar squaredExteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:408
static const double r1
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
AlignedBox< double, 2 > Box2d
Definition: BVH_Example.cpp:6
double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2)
Definition: BVH_Example.cpp:20
A simple bounding volume hierarchy based on AlignedBox.
Definition: KdBVH.h:68
std::ptrdiff_t j
double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2)
Definition: BVH_Example.cpp:17


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:59