コード例 #1
0
ファイル: disparity.cpp プロジェクト: sourishg/stereo-vision
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);
}
コード例 #2
0
ファイル: disparity.cpp プロジェクト: sourishg/stereo-vision
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);
}