private TXYW DrawToOutput(IEnumerable <OPoint> contour, Parameters para)
{
bool detectAngle = para.DetectAngle && (para.Shape != Parameters.TargetShape.Circle);
rawmat.CopyTo(output3);
Moments m = Cv2.Moments(contour);
TXYW txyw = new TXYW();
output3.DrawContours(new IEnumerable <OPoint>[] { contour }, 0, Scalar.Red, 5);
double comx = m.M10 / m.M00;
double comy = m.M01 / m.M00;
Cv2.Circle(output3, (int)comx, (int)comy, 10, Scalar.Red, -1);
txyw.t = PosFrames;
txyw.x = comx;
txyw.y = comy;
if (detectAngle)
{
double angle = 0.5 * Math.Atan2(2 * m.Mu11, m.Mu20 - m.Mu02);
const double r = 50;
OPoint arrow = new OPoint(comx + r * Math.Cos(angle), comy + r * Math.Sin(angle));
Cv2.Line(output3, new OPoint(comx, comy), arrow, Scalar.Red, 5);
txyw.w = angle;
}
Cv2.PutText(output3, $"frame:{txyw.t}",
new OPoint(20, videoCapture.FrameHeight - 20),
HersheyFonts.HersheySimplex, 2, Scalar.Red, 3);
return(txyw);
}
public void AddRawData(TXYW txyw)
{
if (RawData.Count != 0)
{
double preW = RawData.Last().w;
while (txyw.w - preW > Math.PI)
{
txyw.w -= Math.PI;
}
while (txyw.w - preW < -Math.PI)
{
txyw.w += Math.PI;
}
}
RawData.Add(txyw);
}
public MotionData AnalizeAll(Parameters para)
{
string targetDir = Settings.TargetDir(para.VideoFile);
MotionData md = new MotionData();
Action <object, DoWorkEventArgs> ProgressDialogDoWork = (sender, e) => {
using (VideoWriter writer = new VideoWriter(Settings.DetectVideoname(para.VideoFile),
FourCC.MPG4, 30, output3.Size())) {
var bw = sender as BackgroundWorker;
int num = para.EndFrame - para.StartFrame + 1;
for (int t = para.StartFrame; t < para.EndFrame; t++)
{
PosFrames = t;
var contour = GetContour(para);
if (contour != null)
{
TXYW txyw = DrawToOutput(contour, para);
md.AddRawData(txyw);
}
writer.Write(output3);
if (bw.CancellationPending)
{
e.Cancel = true;
break;
}
string message = string.Format("{0}/{1}", t - para.StartFrame, num);
bw.ReportProgress((int)((t - para.StartFrame) * 100 / num), message);
}
if (!e.Cancel)
{
md.UpdatePlotData();
}
writer.Release();
}
};
ProgressDialog pd = new ProgressDialog($"解析中:{para.VideoFile}", new DoWorkEventHandler(ProgressDialogDoWork));
DialogResult result = pd.ShowDialog();
return(result == DialogResult.OK ? md : null);
}
public static MotionData TryLoadRawData(string videoFile, Parameters parameters)
{
string file = Settings.RawDataname(videoFile);
if (File.Exists(file))
{
MotionData md = new MotionData();
md.parameters = parameters;
using (var sr = new StreamReader(file)) {
sr.ReadLine();
while (!sr.EndOfStream)
{
md.AddRawData(TXYW.FromStr(sr.ReadLine()));
}
}
return(md);
}
else
{
return(null);
}
}
public void UpdatePlotData()
{
double spf = 1d / parameters.FPS;
double width = parameters.LSWindow * spf;
double theta = parameters.XaxisAngle / 180d * Math.PI;
double rularDia = parameters.RularDia() ?? 1;
double cos = Math.Cos(theta);
double sin = Math.Sin(theta);
int revY = parameters.ReverseYaxis ? 1 : -1;
int N = RawData.Count;
for (int c = 0; c < PlotData.Length; c++)
{
if (PlotData[c].Length != N)
{
PlotData[c] = new double[N];
}
}
//offset and rotate
TXYW ini = RawData[0];
for (int i = 0; i < RawData.Count; i++)
{
double x0 = RawData[i].x - ini.x;
double y0 = RawData[i].y - ini.y;
PlotData[0][i] = (RawData[i].t - ini.t) * spf;
PlotData[1][i] = (x0 * cos + y0 * sin) * rularDia;
PlotData[2][i] = (-x0 * sin + y0 * cos) * revY * rularDia;
PlotData[3][i] = (RawData[i].w - ini.w - theta) * revY;
}
//least square
for (int i = 0; i < N; i++)
{
double ti = PlotData[0][i];
for (int c = 1; c <= 3; c++)
{
double t0 = 0, t1 = 0, t2 = 0, t3 = 0, t4 = 0;
double x0 = 0, x1 = 0, x2 = 0;
for (int j = 0; j < N; j++)
{
double t = PlotData[0][j];
double x = PlotData[c][j];
double w = Math.Exp(-0.5 * Tw((ti - t) / width));
double tt = t * t;
t0 += w;
t1 += w * t;
t2 += w * tt;
t3 += w * tt * t;
t4 += w * tt * tt;
x0 += w * x;
x1 += w * x * t;
x2 += w * x * tt;
}
double D = (t4 * t0 - t2 * t2) * (t2 * t0 - t1 * t1) - Tw(t3 * t0 - t2 * t1);
double A = ((x2 * t0 - x0 * t2) * (t2 * t0 - t1 * t1) - (x1 * t0 - x0 * t1) * (t3 * t0 - t2 * t1)) / D;
double B = ((x1 * t0 - x0 * t1) * (t4 * t0 - t2 * t2) - (x2 * t0 - x0 * t2) * (t3 * t0 - t2 * t1)) / D;
PlotData[c + 3][i] = B + 2 * A * ti;
PlotData[c + 6][i] = 2 * A;
}
}
//minmax
for (int c = 0; c < PlotData.Length; c++)
{
Max[c] = PlotData[c].Max();
Min[c] = PlotData[c].Min();
}
SavePlotData(parameters.VideoFile);
}
public OpenCvSharp.Point GetOrigin()
{
TXYW ini = RawData[0];
return(new OpenCvSharp.Point(ini.x, ini.y));
}
