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 }
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
PointPointMinimizer
Definition: BVH_Example.cpp:12
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Box2d
AlignedBox< double, 2 > Box2d
Definition: BVH_Example.cpp:6
PointPointMinimizer::calls
int calls
Definition: BVH_Example.cpp:22
PointPointMinimizer::Scalar
double Scalar
Definition: BVH_Example.cpp:15
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
Eigen::bounding_box
Box2d bounding_box(const Vector2d &v)
Definition: BVH_Example.cpp:9
Eigen::KdBVH
A simple bounding volume hierarchy based on AlignedBox.
Definition: KdBVH.h:68
PointPointMinimizer::minimumOnObjectObject
double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2)
Definition: BVH_Example.cpp:20
PointPointMinimizer::minimumOnVolumeVolume
double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2)
Definition: BVH_Example.cpp:17
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
Eigen::AlignedBox
An axis aligned box.
Definition: ForwardDeclarations.h:292
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
PointPointMinimizer::PointPointMinimizer
PointPointMinimizer()
Definition: BVH_Example.cpp:14
main
int main()
Definition: BVH_Example.cpp:25
PointPointMinimizer::minimumOnObjectVolume
double minimumOnObjectVolume(const Vector2d &v, const Box2d &r)
Definition: BVH_Example.cpp:19
v2
Vector v2
Definition: testSerializationBase.cpp:39
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
Eigen::AlignedBox::squaredExteriorDistance
EIGEN_DEVICE_FUNC Scalar squaredExteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:408
PointPointMinimizer::minimumOnVolumeObject
double minimumOnVolumeObject(const Box2d &r, const Vector2d &v)
Definition: BVH_Example.cpp:18
min
#define min(a, b)
Definition: datatypes.h:19
max
#define max(a, b)
Definition: datatypes.h:20
Eigen::BVMinimize
Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
Definition: BVAlgorithms.h:219
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
v1
Vector v1
Definition: testSerializationBase.cpp:38


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:03