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 
30 /** Project world coordinate into 2D image projection.
31  * This takes the input world coordinates and projects them into the 2D
32  * image plane. Unlike the OpenNI DepthGenerator function this function
33  * can accept a custom width and height. This erases the limitation to
34  * be bound to the configured depth image dimensions, and rather use
35  * coordinates as anticipated from, say, a GUI.
36  * @param depthgen depth generator, used to get field of view and possibly
37  * width and height
38  * @param num_points number of points to convert
39  * @param world array of @p num_points world points
40  * @param proj array of @p num_points projective points
41  * @param width width of the image, 0 to use the actual value from the depth
42  * generator
43  * @param height height of the image, 0 to use the actual value from the depth
44  * generator
45  */
46 void
47 world2projection(xn::DepthGenerator *depthgen,
48  unsigned int num_points,
49  const XnPoint3D * world,
50  XnPoint3D * proj,
51  unsigned int width,
52  unsigned int height)
53 {
54  if (width == 0 || height == 0) {
55  xn::DepthMetaData depth_md;
56  depthgen->GetMetaData(depth_md);
57  width = depth_md.XRes();
58  height = depth_md.YRes();
59  }
60 
61  XnFieldOfView fov;
62  XnStatus st;
63  if ((st = depthgen->GetFieldOfView(fov)) != XN_STATUS_OK) {
64  throw Exception("Failed to get field of view, ignoring. (%s)", xnGetStatusString(st));
65  }
66 
67  float world_x_to_z = tan(fov.fHFOV / 2) * 2;
68  ;
69  float world_y_to_z = tan(fov.fVFOV / 2) * 2;
70 
71  XnFloat coeff_x = width / world_x_to_z;
72  XnFloat coeff_y = height / world_y_to_z;
73 
74  XnUInt32 half_res_x = width / 2;
75  XnUInt32 half_res_y = height / 2;
76 
77  for (unsigned int i = 0; i < num_points; ++i) {
78  proj[i].X = coeff_x * world[i].X / world[i].Z + half_res_x;
79  proj[i].Y = half_res_y - coeff_y * world[i].Y / world[i].Z;
80  proj[i].Z = world[i].Z;
81  }
82 }
83 
84 } // namespace openni
85 } // end namespace fawkes
fawkes
Fawkes library namespace.