private void draw_elevation(List <gpx_master.gpx_trkpt> pt_list)
{
if (pt_list == null)
{
return;
}
Bitmap bmp = new Bitmap(alt_pictureBox.Width, alt_pictureBox.Height);
Graphics g = Graphics.FromImage(bmp);
Pen pen = new Pen(Color.Black, 3.0f);
PointF[] point_list = new PointF[pt_list.Count];
int p_count = 0;
foreach (gpx_master.gpx_trkpt point in pt_list)
{
point_list[p_count] = new PointF(((float)p_count / point_list.Count()) * alt_pictureBox.Width, (float)(alt_pictureBox.Height - (((point.ele - track_statistic_infos.min_ele) / (track_statistic_infos.max_ele - track_statistic_infos.min_ele)) * alt_pictureBox.Height)));
p_count++;
}
g.Clear(Color.LightGray);
g.DrawLines(pen, point_list);
elevation_line = bmp;
alt_pictureBox.Image = bmp;
}
public SharpPath()
{
Beziers = new PointF[0][];
Lines = new PointF[2][];
Lines[0] = new PointF[] {
new PointF(86.10F, 447.45F), new PointF(86.10F, 442.75F), new PointF(88.10F, 442.20F),
new PointF(88.10F, 446.88F), new PointF(86.10F, 447.45F)
};
Lines[1] = new PointF[] {
new PointF(90.04F, 446.31F), new PointF(88.66F, 446.71F), new PointF(88.66F, 442.03F),
new PointF(90.04F, 441.64F), new PointF(90.04F, 439.70F), new PointF(88.66F, 440.08F),
new PointF(88.66F, 435.30F), new PointF(88.10F, 435.30F), new PointF(88.10F, 440.23F),
new PointF(86.10F, 440.80F), new PointF(86.10F, 436.15F), new PointF(85.57F, 436.15F),
new PointF(85.57F, 440.98F), new PointF(84.19F, 441.37F), new PointF(84.19F, 443.31F),
new PointF(85.57F, 442.93F), new PointF(85.57F, 447.60F), new PointF(84.19F, 447.98F),
new PointF(84.19F, 449.92F), new PointF(85.57F, 449.54F), new PointF(85.57F, 454.29F),
new PointF(86.10F, 454.29F), new PointF(86.10F, 449.37F), new PointF(88.10F, 448.82F),
new PointF(88.10F, 453.45F), new PointF(88.66F, 453.45F), new PointF(88.66F, 448.65F),
new PointF(90.04F, 448.26F), new PointF(90.04F, 446.31F)
};
for (int i = 0; i < Lines.Count(); i++)
{
for (int j = 0; j < Lines[i].Count(); j++)
{
Lines[i][j].X += -84.19F;
Lines[i][j].Y += -436.06F - 7F;
}
}
}
public MinimNotePath()
{
Beziers = new PointF[][] {
new PointF[] {
new PointF(183.32F, 537.99F),
new PointF(183.25F, 538.79F), new PointF(182.64F, 539.39F), new PointF(182.13F, 539.95F),
new PointF(180.97F, 541.06F), new PointF(179.56F, 541.94F), new PointF(178.01F, 542.32F),
new PointF(177.55F, 542.40F), new PointF(176.80F, 542.47F), new PointF(176.67F, 541.87F),
new PointF(176.54F, 541.23F), new PointF(177.02F, 540.67F), new PointF(177.38F, 540.20F),
new PointF(178.32F, 539.11F), new PointF(179.54F, 538.29F), new PointF(180.84F, 537.71F),
new PointF(181.52F, 537.46F), new PointF(182.30F, 537.13F), new PointF(183.01F, 537.44F),
new PointF(183.20F, 537.55F), new PointF(183.34F, 537.76F), new PointF(183.32F, 537.99F)
},
new PointF[] {
new PointF(181.39F, 536.34F),
new PointF(179.63F, 536.36F), new PointF(177.96F, 537.33F), new PointF(176.88F, 538.70F),
new PointF(176.15F, 539.64F), new PointF(175.67F, 541.00F), new PointF(176.26F, 542.13F),
new PointF(176.70F, 543.03F), new PointF(177.76F, 543.39F), new PointF(178.70F, 543.34F),
new PointF(180.33F, 543.30F), new PointF(181.83F, 542.40F), new PointF(182.91F, 541.21F),
new PointF(183.71F, 540.30F), new PointF(184.29F, 538.98F), new PointF(183.83F, 537.77F),
new PointF(183.50F, 536.89F), new PointF(182.58F, 536.37F), new PointF(181.68F, 536.35F),
new PointF(181.58F, 536.35F), new PointF(181.48F, 536.34F), new PointF(181.39F, 536.34F)
}
};
for (int i = 0; i < Beziers.Count(); i++)
{
for (int j = 0; j < Beziers[i].Count(); j++)
{
Beziers[i][j].X += -180.25F + 5F;
Beziers[i][j].Y += -538.5F;
}
}
Lines = new PointF[][] { new PointF[] {} };
}
public RestFlagPath()
{
Lines = new PointF[0][];
Beziers = new PointF[1][];
Beziers[0] = new PointF[] {
new PointF(192.60F, 579.98F),
new PointF(191.53F, 583.48F), new PointF(190.45F, 586.98F), new PointF(189.38F, 590.48F),
new PointF(189.17F, 590.48F), new PointF(188.97F, 590.48F), new PointF(188.76F, 590.49F),
new PointF(189.58F, 587.76F), new PointF(190.41F, 585.02F), new PointF(191.24F, 582.29F),
new PointF(190.29F, 583.17F), new PointF(188.92F, 583.40F), new PointF(187.67F, 583.30F),
new PointF(186.75F, 583.21F), new PointF(185.86F, 582.45F), new PointF(185.86F, 581.48F),
new PointF(185.76F, 580.59F), new PointF(186.52F, 579.69F), new PointF(187.43F, 579.73F),
new PointF(188.41F, 579.70F), new PointF(189.26F, 580.73F), new PointF(188.96F, 581.68F),
new PointF(188.92F, 582.14F), new PointF(188.48F, 582.37F), new PointF(188.28F, 582.68F),
new PointF(188.62F, 582.89F), new PointF(189.07F, 582.68F), new PointF(189.44F, 582.61F),
new PointF(190.40F, 582.31F), new PointF(191.32F, 581.66F), new PointF(191.72F, 580.71F),
new PointF(191.79F, 580.47F), new PointF(191.87F, 580.23F), new PointF(191.95F, 579.99F),
new PointF(192.17F, 579.99F), new PointF(192.38F, 579.98F), new PointF(192.60F, 579.98F)
};
for (int i = 0; i < Beziers.Count(); i++)
{
for (int j = 0; j < Beziers[i].Count(); j++)
{
Beziers[i][j].X += -185.85F + 5F;
Beziers[i][j].Y += -583.00F + 3F;
}
}
}
public CrotchetRestPath()
{
Beziers = new PointF[][] {
new PointF[] {
new PointF(181.69F, 588.76F),
new PointF(182.47F, 589.08F), new PointF(182.37F, 590.46F), new PointF(181.42F, 589.80F),
new PointF(180.70F, 588.91F), new PointF(179.33F, 588.14F), new PointF(178.34F, 589.11F),
new PointF(177.51F, 589.87F), new PointF(177.78F, 591.27F), new PointF(178.71F, 591.81F),
new PointF(179.37F, 591.94F), new PointF(179.86F, 593.05F), new PointF(178.79F, 592.64F),
new PointF(177.25F, 592.11F), new PointF(175.60F, 590.79F), new PointF(175.72F, 588.99F),
new PointF(175.70F, 587.77F), new PointF(176.95F, 587.02F), new PointF(178.07F, 587.10F),
new PointF(178.52F, 586.94F), new PointF(179.75F, 587.58F), new PointF(179.70F, 587.30F),
new PointF(178.94F, 586.14F), new PointF(178.08F, 585.05F), new PointF(177.41F, 583.84F),
new PointF(177.17F, 583.12F), new PointF(177.69F, 582.42F), new PointF(177.91F, 581.75F),
new PointF(178.39F, 580.55F), new PointF(178.93F, 579.37F), new PointF(179.37F, 578.16F),
new PointF(179.41F, 577.41F), new PointF(178.75F, 576.88F), new PointF(178.40F, 576.27F),
new PointF(178.01F, 575.64F), new PointF(177.49F, 575.07F), new PointF(177.20F, 574.38F),
new PointF(177.22F, 573.50F), new PointF(178.13F, 574.48F), new PointF(178.37F, 574.84F),
new PointF(179.45F, 576.11F), new PointF(180.57F, 577.34F), new PointF(181.61F, 578.65F),
new PointF(182.30F, 579.49F), new PointF(181.66F, 580.48F), new PointF(181.32F, 581.32F),
new PointF(180.82F, 582.58F), new PointF(180.19F, 583.79F), new PointF(179.82F, 585.09F),
new PointF(179.77F, 586.16F), new PointF(180.56F, 587.05F), new PointF(181.06F, 587.94F),
new PointF(181.25F, 588.22F), new PointF(181.47F, 588.50F), new PointF(181.69F, 588.76F)
}
};
for (int i = 0; i < Beziers.Count(); i++)
{
for (int j = 0; j < Beziers[i].Count(); j++)
{
Beziers[i][j].X += -178.72F + 5F;
Beziers[i][j].Y += -582.00F;
}
}
Lines = new PointF[][] { new PointF[] {} };
}
private void DrawPic(ref Graphics gr, Func <float, float, float> f, float lb, float ub, float a, float[] ar)
{
Array.Sort(ar);
List <float> arr = new List <float>();
//Построение списка из неповторяющихся значений входных данных.
arr.Add(ar[0]);
for (int i = 1; i < ar.Length; ++i)
{
if (ar[i] != ar[i - 1])
{
arr.Add(ar[i]);
}
}
dataGridView1.Rows.Clear();
//Нахождение пар (x, y(x)).
PointF[] pf = new PointF[arr.Count];
for (int i = 0; i < arr.Count; ++i)
{
float x = arr[i];
float y = f(a, x);
dataGridView1.Rows.Add(x, y);
if (y > 32767)
{
y = 32767;
}
else
if (y < -32767)
{
y = -32767;
}
pf[i] = new PointF(x, y);
}
minv = pf.Min((PointF x) => { return(x.Y); });
maxv = pf.Max((PointF x) => { return(x.Y); });
//Трансформация полотна для корректного отображения.
transform = new Matrix();
transform.Translate(pictureBox1.Width / 2, pictureBox1.Height / 2);
transform.Scale(pictureBox1.Width / (ub - lb), -pictureBox1.Height / (maxv - minv));
PointF[] lims = new[]
{
new PointF(pf.Min((PointF x) => { return(x.X); }), 0), //lx
new PointF(pf.Max((PointF x) => { return(x.X); }), 0), //rx
new PointF(0, pf.Min((PointF x) => { return(x.Y); })), //dy
new PointF(0, pf.Max((PointF x) => { return(x.Y); })) //uy
};
transform.Translate(-(lims[0].X + lims[1].X) / 2,
-(lims[2].Y + lims[3].Y) / 2);
transform.TransformPoints(pf);
//Отрисовка линий и кругов на полотне.
gr.DrawLines(Pens.Black, pf);
for (int i = 0; i < pf.Count(); ++i)
{
gr.DrawEllipse(Pens.Black, new RectangleF(pf[i].X - 2, pf[i].Y - 2, 4, 4));
}
}
/// <summary>
/// Plotting a vector into new window for 2D function with MindFusion.
/// </summary>
/// <param name="drawingVector">
/// The vector to be drawn into the graph.
/// The x axis is the size of the vecor.
/// The y axis is the vector.
/// </param>
public void MindFusionPlotFunction(Vector <double> drawingVector)
{
//the point array to insret to the point series object.
PointF[] point = new PointF[drawingVector.Count()];
//the list to put all the points to.
List <PointF> pointsList = new List <PointF>();
//the list to put the label for each point (can be 'x' , '.' , or "")
List <string> labelsList = new List <string>();
//put all the points and labels for each point to the list.
for (int i = 0; i < point.Count(); i++)
{
point[i] = new PointF(i, (float)drawingVector[i]);
pointsList.Add(point[i]);
labelsList.Add("");
}
//determine the poit series with both the lists.
PointSeries ps = new PointSeries(pointsList, labelsList);
//draw the line chart in new window.
LineChart lc = new LineChart();
lc.Series.Add(ps);
lc.Width = 400;
lc.Height = 400;
lc.ShowZoomWidgets = true;
lc.AllowZoom = true;
lc.XAxis.MinValue = 0;
lc.XAxis.MaxValue = point.Count();
//make a new form to put the graph on.
Form f = new Form();
f.Controls.Add(lc);
f.Show();
}
public FlatPath()
{
Beziers = new PointF[2][];
Beziers[0] = new PointF[] {
new PointF(95.51F, 443.84F),
new PointF(95.66F, 443.44F), new PointF(95.90F, 443.12F), new PointF(96.21F, 442.87F),
new PointF(96.51F, 442.63F), new PointF(96.83F, 442.50F), new PointF(97.14F, 442.50F),
new PointF(97.66F, 442.50F), new PointF(97.99F, 442.80F), new PointF(98.14F, 443.39F),
new PointF(98.15F, 443.44F), new PointF(98.16F, 443.52F), new PointF(98.16F, 443.65F),
new PointF(98.16F, 444.23F), new PointF(97.95F, 444.78F), new PointF(97.35F, 445.52F),
new PointF(96.73F, 446.30F), new PointF(96.20F, 446.75F), new PointF(95.51F, 447.28F)
};
Beziers[1] = new PointF[] {
new PointF(94.94F, 447.75F),
new PointF(94.94F, 448.10F), new PointF(95.04F, 448.28F), new PointF(95.23F, 448.28F),
new PointF(95.34F, 448.28F), new PointF(95.48F, 448.18F), new PointF(95.69F, 448.06F),
new PointF(96.27F, 447.71F), new PointF(96.63F, 447.48F), new PointF(97.03F, 447.23F),
new PointF(97.48F, 446.95F), new PointF(97.99F, 446.63F), new PointF(98.66F, 445.99F),
new PointF(99.12F, 445.52F), new PointF(99.45F, 445.05F), new PointF(99.66F, 444.58F),
new PointF(99.87F, 444.11F), new PointF(99.97F, 443.64F), new PointF(99.97F, 443.17F),
new PointF(99.97F, 442.49F), new PointF(99.79F, 442.00F), new PointF(99.42F, 441.71F),
new PointF(99.01F, 441.40F), new PointF(98.56F, 441.25F), new PointF(98.09F, 441.25F),
new PointF(97.66F, 441.25F), new PointF(97.22F, 441.37F), new PointF(96.77F, 441.61F),
new PointF(96.33F, 441.85F), new PointF(95.90F, 442.17F), new PointF(95.51F, 442.56F) //,
};
for (int i = 0; i < Beziers.Count(); i++)
{
for (int j = 0; j < Beziers[i].Count(); j++)
{
Beziers[i][j].X += -94.94F;
Beziers[i][j].Y += -433.75F - 9F;
}
}
Lines = new PointF[1][];
Lines[0] = new PointF[] {
new PointF(95.51F, 442.56F),
new PointF(95.51F, 435.29F),
new PointF(94.94F, 435.29F),
new PointF(94.94F, 447.75F)
};
for (int i = 0; i < Lines.Count(); i++)
{
for (int j = 0; j < Lines[i].Count(); j++)
{
Lines[i][j].X += -94.947F;
Lines[i][j].Y += -433.75F - 9F;
}
}
}
public void Draw(Graphics G)
{
Pen pen = new Pen(this.HairColor, 6);
Brush brush = new SolidBrush(this.EyesColor);
PointF[] formToDraw = new PointF[forme.Count()];
for (int i = 0; i < formToDraw.Count(); i++)
{
formToDraw[i].X = forme[i].X + this.Position.X;
formToDraw[i].Y = forme[i].Y + this.Position.Y;
}
G.DrawPolygon(pen, formToDraw);
G.FillPolygon(brush, formToDraw);
}
public NoteFlagPath()
{
Beziers = new PointF[][] {
new PointF[] {
new PointF(355.64087F, 534.50670F), new PointF(356.05925F, 536.43346F),
new PointF(357.79685F, 537.93980F), new PointF(359.09566F, 539.29772F),
new PointF(360.00232F, 540.26777F), new PointF(361.07070F, 541.12502F),
new PointF(361.68096F, 542.33020F), new PointF(362.22847F, 543.32497F),
new PointF(362.53043F, 544.45397F), new PointF(362.52561F, 545.59100F),
new PointF(362.55523F, 547.70041F), new PointF(361.59345F, 549.72003F),
new PointF(360.14610F, 551.22017F), new PointF(359.85055F, 551.58759F),
new PointF(359.04629F, 552.27401F), new PointF(358.90096F, 552.17196F),
new PointF(358.71377F, 552.04052F), new PointF(359.37760F, 551.43876F),
new PointF(359.71032F, 551.11874F), new PointF(361.17865F, 549.59402F),
new PointF(362.04044F, 547.42643F), new PointF(361.74674F, 545.30372F),
new PointF(361.36799F, 543.36784F), new PointF(359.78964F, 541.96920F),
new PointF(358.24068F, 540.89646F), new PointF(357.26129F, 540.22519F),
new PointF(356.21650F, 539.64737F), new PointF(355.11860F, 539.19400F)
}
};
for (int i = 0; i < Beziers.Count(); i++)
{
for (int j = 0; j < Beziers[i].Count(); j++)
{
Beziers[i][j].X += -355.1186F;
Beziers[i][j].Y += -534.5257F + 5F;
}
}
Lines = new PointF[][] {
new PointF[] {
new PointF(355.11860F, 533.90500F),
new PointF(355.61860F, 533.90500F)
}
};
for (int i = 0; i < Lines.Count(); i++)
{
for (int j = 0; j < Lines[i].Count(); j++)
{
Lines[i][j].X += -355.1186F;
Lines[i][j].Y += -534.5257F + 5F;
}
}
}
/// /////////////////////////////////////////////////////////////////////
//Method: public static GraphicsPath MakePolyPath(double radius, int vertexCount, double variance)
//Parameter: double radius- radius of the shape
// int vertex count- the number of vertex for the shape
// double variance- the change...
//Return: GraphicsPath- the graphic path connected by all the points
//Purpose: Generate a GraphicsPath from your collection of generated points and return i
/////////////////////////////////////////////////////////////////////////
public static GraphicsPath MakePolyPath(double radius, int vertexCount, double variance)
{
Random rnd = new Random();
GraphicsPath gp = new GraphicsPath();
PointF[] myList = new PointF[vertexCount];
//find all the point aof the vertex
for (int i = 0; i < vertexCount; i++)
{
myList[i] = (new PointF((float)((radius - rnd.NextDouble() * variance) * Math.Cos(2.0 * Math.PI * ((double)(i) / (double)(vertexCount)))),
(float)((radius - rnd.NextDouble() * variance) * Math.Sin(2.0 * Math.PI * ((double)(i) / (double)(vertexCount))))
));
}
gp.AddLine(myList[myList.Count() - 2], myList.Last());
gp.StartFigure();
gp.AddLines(myList);
gp.CloseFigure();
return(gp);
}
public void Draw(Graphics g)
{
PointF[] pointArray = new PointF[NbPoints];
int i = 0;
foreach (Polyline tmpPolyline in LPolyline)
{
foreach (POI tmpPOI in tmpPolyline.LPOI)
{
pointArray[i] = new PointF((float)tmpPOI.Lat, (float)tmpPOI.Longitude);
i++;
}
}
if (pointArray.Count <PointF>() < 1)
{
throw new Exception("Impossible de dessiner la polygon id : " + Id + " !");
}
else
{
g.FillPolygon(Brushes.Gray, pointArray, System.Drawing.Drawing2D.FillMode.Alternate);
}
}
public bool RefineMatchesWithHomography(VectorOfKeyPoint evalKeypoints, VectorOfKeyPoint trainKeypoints, VectorOfDMatch matches,
ref Mat homographyOut, VectorOfDMatch inliersOut, VectorOfInt inliersMaskOut,
float reprojectionThreshold, int minNumberMatchesAllowed)
{
if (matches.Size < minNumberMatchesAllowed)
{
return(false);
}
PointF[] srcPoints = new PointF[matches.Size];
PointF[] dstPoints = new PointF[matches.Size];
for (int i = 0; i < matches.Size; ++i)
{
srcPoints[i] = trainKeypoints[matches[i].TrainIdx].Point;
dstPoints[i] = evalKeypoints[matches[i].QueryIdx].Point;
}
inliersMaskOut.Clear();
inliersMaskOut = new VectorOfInt(srcPoints.Count());
//for(int i = 0; i < srcPoints.Count(); ++i)
//{
// inliersMaskOut = 0;
//}
CvInvoke.FindHomography(srcPoints, dstPoints, homographyOut, Emgu.CV.CvEnum.HomographyMethod.Ransac, reprojectionThreshold, inliersMaskOut);
for (int i = 0; i < inliersMaskOut.Size; ++i)
{
if (inliersMaskOut[i] > 0)
{
inliersOut.Push(new MDMatch[] { matches[i] });
}
}
return(inliersOut.Size >= minNumberMatchesAllowed);
}
protected override void OnPaint(PaintEventArgs e)
{
Graphics g = e.Graphics;
g.SmoothingMode = SmoothingMode.AntiAlias;
g.InterpolationMode = InterpolationMode.HighQualityBilinear;
g.TextRenderingHint = TextRenderingHint.ClearTypeGridFit;
// DRAW SPACEITEMS
foreach (KeyValuePair <string, SpaceItem> kvp in list)
{
SpaceItem s = kvp.Value;
float cX = (float)((this.Width / 2f) + (s.position.Item1 * scale) - (s.radius * scale) + (offsetX * scale));
float cY = (float)((this.Height / 2f) - (s.position.Item2 * scale) - (s.radius * scale) + (offsetY * scale));
float size = (float)(2 * (s.radius * scale));
g.FillEllipse(center, cX, cY, size, size);
if (s.type == SpaceItem.SpaceItemType.SPACECRAFT || s.type == SpaceItem.SpaceItemType.MOON)
{
List <Tuple <double, double, double> > future = OrbitHelper.IteratePosition(s, list, iterations);
for (int i = 0; i < future.Count - 1; i++)
{
g.DrawLine(orbitPen,
(float)((this.Width / 2f) + (future[i].Item1 * scale)),
(float)((this.Height / 2f) + (future[i].Item2 * -scale)),
(float)((this.Width / 2f) + (future[i + 1].Item1 * scale)),
(float)((this.Height / 2f) + (future[i + 1].Item2 * -scale))
);
}
List <Tuple <double, double, double> > points = s.orbitPoints.ToList();
PointF[] pointsXY = new PointF[points.Count];
for (int i = 0; i < points.Count; i++)
{
pointsXY[i] = new PointF(
(float)((this.Width / 2f) + (points[i].Item1 * scale) + (offsetX * scale)),
(float)((this.Height / 2f) + (points[i].Item2 * -scale) + (offsetY * scale))
);
}
if (pointsXY.Count() >= 2)
{
g.DrawLines(orbitPenG, pointsXY);
}
}
}
// DRAW EARTH
//g.FillEllipse(center, (float)((this.Width / 2f) - (earthRadius * scale)), (float)((this.Height / 2f) - (earthRadius * scale)), (float)(2 * (earthRadius * scale)), (float)(2 * (earthRadius * scale)));
// DRAW PAST POSITIONS
/**
* if(orbitPointsXY.Count > 1)
* {
* for(int i = 0; i < orbitPointsXY.Count - 1; i++)
* {
* g.DrawLine(orbitPen,
* (float)((this.Width / 2f) + (orbitPointsXY[i].Item1 * scale)),
* (float)((this.Height / 2f) + (orbitPointsXY[i].Item2 * -scale)),
* (float)((this.Width / 2f) + (orbitPointsXY[i+1].Item1 * scale)),
* (float)((this.Height / 2f) + (orbitPointsXY[i+1].Item2 * -scale))
* );
* }
* }**/
// DRAW SPACECRAFT-DOTS
foreach (KeyValuePair <string, SpaceItem> kvp in list)
{
SpaceItem s = kvp.Value;
if (s.type == SpaceItem.SpaceItemType.SPACECRAFT)
{
float size = 2f;
float cX = (float)((this.Width / 2f) + (s.position.Item1 * scale) - (size / 2f) + (offsetX * scale));
float cY = (float)((this.Height / 2f) - (s.position.Item2 * scale) - (size / 2f) + (offsetY * scale));
g.FillEllipse(craft, cX, cY, size, size);
}
}
}
public override void Render(IMapContext map)
{
double coeff = map.DiagonalCoefficient();
if (settings.Get <bool>("Ground"))
{
const int POINTS_COUNT = 64;
PointF[] hor = new PointF[POINTS_COUNT];
double step = 2 * map.ViewAngle / (POINTS_COUNT - 1);
SolidBrush brushGround = new SolidBrush(map.GetColor(colorGroundNight, colorGroundDay));
// Bottom part of ground shape
for (int i = 0; i < POINTS_COUNT; i++)
{
var h = new CrdsHorizontal(map.Center.Azimuth - map.ViewAngle + step * i, 0);
hor[i] = map.Project(h);
}
if (hor[0].X >= 0)
{
hor[0].X = -1;
}
if (hor[POINTS_COUNT - 1].X <= map.Width)
{
hor[POINTS_COUNT - 1].X = map.Width + 1;
}
if (hor.Any(h => !map.IsOutOfScreen(h)))
{
GraphicsPath gp = new GraphicsPath();
gp.AddCurve(hor);
gp.AddLines(new PointF[]
{
new PointF(map.Width + 1, map.Height + 1),
new PointF(-1, map.Height + 1)
});
map.Graphics.FillPath(brushGround, gp);
}
else if (map.Center.Altitude <= 0)
{
map.Graphics.FillRectangle(brushGround, 0, 0, map.Width, map.Height);
}
// Top part of ground shape
if (map.Center.Altitude > 0)
{
for (int i = 0; i < POINTS_COUNT; i++)
{
var h = new CrdsHorizontal(map.Center.Azimuth - map.ViewAngle - step * i, 0);
hor[i] = map.Project(h);
}
if (hor.Count(h => !map.IsOutOfScreen(h)) > 2)
{
GraphicsPath gp = new GraphicsPath();
gp.AddCurve(hor);
gp.AddLines(new PointF[]
{
new PointF(map.Width + 1, -1),
new PointF(-1, -1),
});
map.Graphics.FillPath(brushGround, gp);
}
}
}
if (map.Schema == ColorSchema.White || (!settings.Get <bool>("Ground") && settings.Get <bool>("HorizonLine")))
{
const int POINTS_COUNT = 64;
PointF[] hor = new PointF[POINTS_COUNT];
double step = 2 * map.ViewAngle / (POINTS_COUNT - 1);
for (int i = 0; i < POINTS_COUNT; i++)
{
var h = new CrdsHorizontal(map.Center.Azimuth - map.ViewAngle + step * i, 0);
hor[i] = map.Project(h);
}
if (hor[0].X >= 0)
{
hor[0].X = -1;
}
if (hor[POINTS_COUNT - 1].X <= map.Width)
{
hor[POINTS_COUNT - 1].X = map.Width + 1;
}
if (hor.Any(h => !map.IsOutOfScreen(h)))
{
Pen penHorizonLine = new Pen(map.GetColor("ColorHorizon"), 2);
map.Graphics.DrawCurve(penHorizonLine, hor);
}
}
if (settings.Get <bool>("LabelCardinalDirections"))
{
Brush brushCardinalLabels = new SolidBrush(map.GetColor("ColorCardinalDirections"));
StringFormat format = new StringFormat()
{
LineAlignment = StringAlignment.Center, Alignment = StringAlignment.Center
};
for (int i = 0; i < cardinalDirections.Length; i++)
{
var h = new CrdsHorizontal(i * 360 / cardinalDirections.Length, 0);
if (Angle.Separation(h, map.Center) < map.ViewAngle * coeff)
{
PointF p = map.Project(h);
p.Y += fontCardinalLabels[i % 2].Height;
using (var gp = new GraphicsPath())
{
map.Graphics.DrawString(Text.Get($"CardinalDirections.{cardinalDirections[i]}"), fontCardinalLabels[i % 2], brushCardinalLabels, p, format);
}
}
}
}
}
private void ErrCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Orange, 1);
PointF[] Err = new PointF[tempString.Count()];
int i = 0;
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
Err[i].X = float.Parse(sArray[0]);
Err[i].Y = float.Parse(sArray[7]);
i++;
}
pathMax = FindMaxAbs(Err).X;
float iii = FindMaxAbs(Err).Y;
zoom = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(Err).Y;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
for (int j = 0; j < Err.Count(); j++)
{
Err[j] = GetPanelAxes(Err[j].X, Err[j].Y);
}
for (int m = 0; m < 4; m++)
{
for (int j = 1; j < Err.Count() - 1; j++)
{
Err[j].Y = (Err[j - 1].Y + Err[j + 1].Y) / 2;
}
}
myGra.DrawLines(myPen, Err);
}
//绘制合速度图像
void ResultantVelCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Red, 1);
PointF[] velWheel1 = new PointF[tempString.Count()];
PointF[] velWheel2 = new PointF[tempString.Count()];
PointF[] velWheel3 = new PointF[tempString.Count()];
PointF[] targetVelWheel1 = new PointF[tempString.Count()];
PointF[] targetVelWheel2 = new PointF[tempString.Count()];
PointF[] targetVelWheel3 = new PointF[tempString.Count()];
float[] zAngle = new float[tempString.Count()];
int i = 0;
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i].X = velWheel2[i].X = velWheel3[i].X
= targetVelWheel1[i].X = targetVelWheel2[i].X = targetVelWheel3[i].X
= float.Parse(sArray[0]);
velWheel1[i].Y = float.Parse(sArray[1]);
velWheel2[i].Y = float.Parse(sArray[2]);
velWheel3[i].Y = float.Parse(sArray[3]);
targetVelWheel1[i].Y = float.Parse(sArray[4]);
targetVelWheel2[i].Y = float.Parse(sArray[5]);
targetVelWheel3[i].Y = float.Parse(sArray[6]);
i++;
}
PointF[] trueVel = new PointF[tempString.Count()];
PointF[] targetVel = new PointF[tempString.Count()];
PointF[] trueDir = new PointF[tempString.Count()];
PointF[] targetDir = new PointF[tempString.Count()];
PointF[] trueRotateVel = new PointF[tempString.Count()];
PointF[] targetRotateVel = new PointF[tempString.Count()];
PointF[] DirectionErr = new PointF[tempString.Count()];
PointF[] binaryzation = new PointF[tempString.Count()];
//由三个轮子的速度计算合成速度
for (int j = 0; j < targetVelWheel1.Count(); j++)
{
Calculate.TriWheelVel2_t tempVel;
tempVel = Calculate.TriWheelVel2ResultantVel(velWheel1[j].Y, velWheel2[j].Y, velWheel3[j].Y, zAngle[j]);
trueVel[j].Y = tempVel.speed;
trueVel[j].X = velWheel1[j].X;
trueDir[j].Y = tempVel.direction;
trueDir[j].X = velWheel1[j].X;
trueRotateVel[j].Y = tempVel.rotationVel;
trueRotateVel[j].X = velWheel1[j].X;
tempVel = Calculate.TriWheelVel2ResultantVel(targetVelWheel1[j].Y, targetVelWheel2[j].Y, targetVelWheel3[j].Y, zAngle[j]);
targetVel[j].Y = tempVel.speed;
targetVel[j].X = targetVelWheel1[j].X;
targetDir[j].Y = tempVel.direction;
targetDir[j].X = targetVelWheel1[j].X;
targetRotateVel[j].Y = tempVel.rotationVel;
targetRotateVel[j].X = targetVelWheel1[j].X;
}
pathMax = FindMaxAbs(targetDir).X;
zoom = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(targetDir).Y;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
for (int j = 0; j < targetVelWheel1.Count(); j++)
{
DirectionErr[j].X = targetDir[j].X;
DirectionErr[j].Y = Math.Abs(targetDir[j].Y - trueDir[j].Y);
if (DirectionErr[j].Y > 180)
{
DirectionErr[j].Y = 360 - DirectionErr[j].Y;
}
else if (DirectionErr[j].Y < -180)
{
DirectionErr[j].Y = 360 + DirectionErr[j].Y;
}
}
//进行滤波处理
for (int m = 0; m < 4; m++)
{
for (int j = 1; j < DirectionErr.Count() - 1; j++)
{
DirectionErr[j].Y = (DirectionErr[j - 1].Y + DirectionErr[j + 1].Y) / 2;
}
}
//进行阈值判断，二值化
for (int j = 0; j < DirectionErr.Count(); j++)
{
if (DirectionErr[j].Y < 8)
{
binaryzation[j].Y = 0;
binaryzation[j].X = DirectionErr[j].X;
}
else
{
binaryzation[j].Y = DirectionErr[j].Y;
binaryzation[j].X = DirectionErr[j].X;
}
}
//将二值化的文件存入txt文档中
StreamWriter SpeedFile = new StreamWriter(SpeedReplanInfo, false);
for (int m = 0; m < binaryzation.Count(); m++)
{
SpeedFile.WriteLine("{0:f0},{1:f0}", binaryzation[m].X, binaryzation[m].Y);
}
SpeedFile.Close();
//进行阈值判断，二值化
for (int j = 0; j < DirectionErr.Count(); j++)
{
binaryzation[j] = GetPanelAxes(binaryzation[j].X, binaryzation[j].Y);
}
myPen.Color = Color.Black;
myGra.DrawLines(myPen, binaryzation);
}
public override void Render(IMapContext map)
{
if (settings.Get <bool>("Ground"))
{
const int POINTS_COUNT = 64;
PointF[] hor = new PointF[POINTS_COUNT];
double step = 2 * map.ViewAngle / (POINTS_COUNT - 1);
SolidBrush brushGround = new SolidBrush(map.GetColor(colorGroundNight, colorGroundDay));
// Bottom part of ground shape
for (int i = 0; i < POINTS_COUNT; i++)
{
var h = new CrdsHorizontal(map.Center.Azimuth - map.ViewAngle + step * i, 0);
hor[i] = map.Project(h);
}
if (hor.Any(h => !map.IsOutOfScreen(h)))
{
GraphicsPath gp = new GraphicsPath();
gp.AddCurve(hor);
var pts = map.IsInverted ?
new PointF[]
{
new PointF(map.Width + 1, -1),
new PointF(-1, -1),
} : new PointF[]
{
new PointF(map.Width + 1, map.Height + 1),
new PointF(-1, map.Height + 1)
};
if (hor.Last().X > map.Width / 2)
{
gp.AddLines(pts);
}
else
{
gp.AddLines(pts.Reverse().ToArray());
}
map.Graphics.FillPath(brushGround, gp);
}
else if (map.Center.Altitude <= 0)
{
map.Graphics.FillRectangle(brushGround, 0, 0, map.Width, map.Height);
}
// Top part of ground shape
if (map.Center.Altitude > 0)
{
for (int i = 0; i < POINTS_COUNT; i++)
{
var h = new CrdsHorizontal(map.Center.Azimuth - map.ViewAngle - step * i, 0);
hor[i] = map.Project(h);
}
if (hor.Count(h => !map.IsOutOfScreen(h)) > 2)
{
GraphicsPath gp = new GraphicsPath();
gp.AddCurve(hor);
gp.AddLines(new PointF[]
{
new PointF(map.Width + 1, -1),
new PointF(-1, -1),
});
map.Graphics.FillPath(brushGround, gp);
}
}
}
if (map.Schema == ColorSchema.White || (!settings.Get <bool>("Ground") && settings.Get <bool>("HorizonLine")))
{
const int POINTS_COUNT = 64;
PointF[] hor = new PointF[POINTS_COUNT];
double step = 2 * map.ViewAngle / (POINTS_COUNT - 1);
for (int i = 0; i < POINTS_COUNT; i++)
{
var h = new CrdsHorizontal(map.Center.Azimuth - map.ViewAngle + step * i, 0);
hor[i] = map.Project(h);
}
if (hor.Any(h => !map.IsOutOfScreen(h)))
{
Pen penHorizonLine = new Pen(map.GetColor("ColorHorizon"), 2);
map.Graphics.DrawCurve(penHorizonLine, hor);
}
}
if (settings.Get <bool>("LabelCardinalDirections"))
{
Brush brushCardinalLabels = new SolidBrush(map.GetColor("ColorCardinalDirections"));
StringFormat format = new StringFormat()
{
LineAlignment = StringAlignment.Near, Alignment = StringAlignment.Center
};
for (int i = 0; i < cardinalDirections.Length; i++)
{
var h = new CrdsHorizontal(i * 360 / cardinalDirections.Length, 0);
if (Angle.Separation(h, map.Center) < map.ViewAngle)
{
PointF p = map.Project(h);
var fontBase = settings.Get <Font>("CardinalDirectionsFont");
var font = new Font(fontBase.FontFamily, fontBase.Size * (i % 2 == 0 ? 1 : 0.75f), fontBase.Style);
using (var gp = new GraphicsPath())
{
map.Graphics.DrawString(Text.Get($"CardinalDirections.{cardinalDirections[i]}"), font, brushCardinalLabels, p, format);
}
}
}
}
}
//绘画三个轮速度图像
private void WheelVelCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Red, 1);
PointF[] velWheel1 = new PointF[tempString.Count()];
PointF[] velWheel2 = new PointF[tempString.Count()];
PointF[] velWheel3 = new PointF[tempString.Count()];
PointF[] targetVelWheel1 = new PointF[tempString.Count()];
PointF[] targetVelWheel2 = new PointF[tempString.Count()];
PointF[] targetVelWheel3 = new PointF[tempString.Count()];
int i = 0;
//设置zoom值
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i].X = velWheel2[i].X = velWheel3[i].X = float.Parse(sArray[0]);
velWheel1[i].Y = float.Parse(sArray[4]);
velWheel2[i].Y = float.Parse(sArray[5]);
velWheel3[i].Y = float.Parse(sArray[6]);
i++;
}
//计算速度最大值，用于调整画面比例
float[] vel = new float[6];
vel[0] = FindMaxAbs(velWheel1).Y;
vel[1] = FindMaxAbs(velWheel2).Y;
vel[2] = FindMaxAbs(velWheel3).Y;
zoom = (float)(VelDisplay.Bottom / 2) / vel.Max();
pathMax = FindMaxAbs(velWheel1).X;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
i = 0;
//读取文件
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[1]));
velWheel2[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[2]));
velWheel3[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[3]));
targetVelWheel1[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[4]));
targetVelWheel2[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[5]));
targetVelWheel3[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[6]));
i++;
}
//进行滤波处理
for (int n = 0; n < 5; n++)
{
for (int j = 1; j < targetVelWheel1.Count() - 1; j++)
{
velWheel1[j].Y = (velWheel1[j - 1].Y + velWheel1[j + 1].Y) / 2;
velWheel2[j].Y = (velWheel2[j - 1].Y + velWheel2[j + 1].Y) / 2;
velWheel3[j].Y = (velWheel3[j - 1].Y + velWheel3[j + 1].Y) / 2;
targetVelWheel1[j].Y = (targetVelWheel1[j - 1].Y + targetVelWheel1[j + 1].Y) / 2;
targetVelWheel2[j].Y = (targetVelWheel2[j - 1].Y + targetVelWheel2[j + 1].Y) / 2;
targetVelWheel3[j].Y = (targetVelWheel3[j - 1].Y + targetVelWheel3[j + 1].Y) / 2;
}
}
switch (VelDisplayBox.Text)
{
case "1号轮速度":
myPen.Color = Color.Red;
myGra.DrawLines(myPen, velWheel1);
myPen.Color = Color.Blue;
myGra.DrawLines(myPen, targetVelWheel1);
break;
case "2号轮速度":
myPen.Color = Color.Red;
myGra.DrawLines(myPen, velWheel2);
myPen.Color = Color.Blue;
myGra.DrawLines(myPen, targetVelWheel2);
break;
case "3号轮速度":
myPen.Color = Color.Red;
myGra.DrawLines(myPen, velWheel3);
myPen.Color = Color.Blue;
myGra.DrawLines(myPen, targetVelWheel3);
break;
}
}
private void Calibrate(IEnumerable <Image <Gray, byte> > images, int nx, int ny, int useUncalibrated, float _squareSize /* Chessboard square size in cm */)
{
int displayCorners = 1;
int showUndistorted = 1;
bool isVerticalStereo = false;//OpenCV can handle left-right
//or up-down camera arrangements
const int maxScale = 1;
//FILE* f = fopen(imageList, "rt");
int i, j, lr, nframes, n = nx * ny, N = 0;
List <MCvPoint3D32f> objectPoints;
var points = new List <MCvPoint2D64f> [2];
List <int> npoints;
List <char>[] active = new List <char> [2];
var temp = new PointF[n];
var imageSize = new System.Drawing.Size(0, 0);
// ARRAY AND VECTOR STORAGE:
Matrix <double> _M1 = new Matrix <double>(3, 3);
Matrix <double> _M2 = new Matrix <double>(3, 3);
Matrix <double> _D1 = new Matrix <double>(1, 5);
Matrix <double> _D2 = new Matrix <double>(1, 5);
Matrix <double> _R = new Matrix <double>(3, 3);
Matrix <double> _T = new Matrix <double>(3, 1);
Matrix <double> _E = new Matrix <double>(3, 3);
Matrix <double> _F = new Matrix <double>(3, 3);
Matrix <double> _Q = new Matrix <double>(4, 4);
double [,] M1 = _M1.Data;
double [,] M2 = _M2.Data;
double [,] D1 = _D1.Data;
double [,] D2 = _D2.Data;
double [,] R = _R.Data;
double [,] T = _T.Data;
double [,] E = _E.Data;
double [,] F = _F.Data;
double [,] Q = _Q.Data;
for (i = 0; i < images.Count(); i++)
{
int count = 0, result = 0;
lr = i % 2;
List <MCvPoint2D64f> pts = points[lr];
var img = images.ElementAt(i);
imageSize = img.Size;
//FIND CHESSBOARDS AND CORNERS THEREIN:
for (int s = 1; s <= maxScale; s++)
{
var timg = img;
if (s > 1)
{
timg = new Image <Gray, byte>(img.Width * s, img.Height * s);
img.Resize(timg.Width, timg.Height, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC);
}
temp = Emgu.CV.CameraCalibration.FindChessboardCorners(timg, new Size(nx, ny),
Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH | Emgu.CV.CvEnum.CALIB_CB_TYPE.NORMALIZE_IMAGE);
Emgu.CV.CameraCalibration.
result = temp.Count();
if (timg != img)
{
timg.Dispose();
}
if (result == 0 || s == maxScale)
{
for (j = 0; j < count; j++)
{
temp[j].X /= s;
temp[j].Y /= s;
}
}
if (result)
{
break;
}
}
if (displayCorners)
{
printf("%s\n", buf);
IplImage *cimg = cvCreateImage(imageSize, 8, 3);
cvCvtColor(img, cimg, CV_GRAY2BGR);
cvDrawChessboardCorners(cimg, cvSize(nx, ny), &temp[0],
count, result);
cvShowImage("corners", cimg);
cvReleaseImage(&cimg);
if (cvWaitKey(0) == 27) //Allow ESC to quit
{
exit(-1);
}
}
else
{
putchar('.');
}
N = pts.size();
pts.resize(N + n, cvPoint2D32f(0, 0));
active[lr].push_back((uchar)result);
//assert( result != 0 );
if (result)
{
//Calibration will suffer without subpixel interpolation
cvFindCornerSubPix(img, &temp[0], count,
cvSize(11, 11), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS,
30, 0.01));
copy(temp.begin(), temp.end(), pts.begin() + N);
}
cvReleaseImage(&img);
}
fclose(f);
printf("\n");
// HARVEST CHESSBOARD 3D OBJECT POINT LIST:
nframes = active[0].size();//Number of good chessboads found
objectPoints.resize(nframes * n);
for (i = 0; i < ny; i++)
{
for (j = 0; j < nx; j++)
{
objectPoints[i * nx + j] = cvPoint3D32f(i * squareSize, j * squareSize, 0);
}
}
for (i = 1; i < nframes; i++)
{
copy(objectPoints.begin(), objectPoints.begin() + n,
objectPoints.begin() + i * n);
}
npoints.resize(nframes, n);
N = nframes * n;
CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0]);
CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]);
CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]);
CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0]);
cvSetIdentity(&_M1);
cvSetIdentity(&_M2);
cvZero(&_D1);
cvZero(&_D2);
// CALIBRATE THE STEREO CAMERAS
printf("Running stereo calibration ...");
fflush(stdout);
cvStereoCalibrate(&_objectPoints, &_imagePoints1,
&_imagePoints2, &_npoints,
&_M1, &_D1, &_M2, &_D2,
imageSize, &_R, &_T, &_E, &_F,
cvTermCriteria(CV_TERMCRIT_ITER +
CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO +
CV_CALIB_ZERO_TANGENT_DIST +
CV_CALIB_SAME_FOCAL_LENGTH);
printf(" done\n");
// CALIBRATION QUALITY CHECK
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
vector <CvPoint3D32f> lines[2];
points[0].resize(N);
points[1].resize(N);
_imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]);
_imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]);
lines[0].resize(N);
lines[1].resize(N);
CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);
//Always work in undistorted space
cvUndistortPoints(&_imagePoints1, &_imagePoints1,
&_M1, &_D1, 0, &_M1);
cvUndistortPoints(&_imagePoints2, &_imagePoints2,
&_M2, &_D2, 0, &_M2);
cvComputeCorrespondEpilines(&_imagePoints1, 1, &_F, &_L1);
cvComputeCorrespondEpilines(&_imagePoints2, 2, &_F, &_L2);
double avgErr = 0;
for (i = 0; i < N; i++)
{
double err = fabs(points[0][i].x * lines[1][i].x +
points[0][i].y * lines[1][i].y + lines[1][i].z)
+ fabs(points[1][i].x * lines[0][i].x +
points[1][i].y * lines[0][i].y + lines[0][i].z);
avgErr += err;
}
printf("avg err = %g\n", avgErr / (nframes * n));
//COMPUTE AND DISPLAY RECTIFICATION
if (showUndistorted)
{
CvMat *mx1 = cvCreateMat(imageSize.height,
imageSize.width, CV_32F);
CvMat *my1 = cvCreateMat(imageSize.height,
imageSize.width, CV_32F);
CvMat *mx2 = cvCreateMat(imageSize.height,
imageSize.width, CV_32F);
CvMat *my2 = cvCreateMat(imageSize.height,
imageSize.width, CV_32F);
CvMat *img1r = cvCreateMat(imageSize.height,
imageSize.width, CV_8U);
CvMat *img2r = cvCreateMat(imageSize.height,
imageSize.width, CV_8U);
CvMat *disp = cvCreateMat(imageSize.height,
imageSize.width, CV_16S);
CvMat *vdisp = cvCreateMat(imageSize.height,
imageSize.width, CV_8U);
CvMat *pair;
double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
public int Track(Bitmap bm)
{
if (!Active)
{
return(0);
}
Rectangle rect = new Rectangle(0, 0, bm.Width, bm.Height);
BitmapData bmpData = bm.LockBits(rect, ImageLockMode.ReadWrite, bm.PixelFormat);
Mat m = new Mat(bm.Height, bm.Width, Emgu.CV.CvEnum.DepthType.Cv8U, 3, bmpData.Scan0, bmpData.Stride);
PointF[] nextPts = new PointF[prevPoints.Count()];
byte[] status = new byte[prevPoints.Count()];
float[] err = new float[prevPoints.Count()];
CvInvoke.CalcOpticalFlowPyrLK(PrevImg, m, prevPoints, pyrWinSize, pyrLevel, new MCvTermCriteria(maxIter, eps), out nextPts, out status, out err);
// foreach (var kp in keyPoints)
//{
// CvInvoke.Circle(m, new Point((int)kp.Point.X+searchROI.Left, (int)kp.Point.Y+searchROI.Top), 5, new MCvScalar(100, 100, 255));
//}
PrevImg = m.Clone();
CvInvoke.Rectangle(m, new Rectangle(searchLocation, searchSize), new MCvScalar(100, 100, 255, 233));
prevPoints = prevPoints.Where((p, idx) => status[idx] == 1).ToArray();
nextPts = nextPts.Where((p, idx) => status[idx] == 1).ToArray();
double meanX = nextPts.Sum(p => p.X) / nextPts.Count();
double meanY = nextPts.Sum(p => p.Y) / nextPts.Count();
for (int i = 0; i < nextPts.Count(); i++)
//foreach (var np in nextPts)
{
PointF pp = prevPoints[i];
PointF np = nextPts[i];
if (olflags.HasFlag(OLFlags.Points))
{
CvInvoke.Circle(m, new Point((int)np.X, (int)np.Y), 5, new MCvScalar(100, 100, 255));
CvInvoke.Line(m, new Point((int)pp.X, (int)pp.Y), new Point((int)np.X, (int)np.Y), new MCvScalar(100, 255, 100), 1);
}
double dist = Math.Sqrt(Math.Pow(np.X - pp.X, 2) + Math.Pow(np.Y - pp.Y, 2));
if (olflags.HasFlag(OLFlags.Text))
{
CvInvoke.PutText(m, i.ToString(), new Point((int)np.X, (int)np.Y), Emgu.CV.CvEnum.FontFace.HersheyComplexSmall, 1, new MCvScalar(100, 100, 255), 1);
string text = i.ToString() + ": " + dist.ToString("0.00");// + ", " + err[i].ToString("#.##");
CvInvoke.PutText(m, text, new Point(0, i * 15), Emgu.CV.CvEnum.FontFace.HersheyComplexSmall, 1, new MCvScalar(100, 100, 255));
}
}
if (olflags.HasFlag(OLFlags.Center))
{
CvInvoke.Circle(m, new Point((int)meanX, (int)meanY), 7, new MCvScalar(255, 255, 100));
}
bm.UnlockBits(bmpData);
prevPoints = nextPts;
return(0);
}