static
void computeCorrespsFiltered(const Mat& K, const Mat& K_inv, const Mat& Rt,
const Mat& depth0, const Mat& validMask0,
const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
Mat& corresps,
const Mat& normals0, const Mat& transformedNormals1,
const Mat& image0, const Mat& image1)
{
const double maxNormalsDiff = 30; // in degrees
const double maxColorDiff = 50;
const double maxNormalAngleDev = 75; // in degrees
computeCorresps(K, K_inv, Rt, depth0, validMask0, depth1, selectMask1,
maxDepthDiff, corresps);
const Point3f Oz_inv(0,0,-1); // TODO replace by vector to camera position?
for(int v0 = 0; v0 < corresps.rows; v0++)
{
for(int u0 = 0; u0 < corresps.cols; u0++)
{
int c = corresps.at<int>(v0, u0);
if(c != -1)
{
Point3f curNormal = normals0.at<Point3f>(v0,u0);
curNormal *= 1./cv::norm(curNormal);
if(std::abs(curNormal.ddot(Oz_inv)) < std::cos(maxNormalAngleDev / 180 * CV_PI))
{
corresps.at<int>(v0, u0) = -1;
continue;
}
int u1, v1;
get2shorts(c, u1, v1);
Point3f transfPrevNormal = transformedNormals1.at<Point3f>(v1,u1);
transfPrevNormal *= 1./cv::norm(transfPrevNormal);
if(std::abs(curNormal.ddot(transfPrevNormal)) < std::cos(maxNormalsDiff / 180 * CV_PI))
{
corresps.at<int>(v0, u0) = -1;
continue;
}
if(std::abs(image0.at<uchar>(v0,u0) - image1.at<uchar>(v1,u1)) > maxColorDiff)
{
corresps.at<int>(v0, u0) = -1;
continue;
}
}
}
}
}
static Mat angularError( const Mat_<Point2f>& flow1, const Mat_<Point2f>& flow2 )
{
Mat result(flow1.size(), CV_32FC1);
for ( int i = 0; i < flow1.rows; ++i )
{
for ( int j = 0; j < flow1.cols; ++j )
{
const Point2f u1_2d = flow1(i, j);
const Point2f u2_2d = flow2(i, j);
const Point3f u1(u1_2d.x, u1_2d.y, 1);
const Point3f u2(u2_2d.x, u2_2d.y, 1);
if ( isFlowCorrect(u1) && isFlowCorrect(u2) )
result.at<float>(i, j) = acos((float)(u1.ddot(u2) / norm(u1) * norm(u2)));
else
result.at<float>(i, j) = std::numeric_limits<float>::quiet_NaN();
}
}
return result;
}