Image Component Library (ICL)
|
00001 /******************************************************************** 00002 ** Image Component Library (ICL) ** 00003 ** ** 00004 ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** 00005 ** Neuroinformatics Group ** 00006 ** Website: www.iclcv.org and ** 00007 ** http://opensource.cit-ec.de/projects/icl ** 00008 ** ** 00009 ** File : ICLGeom/src/ICLGeom/RGBDMapping.h ** 00010 ** Module : ICLGeom ** 00011 ** Authors: Christof Elbrechter ** 00012 ** ** 00013 ** ** 00014 ** GNU LESSER GENERAL PUBLIC LICENSE ** 00015 ** This file may be used under the terms of the GNU Lesser General ** 00016 ** Public License version 3.0 as published by the ** 00017 ** ** 00018 ** Free Software Foundation and appearing in the file LICENSE.GPL ** 00019 ** included in the packaging of this file. Please review the ** 00020 ** following information to ensure the license requirements will ** 00021 ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** 00022 ** ** 00023 ** The development of this software was supported by the ** 00024 ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** 00025 ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** 00026 ** Forschungsgemeinschaft (DFG) in the context of the German ** 00027 ** Excellence Initiative. ** 00028 ** ** 00029 ********************************************************************/ 00030 00031 00032 #pragma once 00033 00034 #include <ICLGeom/Camera.h> 00035 #include <ICLUtils/Array2D.h> 00036 00037 namespace icl{ 00038 namespace geom{ 00039 00040 00042 00045 class RGBDMapping{ 00046 protected: 00047 Mat colorCamMatrix; 00048 utils::Array2D<Vec> depthCamRays; 00049 Vec depthCamPos; 00050 00052 static inline utils::Point map_rgbd(const Mat &M, const Vec &v){ 00053 const float phInv = 1.0/ ( M(0,3) * v[0] + M(1,3) * v[1] + M(2,3) * v[2] + M(3,3) ); 00054 const int px = phInv * ( M(0,0) * v[0] + M(1,0) * v[1] + M(2,0) * v[2] + M(3,0) ); 00055 const int py = phInv * ( M(0,1) * v[0] + M(1,1) * v[1] + M(2,1) * v[2] + M(3,1) ); 00056 return utils::Point(px,py); 00057 } 00058 00059 public: 00061 RGBDMapping(){} 00062 00064 RGBDMapping(const Camera &colorCam, const utils::Array2D<Vec> &depthCamRays, 00065 const Vec &depthCamPos): 00066 colorCamMatrix(colorCam.getProjectionMatrix() * colorCam.getCSTransformationMatrix()), 00067 depthCamRays(depthCamRays){} 00068 00070 RGBDMapping(const Camera &colorCam, const Camera &depthCamera): 00071 colorCamMatrix(colorCam.getProjectionMatrix() * colorCam.getCSTransformationMatrix()), 00072 depthCamRays(depthCamera.getResolution()){ 00073 00074 utils::Array2D<ViewRay> rays = depthCamera.getAllViewRays(); 00075 for(int i=0;i<rays.getDim();++i){ 00076 depthCamRays[i] = rays[i].direction; 00077 } 00078 00079 depthCamPos = depthCamera.getPosition(); 00080 } 00081 00083 utils::Point apply(const utils::Point &p, float dMM) const{ 00084 Vec pW = depthCamPos + depthCamRays(p.x,p.y) * dMM; 00085 return map_rgbd(colorCamMatrix,pW); 00086 } 00087 00089 utils::Point operator()(const utils::Point &p, float dMM) const { 00090 return apply(p,dMM); 00091 } 00092 00094 00096 void detach(){ 00097 depthCamRays.detach(); 00098 } 00099 }; 00100 } 00101 }