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;
}
}
