public MatchVisualization(MatchVisualization other)
{
raysUpSpace = other.raysUpSpace.ToList();
poseOfUpSpace = other.poseOfUpSpace;
projectedRays = other.projectedRays.ToList();
markersIndices = other.markersIndices.ToList();
}
Bool IEnvironment.match(Vector3[] Rays, out MarkerIndex[] Markers, out Pose PositionofUp)
{
const float projectionsMatchTolerance = 0.06f;
Markers = Enumerable.Repeat(MarkerIndex.Unknown, Rays.Length).ToArray();
PositionofUp = new Pose();
if (Rays.Length != 3)
{
return(false);
}
var projecties = projectRaysOnFloor(Rays);
var M = new float[6, 4];
var b = new float[6];
for (int i = 0; i < 3; ++i)
{
M[2 * i, 0] = projecties[i].x;
M[2 * i, 1] = -projecties[i].y;
M[2 * i, 2] = 1;
M[2 * i + 1, 0] = projecties[i].y;
M[2 * i + 1, 1] = projecties[i].x;
M[2 * i + 1, 3] = 1;
b[2 * i] = _markers[i].x;
b[2 * i + 1] = _markers[i].y;
}
var invM = M.transpose().multiply(M).inverse().multiply(M.transpose());
int BestMatch = -1;
float bestError = float.MaxValue;
var bestTransform2d = new ConvertTo2D();
for (int idPermutation = 0; idPermutation < 6; ++idPermutation)
{
var curB = new float[6];
for (int i = 0; i < 4; ++i)
{
int j = permutations[idPermutation][i];
curB[2 * i] = b[2 * j];
curB[2 * i + 1] = b[2 * j + 1];
}
var transformParams = invM.multiply(curB);
var transform2d = new ConvertTo2D(transformParams[0], transformParams[1], transformParams[2], transformParams[3]);
float error = 0;
for (int idMarker = 0; idMarker < 3; ++idMarker)
{
error += (transform2d.apply(projecties[idMarker]) - _markers[permutations[idPermutation][idMarker]]).sqrMagnitude;
}
if (error < bestError)
{
bestError = error;
BestMatch = idPermutation;
bestTransform2d = transform2d;
}
}
if (bestError > projectionsMatchTolerance)
{
return(false);
}
for (int i = 0; i < Rays.Length; ++i)
{
Markers[i].value = (uint)permutations[BestMatch][i];
}
var position = new Vector3(bestTransform2d.Translate.x, bestTransform2d.Scale, bestTransform2d.Translate.y);
var rotation = Quaternion.AxisAngle(Vector3.up, -bestTransform2d.Angle);
PositionofUp = new Pose(position, rotation);
lock (_visobject)
{
_matchViz = new MatchVisualization(
Rays.ToList(),
PositionofUp,
projecties.Select(p => bestTransform2d.apply(p)).ToList(),
Markers);
}
return(true);
}
Bool IEnvironment.match(Vector3[] raysUpSpace, out MarkerIndex[] markersIndices, out Pose poseOfUpSpace)
{
const float projectionsMatchTolerance = 0.05f;
markersIndices = Enumerable.Repeat(MarkerIndex.Unknown, raysUpSpace.Length).ToArray();
poseOfUpSpace = new Pose();
// For the sake of simplicity, we won't deal with cases when more than three rays is visible.
if (raysUpSpace.Length != 3)
{
return(false);
}
// rays projection on X-Y plane of "up space" at height of 1 meter
var projections = projectRaysOnFloor(raysUpSpace);
// We have to find transform that match rays projections to markers in world space in form f(x) = SR*x + t, where
// SR - scale and rotation matrix: s*{{cos(r), -sin(r)}, {sin(r), cos(r)}}; s - scale coefficient, r - rotation angle,
// t - translation vector.
// Lets solve system of linear equations {SR*ai + t = bi} for SR and t
// System can be rewritten as M*x = b
// / a0.x -a0.y 1 0 \ / s*cos(r) \ / b0.x \
// | a0.y a0.x 0 1 | * | s*sin(r) | = | b0.y |
// | ... | | t.x | | ... |
// \ ... / \ t.y / \ ... /
var M = new float[6, 4];
var b = new float[6];
for (int i = 0; i < 3; ++i)
{
M[2 * i, 0] = projections[i].x;
M[2 * i, 1] = -projections[i].y;
M[2 * i, 2] = 1;
M[2 * i + 1, 0] = projections[i].y;
M[2 * i + 1, 1] = projections[i].x;
M[2 * i + 1, 3] = 1;
b[2 * i] = _markers[i].x;
b[2 * i + 1] = _markers[i].y;
}
// left inverse
var invM = M.transpose().multiply(M).inverse().multiply(M.transpose());
// Iterate over all possible permutations and pick the best one if its good enough.
int bestPermutationId = -1;
float bestError = float.MaxValue;
var bestTransform2d = new Transform2d();
for (int idPermutation = 0; idPermutation < 6; ++idPermutation)
{
// make vector of constant terms for current markers order
var curB = new float[6];
for (int i = 0; i < 3; ++i)
{
int j = permutations[idPermutation][i];
curB[2 * i] = b[2 * j];
curB[2 * i + 1] = b[2 * j + 1];
}
var transformParams = invM.multiply(curB); // {scale*cos(r), scale*sin(r), t.x, t.y}
var transform2d = new Transform2d(transformParams[0], transformParams[1], transformParams[2], transformParams[3]);
float error = 0; // sum of square distances between corresponding markers and transformed rays projections
for (int idMarker = 0; idMarker < 3; ++idMarker)
{
error += (transform2d.apply(projections[idMarker]) - _markers[permutations[idPermutation][idMarker]]).sqrMagnitude;
}
if (error < bestError)
{
bestError = error;
bestPermutationId = idPermutation;
bestTransform2d = transform2d;
}
}
if (bestError > projectionsMatchTolerance)
{
return(false);
}
for (int i = 0; i < raysUpSpace.Length; ++i)
{
markersIndices[i].value = (uint)permutations[bestPermutationId][i];
}
var position = new Vector3(bestTransform2d.Translate.x, bestTransform2d.Scale, bestTransform2d.Translate.y);
var rotation = Quaternion.AxisAngle(Vector3.up, -bestTransform2d.Angle);
poseOfUpSpace = new Pose(position, rotation);
lock (_visualizationLocker) {
_matchVisualization = new MatchVisualization(
raysUpSpace.ToList(),
poseOfUpSpace,
projections.Select(p => bestTransform2d.apply(p)).ToList(),
markersIndices);
}
return(true);
}
