Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * conversions.h - OpenNI utility methods: conversions 00004 * 00005 * Created: Thu Mar 31 21:22:19 2011 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include <plugins/openni/utils/conversions.h> 00024 00025 #include <cmath> 00026 00027 namespace fawkes { 00028 namespace openni { 00029 #if 0 /* just to make Emacs auto-indent happy */ 00030 } 00031 } 00032 #endif 00033 00034 /** Project world coordinate into 2D image projection. 00035 * This takes the input world coordinates and projects them into the 2D 00036 * image plane. Unlike the OpenNI DepthGenerator function this function 00037 * can accept a custom width and height. This erases the limitation to 00038 * be bound to the configured depth image dimensions, and rather use 00039 * coordinates as anticipated from, say, a GUI. 00040 * @param depthgen depth generator, used to get field of view and possibly 00041 * width and height 00042 * @param num_points number of points to convert 00043 * @param world array of @p num_points world points 00044 * @param proj array of @p num_points projective points 00045 * @param width width of the image, 0 to use the actual value from the depth 00046 * generator 00047 * @param height height of the image, 0 to use the actual value from the depth 00048 * generator 00049 */ 00050 void 00051 world2projection(xn::DepthGenerator *depthgen, 00052 unsigned int num_points, const XnPoint3D *world, XnPoint3D *proj, 00053 unsigned int width, unsigned int height) 00054 { 00055 if (width == 0 || height == 0) { 00056 xn::DepthMetaData depth_md; 00057 depthgen->GetMetaData(depth_md); 00058 width = depth_md.XRes(); 00059 height = depth_md.YRes(); 00060 } 00061 00062 XnFieldOfView fov; 00063 XnStatus st; 00064 if ((st = depthgen->GetFieldOfView(fov)) != XN_STATUS_OK) { 00065 throw Exception("Failed to get field of view, ignoring. (%s)", 00066 xnGetStatusString(st)); 00067 } 00068 00069 float world_x_to_z = tan(fov.fHFOV / 2) * 2;; 00070 float world_y_to_z = tan(fov.fVFOV / 2) * 2; 00071 00072 XnFloat coeff_x = width / world_x_to_z; 00073 XnFloat coeff_y = height / world_y_to_z; 00074 00075 XnUInt32 half_res_x = width / 2; 00076 XnUInt32 half_res_y = height / 2; 00077 00078 for (unsigned int i = 0; i < num_points; ++i) { 00079 proj[i].X = coeff_x * world[i].X / world[i].Z + half_res_x; 00080 proj[i].Y = half_res_y - coeff_y * world[i].Y / world[i].Z; 00081 proj[i].Z = world[i].Z; 00082 } 00083 } 00084 00085 00086 00087 } // end namespace fawkes::openni 00088 } // end namespace fawkes