bool Cone::LeastSquaresFit(const PointCloud &pc,
MiscLib::Vector< size_t >::const_iterator begin,
MiscLib::Vector< size_t >::const_iterator end)
{
bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
GfxTL::IndexIterate(end, pc.begin()));
return retVal;
}
/* Estimate a transform between two sets of keypoints */
std::vector<int> EstimateTransform(const std::vector<Keypoint> &k1,
const std::vector<Keypoint> &k2,
const std::vector<KeypointMatch> &matches,
MotionModel mm,
int nRANSAC, double RANSACthresh,
double *Mout)
{
int min_matches = -1;
switch (mm) {
case MotionRigid:
min_matches = 3;
break;
case MotionHomography:
min_matches = 4;
break;
}
int *match_idxs = new int[min_matches];
int num_matches = (int) matches.size();
int max_inliers = 0;
double Mbest[9];
if (num_matches < min_matches) {
std::vector<int> empty;
printf("Cannot estimate rigid transform\n");
return empty;
}
v3_t *r_pts = new v3_t[min_matches];
v3_t *l_pts = new v3_t[min_matches];
double *weight = new double[min_matches];
for (int round = 0; round < nRANSAC; round++) {
for (int i = 0; i < min_matches; i++) {
bool found;
int idx;
do {
found = true;
idx = rand() % num_matches;
for (int j = 0; j < i; j++) {
if (match_idxs[j] == idx) {
found = false;
break;
}
}
} while (!found);
match_idxs[i] = idx;
}
/* Solve for the motion */
for (int i = 0; i < min_matches; i++) {
int idx1 = matches[match_idxs[i]].m_idx1;
int idx2 = matches[match_idxs[i]].m_idx2;
Vx(l_pts[i]) = k1[idx1].m_x;
Vy(l_pts[i]) = k1[idx1].m_y;
Vz(l_pts[i]) = 1.0;
Vx(r_pts[i]) = k2[idx2].m_x;
Vy(r_pts[i]) = k2[idx2].m_y;
Vz(r_pts[i]) = 1.0;
weight[i] = 1.0;
}
double Mcurr[9];
switch (mm) {
case MotionRigid: {
double R[9], T[9], Tout[9], scale;
align_horn(min_matches, r_pts, l_pts, R, T, Tout, &scale, weight);
memcpy(Mcurr, Tout, 9 * sizeof(double));
break;
}
case MotionHomography: {
align_homography(min_matches, r_pts, l_pts, Mcurr, 0);
break;
}
}
std::vector<int> inliers;
int num_inliers = CountInliers(k1, k2, matches, Mcurr,
RANSACthresh, inliers);
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Mbest, Mcurr, 9 * sizeof(double));
}
}
std::vector<int> inliers;
CountInliers(k1, k2, matches, Mbest, RANSACthresh, inliers);
memcpy(Mout, Mbest, 9 * sizeof(double));
LeastSquaresFit(k1, k2, matches, mm, inliers, Mout);
// memcpy(Mout, Mbest, 9 * sizeof(double));
delete [] match_idxs;
delete [] r_pts;
delete [] l_pts;
delete [] weight;
return inliers;
}
sBool sBSpline::FitCurveImpl(const Sample *samples,sInt nSamples,sF32 maxError,sInt maxRefinements)
{
sArray<sF32> NewKnots;
sBool Subdivided = sFALSE;
// Initial fit
LeastSquaresFit(samples,nSamples);
// Refine as necessary
NewKnots.Init(Knots.Count);
while(maxRefinements-- > 0)
{
// Calculate new knot vector: Setup
NewKnots.Count = 0;
const Sample *sample = samples;
const Sample *sampleEnd = samples + nSamples;
Subdivided = sFALSE;
*NewKnots.Add() = Knots[0];
// Go through knot intervals, measuring error and subdiving as necesarry
for(sInt i=1; i<Knots.Count; i++)
{
if(Knots[i] != Knots[i-1]) // nonempty interval
{
// Measure max. error (currently without derivatives)
sF32 error = 0.0f;
while(sample < sampleEnd && sample->Time < Knots[i])
{
if(sample->Type == 0) // only direct values
error = sMax<sF32>(error,sFAbs(sample->Value - Evaluate(sample->Time)));
sample++;
}
// Add new knot in middle if error bigger than threshold
if(error >= maxError)
{
*NewKnots.Add() = (Knots[i-1] + Knots[i]) * 0.5f;
Subdivided = sTRUE;
}
}
// Add old knot value in any case
*NewKnots.Add() = Knots[i];
}
// If not subdivided, we're done, else compute new approximation
// and iterate
if(!Subdivided)
break;
else
{
Knots.Swap(NewKnots);
Values.Resize(Knots.Count);
LeastSquaresFit(samples,nSamples);
}
}
NewKnots.Exit();
return !Subdivided;
}
