pair< Point, bool > findCorresPointLeft(Mat img1, Mat img2, Point p, LineIterator it2) {
Point match_pos = p;
long min_error = 10000000000;
int error_threshold = 10000;
vector< long > errors;
for(int i = 0; i < it2.count; i+=4)
{
Point cur_pt = it2.pos();
int disp = cur_pt.x - p.x;
if (abs(disp) > 60) {
for (int z = 0; z < 4; z++) it2++;
continue;
}
long error = 0;
int w = 7;
for (int k = -w; k <= w; k++) {
for (int j = -w; j <= w; j++) {
if (inImg(p.x + k, p.y + j) && inImg(cur_pt.x + k, cur_pt.y + j)) {
error += (long)(abs((img1.at<Vec3b>(p.y + j, p.x + k)[0] - img2.at<Vec3b>(cur_pt.y + j, cur_pt.x + k)[0]))
+abs((img1.at<Vec3b>(p.y + j, p.x + k)[1] - img2.at<Vec3b>(cur_pt.y + j, cur_pt.x + k)[1]))
+abs((img1.at<Vec3b>(p.y + j, p.x + k)[2] - img2.at<Vec3b>(cur_pt.y + j, cur_pt.x + k)[2])));
}
}
}
if (error < min_error) {
min_error = error;
match_pos = cur_pt;
}
errors.push_back(error);
for (int z = 0; z < 4; z++) ++it2;
}
bool good_match = true;
/*
int bad_points = 0;
float error_thresh = 1.1 * min_error;
for (int i = 0; i < errors.size(); i++) {
if (errors[i] < (long)error_thresh) bad_points++;
}
if (bad_points > 5) {
good_match = false;
}
*/
return make_pair(match_pos, good_match);
}
pair< Point, bool > findCorresPointRight(Mat img1, Mat img2, Point p, LineIterator it2) {
Point match_pos = p;
float min_error = 10000000000;
vector< long > errors;
Point2d p_dis = getDistortedPoint(p, K1, D1);
for(int i = 0; i < it2.count; i+=2)
{
Point cur_pt = it2.pos();
int disp = cur_pt.x - p.x;
if (abs(disp) > 70) {
for (int z = 0; z < 2; z++) it2++;
continue;
}
Point2d cur_pt_dis = getDistortedPoint(Point2d(cur_pt.x, cur_pt.y), K2, D2);
float error = 0;
//float error = costFunction(img1_distorted, img2_distorted, Point(p_dis.x, p_dis.y), Point(cur_pt_dis.x, cur_pt_dis.y), "SAD");
for (int k = -w; k <= w; k++) {
for (int l = -w; l <= w; l++) {
for (int ch = 0; ch < channels; ch++) {
error += abs(img1_distorted.at<Vec3b>(k+p_dis.y,l+p_dis.x)[ch] - img2_distorted.at<Vec3b>(k+cur_pt_dis.y,l+cur_pt_dis.x)[ch]);
}
}
}
if (error < min_error) {
min_error = error;
match_pos = cur_pt;
}
errors.push_back(error);
for (int z = 0; z < 2; z++) ++it2;
}
bool good_match = true;
/*
int bad_points = 0;
float error_thresh = 1.1 * min_error;
for (int i = 0; i < errors.size(); i++) {
if (errors[i] < (long)error_thresh) bad_points++;
}
if (bad_points > 5) {
good_match = false;
}
*/
return make_pair(match_pos, good_match);
}