Fawkes API  Fawkes Development Version
conversions.cpp
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