private void sort(ref MatOfPoint2f fourPoints)
{
// the argument needs to contain 4 points precisely
// sort the bounding box to (topleft, topright, bottomright, bottomleft
double[] p1, p2, p3, p4;
List <Point> points = new List <Point>();
p1 = new double[2] {
fourPoints.get(0, 0)[0], fourPoints.get(0, 0)[1]
};
p2 = new double[2] {
fourPoints.get(1, 0)[0], fourPoints.get(1, 0)[1]
};
p3 = new double[2] {
fourPoints.get(2, 0)[0], fourPoints.get(2, 0)[1]
};
p4 = new double[2] {
fourPoints.get(3, 0)[0], fourPoints.get(3, 0)[1]
};
print("p1: " + (char)p1[0] + "," + (char)p1[1]);
print("p2: " + (char)p2[0] + "," + (char)p2[1]);
print("p3: " + (char)p3[0] + "," + (char)p3[1]);
print("p4: " + (char)p4[0] + "," + (char)p4[1]);
if (p1[0] < p2[0] && p1[0] < p3[0] || p1[0] < p4[0] && p1[0] < p3[0] || p1[0] < p2[0] && p1[0] < p4[0])
{
if (p1[1] < p2[1] && p1[1] < p3[1] || p1[1] < p4[1] && p1[1] < p3[1] || p1[1] < p2[1] && p1[1] < p4[1])
{
points.Add(new Point(p1[0], p1[1]));
if (p2[1] < p3[1] && p2[1] < p4[1])
{
points.Add(new Point(p2[0], p2[1]));
if (p3[0] < p4[0])
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p3[0], p3[1]));
}
else
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p4[0], p4[1]));
}
}
else if (p3[1] < p2[1] && p3[1] < p4[1])
{
points.Add(new Point(p3[0], p3[1]));
if (p2[0] < p4[0])
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p4[0], p4[1]));
}
}
else if (p4[1] < p2[1] && p4[1] < p3[1])
{
points.Add(new Point(p4[0], p4[1]));
if (p2[0] < p3[0])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p3[0], p3[1]));
}
}
}
else if (p2[0] < p3[0] && p2[0] < p4[0])
{
points.Add(new Point(p2[0], p2[1]));
if (p3[1] < p4[1])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p4[0], p4[1]));
}
else
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p3[0], p3[1]));
}
points.Add(new Point(p1[0], p1[1]));
}
else if (p3[0] < p2[0] && p3[0] < p4[0])
{
points.Add(new Point(p3[0], p3[1]));
if (p2[1] < p4[1])
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p4[0], p4[1]));
}
else
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p2[0], p2[1]));
}
points.Add(new Point(p1[0], p1[1]));
}
else if (p4[0] < p3[0] && p4[0] < p2[0])
{
points.Add(new Point(p4[0], p4[1]));
if (p3[1] < p2[1])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p3[0], p3[1]));
}
points.Add(new Point(p1[0], p1[1]));
}
}
else if (p2[0] < p1[0] && p2[0] < p3[0] || p2[0] < p4[0] && p2[0] < p3[0] || p2[0] < p1[0] && p2[0] < p4[0])
{
if (p2[1] < p1[1] && p2[1] < p3[1] || p2[1] < p4[1] && p2[1] < p3[1] || p2[1] < p1[1] && p2[1] < p4[1])
{
points.Add(new Point(p2[0], p2[1]));
if (p1[1] < p3[1] && p1[1] < p4[1])
{
points.Add(new Point(p1[0], p1[1]));
if (p3[0] < p4[0])
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p3[0], p3[1]));
}
else
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p4[0], p4[1]));
}
}
else if (p3[1] < p1[1] && p3[1] < p4[1])
{
points.Add(new Point(p3[0], p3[1]));
if (p1[0] < p4[0])
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p1[0], p1[1]));
}
else
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p4[0], p4[1]));
}
}
else if (p4[1] < p1[1] && p4[1] < p3[1])
{
points.Add(new Point(p4[0], p4[1]));
if (p1[0] < p3[0])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p1[0], p1[1]));
}
else
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p3[0], p3[1]));
}
}
}
else if (p1[0] < p3[0] && p1[0] < p4[0])
{
points.Add(new Point(p1[0], p1[1]));
if (p3[1] < p4[1])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p4[0], p4[1]));
}
else
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p3[0], p3[1]));
}
points.Add(new Point(p2[0], p2[1]));
}
else if (p3[0] < p1[0] && p3[0] < p4[0])
{
points.Add(new Point(p3[0], p3[1]));
if (p1[1] < p4[1])
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p4[0], p4[1]));
}
else
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p1[0], p1[1]));
}
points.Add(new Point(p2[0], p2[1]));
}
else if (p4[0] < p3[0] && p4[0] < p1[0])
{
points.Add(new Point(p4[0], p4[1]));
if (p3[1] < p1[1])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p1[0], p1[1]));
}
else
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p3[0], p3[1]));
}
points.Add(new Point(p2[0], p2[1]));
}
}
else if (p3[0] < p2[0] && p3[0] < p1[0] || p3[0] < p4[0] && p3[0] < p1[0] || p3[0] < p2[0] && p3[0] < p4[0])
{
if (p3[1] < p2[1] && p3[1] < p1[1] || p3[1] < p4[1] && p3[1] < p1[1] || p3[1] < p2[1] && p3[1] < p4[1])
{
points.Add(new Point(p3[0], p3[1]));
if (p2[1] < p1[1] && p2[1] < p4[1])
{
points.Add(new Point(p2[0], p2[1]));
if (p1[0] < p4[0])
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p1[0], p1[1]));
}
else
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p4[0], p4[1]));
}
}
else if (p1[1] < p2[1] && p1[1] < p4[1])
{
points.Add(new Point(p1[0], p1[1]));
if (p2[0] < p4[0])
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p4[0], p4[1]));
}
}
else if (p4[1] < p2[1] && p4[1] < p1[1])
{
points.Add(new Point(p4[0], p4[1]));
if (p2[0] < p1[0])
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p1[0], p1[1]));
}
}
}
else if (p2[0] < p1[0] && p2[0] < p4[0])
{
points.Add(new Point(p2[0], p2[1]));
if (p1[1] < p4[1])
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p4[0], p4[1]));
}
else
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p1[0], p1[1]));
}
points.Add(new Point(p3[0], p3[1]));
}
else if (p1[0] < p2[0] && p1[0] < p4[0])
{
points.Add(new Point(p1[0], p1[1]));
if (p2[1] < p4[1])
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p4[0], p4[1]));
}
else
{
points.Add(new Point(p4[0], p4[1]));
points.Add(new Point(p2[0], p2[1]));
}
points.Add(new Point(p3[0], p3[1]));
}
else if (p4[0] < p1[0] && p4[0] < p2[0])
{
points.Add(new Point(p4[0], p4[1]));
if (p1[1] < p2[1])
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p1[0], p1[1]));
}
points.Add(new Point(p3[0], p3[1]));
}
}
else if (p4[0] < p2[0] && p4[0] < p3[0] || p4[0] < p1[0] && p4[0] < p3[0] || p4[0] < p2[0] && p4[0] < p1[0])
{
if (p4[1] < p2[1] && p4[1] < p3[1] || p4[1] < p1[1] && p4[1] < p3[1] || p4[1] < p2[1] && p4[1] < p1[1])
{
points.Add(new Point(p4[0], p4[1]));
if (p2[1] < p3[1] && p2[1] < p1[1])
{
points.Add(new Point(p2[0], p2[1]));
if (p3[0] < p1[0])
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p3[0], p3[1]));
}
else
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p1[0], p1[1]));
}
}
else if (p3[1] < p2[1] && p3[1] < p1[1])
{
points.Add(new Point(p3[0], p3[1]));
if (p2[0] < p1[0])
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p1[0], p1[1]));
}
}
else if (p1[1] < p2[1] && p1[1] < p3[1])
{
points.Add(new Point(p1[0], p1[1]));
if (p2[0] < p3[0])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p3[0], p3[1]));
}
}
}
else if (p2[0] < p3[0] && p2[0] < p1[0])
{
points.Add(new Point(p2[0], p2[1]));
if (p3[1] < p1[1])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p1[0], p1[1]));
}
else
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p3[0], p3[1]));
}
points.Add(new Point(p4[0], p4[1]));
}
else if (p3[0] < p2[0] && p3[0] < p1[0])
{
points.Add(new Point(p3[0], p3[1]));
if (p2[1] < p1[1])
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p1[0], p1[1]));
}
else
{
points.Add(new Point(p1[0], p1[1]));
points.Add(new Point(p2[0], p2[1]));
}
points.Add(new Point(p4[0], p4[1]));
}
else if (p1[0] < p3[0] && p1[0] < p2[0])
{
points.Add(new Point(p1[0], p1[1]));
if (p3[1] < p2[1])
{
points.Add(new Point(p3[0], p3[1]));
points.Add(new Point(p2[0], p2[1]));
}
else
{
points.Add(new Point(p2[0], p2[1]));
points.Add(new Point(p3[0], p3[1]));
}
points.Add(new Point(p4[0], p4[1]));
}
}
// MatOfPoint2f pts2 = new MatOfPoint2f();
fourPoints = new MatOfPoint2f();
fourPoints.release();
fourPoints.fromList(points);
}
/// <summary>
/// Refines the matches with homography.
/// </summary>
/// <returns><c>true</c>, if matches with homography was refined, <c>false</c> otherwise.</returns>
/// <param name="queryKeypoints">Query keypoints.</param>
/// <param name="trainKeypoints">Train keypoints.</param>
/// <param name="reprojectionThreshold">Reprojection threshold.</param>
/// <param name="matches">Matches.</param>
/// <param name="homography">Homography.</param>
static bool refineMatchesWithHomography
(
MatOfKeyPoint queryKeypoints,
MatOfKeyPoint trainKeypoints,
float reprojectionThreshold,
MatOfDMatch matches,
Mat homography
)
{
// Debug.Log ("matches " + matches.ToString ());
int minNumberMatchesAllowed = 8;
List <KeyPoint> queryKeypointsList = queryKeypoints.toList();
List <KeyPoint> trainKeypointsList = trainKeypoints.toList();
List <DMatch> matchesList = matches.toList();
if (matchesList.Count < minNumberMatchesAllowed)
{
return(false);
}
// Prepare data for cv::findHomography
List <Point> srcPointsList = new List <Point> (matchesList.Count);
List <Point> dstPointsList = new List <Point> (matchesList.Count);
for (int i = 0; i < matchesList.Count; i++)
{
srcPointsList.Add(trainKeypointsList [matchesList [i].trainIdx].pt);
dstPointsList.Add(queryKeypointsList [matchesList [i].queryIdx].pt);
}
// Find homography matrix and get inliers mask
using (MatOfPoint2f srcPoints = new MatOfPoint2f())
using (MatOfPoint2f dstPoints = new MatOfPoint2f())
using (MatOfByte inliersMask = new MatOfByte(new byte[srcPointsList.Count])) {
srcPoints.fromList(srcPointsList);
dstPoints.fromList(dstPointsList);
// Debug.Log ("srcPoints " + srcPoints.ToString ());
// Debug.Log ("dstPoints " + dstPoints.ToString ());
Calib3d.findHomography(srcPoints,
dstPoints,
Calib3d.FM_LMEDS,
reprojectionThreshold,
inliersMask, 2000, 0.9).copyTo(homography);
if (homography.rows() != 3 || homography.cols() != 3)
{
return(false);
}
//Debug.Log ("homography " + homography.ToString ());
//Debug.Log ("inliersMask " + inliersMask.dump ());
List <byte> inliersMaskList = inliersMask.toList();
List <DMatch> inliers = new List <DMatch> ();
for (int i = 0; i < inliersMaskList.Count; i++)
{
if (inliersMaskList [i] == 1)
{
inliers.Add(matchesList [i]);
}
}
matches.fromList(inliers);
//Debug.Log ("matches " + matches.ToString ());
}
return(matchesList.Count > minNumberMatchesAllowed);
}
/// <summary>
/// Processes points by filter.
/// </summary>
/// <param name="img">Image mat.</param>
/// <param name="srcPoints">Input points.</param>
/// <param name="dstPoints">Output points.</param>
/// <param name="drawDebugPoints">if true, draws debug points.</param>
/// <returns>Output points.</returns>
public override List <Vector2> Process(Mat img, List <Vector2> srcPoints, List <Vector2> dstPoints = null, bool drawDebugPoints = false)
{
if (srcPoints != null && srcPoints.Count != numberOfElements)
{
throw new ArgumentException("The number of elements is different.");
}
if (srcPoints == null)
{
return(dstPoints == null ? srcPoints : dstPoints);
}
if (!flag)
{
if (img.channels() == 4)
{
Imgproc.cvtColor(img, prevgray, Imgproc.COLOR_RGBA2GRAY);
}
else if (img.channels() == 3)
{
Imgproc.cvtColor(img, prevgray, Imgproc.COLOR_RGB2GRAY);
}
else
{
if (prevgray.total() == 0)
{
prevgray = img.clone();
}
else
{
img.copyTo(prevgray);
}
}
for (int i = 0; i < numberOfElements; i++)
{
prevTrackPts[i] = new Point(srcPoints [i].x, srcPoints [i].y);
}
flag = true;
}
if (srcPoints != null)
{
if (dstPoints == null)
{
dstPoints = new List <Vector2> ();
}
if (dstPoints != null && dstPoints.Count != numberOfElements)
{
dstPoints.Clear();
for (int i = 0; i < numberOfElements; i++)
{
dstPoints.Add(new Vector2());
}
}
if (img.channels() == 4)
{
Imgproc.cvtColor(img, gray, Imgproc.COLOR_RGBA2GRAY);
}
else if (img.channels() == 3)
{
Imgproc.cvtColor(img, gray, Imgproc.COLOR_RGB2GRAY);
}
else
{
if (gray.total() == 0)
{
gray = img.clone();
}
else
{
img.copyTo(gray);
}
}
if (prevgray.total() > 0)
{
mOP2fPrevTrackPts.fromList(prevTrackPts);
mOP2fNextTrackPts.fromList(nextTrackPts);
Video.calcOpticalFlowPyrLK(prevgray, gray, mOP2fPrevTrackPts, mOP2fNextTrackPts, status, err);
prevTrackPts = mOP2fPrevTrackPts.toList();
nextTrackPts = mOP2fNextTrackPts.toList();
// clac diffDlib
prevTrackPtsMat.fromList(prevTrackPts);
OpenCVForUnity.Rect rect = Imgproc.boundingRect(prevTrackPtsMat);
double diffDlib = this.diffDlib * rect.area() / 40000.0 * diffCheckSensitivity;
// if the face is moving so fast, use dlib to detect the face
double diff = calDistanceDiff(prevTrackPts, nextTrackPts);
if (drawDebugPoints)
{
Debug.Log("variance:" + diff);
}
if (diff > diffDlib)
{
for (int i = 0; i < numberOfElements; i++)
{
nextTrackPts [i].x = srcPoints [i].x;
nextTrackPts [i].y = srcPoints [i].y;
dstPoints [i] = srcPoints [i];
}
if (drawDebugPoints)
{
Debug.Log("DLIB");
for (int i = 0; i < numberOfElements; i++)
{
Imgproc.circle(img, new Point(srcPoints [i].x, srcPoints [i].y), 2, new Scalar(255, 0, 0, 255), -1);
}
}
}
else
{
// In this case, use Optical Flow
for (int i = 0; i < numberOfElements; i++)
{
dstPoints [i] = new Vector2((float)nextTrackPts [i].x, (float)nextTrackPts [i].y);
}
if (drawDebugPoints)
{
Debug.Log("Optical Flow");
for (int i = 0; i < numberOfElements; i++)
{
Imgproc.circle(img, nextTrackPts [i], 2, new Scalar(0, 0, 255, 255), -1);
}
}
}
}
Swap(ref prevTrackPts, ref nextTrackPts);
Swap(ref prevgray, ref gray);
}
return(dstPoints);
}
