/******************************************************************** ** Image Component Library (ICL) ** ** ** ** Copyright (C) 2006-2010 CITEC, University of Bielefeld ** ** Neuroinformatics Group ** ** Website: www.iclcv.org and ** ** http://opensource.cit-ec.de/projects/icl ** ** ** ** File : ICLGeom/src/Camera.cpp ** ** Module : ICLGeom ** ** Authors: Erik Weitnauer, Christof Elbrechter ** ** ** ** ** ** Commercial License ** ** ICL can be used commercially, please refer to our website ** ** www.iclcv.org for more details. ** ** ** ** GNU General Public License Usage ** ** Alternatively, this file may be used under the terms of the ** ** GNU General Public License version 3.0 as published by the ** ** Free Software Foundation and appearing in the file LICENSE.GPL ** ** included in the packaging of this file. Please review the ** ** following information to ensure the GNU General Public License ** ** version 3.0 requirements will be met: ** ** http://www.gnu.org/copyleft/gpl.html. ** ** ** ** The development of this software was supported by the ** ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** ** Forschungsgemeinschaft (DFG) in the context of the German ** ** Excellence Initiative. ** ** ** *********************************************************************/ #include <ICLUtils/ConfigFile.h> #include <ICLUtils/XMLDocument.h> #include <ICLUtils/DynMatrixUtils.h> #include <ICLGeom/Camera.h> #include <fstream> namespace icl { void Camera::setRotation(const Vec &rot) { // see http://en.wikipedia.org/wiki/Rotation_matrix#Dimension_three float sin_a = sin(rot[0]); float cos_a = cos(rot[0]); Mat3x3 R_x(1, 0, 0, 0, cos_a, -sin_a, 0, sin_a, cos_a); float sin_b = sin(rot[1]); float cos_b = cos(rot[1]); Mat3x3 R_y(cos_b, 0, sin_b, 0, 1, 0, -sin_b, 0, cos_b); float sin_c = sin(rot[2]); float cos_c = cos(rot[2]); Mat3x3 R_z(cos_c, -sin_c, 0, sin_c, cos_c, 0, 0, 0, 1); setRotation(R_x*R_y*R_z); } Mat Camera::createTransformationMatrix(const Vec &norm, const Vec &up, const Vec &pos) { // see http://en.wikipedia.org/wiki/Cross_product#Cross_product_and_handedness /* [ --- hh --- | -hh.p ] [ --- up --- | -up.p ] [ -- norm -- | -no.p ] [ 0 0 0 | 1 ] */ // double cross product to ensure that they are all orthogonal Vec hh = cross(up,norm); Vec uu = cross(norm,hh); Mat T; T.row(0) = hh; T.row(1) = uu; T.row(2) = norm; T.row(3) = Vec(0.0); T.col(3) = Vec(0.0); T.col(3) = -(T*pos); T(3,3) = 1; return T; } Mat Camera::getCSTransformationMatrix() const { return createTransformationMatrix(m_norm, m_up, m_pos); } Mat Camera::getCSTransformationMatrixGL() const { // for OpenGL, the camera looks in negative z direction return createTransformationMatrix(-m_norm, m_up, m_pos); } FixedMatrix<icl32f,4,3> Camera::getQMatrix() const { Mat K = getProjectionMatrix(); Mat CS = getCSTransformationMatrix(); FixedMatrix<icl32f,4,3> cs(CS.begin()); FixedMatrix<icl32f,3,3> k( K(0,0), K(1,0), K(2,0), K(0,1), K(1,1), K(2,1), K(0,3), K(1,3), K(2,3) ); return k*cs; } Mat Camera::getProjectionMatrix() const { return Mat(m_f * m_mx, m_skew, m_px, 0, 0, m_f*m_my, m_py, 0, 0, 0, 0, 0, 0, 0, 1, 0); } Mat Camera::getProjectionMatrixGL() const { float clip_near = m_f; float A = (m_renderParams.clipZFar + clip_near)/(clip_near - m_renderParams.clipZFar); float B = (2*m_renderParams.clipZFar*clip_near)/(clip_near - m_renderParams.clipZFar); float w2 = m_renderParams.chipSize.width/2; float h2 = m_renderParams.chipSize.height/2; // Because OpenGL will automatically flip the y-coordinates in the end, // we need switch the sign of the skew and py components of the matrix. // Also we need switch the sign back when doing the projection by hand. return Mat( -m_f*m_mx/w2, m_skew/w2, -(m_px-w2)/w2, 0, 0, -m_f*m_my/h2, (m_py-h2)/h2, 0, 0, 0, A, B, 0, 0, -1, 0); } Mat Camera::getViewportMatrixGL() const { float w2 = m_renderParams.viewport.width/2; float h2 = m_renderParams.viewport.height/2; const float &zFar = m_renderParams.viewportZMax; const float &zNear = m_renderParams.viewportZMin; return Mat(w2, 0, 0, w2 + m_renderParams.viewport.x, 0, h2, 0, h2 + m_renderParams.viewport.y, 0, 0, (zFar-zNear)/2, (zFar+zNear)/2, 0, 0, 0, 1); } // Projects a world point to the screen Point32f Camera::project(const Vec &Xw) const { Mat T = getCSTransformationMatrix(); Mat P = getProjectionMatrix(); //#warning "testing project fix here!" //P(1,0) *= -1; P(2,1) *= -1; Vec xi = homogenize(P*T*Xw); return Point32f(xi[0],xi[1]); } // Projects a set of points void Camera::project(const std::vector<Vec> &Xws, std::vector<Point32f> &dst) const{ dst.resize(Xws.size()); Mat T = getCSTransformationMatrix(); Mat P = getProjectionMatrix(); //#warning "testing project fix here!" //P(1,0) *= -1; P(2,1) *= -1; Mat M = P*T; for(unsigned int i=0;i<Xws.size();++i){ Vec xi = homogenize(M*Xws[i]); dst[i].x = xi[0]; dst[i].y = xi[1]; } } // Projects a set of points const std::vector<Point32f> Camera::project(const std::vector<Vec> &Xws) const{ std::vector<Point32f> xis; project(Xws,xis); return xis; } /// Project a world point onto the image plane. Vec Camera::projectGL(const Vec &Xw) const { Mat T = getCSTransformationMatrix(); Mat P = getProjectionMatrixGL(); // correct the sign of skew and y-offset component P(1,0) *= -1; P(2,1) *= -1; Mat V = getViewportMatrixGL(); return homogenize(V*P*T*Xw); } /// Project a vector of world points onto the image plane. void Camera::projectGL(const std::vector<Vec> &Xws, std::vector<Vec> &dst) const { dst.resize(Xws.size()); Mat T = getCSTransformationMatrix(); Mat P = getProjectionMatrixGL(); // correct the sign of skew and y-offset component P(1,0) *= -1; P(2,1) *= -1; Mat V = getViewportMatrixGL(); Mat M = V*P*T; for(unsigned int i=0;i<Xws.size();++i) { dst[i] = homogenize(M*Xws[i]); } } /// Project a vector of world points onto the image plane. const std::vector<Vec> Camera::projectGL(const std::vector<Vec> &Xws) const { std::vector<Vec> dst; projectGL(Xws, dst); return dst; } void Camera::setRotation(const Mat3x3 &rot) { m_norm = Vec(rot(0,2), rot(1,2), rot(2,2), 0).normalized(); m_norm[3] = 1; m_up = Vec(rot(0,1), rot(1,1), rot(2,1), 0).normalized(); m_up[3] = 1; // we need to check the handedness Vec v = cross(m_up,m_norm); // get the component with biggest abs. value of v int idx = 0; if (abs(v[1]) > abs(v[idx])) idx = 1; if (abs(v[2]) > abs(v[idx])) idx = 2; // check if the signs are same if (v[idx] * rot(idx,0) < 0) { // not the same sign -- switch directions to make right handed cs m_norm *= -1; m_norm[3] = 1; m_up *= -1; m_up[3] = 1; } } Camera Camera::createFromProjectionMatrix(const FixedMatrix<icl32f,4,3> &Q, float focalLength) { FixedMatrix<float,3,3> M = Q.part<0,0,3,3>(); FixedMatrix<float,1,3> c4 = Q.col(3); FixedMatrix<float,3,3> K; // intrinsic parameters FixedMatrix<float,3,3> R; // extrinsic (rotation matrix) FixedMatrix<float,1,3> T; // extrinsic (tranlation vector) M.decompose_RQ(K,R); K = K/K(2,2); // normalize K T = -M.inv() * c4; Camera cam; cam.setPosition(Vec(T[0],T[1],T[2],1)); cam.setRotation(R); cam.setFocalLength(focalLength); cam.setSamplingResolution(K(0,0)/focalLength,K(1,1)/focalLength); cam.setPrincipalPointOffset(Point32f(K(2,0),K(2,1))); cam.setSkew(K(1,0)); return cam; } Camera Camera::calibrate_pinv(std::vector<Vec> Xws, std::vector<Point32f> xis, float focalLength) throw (NotEnoughDataPointsException) { // TODO: normalize points checkAndFixPoints(Xws,xis); int N = (int)Xws.size(); DynMatrix<float> U(1,2*N); for(int i=0;i<N;++i){ U[2*i] = xis[i].x; U[2*i+1] = xis[i].y; } DynMatrix<float> B(11,2*N); for(int i=0;i<N;++i){ float x=Xws[i][0], y=Xws[i][1],z=Xws[i][2], u=-xis[i].x,v=-xis[i].y; float r1[11] = {x,y,z,1,0,0,0,0,u*x,u*y,u*z}; float r2[11] = {0,0,0,0,x,y,z,1,v*x,v*y,v*z}; std::copy(r1,r1+11,B.row_begin(2*i)); std::copy(r2,r2+11,B.row_begin(2*i+1)); } DynMatrix<float> Cv = B.pinv() * U; FixedMatrix<float,4,3> Q(Cv[0],Cv[1],Cv[2],Cv[3], Cv[4],Cv[5],Cv[6],Cv[7], Cv[8],Cv[9],Cv[10],1); return Camera::createFromProjectionMatrix(Q, focalLength); } Camera Camera::calibrate(std::vector<Vec> Xws, std::vector<Point32f> xis, float focalLength) throw (NotEnoughDataPointsException) { #ifndef HAVE_MKL return calibrate_pinv(Xws,xis,focalLength); #else // TODO: normalize points // TODO: check whether we have svd (IPP) available checkAndFixPoints(Xws,xis); unsigned int n = Xws.size(); DynMatrix<icl32f> A(12,2*n); for (unsigned int k=0; k<n; ++k) { int i = 2*k; float x = xis[k].x; float y = xis[k].y; float X = Xws[k][0]; float Y = Xws[k][1]; float Z = Xws[k][2]; float W = Xws[k][3]; A(0,i) = 0; A(1,i) = 0; A(2,i) = 0; A(3,i) = 0; A(4,i) = -X; A(5,i) = -Y; A(6,i) = -Z; A(7,i) = -W; A(8,i) = y*X; A(9,i) = y*Y; A(10,i) = y*Z; A(11,i) = y*W; i = 2*k+1; A(0,i) = X; A(1,i) = Y; A(2,i) = Z; A(3,i) = W; A(4,i) = 0; A(5,i) = 0; A(6,i) = 0; A(7,i) = 0; A(8,i) = -x*X; A(9,i) = -x*Y; A(10,i) = -x*Z; A(11,i) = -x*W; } DynMatrix<icl32f> U,s,V; svd_dyn(A,U,s,V); FixedMatrix<float,4,3> Q; for (int i=0; i<4; i++) for (int j=0; j<3; j++) { Q(i,j) = V(11,j*4+i); } return Camera::createFromProjectionMatrix(Q, focalLength); #endif } void Camera::checkAndFixPoints(std::vector<Vec> &Xws, std::vector<Point32f> &xis) throw (NotEnoughDataPointsException) { if(Xws.size() > xis.size()){ ERROR_LOG("got more world points than image points (erasing additional world points)"); Xws.resize(xis.size()); } else if(Xws.size() < xis.size()){ ERROR_LOG("got less world points than image points (erasing additional image points)"); xis.resize(Xws.size()); } if(Xws.size() < 6){ throw NotEnoughDataPointsException(); } for(unsigned int i=0;i<Xws.size();++i){ Xws[i][3]=1; } } void Camera::load_camera_from_stream(std::istream &is, const std::string &prefix, Camera &cam){ cam = Camera(); // load default values ConfigFile f(new XMLDocument(is)); f.setPrefix(prefix); #define LOAD_FROM_STREAM(KEY,FNAME) \ if (f.contains("camera." #KEY)) cam.set##FNAME(f["camera." #KEY]); \ else WARNING_LOG("No " #KEY " found in configuration (using default: '" << cam.get##FNAME() << "')"); LOAD_FROM_STREAM(name,Name); LOAD_FROM_STREAM(position,Position); LOAD_FROM_STREAM(norm,Norm); LOAD_FROM_STREAM(up,Up); LOAD_FROM_STREAM(f,FocalLength); LOAD_FROM_STREAM(principal-point-offset,PrincipalPointOffset); LOAD_FROM_STREAM(sampling-resolution-x,SamplingResolutionX); LOAD_FROM_STREAM(sampling-resolution-y,SamplingResolutionY); LOAD_FROM_STREAM(skew,Skew); #undef LOAD_FROM_STREAM f.setPrefix(prefix+"camera.render-params."); #define LOAD_FROM_STREAM(KEY,ATTR) \ if (f.contains(#KEY)) cam.getRenderParams().ATTR = f[#KEY]; \ else WARNING_LOG("No " #KEY " found in configuration (using default: '" << cam.getRenderParams().ATTR << "')"); LOAD_FROM_STREAM(chip-size, chipSize); LOAD_FROM_STREAM(clip-z-far, clipZFar); LOAD_FROM_STREAM(viewport, viewport); LOAD_FROM_STREAM(viewport-z-min, viewportZMin); LOAD_FROM_STREAM(viewport-z-max, viewportZMax); #undef LOAD_FROM_STREAM } std::string Camera::toString() const { std::ostringstream os; os << "Position: " << getPosition().transp() << std::endl; os << "Norm: " << getNorm().transp() << std::endl; os << "Up: " << getUp().transp() << std::endl; os << "Focal Length: " << getFocalLength() << std::endl; os << "Principal Point Offset: " << getPrincipalPointOffset() << std::endl; os << "Sampling Resolution: " << getSamplingResolutionX() << ", " << getSamplingResolutionY() << std::endl; os << "Skew: " << getSkew() << std::endl; return os.str(); } /// ostream operator std::ostream &operator<<(std::ostream &os, const Camera &cam){ ConfigFile f; f.setPrefix("config.camera."); #define WRITE_TO_STREAM(KEY,FNAME) \ f[#KEY] = cam.get##FNAME(); WRITE_TO_STREAM(name,Name); WRITE_TO_STREAM(position,Position); WRITE_TO_STREAM(norm,Norm); WRITE_TO_STREAM(up,Up); WRITE_TO_STREAM(f,FocalLength); WRITE_TO_STREAM(principal-point-offset,PrincipalPointOffset); WRITE_TO_STREAM(sampling-resolution-x,SamplingResolutionX); WRITE_TO_STREAM(sampling-resolution-y,SamplingResolutionY); WRITE_TO_STREAM(skew,Skew); #undef WRITE_TO_STREAM f["render-params.chip-size"] = cam.getRenderParams().chipSize; f["render-params.clip-z-far"] = cam.getRenderParams().clipZFar; f["render-params.viewport"] = cam.getRenderParams().viewport; f["render-params.viewport-z-min"] = cam.getRenderParams().viewportZMin; f["render-params.viewport-z-max"] = cam.getRenderParams().viewportZMax; return os << f; } /// istream operator parses a camera from an XML-string std::istream &operator>>(std::istream &is, Camera &cam) throw (ParseException) { cam = Camera(is,"config."); return is; } Camera::Camera(const std::string &filename, const std::string &prefix) throw (ParseException){ std::ifstream is(filename.c_str()); load_camera_from_stream(is,prefix,*this); } Camera::Camera(std::istream &is, const std::string &prefix) throw (ParseException){ load_camera_from_stream(is,prefix,*this); } ViewRay Camera::getViewRay(const Point32f &pixel) const { Mat T = getCSTransformationMatrix(); Mat P = getProjectionMatrix(); Mat M = P*T; FixedMatrix<icl32f,4,3> Q; Q.row(0) = M.row(0); Q.row(1) = M.row(1); Q.row(2) = M.row(3); Vec dir = Q.pinv()*FixedColVector<icl32f,3>(pixel.x, pixel.y, 1); if ((m_pos[0]*m_pos[0] + m_pos[1]*m_pos[1] + m_pos[2]*m_pos[2]) < 1e-20) { // Special case: the camera is very close to origin. The last component of // 'dir' is about zero, so homogenize could fail. We can just skip this // step, because we normalize 'dir' later, anyway. } else dir = m_pos - homogenize(dir); dir[3] = 0; dir.normalize(); dir[3] = 1; return ViewRay(m_pos,dir); } ViewRay Camera::getViewRay(const Vec &Xw) const{ return ViewRay(m_pos, Xw-m_pos); } static inline float sprod_3(const Vec &a, const Vec &b){ return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; } Vec Camera::getIntersection(const ViewRay &v, const PlaneEquation &plane) throw (ICLException) { float denom = sprod_3(v.direction, plane.normal); if(!denom) throw ICLException("no intersection -> plane normal is perdendicular to view-ray direction"); float lambda = - sprod_3(v.offset-plane.offset,plane.normal) / denom; return v(lambda); } Vec Camera::estimate3DPosition(const Point32f &pixel, const PlaneEquation &plane) const throw (ICLException) { return getIntersection(getViewRay(pixel),plane); } /* static Point32f to_normalized_viewport(const Point32f &p, const Camera &cam){ // {{{ open Vec uv = cam.getViewportMatrixGL().inv() * Vec(p.x,p.y,0,1); return Point32f(-uv[0],-uv[1]); } // }} } */ static Vec estimate_3D_internal(const std::vector<Camera*> cams, const std::vector<Point32f> &ps) throw (ICLException){ // {{{ open int K = (int)cams.size(); ICLASSERT_THROW(K > 1,ICLException("Camera::estimate_3D_internal: 3D point estimation needs at least 2 views")); DynMatrix<float> A(3,2*K), B(1,2*K); for(int i=0;i<K;++i){ const float &u = ps[i].x; const float &v = ps[i].y; FixedMatrix<float,4,3> Q = cams[i]->getQMatrix(); FixedRowVector<float,3> x = Q.part<0,0,3,1>(); FixedRowVector<float,3> y = Q.part<0,1,3,1>(); FixedRowVector<float,3> z = Q.part<0,2,3,1>(); FixedColVector<float,3> t = Q.part<3,0,1,3>(); FixedRowVector<float,3> a1 = z*u - x; FixedRowVector<float,3> a2 = z*v - y; std::copy(a1.begin(),a1.end(),A.row_begin(2*i)); std::copy(a2.begin(),a2.end(),A.row_begin(2*i+1)); B[2*i] = t[0]-u*t[2]; B[2*i+1] = t[1]-v*t[2]; } try{ DynMatrix<float> pEst = A.pinv() * B; return Vec(pEst[0],pEst[1],pEst[2],1); }catch(const ICLException &ex){ throw ICLException(str("Camera::estimate_3D_internal: unable to solve linear equation (")+ex.what()+")"); } return Vec(0,0,0,1); } // }}} Vec Camera::estimate_3D(const std::vector<Camera*> cams, const std::vector<Point32f> &UVs, bool removeInvalidPoints) throw (ICLException){ // {{{ open ICLASSERT_THROW(cams.size() == UVs.size(),ICLException("Camera::estimate_3D: given camera count and point count differs")); if(removeInvalidPoints){ std::vector<Camera*> camsOk; std::vector<Point32f> uvsOk; for(unsigned int i=0;i<cams.size();++i){ const Rect32f &vp = cams[i]->getRenderParams().viewport; if(vp.contains(UVs[i].x,UVs[i].y)){ camsOk.push_back(cams[i]); uvsOk.push_back(UVs[i]); } } return estimate_3D_internal(camsOk,uvsOk); }else{ return estimate_3D_internal(cams,UVs); } } // }}} /// returns the tensor obtained by contraction of v with epsilon tensor. static inline FixedMatrix<icl32f,3,3> contr_eps(const FixedMatrix<icl32f,1,3> &v){ return FixedMatrix<icl32f,3,3>(0, v[2], -v[1], -v[2], 0, v[0], v[1], -v[0], 0); } template<class ForwardIterator> static bool all_negative(ForwardIterator begin, ForwardIterator end){ while(begin != end){ if(*begin++ > 0) return false; } return true; } template<class ForwardIterator> static bool all_positive(ForwardIterator begin, ForwardIterator end){ while(begin != end){ if(*begin++ < 0) return false; } return true; } /// multiview 3D point estimation using svd-based linear optimization Vec Camera::estimate_3D_svd(const std::vector<Camera*> cams, const std::vector<Point32f> &UVs){ int K = (int)cams.size(); ICLASSERT_THROW(K>1,ICLException("estimate_3D_svd needs at least 2 views")); ICLASSERT_THROW((int)UVs.size() == (int)cams.size(), ICLException("estimate_3D_svd got more or less cameras than points")); std::vector<FixedMatrix<icl32f,4,3> > P(K); std::vector<FixedMatrix<icl32f,1,2> > u(K); for(int i=0;i<K;++i) { P[i] = cams[i]->getQMatrix(); u[i] = FixedMatrix<icl32f,1,2>(UVs[i].x,UVs[i].y); } #if 0 // this does not work for some reason!, however it works without! for(int k=0;k<K;++k){ Mat33 H( 2.0/imageSizes[k].width, 0, -1, 0, 2.0/imageSizes[k].height, -1, 0, 0, 1); P[k] = H*P[k]; u[k] = Mat22(H.part<0,0,2,2>()) * u[k] + Vec2(H.part<2,2,1,2>()); } #endif DynMatrix<float> A(4,K*3); for(int k=0;k<K;++k){ FixedMatrix<icl32f,4,3> m = contr_eps(FixedMatrix<icl32f,1,3>(u[k][0],u[k][1],1))*P[k]; for(unsigned int y=0;y<3;++y){ for(unsigned int x=0;x<4;++x){ A(x,k*3+y) = m(x,y); } } } DynMatrix<float> _U,_s,V; svd_dyn(A,_U,_s,V); // search eigenvector to lowest eigenvalue DynMatrix<float> X(1,V.rows()); X = V.col(V.cols()-1); // create matrix with all 3rd rows of the camera matrices DynMatrix<float> M(4,K); for(int k=0;k<K;++k){ std::copy(P[k].row_begin(2),P[k].row_end(2),M.row_begin(k)); } DynMatrix<float> s = M*X; if(all_negative(s.begin(),s.end())){ return homogenize(-Vec(X.begin())); }else if(all_positive(s.begin(),s.end())){ return homogenize(Vec(X.begin())); }else{ throw ICLException("estimate_3D_svd: Inconsistent orientation of point match"); return Vec(0,0,0,1); } } void Camera::setResolution(const Size &newScreenSize){ getRenderParams().chipSize = newScreenSize; getRenderParams().viewport = Rect(Point::null,newScreenSize); setPrincipalPointOffset(newScreenSize.width/2,newScreenSize.height/2); } void Camera::setResolution(const Size &newScreenSize, const Point &newPrincipalPointOffset){ getRenderParams().chipSize = newScreenSize; getRenderParams().viewport = Rect(Point::null,newScreenSize); setPrincipalPointOffset(newPrincipalPointOffset); } }