Fawkes API  Fawkes Development Version
conversions.cpp
1 
2 /***************************************************************************
3  * conversions.h - OpenNI utility methods: conversions
4  *
5  * Created: Thu Mar 31 21:22:19 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include <plugins/openni/utils/conversions.h>
24 
25 #include <cmath>
26 
27 namespace fawkes {
28  namespace openni {
29 #if 0 /* just to make Emacs auto-indent happy */
30  }
31 }
32 #endif
33 
34 /** Project world coordinate into 2D image projection.
35  * This takes the input world coordinates and projects them into the 2D
36  * image plane. Unlike the OpenNI DepthGenerator function this function
37  * can accept a custom width and height. This erases the limitation to
38  * be bound to the configured depth image dimensions, and rather use
39  * coordinates as anticipated from, say, a GUI.
40  * @param depthgen depth generator, used to get field of view and possibly
41  * width and height
42  * @param num_points number of points to convert
43  * @param world array of @p num_points world points
44  * @param proj array of @p num_points projective points
45  * @param width width of the image, 0 to use the actual value from the depth
46  * generator
47  * @param height height of the image, 0 to use the actual value from the depth
48  * generator
49  */
50 void
51 world2projection(xn::DepthGenerator *depthgen,
52  unsigned int num_points, const XnPoint3D *world, XnPoint3D *proj,
53  unsigned int width, unsigned int height)
54 {
55  if (width == 0 || height == 0) {
56  xn::DepthMetaData depth_md;
57  depthgen->GetMetaData(depth_md);
58  width = depth_md.XRes();
59  height = depth_md.YRes();
60  }
61 
62  XnFieldOfView fov;
63  XnStatus st;
64  if ((st = depthgen->GetFieldOfView(fov)) != XN_STATUS_OK) {
65  throw Exception("Failed to get field of view, ignoring. (%s)",
66  xnGetStatusString(st));
67  }
68 
69  float world_x_to_z = tan(fov.fHFOV / 2) * 2;;
70  float world_y_to_z = tan(fov.fVFOV / 2) * 2;
71 
72  XnFloat coeff_x = width / world_x_to_z;
73  XnFloat coeff_y = height / world_y_to_z;
74 
75  XnUInt32 half_res_x = width / 2;
76  XnUInt32 half_res_y = height / 2;
77 
78  for (unsigned int i = 0; i < num_points; ++i) {
79  proj[i].X = coeff_x * world[i].X / world[i].Z + half_res_x;
80  proj[i].Y = half_res_y - coeff_y * world[i].Y / world[i].Z;
81  proj[i].Z = world[i].Z;
82  }
83 }
84 
85 
86 
87 } // end namespace fawkes::openni
88 } // end namespace fawkes
Fawkes library namespace.