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.LGPL ** 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 <ICLUtils/CompatMacros.h> 00035 #include <ICLGeom/Camera.h> 00036 #include <ICLUtils/Array2D.h> 00037 00038 namespace icl{ 00039 namespace geom{ 00040 00041 00043 00046 class ICLGeom_API RGBDMapping{ 00047 protected: 00048 Mat colorCamMatrix; 00049 utils::Array2D<Vec> depthCamRays; 00050 Vec depthCamPos; 00051 00053 static inline utils::Point map_rgbd(const Mat &M, const Vec &v){ 00054 const float phInv = 1.0/ ( M(0,3) * v[0] + M(1,3) * v[1] + M(2,3) * v[2] + M(3,3) ); 00055 const int px = phInv * ( M(0,0) * v[0] + M(1,0) * v[1] + M(2,0) * v[2] + M(3,0) ); 00056 const int py = phInv * ( M(0,1) * v[0] + M(1,1) * v[1] + M(2,1) * v[2] + M(3,1) ); 00057 return utils::Point(px,py); 00058 } 00059 00060 public: 00062 RGBDMapping(){} 00063 00065 RGBDMapping(const Camera &colorCam, const utils::Array2D<Vec> &depthCamRays, 00066 const Vec &depthCamPos): 00067 colorCamMatrix(colorCam.getProjectionMatrix() * colorCam.getCSTransformationMatrix()), 00068 depthCamRays(depthCamRays){} 00069 00071 RGBDMapping(const Camera &colorCam, const Camera &depthCamera): 00072 colorCamMatrix(colorCam.getProjectionMatrix() * colorCam.getCSTransformationMatrix()), 00073 depthCamRays(depthCamera.getResolution()){ 00074 00075 utils::Array2D<ViewRay> rays = depthCamera.getAllViewRays(); 00076 for(int i=0;i<rays.getDim();++i){ 00077 depthCamRays[i] = rays[i].direction; 00078 } 00079 00080 depthCamPos = depthCamera.getPosition(); 00081 } 00082 00084 utils::Point apply(const utils::Point &p, float dMM) const{ 00085 Vec pW = depthCamPos + depthCamRays(p.x,p.y) * dMM; 00086 return map_rgbd(colorCamMatrix,pW); 00087 } 00088 00090 utils::Point operator()(const utils::Point &p, float dMM) const { 00091 return apply(p,dMM); 00092 } 00093 00095 00097 void detach(){ 00098 depthCamRays.detach(); 00099 } 00100 }; 00101 } 00102 }