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
27namespace fawkes {
28namespace 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 */
46void
47world2projection(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 library namespace.