protected virtual Vector2[] ToLocal(Vector2[] points, Pose2D target)
{
TMatrix2D rotmx = target.ToGlobalMatrix();
Vector2[] local = new Vector2[(points.Length - 1) + 1];
int len = points.Length - 1;
for (int i = 0; i <= len; i++)
{
local[i] = (Vector2)(rotmx * points[i]);
}
return(local);
}
protected virtual Vector2[] ToLocal(Vector2[] points, Pose2D target)
{
int i2;
TMatrix2D rotmx = target.ToGlobalMatrix();
Vector2[] local = new Vector2[checked (checked (checked ((int)points.Length) - 1) + 1)];
int i1 = checked (checked ((int)points.Length) - 1);
int i = 0;
while (i <= i2)
{
local[i] = rotmx * points[i];
i = checked (i + 1);
i2 = i1;
}
return(local);
}
// Methods
protected internal override MatchResult MatchPoints(Vector2[] points1, Vector2[] points2, Pose2D seed, double dangle, bool[] filter1, bool[] filter2)
{
int startTime = Environment.TickCount;
Pose2D rawdpose = seed;
Pose2D curdpose = rawdpose;
Vector2[] curpoints2 = null;
Correlation <Vector2>[] curpairs = null;
Queue <double> results = new Queue <double>();
int nconverged = 0;
Vector2 uavg = new Vector2();
double sumerror = 0.0;
double sumangcov = 0.0;
int n = 1;
while (n <= base.MAX_ITERATIONS)
{
curpoints2 = this.ToLocal(points2, curdpose);
curpairs = this.CorrelatePointsDM(points1, curpoints2, filter1, filter2);
if (curpairs.Length > 0)
{
Vector2 sumpoint1 = new Vector2();
Vector2 sumpoint2 = new Vector2();
foreach (Correlation <Vector2> pair in curpairs)
{
sumpoint1 += pair.Point1;
sumpoint2 += pair.Point2;
}
Vector2 u1 = sumpoint1 / curpairs.GetLength(0);
Vector2 u2 = sumpoint2 / curpairs.GetLength(0);
uavg = (u1 + u2) / 2;
sumpoint1 = new Vector2();
sumpoint2 = new Vector2();
sumerror = 0.0;
sumangcov = 0.0;
foreach (Correlation <Vector2> pair in curpairs)
{
Vector2 point1 = pair.Point1;
Vector2 point2 = pair.Point2;
sumpoint1 += new Vector2((point1[0] - u1[0]) * (point2[0] - u2[0]), (point1[1] - u1[1]) * (point2[1] - u2[1]));
sumpoint2 += new Vector2((point1[0] - u1[0]) * (point2[1] - u2[0]), (point1[1] - u1[1]) * (point2[0] - u2[1]));
Vector2 dpoint = point1 - point2;
Vector2 mpoint = (point1 + point2) / 2;
sumerror += dpoint * dpoint;
sumangcov += mpoint * mpoint;
}
double rotest = System.Math.Atan2(sumpoint2[0] - sumpoint2[1], sumpoint1[0] + sumpoint1[1]);
TMatrix2D rotmx = new TMatrix2D(rotest);
Vector2 transest = u2 - (rotmx * u1);
curdpose = new Pose2D(curdpose.Position - transest, curdpose.Rotation - rotest);
rotmx = new TMatrix2D(-rotest);
curdpose = (Pose2D)(rotmx * curdpose);
bool converged = false;
foreach (double result in results)
{
if (sumerror == 0.0)
{
converged = converged || (result < base.RESULTS_MAXERROR);
}
else
{
converged = converged || (System.Math.Abs((double)((result / sumerror) - 1.0)) < base.RESULTS_MAXERROR);
}
}
results.Enqueue(sumerror);
while (results.Count > base.RESULTS_CONSIDERED)
{
results.Dequeue();
}
if (converged)
{
nconverged++;
}
}
if (nconverged >= base.RESULTS_CONVERGED)
{
break;
}
n++;
}
Matrix3 mcov = new Matrix3();
bool inverseError = false;
double mdist = 0.0;
int num = 0;
if (curpairs.Length > 0)
{
foreach (Correlation <Vector2> pair in curpairs)
{
Vector2 dpoint = pair.Point2 - pair.Point1;
mdist += System.Math.Pow(dpoint.X, 2.0) + System.Math.Pow(dpoint.Y, 2.0);
}
mdist = System.Math.Pow(mdist, 0.5);
num = curpairs.GetLength(0);
MathNet.Numerics.LinearAlgebra.Matrix cov = new MathNet.Numerics.LinearAlgebra.Matrix(3, 3);
cov[0, 0] = num;
cov[0, 1] = 0.0;
cov[0, 2] = -num * uavg[1];
cov[1, 0] = 0.0;
cov[1, 1] = num;
cov[1, 2] = num * uavg[0];
cov[2, 0] = -num * uavg[1];
cov[2, 1] = num * uavg[0];
cov[2, 2] = sumangcov;
try
{
cov = (sumerror / (2 * num - 3)) * cov.Inverse();
}
catch
{
inverseError = true;
}
mcov[0, 0] = (float)cov[0, 0];
mcov[0, 1] = (float)cov[0, 1];
mcov[0, 2] = (float)cov[0, 2];
mcov[1, 0] = (float)cov[1, 0];
mcov[1, 1] = (float)cov[1, 1];
mcov[1, 2] = (float)cov[1, 2];
mcov[2, 0] = (float)cov[2, 0];
mcov[2, 1] = (float)cov[2, 1];
mcov[2, 2] = (float)cov[2, 2];
}
double duration = (Environment.TickCount - startTime) / 1000.0;
return(new MatchResult(rawdpose, curdpose, n, duration, this.HasConverged(points1.Length, num, n) && !inverseError));
}
protected internal override MatchResult MatchPoints(Vector2[] points1, Vector2[] points2, Pose2D seed, double dangle, bool[] filter1, bool[] filter2)
{
//Debug.DrawPoints(points1, 0);
//Vector2[] pt2Local = this.ToLocal(points2, seed);
//Debug.DrawPoints(pt2Local, 1);
int startTime = Environment.TickCount;
Matrix2 J = new Matrix2(0, -1, 1, 0);
Matrix2 Jt = J.Transpose();
double[] iangles1 = this.ComputeIncidenceAngles(points1);
double[] iangles2 = this.ComputeIncidenceAngles(points2);
//covariance matrixes
Matrix2[] ecovs1 = new Matrix2[points1.Length];
Matrix2[] ecovs2 = new Matrix2[points2.Length];
for (int i = 0; i < ecovs1.Length; i++)
{
ecovs1[i] = this.ComputeErrorCovariance(points1[i], iangles1[i], dangle);
}
Pose2D rawdpose = seed;
Pose2D curdpose = rawdpose;
Vector2[] curpoints2 = null;
Correlation <Vector2>[] curpairs = null;
//indexers, for fast access to covariance matrices
int index = 0;
int[] index1 = null;
int[] index2 = null;
Queue <double> results = new Queue <double>();
int nconverged = 0;
Matrix2 suminvsumcov = new Matrix2();
Vector2 midpoint = new Vector2();
Matrix2 sumsumcov = new Matrix2();
Vector2 transest = new Vector2();
double rotest = 0;
int n = 0;
for (n = 1; n <= MAX_ITERATIONS; n++)
{
//recompute relative coords for range-scan 2 given the new dpose estimate
curpoints2 = this.ToLocal(points2, curdpose);
//Debug.DrawPoints(curpoints2, 2);
//recompute covariances for curpoints2 (= covs2)
for (int i = 0; i < ecovs2.Length; i++)
{
ecovs2[i] = this.ComputeErrorCovariance(curpoints2[i], iangles2[i], dangle);
}
//find all correlated point-pairs
Heap assoc = this.CorrelatePointsQT(points1, curpoints2, filter1, filter2);
curpairs = getFilteredCorrespondances(assoc);
//Debug.DrawPointRelations(curpairs);
if (curpairs.Length > 0)
{
//recompute indexes only once per iteration
index1 = new int[curpairs.Length];
index2 = new int[curpairs.Length];
index = 0;
foreach (Correlation <Vector2> pair in curpairs)
{
index1[index] = Array.IndexOf(points1, pair.Point1);
index2[index] = Array.IndexOf(curpoints2, pair.Point2);
index++;
}
//these vars will hold summaries for a single iteration
Vector2 sumipoint1 = new Vector2();
Vector2 sumipoint2 = new Vector2();
Matrix2 sumirotmid = new Matrix2();
Vector2 sumrotmid = new Vector2();
double sumerror = 0;
suminvsumcov = new Matrix2();
index = 0;
foreach (Correlation <Vector2> pair in curpairs)
{
//get corresponding points and error covariances ...
Vector2 point1 = pair.Point1;
Vector2 point2 = pair.Point2;
Matrix2 cov1 = ecovs1[index1[index]];
Matrix2 cov2 = ecovs2[index2[index]];
//translation estimate ...
Matrix2 sumcov = cov1 + cov2;
Matrix2 invsumcov = sumcov.Invert();
suminvsumcov = suminvsumcov + invsumcov;
sumipoint1 = sumipoint1 + invsumcov * point1;
sumipoint2 = sumipoint2 + invsumcov * point2;
//error metric ...
Vector2 dpoint = point1 - point2;
sumerror = sumerror + (dpoint * invsumcov * dpoint);
//rotation estimate ...
Matrix2 irotmid = Jt * invsumcov * J;
sumirotmid = sumirotmid + 2 * irotmid;
sumrotmid = sumrotmid + irotmid * point1 + irotmid * point2;
index++;
}
//get rotational midpoint and translation estimate
midpoint = sumirotmid.Invert() * sumrotmid;
sumsumcov = suminvsumcov.Invert();
transest = sumsumcov * (sumipoint2 - sumipoint1);
double sumnval = 0;
double sumdval = 0;
index = 0;
foreach (Correlation <Vector2> pair in curpairs)
{
//get corresponding points and error covariances ...
Vector2 point1 = pair.Point1;
Vector2 point2 = pair.Point2;
Matrix2 cov1 = ecovs1[index1[index]];
Matrix2 cov2 = ecovs2[index2[index]];
//apply estimated midpoint and translation
point1 = point1 - midpoint;
point2 = point2 - transest - midpoint;
Matrix2 sumcov = cov1 + cov2;
Matrix2 invsumcov = sumcov.Invert();
//update numerator and denominator summaries
Vector2 dpoint = point2 - point1;
double nval = dpoint * invsumcov * J * point2;
double dval = dpoint * invsumcov * point2 + point2 * J * invsumcov * J * point2;
sumnval = sumnval + nval;
sumdval = sumdval + dval;
index++;
}
//get rotational estimate
rotest = -sumnval / sumdval;
//update odometry estimate
Vector2 vdpose = curdpose.Position - transest - midpoint;
TMatrix2D rotmx = new TMatrix2D(-rotest);
vdpose = rotmx * vdpose;
vdpose = vdpose + midpoint;
curdpose = new Pose2D(vdpose, curdpose.Rotation - rotest);
//check for convergence
bool converged = false;
foreach (double result in results)
{
if (sumerror == 0)
{
converged = converged || (result < RESULTS_MAXERROR);
}
else
{
converged = converged || (System.Math.Abs(result / sumerror - 1) < RESULTS_MAXERROR);
}
}
results.Enqueue(sumerror);
while (results.Count > RESULTS_CONSIDERED)
{
results.Dequeue();
}
if (converged)
{
nconverged++;
}
}
if (nconverged >= RESULTS_CONVERGED)
{
break;
}
}
int untilTime = Environment.TickCount;
double duration = (untilTime - startTime) / 1000.0;
int num = (curpairs != null ? curpairs.Length : -1);
return(new MatchResult(rawdpose, curdpose, n, duration, this.HasConverged(points1.Length, num, n)));
}
