intrinsics.hpp

// This file is part of OpenCV project.




// It is subject to the license terms in the LICENSE file found in the top-level directory




// of this distribution and at http://opencv.org/license.html








#ifndef __OPENCV_RGBD_INTRINSICS_HPP__




#define __OPENCV_RGBD_INTRINSICS_HPP__








#include "opencv2/core/matx.hpp"








namespace

cv



{



namespace
kinfu


{







struct

Intr



{



struct

Reprojector




{



Reprojector() {}



inline
Reprojector(Intr
intr)



{



fxinv = 1.f/intr.fx, fyinv = 1.f/intr.fy;



cx = intr.cx, cy = intr.cy;



}



template<typename
T>



inline
cv::Point3_<T>
operator()(cv::Point3_<T>
p)
const





{



T x = p.z
* (p.x
- cx) * fxinv;



T y = p.z
* (p.y
- cy) * fyinv;



return
cv::Point3_<T>(x, y, p.z);



}







float
fxinv, fyinv, cx, cy;



};







struct

Projector




{



inline
Projector(Intr
intr) : fx(intr.fx), fy(intr.fy), cx(intr.cx), cy(intr.cy) { }



template<typename
T>



inline
cv::Point_<T>
operator()(cv::Point3_<T>
p)
const





{



T invz = T(1)/p.z;



T x = fx*(p.x*invz) + cx;



T y = fy*(p.y*invz) + cy;



return
cv::Point_<T>(x, y);



}



template<typename
T>



inline
cv::Point_<T>
operator()(cv::Point3_<T>
p,
cv::Point3_<T>& pixVec)
const





{



T invz = T(1)/p.z;



pixVec =
cv::Point3_<T>(p.x*invz, p.y*invz, 1);



T x = fx*pixVec.x
+ cx;



T y = fy*pixVec.y
+ cy;



return
cv::Point_<T>(x, y);



}



float
fx, fy, cx, cy;



};



Intr() : fx(), fy(), cx(), cy() { }



Intr(float
_fx,
float
_fy,
float
_cx,
float
_cy) : fx(_fx), fy(_fy), cx(_cx), cy(_cy) { }



Intr(cv::Matx33f
m) : fx(m(0, 0)), fy(m(1, 1)), cx(m(0, 2)), cy(m(1, 2)) { }



// scale intrinsics to pyramid level




inline
Intr scale(int
pyr)
const





{



float
factor = (1.f /(1 << pyr));



return
Intr(fx*factor, fy*factor, cx*factor, cy*factor);



}



inline
Reprojector makeReprojector()
const
{
return
Reprojector(*this); }



inline
Projector   makeProjector()
const
{
return
Projector(*this);   }







inline
cv::Matx33f
getMat()
const
{
return
Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); }







float
fx, fy, cx, cy;


};






}
// namespace rgbd



}
// namespace cv








#endif