DepthFinder::DepthFinder( const cv::Mat_<cv::Vec3f>& pc, const cv::Vec3d &focVec)
{
imgRct_ = cv::Rect( 0, 0, pc.cols, pc.rows);
// Integral image with single channel for depth
const cv::Size imgSz( imgRct_.width, imgRct_.height);
cv::Mat_<double> dmap(imgSz);
cv::Mat_<byte> dcnts(imgSz);
for ( int i = 0; i < imgSz.height; ++i)
{
double* drow = dmap.ptr<double>(i);
byte* irow = dcnts.ptr<byte>(i);
for ( int j = 0; j < imgSz.width; ++j)
{
const cv::Vec3f& fp = pc.at<cv::Vec3f>(i,j);
cv::Vec3d p( fp[0], fp[1], fp[2]);
double depth = focVec.dot( p);
if ( depth < 0)
depth = 0;
if ( depth > MAX_DEPTH)
depth = MAX_DEPTH;
drow[j] = depth;
irow[j] = depth > 0 ? 1 : 0; // Only points with valid depth are set
} // end for
} // end for
cv::integral( dmap, depthIntImg, CV_64F);
cv::integral( dcnts, depthCntImg, CV_32S);
} // end ctor
// public static
double ObjModelPolygonAngles::calcAngle( const cv::Vec3f& v, const cv::Vec3f& v0, const cv::Vec3f& v1)
{
const cv::Vec3d u0 = v0 - v;
const cv::Vec3d u1 = v1 - v;
return acos( u0.dot(u1) / (cv::norm(u0) * cv::norm(u1)));
} // end calcAngle
void
set_d(float d)
{
p = cv::Vec3d(0, 0, d / n[2]);
p_dot_n = p.dot(n);
}