Emgu.CV Object Marker

Question

I built a pan-tilt system with 2 servos and laser diode. The system was controlled by Arduino Nano. By using Emgu.CV in C#, I created a grayscale image and convert it to a black-white image using GrayImg.ThresholdBinaryInv . I'm using Inverse Kinematics calculations but the laser diode couldn't mark the object well. Where did I make a mistake? Image coordinate system and servos coordinate systems are not same. Thus, I did mapping and scaling. It should work but the accuracy of the laser diode is inaccurate.

Arduino Code

#include <Servo.h>

int Th1, Th2, tmp;
Servo M1;
Servo M2;
void setup() 
{
  Serial.begin(9600);
  pinMode(6,OUTPUT; // Laser Diode
  digitalWrite(6,0);
  Th1 = 0;
  Th2 = 0;
  M1.attach(3);
  M1.write(90);
  M2.attach(9);
  M2.write(90);
}

void loop() 
{
  delay(200); //sync issue only

  if(Serial.available()>=2)
  {
    Th1 = Serial.read(); //read only one byte
    Th2 = Serial.read();
    M1.write(Th1);
    M2.write(Th2);

    //Remove any extra worng reading
    while(Serial.available()) tmp = Serial.read();    
    
    // Run the robotic arm here. For testing, we will    
    digitalWrite(6,1);
    delay(500);
    digitalWrite(6,0);
    delay(500);
     
    //switch On or switch off a LED according to Th1 value


    
    //Serial.print('1'); // This tell the PC that Arduino is Ready for next angles
  }
}

C# Code

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.IO.Ports;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.Util;
using Emgu.CV.Structure;

namespace ObjectMarker
{
    public partial class Form1 : Form
    {
        private Capture capture; 
        private Image<Bgr, Byte> IMG;
        private Image<Gray,Byte> blackWhite;
        private Image<Gray, Byte> GrayImg; 
        
        private int N, Xpx, Ypx; // N -> Number of non-black pixels
        private double Xcm, Ycm;
        private double myScale;
        private double Th1, Th2;
        static SerialPort _serialPort;
        public byte []Buff = new byte[2];
        
        public Form1() //Constructor of the Form1
        {
            InitializeComponent();
            //myScale = 65.0 / 480.0;
            myScale = 1.7 / 13;
             _serialPort = new SerialPort();
            _serialPort.PortName = "COM4";//Set your board COM
            _serialPort.BaudRate = 9600;
            _serialPort.Open();
        }
        

        private void processFrame(object sender, EventArgs e) // Most important function
        {                       //You're not connected any camera - null 
            if (capture == null)//very important to handel excption - capture - point of the camera
            {
                try
                {
                    capture = new Capture(1); //creatine a object 
                }
                catch (NullReferenceException excpt)
                {
                    MessageBox.Show(excpt.Message);
                }
            }

            IMG = capture.QueryFrame();// capture the current frame. Get an image.
         
            GrayImg = IMG.Convert<Gray, Byte>();
            blackWhite = GrayImg.ThresholdBinaryInv(new Gray(25),new Gray (255));
            
            Xpx = 0;
            Ypx = 0;
            N = 0;
       
            for (int i = 0; i < blackWhite.Width; i++) {
                for (int j = 0; j < blackWhite.Height; j++) {
                    if(blackWhite[j,i].Intensity > 128){
                        N++;
                        Xpx += i;
                    Ypx += j;
                    }
                }
            }
            
            if(N>0){

                Xpx = Xpx / N;
                Ypx = Ypx / N;
                
                Xpx = (blackWhite.Width / 2) - Xpx; //320 - xpx
                Ypx = Ypx - (blackWhite.Height / 2); // ypx - 240
    
                Xcm = Xpx * myScale;
                Ycm = Ypx * myScale;

                double d3 = 28; // Laser to wall dist. (?)
                double Zcm = 108; // Cam to wall dist.
                double d1 = 3.50; // Joint to joint dist.
                double l2 = 4.50; // Joint to laser dist. (?)


                textBox1.Text = Xcm.ToString();
                textBox2.Text = Ycm.ToString();
                textBox3.Text = N.ToString();
                textBox11.Text = Zcm.ToString();
                textBox12.Text = myScale.ToString();

                //Inverse Calculations

                double Px, Py, Pz,Diff = 0;
        
                // Mapping
                //Px = Zcm;
                Px = -1 * Zcm;
                Py = -1 * Xcm;
                Pz = Ycm + Diff; // The laptop has built-in camera and the pan-tilt above the keyboard so there's a distance between camera and pan-tilt system (laser diode)
                

                textBox8.Text = Px.ToString();
                textBox9.Text = Py.ToString();
                textBox10.Text = Pz.ToString();

                Th1 = Math.Atan((Py / Px));
                Th2 = Math.Atan((Math.Sin(Th1) * (Pz - d1)) / Py);
                

                //Th1 = Math.Atan(Ycm / Xcm);
                //Th2 = Math.Atan((Math.Sin(Th1) * (Zcm - d1)) / Ycm);
                
                textBox4.Text = Th1.ToString();
                textBox5.Text = Th2.ToString();

         
                Th1 = (Th1 * (180 / Math.PI));
                Th2 = (Th2 * (180 / Math.PI));
                Th1 += 90;
                Th2 += 90;

                textBox6.Text = Th1.ToString();
                textBox7.Text = Th2.ToString();

                label11.Text = trackBar1.Value.ToString();
                label12.Text = trackBar2.Value.ToString();
                
                Buff[0] = (byte)trackBar1.Value; //Th1
                Buff[1] = (byte)trackBar2.Value; //Th2
                _serialPort.Write(Buff,0,2);

            }
            else
            {
                textBox1.Text = "";
                textBox2.Text = "";
                textBox3.Text = N.ToString();

                Buff[0] = (byte)90; //Th1
                Buff[1] = (byte)90; //Th2
                _serialPort.Write(Buff, 0, 2);
            }
                
            
      
            try
            {
                
                imageBox1.Image = IMG;
                imageBox2.Image = GrayImg;
                imageBox3.Image = blackWhite;
                
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }



        private void button1_Click(object sender, EventArgs e)
        {
            //Application.Idle += processFrame; //Serial comm between arduino and computer
            timer1.Enabled = true;
            button1.Enabled = false;
            button2.Enabled = true;
        }

        private void button2_Click(object sender, EventArgs e)
        {
            //Application.Idle -= processFrame;
            timer1.Enabled = false;
            button1.Enabled = true;
            button2.Enabled = false;
        }    

        private void button3_Click(object sender, EventArgs e) // I measure the object size from paint. It gave me pixel size. Then I measure the real size of the image. Real size divided by pixel size gives scale.
        {
            IMG.Save("G:\\CurrentFrame" +  ".jpg");
        }
        void Timer1Tick(object sender, EventArgs e)
        {
            processFrame(sender,e);
        }       

        
    }
}

Answer 1

This question is very hard to answer, as there are many places where an error could have crept in. Without further diagnosis and knowledge of the system, it's almost impossible to give a clear answer. I suggest you perform some more diagnosis, and if the answer still eludes you, reformulate your question (or close it when you find the answer yourself).

Some possible sources of error (and points for diagnosis) could be:

• In the Arduino code, you throw away anything after the first two bytes of Serial communication. Why would there be extra bytes? Are you sure the extra bytes are garbage, and, inversely, the first two bytes are the correct ones?
• In the C# mapping of coordinates, you perform a translation of the coordinates. Are you sure the system is exactly axis-parallel? An error of just a few degrees quickly adds up. You might need to add a rotational transformation.
• Your C# code returns an angle (in degrees) for the servo. This could be negative, or above 255, so converting it to a byte could, on overflow, lead to passing a (completely) wrong value to the Arduino.