| Wheelchair Control Diploma Thesis, Author: Ondrej Vozda, FEEC BUT, 2013

- Main Page
- Packages
- Classes
- Files

- File List

All Classes Namespaces Functions Variables Enumerations Enumerator Properties Events Pages
FormMainCV.cs
1 ﻿using System;
2 using System.Collections.Generic;
3 using System.Drawing;
4 using System.Threading;
5 using System.Windows.Forms;
6
7 //EMGU CV library usings
8 using Emgu.CV;
9 using Emgu.CV.Structure;
10
11 //My namespaces
12 using MyImageProcessingClasses;
13 using World;
14
15 namespace WheelchairControl
16 {
21 partial class FormMain
22 {
23 #region Private variables
24
28 private Capture capture = new Capture();
29
33 private FPS_Counter fpsCounter = new FPS_Counter();
34
38 private CameraCalibrator cameraCalibrator = new CameraCalibrator();
39
43 private MarksDetector rightMarkDetector = new MarksDetector();
44
48 private MarksDetector leftMarkDetector = new MarksDetector();
49
53 private FeatureDetector featureDetectorLeft;
54
58 private FeatureDetector featureDetectorRight;
59
63 private WorldMap worldMap = new WorldMap();
64
69 private HomographyMatrix homMatWorldMapToWorld = new HomographyMatrix();
70
75 private HomographyMatrix homMatWorldToPb;
76
80 private Image<Bgr, Byte> lastCaptured;
81
85 private Image<Gray, Byte> imageThresholdedLeft;
86
90 private Image<Gray, Byte> imageThresholdedRight;
91
95 private Hsv hsvSelectionLeft;
96
100 private Hsv hsvSelectionRight;
101
105 private Point leftDetectedMark;
106
110 private Point rightDetectedMark;
111
115 private Object marksLock = new Object();
116
120 private WheelchairModel wheelchairModel = new WheelchairModel();
121
122 //Others
123 private Bitmap frameBitmap, frameWorldMap; //Camera frame and world map frame bitmaps
124 private Graphics graphFrame, graphWorld; //Graphics connected to frameBitmap and frameWorldMap
125 private float scalePrev = -1f; //Previous world map scale
126
127 #endregion
128
129 #region Miscelaneous methods
130
138 private Boolean RecalculateRefPointToMarksCenter(ref Double X, ref Double Y, out Double Phi)
139 {
140 PointF leftMark, rightMark, markCenter;
141 Double phiTranform;
142
143 Phi = 0; //Init out parameter
144
145 if(worldMap.Marks.Count != (Byte)WorldMap.MarkPosition.MARKS_COUNT)
146 return false;
147
148 try
149 {
150 //Get marks
151 leftMark = new PointF(worldMap.Marks[(Byte)WorldMap.MarkPosition.LEFT].Coordinates.X, worldMap.Marks[(Byte)WorldMap.MarkPosition.LEFT].Coordinates.Y);
152 rightMark = new PointF((float)worldMap.Marks[(Byte)WorldMap.MarkPosition.RIGHT].Coordinates.X, (float)worldMap.Marks[(Byte)WorldMap.MarkPosition.RIGHT].Coordinates.Y);
153
154 //Get marks' center
155 markCenter = new PointF((float)((rightMark.X + leftMark.X) / 2.0), (float)((rightMark.Y + leftMark.Y) / 2.0));
156
157 /*Phi (wheelchair coordinate) is calculated with 90° offset -> Y axis is oriented in the direction of car, and X axis runs through the marks to the right,
158 so when 0° angle is measured between marks,
159 wheelchair's angle in the reference frame is in fact 90° etc.*/
160 Phi = ((1000 * Math.PI/2.0) - (1000 * Math.Atan2(rightMark.Y - leftMark.Y, rightMark.X - leftMark.X)));
161
162 //Angle used for tranformation (90° - phi)
163 phiTranform = (Math.PI / 2.0) - (Phi / 1000.0);
164
165 #region Transformation
166 {
167 //Transform coordinates (rotation + translation)
168
169 //Precalculate trigonotmetric functions' values
170 Double tempCos = Math.Cos(phiTranform);
171 Double tempSin = Math.Sin(phiTranform);
172
173 //Create data arrays
174 Double[,] matrixData = new Double[3, 3] {
175 {tempCos, tempSin, (-tempCos*markCenter.X) + (-tempSin*markCenter.Y)},
176 {-tempSin, tempCos, (tempSin*markCenter.X) + (-tempCos*markCenter.Y)},
177 {0.0, 0.0, 1}
178 };
179
180 Double[,] coordinatesData = new Double[3, 1] {
181 { X },
182 { Y },
183 { 1.0 }
184 };
185
186 //Crate matrices
187 Matrix<Double> tranformationMatrix = new Matrix<Double>(matrixData);
188 //CvInvoke.cvInvert(tranformationMatrix.Ptr, tranformationMatrix.Ptr, Emgu.CV.CvEnum.SOLVE_METHOD.CV_SVD);
189 Matrix<Double> sourcePointVector = new Matrix<Double>(coordinatesData);
190
191 //Transform
192 Matrix<Double> matrixResult = tranformationMatrix.Mul(sourcePointVector);
193
194 //Assign value
195 X = matrixResult[0, 0];
196 Y = matrixResult[1, 0];
197 }
198 #endregion
199
200 return true;
201 }
202 catch (Exception)
203 {
204 //Error occured
205 return false;
206 }
207 }
208
214 private Byte AutoDetectReferenceHue(Image<Bgr, Byte> modelImage)
215 {
216 const Int32 BOUNDARY_WIDTH = 10; //Width of white boundary surrounding model
217
218 using (Image<Hsv, Byte> imageHSV = modelImage.Convert<Hsv, Byte>())
219 return (Byte)(Math.Round(imageHSV[BOUNDARY_WIDTH, BOUNDARY_WIDTH].Hue,0));
220 }
221
227 private void ImageGrabbed_EventHandler(object sender, EventArgs e)
228 {
229 //Retreive frame
230 lastCaptured = capture.RetrieveBgrFrame(); //new Image<Bgr,Byte>(@"..\..\Test.jpg");
231
232 //Signal captured frame
233 fpsCounter.FrameCaptured();
234
235 //Create graphics (components are drawn to temporary bitmap first to prevent blinking)
236 frameBitmap = lastCaptured.ToBitmap();
237 graphFrame = Graphics.FromImage(frameBitmap);
238
239 #region Switch detection method
240
241 //Get selected index using invoke
242 Int32 detectionMethodIndex = 0;
243
244 try
245 {
246 this.Invoke((MethodInvoker)delegate() { detectionMethodIndex = comboBoxFeatureDetectionMethod.SelectedIndex; });
247 }
248 catch (Exception ex)
249 {
250 if (ex is ObjectDisposedException || ex is NullReferenceException)
251 { /*Form disposed, nothing to do*/ }
252 else
253 { throw; /*Other exceptions are passed further*/ }
254 }
255
256 //Switch mark detection methods
257 switch(detectionMethodIndex)
258 {
259 default:
260 case (Int32)ComboboxDetectionMethodIndexes.REFERENCE_HSV: //Process images using detection in HSV color space
261
262 //Threshold image in HSV color space
263 imageThresholdedLeft = ImageProcessingTools.ThresholdImageInHSV(lastCaptured, hsvSelectionLeft);
264 imageThresholdedRight = ImageProcessingTools.ThresholdImageInHSV(lastCaptured, hsvSelectionRight);
265
266 //Show thresholded image
267 ibHSV1.Image = imageThresholdedLeft.Resize(1 / 3.0, Emgu.CV.CvEnum.INTER.CV_INTER_NN);
268 ibHSV2.Image = imageThresholdedRight.Resize(1 / 3.0, Emgu.CV.CvEnum.INTER.CV_INTER_NN);
269
270 //Find and draw centers of selected areas, draw L/R label
271 {
272 Rectangle leftRect = ImageProcessingTools.GetCenterRectangle(imageThresholdedLeft);
273 Rectangle rightRect = ImageProcessingTools.GetCenterRectangle(imageThresholdedRight);
274
275 graphFrame.FillEllipse(ImageProcessingTools.myBrushes[0], leftRect);
276 graphFrame.DrawString("L", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[1], leftRect.Location);
277
278 graphFrame.FillEllipse(ImageProcessingTools.myBrushes[1], rightRect);
279 graphFrame.DrawString("R", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[0], rightRect.Location);
280 }
281
282 //Save detected point to mark detector class' buffer
283 leftMarkDetector.AddPoint(ImageProcessingTools.FindCenter(imageThresholdedLeft));
284 rightMarkDetector.AddPoint(ImageProcessingTools.FindCenter(imageThresholdedRight));
285
286 break;
287
288 case (Int32)ComboboxDetectionMethodIndexes.SURF_DETECTOR: //Use SURF detector
289 {
290 List<Image<Gray, Byte>> featuresList = new List<Image<Gray, byte>>();
291 List<Rectangle> rectangleList = new List<Rectangle>();
292
293 //Detect and draw left mark
294 featureDetectorLeft.DetectFeature(lastCaptured, featuresList, rectangleList);
295
296 if (rectangleList.Count > 0)
297 {
298 //Draw
299 graphFrame.DrawRectangle(new Pen(ImageProcessingTools.myBrushes[0]), rectangleList[0]);
300
301 graphFrame.FillEllipse(ImageProcessingTools.myBrushes[0],
302 ImageProcessingTools.GetCenterRectangle(rectangleList[0].Center()));
303
304 graphFrame.DrawString("L", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[0], rectangleList[0].Location);
305
306 //Add mark detector point
307 leftMarkDetector.AddPoint(rectangleList[0].Center());
308 }
309
310 //Clear lists
311 featuresList.Clear();
312 rectangleList.Clear();
313
314 //Detect and draw right mark
315 featureDetectorRight.DetectFeature(lastCaptured, featuresList, rectangleList);
316
317 if (rectangleList.Count > 0)
318 {
319 //Draw
320 graphFrame.DrawRectangle(new Pen(ImageProcessingTools.myBrushes[1]), rectangleList[0]);
321
322 graphFrame.FillEllipse(ImageProcessingTools.myBrushes[1],
323 ImageProcessingTools.GetCenterRectangle(rectangleList[0].Center()));
324
325 graphFrame.DrawString("R", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[1], rectangleList[0].Location);
326
327 //Add mark detector point
328 rightMarkDetector.AddPoint(rectangleList[0].Center());
329 }
330 }
331
332 break;
333 }
334
335 #endregion
336
337 //Draw locked marks & actualize world map
338 lock (marksLock)
339 {
340 if (leftDetectedMark.X > 0 && leftDetectedMark.Y > 0)
341 {
342 Rectangle tempRect = ImageProcessingTools.GetCenterRectangle(leftDetectedMark);
343
344 graphFrame.FillEllipse(ImageProcessingTools.myBrushes[2], tempRect);
345 graphFrame.DrawString("L", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[3], tempRect.Location);
346 worldMap.ModifyMark(WorldMap.MarkPosition.LEFT, cameraCalibrator.TransformPoint(leftDetectedMark), Color.GreenYellow);
347 }
348
349 if (rightDetectedMark.X > 0 && rightDetectedMark.Y > 0)
350 {
351 Rectangle tempRect = ImageProcessingTools.GetCenterRectangle(rightDetectedMark);
352
353 graphFrame.FillEllipse(ImageProcessingTools.myBrushes[2], tempRect);
354 graphFrame.DrawString("R", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[3], tempRect.Location);
355 worldMap.ModifyMark(WorldMap.MarkPosition.RIGHT, cameraCalibrator.TransformPoint(rightDetectedMark), Color.Red);
356 }
357 }
358
359 //Draw fps
360 if (menuItemShowFPSIndicator.Checked)
361 graphFrame.DrawString(fpsCounter.FPS.ToString("F2") + " FPS", Font, ImageProcessingTools.myBrushes[0], (PointF)ImageProcessingTools.defaultPoint);
362
363 //Draw bitmap
364 pbFrame.Image = frameBitmap;
365
366 //Draw world map
367 DrawWorldMap();
368 }
369
374 private void MarkLock_ThreadRoutine(Boolean resetCounters)
375 {
376 Point leftMark, rightMark;
377
378 //Reset detectors if allowed
379 if (resetCounters)
380 {
381 leftMarkDetector.Reset();
382 rightMarkDetector.Reset();
383 }
384
385 //Wait until buffers are full
386 while (!leftMarkDetector.IsBufferFull || !rightMarkDetector.IsBufferFull)
387 Thread.Sleep(1000 / (Int32)Math.Floor(fpsCounter.FPS)); //Wait approximately one frame
388
389 //Verify if marks were detected.
390 if (!leftMarkDetector.DetectMark(out leftMark))
391 leftMark = new Point(-1, -1); //Detection failed, add fake point instead
392
393 if (!rightMarkDetector.DetectMark(out rightMark))
394 rightMark = new Point(-1, -1); //Detection failed, add fake point instead
395
396 //Save thread-safely
397 lock (marksLock)
398 {
399 leftDetectedMark = leftMark;
400 rightDetectedMark = rightMark;
401 }
402 }
403
407 private void DrawWorldMap()
408 {
409 try
410 {
411 const float CIRCLE_WIDTH = 5.0f;
412 Byte renderingModeIndex = (Byte)WorldMapRenderingModes.NONE;
413
414 //Transformed points of the trajectory
415 List<PointF> trajectoryPoints = new List<PointF>();
416
417 //Crate graphics
418 graphWorld = Graphics.FromImage(frameWorldMap);
419 graphWorld.Clear(pbWorldMap.BackColor);
420
421 //Select rendering mode
422 try { this.Invoke((MethodInvoker)delegate() { renderingModeIndex = (Byte)comboBoxRenderingMode.SelectedIndex; }); }
423 catch (Exception) { renderingModeIndex = (Byte)WorldMapRenderingModes.NONE; /*Just in case*/ }
424
425 switch (renderingModeIndex)
426 {
427 default:
428 case (Byte)WorldMapRenderingModes.NONE:
429
430 graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.None;
431
432 break;
433
434 case (Byte)WorldMapRenderingModes.HIGH_SPEED:
435
436 graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighSpeed;
437
438 break;
439
440 case (Byte)WorldMapRenderingModes.HIGH_QUALITY:
441
442 graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
443
444 break;
445
446 case (Byte)WorldMapRenderingModes.ANTIALIAS:
447
448 graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
449
450 break;
451 }
452
453 //Find transformation
454 float scale = 1f;
455
456 //Set scale - trackbar is owned by another thread - use invoker
457 try
458 {
459 if (tbWorldMapScale.InvokeRequired)
460 tbWorldMapScale.Invoke((MethodInvoker)(delegate()
461 {
462 scale = (float)(tbWorldMapScale.Value);
463 }));
464 else
465 {
466 scale = (float)(tbWorldMapScale.Value);
467 }
468 }
469 catch (ObjectDisposedException)
470 {
471 //Set default scale
472 scale = 1f;
473 }
474
475 //Recalculate homography matrices only when neccessary
476 if (scale != scalePrev)
477 {
478 //Source points - points of the calibrating rectangle
479 List<PointF> selectedPoints = new List<PointF> { new PointF(-1*scale, scale), //Upper left
480 new PointF(scale, scale), //Upper right
481 new PointF(scale, -1 * scale), //Lower right
482 new PointF(-1 * scale, -1 * scale) }; //Lower left
483
484 //Target point - corners of the picturebox
485 List<PointF> templatePoints = new List<PointF> { new PointF(0f, 0f),
486 new PointF((float)pbWorldMap.Width, 0f),
487 new PointF((float)pbWorldMap.Width, (float)pbWorldMap.Height),
488 new PointF(0,(float)pbWorldMap.Height)};
489
490 //World coordinates to picturebox coordinates
491 homMatWorldToPb = CameraCalibration.FindHomography(selectedPoints.ToArray(), templatePoints.ToArray(),
492 Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.DEFAULT, 1.0);
493
494 //PbWorldMap coordinates to world coordinates - onversion of previous matrix
495 CvInvoke.cvInvert(homMatWorldToPb.Ptr, homMatWorldMapToWorld.Ptr, Emgu.CV.CvEnum.SOLVE_METHOD.CV_LU);
496
497 /*NOTE: Both homMatWorldToPb and homMatWorldMapToWorld matrices are affine homography matrices (or very close to them),
498 therefore, it is no need to normalize transformed coordinates with w'!*/
499
500 //Save scale
501 scalePrev = scale;
502 }
503
504 #region Draw map
505
506 //Draw marks
507 foreach (Mark mark in worldMap.Marks)
508 {
509 //Create data array
510 Double[,] data = new Double[3, 1]{ {(Double)mark.Coordinates.X}, //X
511 {(Double)mark.Coordinates.Y}, //Y
512 {1.0} //Scale
513 };
514
515 //Matrix from data
516 Matrix<Double> matrixTemp = new Matrix<double>(data);
517
518 //Transform vector using homography matrix
519 matrixTemp = homMatWorldToPb.Mul(matrixTemp);
520
521 graphWorld.FillEllipse(new SolidBrush(mark.MarkColor), (float)(matrixTemp[0, 0] - CIRCLE_WIDTH / 2), (float)(matrixTemp[1, 0] - CIRCLE_WIDTH / 2), CIRCLE_WIDTH, CIRCLE_WIDTH);
522 }
523
524 //Draw platform
525 PointF[] platformRectTmp = Car.PlatformRectangle;
526
527 for (Byte i = 0; i < Car.PlatformRectangle.Length; i++)
528 {
529 //Create data array
530 Double[,] data = new Double[3, 1]{ {(Double)Car.PlatformRectangle[i].X}, //X
531 {(Double)Car.PlatformRectangle[i].Y}, //Y
532 {1.0} //Scale
533 };
534
535 //Matrix from data
536 Matrix<Double> matrixTemp = new Matrix<double>(data);
537
538 //Transform vector using homography matrix
539 matrixTemp = homMatWorldToPb.Mul(matrixTemp);
540
541 //Save point
542 platformRectTmp[i].X = (float)matrixTemp[0, 0];
543 platformRectTmp[i].Y = (float)matrixTemp[1, 0];
544 }
545
546 graphWorld.FillPolygon(new SolidBrush(Car.PLATFORM_COLOR), platformRectTmp);
547
548 //Draw car
549 PointF[] carRectTmp = Car.CarRectangle;
550
551 for (Byte i = 0; i < Car.CarRectangle.Length; i++)
552 {
553 //Create data array
554 Double[,] data = new Double[3, 1]{ {(Double)Car.CarRectangle[i].X}, //X
555 {(Double)Car.CarRectangle[i].Y}, //Y
556 {1.0} //Scale
557 };
558
559 //Matrix from data
560 Matrix<Double> matrixTemp = new Matrix<double>(data);
561
562 //Transform vector using homography matrix
563 matrixTemp = homMatWorldToPb.Mul(matrixTemp);
564
565 //Save point
566 carRectTmp[i].X = (float)matrixTemp[0, 0];
567 carRectTmp[i].Y = (float)matrixTemp[1, 0];
568 }
569
570 graphWorld.FillPolygon(new SolidBrush(Car.CAR_COLOR), carRectTmp);
571
572 //Draw wheelchair
573 Boolean drawWheelchair = false;
574 PointF[] wheelchairRectangle = wheelchairModel.GetDefaultSurroundingRectangle();
575
576 try
577 {
578 //Invoke in formMain's thread
579 this.Invoke((MethodInvoker)delegate() { drawWheelchair = checkBoxDrawWheelchair.Checked; });
580 }
581 catch (Exception)
582 { drawWheelchair = false; /*Just for case value was corrupted*/ }
583
584 if (drawWheelchair) //Draw only if proper checkbox is checked
585 {
586 for (Byte i = 0; i < wheelchairRectangle.Length; i++)
587 {
588 //Create data array
589 Double[,] data = new Double[3, 1]{ {(Double)(wheelchairRectangle[i].X / 1000.0)}, // X [mm]
590 {(Double)(wheelchairRectangle[i].Y / 1000.0)}, //Y [mm]
591 {1.0} //Scale
592 };
593
594 //Matrix from data
595 Matrix<Double> matrixTemp = new Matrix<double>(data);
596
597 //Transform vector using homography matrix
598 matrixTemp = homMatWorldToPb.Mul(matrixTemp);
599
600 //Save point
601 wheelchairRectangle[i].X = (float)matrixTemp[0, 0];
602 wheelchairRectangle[i].Y = (float)matrixTemp[1, 0];
603 }
604
605 graphWorld.FillPolygon(WheelchairModel.WHEELCHAIR_COLOR[0], wheelchairRectangle); //Wheelchair
606
607 graphWorld.FillPolygon(WheelchairModel.WHEELCHAIR_COLOR[1], new PointF[] { //Arrow
608 new PointF((wheelchairRectangle[3].X + wheelchairRectangle[0].X) / 2, (wheelchairRectangle[3].Y + wheelchairRectangle[0].Y) / 2),
609 new PointF((wheelchairRectangle[0].X + wheelchairRectangle[1].X) / 2, (wheelchairRectangle[0].Y + wheelchairRectangle[1].Y) / 2),
610 new PointF((wheelchairRectangle[1].X + wheelchairRectangle[2].X) / 2, (wheelchairRectangle[1].Y + wheelchairRectangle[2].Y) / 2)});
611 }
612
613 //Draw all track points
614 foreach (TrackPoint trPoint in worldMap.TrackPoints)
615 {
616 //Create data array
617 Double[,] data = new Double[3, 1]{ {(Double)trPoint.Coordinates.X}, //X
618 {(Double)trPoint.Coordinates.Y}, //Y
619 {1.0} //Scale
620 };
621
622 //Matrix from data
623 Matrix<Double> matrixTemp = new Matrix<double>(data);
624
625 //Transform vector using homography matrix
626 matrixTemp = homMatWorldToPb.Mul(matrixTemp);
627
628 //Save trajectory point and draw it
629 trajectoryPoints.Add(new PointF((float)matrixTemp[0, 0], (float)matrixTemp[1, 0]));
630
631 graphWorld.FillEllipse(new SolidBrush(Color.Blue), (float)(matrixTemp[0, 0] - CIRCLE_WIDTH / 2), (float)(matrixTemp[1, 0] - CIRCLE_WIDTH / 2), CIRCLE_WIDTH, CIRCLE_WIDTH);
632 }
633
634 //Draw trajectory
635 for (Byte i = 1; i < worldMap.TrackPoints.Count; i++)
636 {
637 graphWorld.DrawLine(new Pen(Color.Blue), trajectoryPoints[i], trajectoryPoints[i - 1]);
638 }
639
640 if (trajectoryPoints.Count > 0)
641 graphWorld.DrawLine(new Pen(Color.Blue), trajectoryPoints[trajectoryPoints.Count - 1], new Point(pbWorldMap.Width / 2, pbWorldMap.Height / 2));
642
643 //Draw cross
644 graphWorld.DrawLine(new Pen(Color.Black), 0, pbWorldMap.Height / 2, pbWorldMap.Width, pbWorldMap.Height / 2);
645 graphWorld.DrawLine(new Pen(Color.Black), pbWorldMap.Width / 2, 0, pbWorldMap.Width / 2, pbWorldMap.Height);
646
647 //Draw scale
648 graphWorld.DrawString(scale.ToString() + " m", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[2],
649 new PointF(pbWorldMap.Width - ImageProcessingTools.myFont.Size * 4 /*Expected characters count = 4*/, pbWorldMap.Height / 2 - ImageProcessingTools.myFont.Height));
650
651 //Draw status info
652 String statusInfoString = null;
653 Byte cbReferenceSelIndex = (Byte)MapReferenceCoordinates.MARKS_CENTER;
654
655 try
656 {
657 //Always invoke, this thread is not member of STAThread apartment
658 this.Invoke((MethodInvoker)delegate() { cbReferenceSelIndex = (Byte)comboBoxCoordinatesReference.SelectedIndex; });
659 }
660 catch (Exception)
661 { cbReferenceSelIndex = (Byte)MapReferenceCoordinates.MARKS_CENTER; /*Just in case value was corrupted*/ }
662
663 if (cbReferenceSelIndex == (Byte)MapReferenceCoordinates.MARKS_CENTER)
664 {
665 //Display odometry info
666 statusInfoString = "Wheelchair odometry:" + Environment.NewLine +
667 "X: " + (wheelchairModel.X / 1000.0).ToString("F2") + " m" + Environment.NewLine +
668 "Y: " + (wheelchairModel.Y / 1000.0).ToString("F2") + " m" + Environment.NewLine +
669 "Phi: " + (wheelchairModel.Phi / 1000.0).ToString("F3") + " rad ~ " + (180 * wheelchairModel.Phi / (1000.0 * Math.PI)).ToString("F2") + "°";
670 }
671 else
672 {
673 //Wheelchair is reference point in this case, so its coordinates are always [0;0;0]
674 statusInfoString = "Wheelchair odometry:" + Environment.NewLine +
675 "X: 0.00 m" + Environment.NewLine +
676 "Y: 0.00 m" + Environment.NewLine +
677 "Phi: 0.000 rad ~ 0.00°";
678 }
679
680 graphWorld.DrawString(statusInfoString, ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[2], new PointF(0f, 0f));
681
682 //Display image
683 pbWorldMap.Image = frameWorldMap;
684
685 #endregion
686 }
687 catch (Exception ex)
688 {
689 //Draw error image
690 graphWorld = Graphics.FromImage(frameWorldMap);
691 graphWorld.Clear(pbWorldMap.BackColor);
692
693 graphWorld.DrawString("Error:" + Environment.NewLine + ex.Message.ToString(), new Font(FontFamily.GenericMonospace, 10f),
694 MyImageProcessingClasses.ImageProcessingTools.myBrushes[2], new RectangleF(new Point(0, 0), pbWorldMap.Size));
695
696 pbWorldMap.Image = frameWorldMap;
697 }
698 }
699
704 private void CalibratePointsMouseClickRoutine(MouseEventArgs e)
705 {
706 //Switch mouse button
707 switch (e.Button)
708 {
709 //Left button - calibrate points
710 case MouseButtons.Left:
711 {
712 PointF mousePoint = new PointF((float)e.X, (float)e.Y);
713
714 //Select point to calibrate
715 if (rbUL.Checked) //Upper left point
716 {
717 cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.UPPER_LEFT);
718
719 //Auto switch
720 rbUR.Select();
721
722 break;
723 }
724
725 if (rbUR.Checked) //Upper right point
726 {
727 cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.UPPER_RIGHT);
728
729 //Auto switch
730 rbLR.Select();
731
732 break;
733 }
734
735 if (rbLR.Checked) //Lower right point
736 {
737 cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.LOWER_RIGHT);
738
739 //Auto switch
740 rbLL.Select();
741
742 break;
743 }
744
745 if (rbLL.Checked) //Lower left point
746 {
747 cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.LOWER_LEFT);
748
749 //Auto switch
750 rbUL.Select();
751
752 break;
753 }
754
755 break;
756 }
757
758 default:
759 //No default action
760 break;
761 }
762
763 //Enable button if all point are specified
764 if (cameraCalibrator.AllCalibrationPointsSpecified)
765 buttonCalibrateHomography.Enabled = true;
766 else
767 buttonCalibrateHomography.Enabled = false;
768 }
769
774 private void SelectColorMouseClickRoutine(MouseEventArgs e)
775 {
776 //Switch mouse buttons
777 switch (e.Button)
778 {
779 //Left button - calibrate color
780 case MouseButtons.Left:
781
782 //check whether index is in bounds
783 if (e.X < capture.Width && e.Y < capture.Height)
784 {
785 if (comboBoxFeatureDetectionMethod.SelectedIndex == (Byte)ComboboxDetectionMethodIndexes.REFERENCE_HSV)
786 {
787 //HSV feature detection method is used -> select color to calibrate
788 if (rbSelectColor1.Checked)
789 {
790 //Paint label with selected color
791 lblSelectedColor1.BackColor = Color.FromArgb((Int32)lastCaptured[e.Y, e.X].Red, (Int32)lastCaptured[e.Y, e.X].Green, (Int32)lastCaptured[e.Y, e.X].Blue);
792
793 //Calculate HSV values
794 hsvSelectionLeft = lastCaptured.Convert<Hsv, Byte>()[e.Y, e.X];
795 lblHSV.Text = "Selected HSV: \nH: " + hsvSelectionLeft.Hue.ToString() + "\nS: " + hsvSelectionLeft.Satuation.ToString() + "\nV: " + hsvSelectionLeft.Value.ToString();
796
797 //Switch color selection
798 rbSelectColor2.Select();
799 }
800 else
801 {
802 //Paint label with selected color
803 lblSelectedColor2.BackColor = Color.FromArgb((Int32)lastCaptured[e.Y, e.X].Red, (Int32)lastCaptured[e.Y, e.X].Green, (Int32)lastCaptured[e.Y, e.X].Blue);
804
805 //Calculate HSV values
806 hsvSelectionRight = lastCaptured.Convert<Hsv, Byte>()[e.Y, e.X];
807 lblHSV.Text = "Selected HSV: \nH: " + hsvSelectionRight.Hue.ToString() + "\nS: " + hsvSelectionRight.Satuation.ToString() + "\nV: " + hsvSelectionRight.Value.ToString();
808
809 //Switch color selection
810 rbSelectColor1.Select();
811 }
812 }
813 else
814 {
815 //If SURF detector is used, click is used only to diplay selected value for user info, but does not modify the reference colors
816 Hsv selectedHSV = lastCaptured.Convert<Hsv, Byte>()[e.Y, e.X];
817 lblHSV.Text = "Selected HSV: \nH: " + selectedHSV.Hue.ToString() + "\nS: " + selectedHSV.Satuation.ToString() + "\nV: " + selectedHSV.Value.ToString();
818 }
819 }
820
821 break;
822
823 default:
824 //No default action
825 break;
826 }
827 }
828
829 #endregion
830 }
831 }