geolib2
test_geolib.cpp
Go to the documentation of this file.
1 #include <geolib/Octree.h>
2 #include <geolib/Box.h>
3 #include <geolib/HeightMap.h>
4 
5 #include <geolib/io/import.h>
6 #include <geolib/serialization.h>
7 
8 #include <geolib/sensors/DepthCamera.h>
9 #include <geolib/sensors/LaserRangeFinder.h>
10 
11 #include <profiling/Timer.h>
12 
13 #include <opencv2/highgui/highgui.hpp>
14 
15 #include <fstream>
16 #include <vector>
17 
18 double CANVAS_WIDTH = 640;
19 double CANVAS_HEIGHT = 480;
20 
21 double renderDepthCamera(cv::Mat& image, const geo::Shape& shape, bool show)
22 {
23  geo::DepthCamera cam(CANVAS_WIDTH, CANVAS_HEIGHT,
24  554.2559327880068 * CANVAS_WIDTH / 640, 554.2559327880068 * CANVAS_HEIGHT / 480,
25  320.5 * CANVAS_WIDTH / 640, 240.5 * CANVAS_HEIGHT / 480,
26  0, 0);
27 
28  Timer timer;
29  timer.start();
30 
31  int N = 0;
32  for(double angle = 0; angle < 6.283; angle += 0.05) {
33  if (show) {
34  image = cv::Mat(image.rows, image.cols, CV_32FC1, 0.0);
35  }
36 
37 
38  geo::Pose3D pose(0, -0.5, -5, -1.57, angle, 0);
39  cam.rasterize(shape, pose, image);
40 
41  if (show) {
42  cv::imshow("visualization", image / 10);
43  cv::waitKey(30);
44  }
45 
46  ++N;
47  }
48 
49  return timer.getElapsedTimeInMilliSec() / N;
50 }
51 
52 double renderLRF(cv::Mat& image, const geo::Shape& shape, bool show) {
53  geo::LaserRangeFinder lrf;
54  lrf.setAngleLimits(-2, 2);
55  lrf.setNumBeams(1000);
56  lrf.setRangeLimits(0, 10);
57 
58  Timer timer;
59  timer.start();
60 
61  int N = 0;
62  for(double angle = 0; angle < 6.283; angle += 0.05) {
63  geo::Pose3D pose(5, 0, -0.5, -1.57, 0, angle);
64 
65  std::vector<double> ranges;
66  lrf.render(shape, geo::Pose3D(0, 0, 0), pose, ranges);
67 
68  if (show) {
69  image = cv::Mat(image.rows, image.cols, CV_32FC1, 0.0);
70 
71  const std::vector<double>& angles = lrf.getAngles();
72  for(unsigned int i = 0; i < angles.size(); ++i) {
73  geo::Vector3 p = lrf.polarTo2D(angles[i], ranges[i]);
74  double x = (p.x * 25) + image.cols / 2;
75  double y = (p.y * 25) + image.rows / 2;
76 
77  image.at<float>(y, x) = 1;
78  }
79  std::cout << std::endl;
80  cv::imshow("visualization", image);
81  cv::waitKey(30);
82  }
83 
84  ++N;
85  }
86 
87  return timer.getElapsedTimeInMilliSec() / N;
88 }
89 
90 
91 int main(int argc, char **argv) {
92  geo::serialization::registerDeserializer<geo::Shape>();
93 
94  geo::ShapePtr mesh;
95  if (argc > 1) {
96  std::string filename = argv[1];
97 
98  double scale = 1;
99  if (argc > 2) {
100  scale = atof(argv[2]);
101  }
102 
103  // first try own file format
104  mesh = geo::serialization::fromFile(filename);
105 
106  if (!mesh) {
107  // If fails, try using assimp
108  mesh = geo::io::readMeshFile(filename, scale);
109 
110  if (!mesh) {
111  std::cout << "Could not load " << filename << std::endl;
112  return 1;
113  }
114  }
115  }
116 
117  if (mesh) {
118  geo::serialization::toFile(mesh, "/tmp/test_geolib.geo");
119  mesh = geo::serialization::fromFile("/tmp/test_geolib.geo");
120  }
121 
122  geo::Octree tree(10);
123 
124  std::vector<geo::Vector3> points;
125 
126  for(double x = -5; x < 5; x += 0.5) {
127  for(double y = -5; y < 5; y += 0.5) {
128  for(double z = -5; z < 5; z += 0.5) {
129  points.push_back(geo::Vector3(x, y, z));
130  }
131  }
132  }
133 
134  Timer timer;
135  timer.start();
136  for(unsigned int i = 0; i < points.size(); ++i) {
137  tree.add(points[i]);
138  }
139  timer.stop();
140  std::cout << "Octree::add(Vector3):\t" << timer.getElapsedTimeInMilliSec() / points.size() << " ms" << std::endl;
141 
142 
143  geo::Octree table(4);
144  double res = table.setResolution(0.1);
145  std::cout << "True resolution = " << res << std::endl;
146 
147  for(double x = -0.8; x < 0.8; x += res) {
148  for(double y = -0.35; y < 0.35; y += res) {
149  table.add(geo::Vector3(x, y, 0.75));
150  }
151  }
152 
153  for(int mx = -1; mx <= 1; mx += 2) {
154  for(int my = -1; my <= 1; my += 2) {
155  if (mx != 1 || my != 1) {
156  for(double z = 0.05; z < 0.75; z += res) {
157  table.add(geo::Vector3(0.7 * mx, 0.25 * my, z));
158  }
159  }
160  }
161  }
162 
163  double distance;
164  geo::Ray r(geo::Vector3(0, 0, 5), geo::Vector3(0, 0, -1));
165 
166  int N = 10000;
167  Timer timer2;
168  timer2.start();
169  for(int i = 0; i < N; ++i) {
170  table.intersect(r, 0, 10, distance);
171  }
172  timer2.stop();
173  std::cout << "Octree::intersect(Ray):\t" << timer2.getElapsedTimeInMilliSec() / N << " ms" << std::endl;
174 
175  geo::Box b(geo::Vector3(-0.5, -0.5, 0.5), geo::Vector3(0.5, 0.5, 2));
176 
177  N = 10000;
178  Timer timer3;
179  timer3.start();
180  for(int i = 0; i < N; ++i) {
181  table.intersect(b);
182  }
183  timer3.stop();
184  std::cout << "Octree::intersect(Box):\t" << timer3.getElapsedTimeInMilliSec() / N << " ms" << std::endl;
185 
186  // * * * * * RAYTRACING * * * * * * *
187 
188  tree.clear();
189  std::vector<geo::Ray> rays;
190  for(double y = -2; y < 2; y += tree.getResolution()) {
191  for(double x = -2; x < 2; x += tree.getResolution()) {
192  geo::Ray r(geo::Vector3(x, y, 5), geo::Vector3(0, 0, -1));
193  r.setLength(5 - x / 2);
194  rays.push_back(r);
195  }
196  }
197 
198  Timer timer4;
199  timer4.start();
200  for(std::vector<geo::Ray>::iterator it = rays.begin(); it != rays.end(); ++it) {
201  tree.raytrace(*it, 0, it->getLength());
202  }
203  timer4.stop();
204  std::cout << "Octree::raytrace(Ray):\t" << timer4.getElapsedTimeInMilliSec() / rays.size() << " ms" << std::endl;
205 
206  rays.clear();
207  for(double y = -2; y < 2; y += tree.getResolution()*2) {
208  for(double x = -2; x < 2; x += tree.getResolution()*2) {
209  geo::Ray r(geo::Vector3(x, y, 5), geo::Vector3(0, 0, -1));
210  r.setLength(5 - y / 2);
211  rays.push_back(r);
212  }
213  }
214 
215 
216  Timer timer5;
217  timer5.start();
218  for(std::vector<geo::Ray>::iterator it = rays.begin(); it != rays.end(); ++it) {
219  tree.raytrace(*it, 0, it->getLength());
220  }
221  timer5.stop();
222  std::cout << "Octree::raytrace(Ray):\t" << timer5.getElapsedTimeInMilliSec() / rays.size() << " ms" << std::endl;
223 
224 
225  geo::Octree axis(2);
226  double res2 = table.setResolution(0.1);
227  for(double v = 0; v < 1; v += res2) { axis.add(geo::Vector3(v, 0, 0)); }
228  for(double v = 0; v < 1; v += res2 * 2) { axis.add(geo::Vector3(0, v, 0)); }
229  for(double v = 0; v < 1; v += res2 * 4) { axis.add(geo::Vector3(0, 0, v)); }
230 
231 
232  // * * * * * * * * * * * * * * * * * * * *
233 
234  cv::Mat image = cv::Mat(CANVAS_HEIGHT, CANVAS_WIDTH, CV_32FC1, 0.0);
235 
236 // DepthCamera cam;
237  //cam.render(Box(Vector3(-2, -5, -5), Vector3(2, 5, 5)), Pose3D(-2.82, 0, 1.82, 0, 0.5, 0), image);
238  geo::Box shape(geo::Vector3(-0.3, -0.5, -0.5), geo::Vector3(0.3, 0.5, 0.5));
239 
240 // std::cout << "DepthCamera::raytrace(box):\t" << render(image, shape, false, false) << " ms" << std::endl;
241 
242  // Create height map
243  int hmap_size = 20;
244  std::vector<std::vector<double> > map(hmap_size);
245  for(int mx = 0; mx < hmap_size; ++mx) {
246  map[mx].resize(hmap_size, 0);
247  }
248 
249  for(int j = 0; j < hmap_size / 2; ++j) {
250  for(int i = 0; i < hmap_size - j * 2; ++i) {
251  double h = 0.1 * j;
252 // if (j > 0) {
253 // h = 0;
254 // }
255 
256  map[i+j][j] = h;
257  map[i+j][hmap_size-j-1] = h;
258  map[j][i+j] = h;
259  map[hmap_size-j-1][i+j] = h;
260  }
261  }
262 
263  geo::HeightMap hmap = geo::HeightMap::fromGrid(map, 0.1);
264 
265  geo::Box plane(geo::Vector3(-10, -10, -0.1), geo::Vector3(10, 10, 0));
266 
267  std::cout << "DepthCamera::rasterize(box):\t" << renderDepthCamera(image, shape, false) << " ms" << std::endl;
268  std::cout << "DepthCamera::rasterize(table):\t" << renderDepthCamera(image, table, false) << " ms" << std::endl;
269  std::cout << "DepthCamera::rasterize(heightmap):\t" << renderDepthCamera(image, hmap, false) << " ms" << std::endl;
270 // std::cout << "DepthCamera::rasterize(abstract_shape):\t" << renderDepthCamera(image, tree, true, false) << " ms" << std::endl;
271 
272  if (mesh) {
273  std::cout << "DepthCamera::rasterize(input_mesh):\t" << renderDepthCamera(image, *mesh, false) << " ms" << std::endl;
274  }
275 
276  std::cout << "LaserRangeFinder::render(box):\t" << renderLRF(image, shape, false) << " ms" << std::endl;
277  std::cout << "LaserRangeFinder::render(table):\t" << renderLRF(image, table, false) << " ms" << std::endl;
278  std::cout << "LaserRangeFinder::render(heightmap):\t" << renderLRF(image, hmap, false) << " ms" << std::endl;
279 // std::cout << "DepthCamera::rasterize(abstract_shape):\t" << renderLRF(image, tree, true, false) << " ms" << std::endl;
280 
281  if (mesh) {
282  std::cout << "LaserRangeFinder::rasterize(input_mesh):\t" << renderLRF(image, *mesh, false) << " ms" << std::endl;
283  }
284 
285  geo::DepthCamera cam(CANVAS_WIDTH, CANVAS_HEIGHT,
286  554.2559327880068 * CANVAS_WIDTH / 640, 554.2559327880068 * CANVAS_HEIGHT / 480,
287  320.5 * CANVAS_WIDTH / 640, 240.5 * CANVAS_HEIGHT / 480,
288  0, 0);
289 
290  geo::LaserRangeFinder lrf;
291  lrf.setAngleLimits(-2.3, 2.3);
292  lrf.setNumBeams(1000);
293  lrf.setRangeLimits(0, 10);
294 
295  cv::Mat display_image(CANVAS_HEIGHT, CANVAS_WIDTH * 2, CV_32FC1, 0.0);
296 
297  double angle = 0;
298 
299  while (true) {
300  geo::Shape* shape1 = &hmap;
301  if (mesh) {
302  shape1 = mesh.get();
303  }
304  geo::Pose3D pose1(5, 1, -0.5, 0, 0, angle);
305 
306  geo::Shape& shape2 = shape;
307  geo::Pose3D pose2(5, 2, -0.5, 0, 0.3, angle);
308 
309  // * * * * * * DEPTH CAMERA * * * * * *
310 
311  cv::Mat depth_image = cv::Mat(CANVAS_HEIGHT, CANVAS_WIDTH, CV_32FC1, 0.0);
312  cam.rasterize(*shape1, geo::Pose3D(0, 0, 0, 1.57, 0, -1.57), pose1, depth_image);
313  cam.rasterize(shape2, geo::Pose3D(0, 0, 0, 1.57, 0, -1.57), pose2, depth_image);
314 
315  cv::Mat depth_image2 = depth_image / 8;
316  cv::Mat destinationROI = display_image(cv::Rect(cv::Point(0, 0), cv::Size(CANVAS_WIDTH, CANVAS_HEIGHT)));
317  depth_image2.copyTo(destinationROI);
318 
319  // * * * * * * LRF * * * * * *
320 
321  std::vector<double> ranges;
322  lrf.render(*shape1, geo::Pose3D(0, 0, 0), pose1, ranges);
323  lrf.render(shape2, geo::Pose3D(0, 0, 0), pose2, ranges);
324  lrf.render(geo::Box(geo::Vector3(-0.5, -3.5, -0.5), geo::Vector3(0.5, 3.5, 0.5)), geo::Pose3D(0, 0, 0), geo::Pose3D(-2, 0, 0), ranges);
325 
326  cv::Mat lrf_image = cv::Mat(CANVAS_HEIGHT, CANVAS_WIDTH, CV_32FC1, 0.0);
327 
328  std::vector<geo::Vector3> points;
329  if (lrf.rangesToPoints(ranges, points)) {
330  for(unsigned int i = 0; i < points.size(); ++i) {
331  const geo::Vector3& p = points[i];
332  double x = (-p.y * 25) + image.cols / 2;
333  double y = (-p.x * 25) + image.rows / 2;
334 
335  if (x >= 0 && y >= 0 && x < image.cols && y < image.rows) {
336  lrf_image.at<float>(y, x) = 1;
337  }
338  }
339  }
340 
341  cv::Mat destinationROI2 = display_image(cv::Rect(cv::Point(CANVAS_WIDTH, 0), cv::Size(CANVAS_WIDTH, CANVAS_HEIGHT)));
342  lrf_image.copyTo(destinationROI2);
343 
344  cv::imshow("visualization", display_image);
345 
346  angle += 0.05;
347 
348  char key = cv::waitKey(3);
349  if (key == 'q')
350  {
351  break;
352  }
353  }
354 
355  return 0;
356 }
geo::Vector3::y
const real & y() const
Definition: matrix.h:32
geo::Ray::setLength
void setLength(const double length)
Definition: Ray.h:26
geo::LaserRangeFinder::setAngleLimits
void setAngleLimits(double min, double max)
Definition: LaserRangeFinder.cpp:285
geo::HeightMap::fromGrid
static HeightMap fromGrid(const std::vector< std::vector< double > > &grid, double resolution)
fromGrid: instantiate a Heightmap from a grid
Definition: HeightMap.cpp:34
renderLRF
double renderLRF(cv::Mat &image, const geo::Shape &shape, bool show)
Definition: test_geolib.cpp:52
std::vector::resize
T resize(T... args)
CANVAS_HEIGHT
double CANVAS_HEIGHT
Definition: test_geolib.cpp:19
fstream
std::string
std::shared_ptr
geo::Octree::setResolution
double setResolution(double resolution)
Definition: Octree.cpp:38
geo::LaserRangeFinder::setRangeLimits
void setRangeLimits(double min, double max)
Definition: LaserRangeFinder.h:67
geo::HeightMap
A geometric description of a Heightmap using a quad tree.
Definition: HeightMap.h:15
vector
std::vector::size
T size(T... args)
geo::Box
Definition: Box.h:15
std::shared_ptr::get
T get(T... args)
geo::DepthCamera::rasterize
RasterizeResult rasterize(const Shape &shape, const Pose3D &pose, cv::Mat &image, PointerMap &pointer_map=EMPTY_POINTER_MAP, void *pointer=0, TriangleMap &triangle_map=EMPTY_TRIANGLE_MAP) const
rasterize: render a 3D shape onto a 2D image
Definition: DepthCamera.cpp:114
geo::Octree::add
void add(const Vector3 &p)
Definition: Octree.cpp:29
geo::Transform3T
Definition: math_types.h:19
geo::LaserRangeFinder::render
void render(const geo::LaserRangeFinder::RenderOptions &options, geo::LaserRangeFinder::RenderResult &res) const
Definition: LaserRangeFinder.cpp:142
geo::serialization::fromFile
static ShapePtr fromFile(const std::string &filename)
Definition: serialization.cpp:46
std::vector::clear
T clear(T... args)
geo::LaserRangeFinder::polarTo2D
static geo::Vector3 polarTo2D(double angle, double range)
Definition: LaserRangeFinder.cpp:366
HeightMap.h
geo::Octree::getResolution
double getResolution() const
Definition: Octree.cpp:46
Timer
std::vector::push_back
T push_back(T... args)
std::cout
geo::Octree::raytrace
void raytrace(const Ray &r, float t0, float t1)
Definition: Octree.cpp:68
std::vector::at
T at(T... args)
Timer::start
void start()
geo::io::readMeshFile
ShapePtr readMeshFile(const std::string &filename, const geo::Vec3 &scale)
Definition: import.cpp:112
renderDepthCamera
double renderDepthCamera(cv::Mat &image, const geo::Shape &shape, bool show)
Definition: test_geolib.cpp:21
Timer.h
geo::Ray
Definition: Ray.h:10
geo::LaserRangeFinder::setNumBeams
void setNumBeams(uint n)
Definition: LaserRangeFinder.cpp:293
geo::Octree::intersect
bool intersect(const Ray &r, float t0, float t1, double &distance) const
intersect: currently always throws a logic error
Definition: Octree.cpp:50
DepthCamera.h
geo::Vector3
Definition: matrix.h:12
geo::Octree
Definition: Octree.h:12
Timer::getElapsedTimeInMilliSec
double getElapsedTimeInMilliSec() const
geo::Octree::clear
void clear()
Definition: Octree.cpp:23
geo::serialization::toFile
static void toFile(ShapeConstPtr shape, const std::string &filename)
Definition: serialization.cpp:54
CANVAS_WIDTH
double CANVAS_WIDTH
Definition: test_geolib.cpp:18
geo::LaserRangeFinder
Definition: LaserRangeFinder.h:13
b
void b()
std::endl
T endl(T... args)
std::vector::begin
T begin(T... args)
geo::Vector3::x
const real & x() const
Definition: matrix.h:31
geo::DepthCamera
Definition: DepthCamera.h:159
serialization.h
Box.h
std::vector::end
T end(T... args)
Octree.h
main
int main(int argc, char **argv)
Definition: test_geolib.cpp:91
LaserRangeFinder.h
geo::LaserRangeFinder::rangesToPoints
bool rangesToPoints(const std::vector< double > &ranges, std::vector< geo::Vector3 > &points) const
Definition: LaserRangeFinder.cpp:355
geo::LaserRangeFinder::getAngles
const std::vector< double > & getAngles() const
Definition: LaserRangeFinder.h:82
Timer::stop
void stop()
geo::Shape
A geometric description of a shape.
Definition: Shape.h:19
import.h
Generated on Sat Apr 19 2025 04:34:12 for geolib2 by   doxygen 1.8.17