private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam)
{
NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44();
// ARdouble err;
int i;
if (matchData.getLength() < 4)
{
return(false);
}
NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength());
NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength());
for (i = 0; i < matchData.getLength(); i++)
{
sCoord[i].x = matchData.getItem(i).query.x;
sCoord[i].y = matchData.getItem(i).query.y;
wCoord[i].x = matchData.getItem(i).ref_.pos3d.x;
wCoord[i].y = matchData.getItem(i).ref_.pos3d.y;
wCoord[i].z = 0.0;
}
NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix());
if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc))
{
return(false);
}
/*
* printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f",
* initMatXw2Xc[j][i]); printf("\n"); }
*/
NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix());
icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam);
if (i_resultparam.last_error > 10.0f)
{
return(false);
}
return(true);
}
/**
* Set the number of bins for translation based on the correspondences.
*/
private bool autoAdjustXYNumBins(int max_dim, FeaturePairStack i_point_pair)
{
int l = i_point_pair.getLength();
//prepare work area
if (this._project_dim.Length < l)
{
this._project_dim = new double[l + 10];
}
double[] projected_dim = this._project_dim;
for (int i = l - 1; i >= 0; i--)
{
// Scale is the 3rd component
FeaturePairStack.Item item = i_point_pair.getItem(i);
// Project the max_dim via the scale
double scale = SafeDivision(item.query.scale, item.ref_.scale);
projected_dim[i] = scale * max_dim;
}
// Find the median projected dim
// float median_proj_dim = FastMedian<float>(&projected_dim[0],
// (int)projected_dim.size());
double median_proj_dim = FastMedian(projected_dim, l);
// Compute the bin size a fraction of the median projected dim
double bin_size = 0.25f * median_proj_dim;
int t;
t = (int)Math.Ceiling((mMaxX - mMinX) / bin_size);
this.mNumXBins = (5 > t ? 5 : t);
t = (int)Math.Ceiling((mMaxY - mMinY) / bin_size);
this.mNumYBins = (5 > t ? 5 : t);
if (mNumXBins >= 128 || mNumYBins >= 128)
{
return(false);
}
return(true);
}
private int vote(FeaturePairStack i_point_pair, int i_center_x, int i_center_y, SubBinLocation[] i_sub_bin_locations)
{
int size = i_point_pair.getLength();
int num_features_that_cast_vote = 0;
for (int i = 0; i < size; i++)
{
//mapCorrespondence(r,i_point_pair.getItem(i),i_center_x,i_center_y);
double rx, ry, rangle, rscale;
{
FreakFeaturePoint ins = i_point_pair.getItem(i).query;
FreakFeaturePoint ref_ = i_point_pair.getItem(i).ref_;
//angle
rangle = ins.angle - ref_.angle;
// Map angle to (-pi,pi]
if (rangle <= -PI)
{
rangle += (2 * PI);
}
else if (rangle > PI)
{
rangle -= (2 * PI);
}
double scale = SafeDivision(ins.scale, ref_.scale);
double c = (scale * Math.Cos(rangle));
double s = (scale * Math.Sin(rangle));
//scale
rscale = (double)(Math.Log(scale) * this.mScaleOneOverLogK);
//x,y
rx = c * i_center_x - s * i_center_y + (ins.x - (c * ref_.x - s * ref_.y));
ry = s * i_center_x + c * i_center_y + (ins.y - (s * ref_.x + c * ref_.y));
// Check that the vote is within range
if (rx < mMinX || rx >= mMaxX || ry < mMinY || ry >= mMaxY ||
rangle <= -PI || rangle > PI ||
rscale < mMinScale || rscale >= mMaxScale)
{
continue;
}
}
// Compute the bin location
SubBinLocation sub_bin = i_sub_bin_locations[num_features_that_cast_vote];
mapVoteToBin(sub_bin, rx, ry, rangle, rscale);
int binX = (int)Math.Floor(sub_bin.x - 0.5f);
int binY = (int)Math.Floor(sub_bin.y - 0.5f);
int binScale = (int)Math.Floor(sub_bin.scale - 0.5f);
int binAngle = ((int)Math.Floor(sub_bin.angle - 0.5f) + mNumAngleBins) % mNumAngleBins;
// Check that we can voting to all 16 bin locations
if (binX < 0 || (binX + 1) >= mNumXBins || binY < 0 ||
(binY + 1) >= mNumYBins || binScale < 0 ||
(binScale + 1) >= mNumScaleBins)
{
continue;
}
sub_bin.index = i;
num_features_that_cast_vote++;
this.votemap.vote16(binX, binY, binAngle, binScale, 1);
}
return(num_features_that_cast_vote);
}
