double sampleEuclideanTransform(std::vector<Vector3d> const& left, std::vector<Vector3d> const& right, int const minSample[3], double inlierThresholdL, double inlierThresholdR, Matrix3x3d& R, Vector3d& T, std::vector<int> &inliers) { inliers.clear(); int const N = left.size(); if (N < 3) throwV3DErrorHere("computeRobustEuclideanTransform(): at least 3 point correspondences required."); vector<Vector3d> ptsLeftTrans(N), ptsRightTrans(N); vector<Vector3d> left_pts(3), right_pts(3); int const j0 = minSample[0]; int const j1 = minSample[1]; int const j2 = minSample[2]; left_pts[0] = left[j0]; left_pts[1] = left[j1]; left_pts[2] = left[j2]; right_pts[0] = right[j0]; right_pts[1] = right[j1]; right_pts[2] = right[j2]; Matrix3x3d R0, R0t; Vector3d T0; getEuclideanTransformation(left_pts, right_pts, R0, T0); R0t = R0.transposed(); for (int i = 0; i < N; ++i) ptsLeftTrans[i] = (R0 * left[i] + T0); for (int i = 0; i < N; ++i) ptsRightTrans[i] = R0t * (right[i] - T0); double score = 0; for (int i = 0; i < N; ++i) { double const distL = distance_L2(left[i], ptsRightTrans[i]); double const distR = distance_L2(right[i], ptsLeftTrans[i]); score += std::min(distL, inlierThresholdL); score += std::min(distR, inlierThresholdR); if (distL < inlierThresholdL && distR < inlierThresholdR) inliers.push_back(i); } // end for (i) R = R0; T = T0; return score; } // end sampleEuclideanTransform()
double Superpixel::distance(const Superpixel& a, const Superpixel& b, int dist_type, cv::Mat weights) { if (dist_type == SPX_DIST_L2) { return distance_L2(a, b); } else if (dist_type == SPX_DIST_L1) { return distance_L1(a, b); } else if (dist_type == SPX_DIST_CUSTOM) { return distance_CUSTOM(a, b, weights); } else { return DBL_MAX; } }