/// <summary>
/// Add the EKF.
/// 追加
/// </summary>
/// <param name="q">Q.</param>
/// <param name="error">Error.</param>
/// <param name="points">Points.</param>
/// <param name="Descriptors">Descriptors.</param>
public IEnumerator AddEnumerator(Quaternion q, float error, IList <Vector3> points, MatOfFloat Descriptors)
{
_updateStatus = true;
if (points.Count != Descriptors.rows())
{
throw new OutLookARException("KeyVectorとDescriptorsの数が合いません");
}
int nowSize = StateMatrix.rows();
int N = (nowSize - 4) / 3;
int pointsCount = points.Count;
int newSize = nowSize + pointsCount * 3;
Mat uStateMatrix = g(StateMatrix, q);
uStateMatrix = uStateMatrix.resize(newSize);
yield return(null);
Mat uVarianceCovarianceMatrix = G(N) * VarianceCovarianceMatrix * GT(N) + FxT(N) * R(error) * Fx(N);
uVarianceCovarianceMatrix = uVarianceCovarianceMatrix.resize(newSize, newSize);
for (int i = 0; i < pointsCount; i++)
{
for (int c = 0; c < 3; c++)
{
uVarianceCovarianceMatrix.put(nowSize + c + 3 * i, nowSize + c + 3 * i, Infinite);
}
yield return(null);
}
Mat I = Mat.eye(newSize, newSize, Type);
Quaternion uStateMatrix_q = new Quaternion((float)uStateMatrix.get(0, 0)[0], (float)uStateMatrix.get(1, 0)[0], (float)uStateMatrix.get(2, 0)[0], (float)uStateMatrix.get(3, 0)[0]);
int j = N + 1;
foreach (Vector3 point in points)
{
Vector3 uStateMatrix_j = uStateMatrix_q * point;
uStateMatrix.put(j * 3 + 1, 0, uStateMatrix_j.x);
uStateMatrix.put(j * 3 + 2, 0, uStateMatrix_j.y);
uStateMatrix.put(j * 3 + 3, 0, uStateMatrix_j.z);
Mat h = H(uStateMatrix_j, uStateMatrix_q) * FxJ(j, N + pointsCount);
Mat K = uVarianceCovarianceMatrix * h.t() * (h * uVarianceCovarianceMatrix * h.t() + Q).inv();
uVarianceCovarianceMatrix = (I - K * h) * uVarianceCovarianceMatrix;
yield return(null);
j++;
}
_stateMatrix = uStateMatrix;
_varianceCovarianceMatrix = uVarianceCovarianceMatrix;
_stateDescriptors.Add(Descriptors);
_updateStatus = false;
}
public IEnumerator UpdateMap(IList <Vector3> KeyVector, MatOfFloat Descriptors)
{
float MinError = 500f;
if (UpdateMapFlag)
{
throw new OutLookARException("Map更新中です。");
}
if (!initFlag)
{
throw new OutLookARException("初期化されていません");
}
if (KeyVector.Count != Descriptors.rows() || Descriptors.rows() <= 0)
{
throw new OutLookARException("KeyVectorとDescriptorsの数が合いません");
}
updateMapFlag = true;
Debug.Log("Update Start");
IList <DMatch> matches;
matches = Utils.CrossMatcher(MapDescriptors, Descriptors).LowPassFilter();
if (matches.Count <= 0)
{
updateMapFlag = false;
throw new OutLookARException("マッチしませんでした");
}
Debug.Log("Updating <1>");
yield return(null);
List <int> IndexL = new List <int>();
List <Vector3> FromPointL = new List <Vector3>();
List <Vector3> ToPointL = new List <Vector3>();
for (int i = 0; i < KeyVector.Count; i++)
{
IndexL.Add(i);
}
Debug.Log("Updating <2>");
foreach (DMatch match in matches)
{
IndexL.Remove(match.trainIdx);
FromPointL.Add(MapKeyVector[match.queryIdx]);
ToPointL.Add(KeyVector[match.trainIdx]);
}
yield return(null);
Debug.Log("Updating <3>");
float minError;
Quaternion q = LMedS(FromPointL, ToPointL, out minError);
Debug.Log(minError);
if (minError < 10f)
{
foreach (int id in IndexL)
{
MapDescriptors.Add(Descriptors.row(id));
MapKeyVector.Add(new Quaternion(-q.x, -q.y, -q.z, q.w) * KeyVector[id]);
MapError.Add(minError + 10f);
}
}
yield return(null);
Debug.Log("Updating <4>");
SortedList <int, int> RemoveIDL = new SortedList <int, int>();
foreach (DMatch match in matches)
{
Vector3 vec = q * MapKeyVector[match.queryIdx];
float distanse = Mathf.Sqrt(Mathf.Pow((vec.x - KeyVector[match.trainIdx].x), 2) + Mathf.Pow((vec.y - KeyVector[match.trainIdx].y), 2) + Mathf.Pow((vec.z - KeyVector[match.trainIdx].z), 2));
if (distanse > MinError)
{
try
{
if (MapError[match.queryIdx] > minError)
{
RemoveIDL.Add(MapKeyVector.Count - match.queryIdx, match.queryIdx);
}
else if (MapError[match.queryIdx] < minError)
{
MapKeyVector[match.queryIdx] = (new Quaternion(-q.x, -q.y, -q.z, q.w) * KeyVector[match.trainIdx]);
MapError[match.queryIdx] = minError;
}
}
catch (InvalidCastException e)
{
Debug.Log(e);
}
}
}
yield return(null);
MatOfFloat MapDescriptorsClone = MapDescriptors.clone() as MatOfFloat;
foreach (int id in RemoveIDL.Values)
{
try
{
MapDescriptorsClone.RemoveAt(id);
MapKeyVector.RemoveAt(id);
MapError.RemoveAt(id);
}
catch (InvalidCastException e)
{
Debug.Log(e);
}
}
MapDescriptorsClone.copyTo(MapDescriptors);
updateMapFlag = false;
Debug.Log("Update Comp");
}