Irak MayerElizabeth VicarteStephanie Vicarte
Published © GPL3+

Stereo Vision on Netduino

Managing two cameras to calculate a depth map using a Netduino board and C#.

IntermediateFull instructions provided4 hours1,533

Things used in this project

Hardware components

Seeed Studio Grove - Serial Camera Kit
×2
Base Shield V2
Seeed Studio Base Shield V2
×1

Software apps and online services

Visual Studio 2015
Microsoft Visual Studio 2015

Story

Read more

Schematics

Board Connection

Code

Stereo vision program

C#
//Visteli labs
//Stereo image acquisition using two UART cameras.
//8-27-2018

using System;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.IO;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
using System.IO.Ports;
using System.IO;
using System.Text;

namespace StereoCamera
{
    public class StereoCamera
    {
        //Define UART ports and properties.
        static SerialPort Camera1 = new SerialPort("COM1", 115200, Parity.None, 8, StopBits.One);
        static SerialPort Camera2 = new SerialPort("COM2", 115200, Parity.None, 8, StopBits.One);

        // An output port allows you to write (send a signal) to a pin
        static OutputPort _led = new OutputPort(Pins.ONBOARD_LED, false);
        // An input port reads the signal from a pin (Should be Pins.ONBOARD_BTN, but there is a bug)
        static InputPort _button = new InputPort((Cpu.Pin)0x15, false, Port.ResistorMode.Disabled);

        //Define variables 
        static byte[] messageFromCam1 = new byte[1024]; 
        static byte[] messageFromCam2 = new byte[1024];
        static bool dataRead1 = false;
        static int dataSize1 = 0;
        static bool dataRead2 = false;
        static int dataSize2 = 0;

        //Camera constants
        const int PIC_PKT_LEN = 128;                 
        const int PIC_FMT_VGA = 1;
        const int PIC_FMT_CIF = 5;
        const int PIC_FMT_OCIF = 3;
        const int CAM_ADDR = 0;
        const string CAM_SERIAL = "Serial";

        const int PIC_FMT = PIC_FMT_VGA;

        const byte cameraAddr = (CAM_ADDR << 5);  // addr
        static ulong picTotalLen = 0;            // picture length

        //Clear UART input port
        static void clearBuffer(SerialPort camDevice)
        {
        }

        //Process response from camera
        static bool readReply(SerialPort CamDevice,byte[] strData, ref int intReceived)
        {
            bool retval = false;
            const int BUFFER_SIZE = 6;
            byte[] buffer = new byte[BUFFER_SIZE];

            //fill the correct buffer depending on which camera
            if (CamDevice == Camera1)
            {
                for (int index = 0; index < dataSize1; index++)
                {
                    strData[index] = messageFromCam1[index];
                }

                if (intReceived == dataSize1)
                    retval = true;
                else
                    intReceived = dataSize1;
            }
            else if (CamDevice == Camera2)
            {
                for (int index = 0; index < dataSize1; index++)
                {
                    strData[index] = messageFromCam2[index];
                }

                if (intReceived == dataSize2)
                    retval = true;
                else
                    intReceived = dataSize2;
            }

            return retval;
        }

        //Send command to the camera
        static bool sendCmd(SerialPort camDevice,char[] cmd,int cmdLength)
        {
            int i;
            
            for (i=0;i<cmdLength;i++)
            {
                camDevice.WriteByte((byte)cmd[i]);
            }

            return true;
        }

        //Set the camera to capture
        static void Set4Capture(SerialPort camDevice)
        {
            //Command fot capture
            char[] cmd = new char[] { (char)0xaa, (char )(0x01 | cameraAddr), (char)0x00, (char)0x07, (char)0x00, (char)PIC_FMT };
            byte[] resp = new byte[1024] ;
            bool cmdRet = false;
            int dataSize2Read=6;

            while (true)
            {
                clearBuffer(camDevice);
                sendCmd(camDevice, cmd,6);

                cmdRet = readReply(camDevice,  resp, ref dataSize2Read);
                if (!cmdRet && dataSize2Read < 6) continue;

                if (resp[0] == 0xaa && resp[1] == (0x0e | cameraAddr) && resp[2] == 0x01 && resp[4] == 0 && resp[5] == 0) break;
            }
        }

        //Snap a single image from the selected camera
        static void Snap(SerialPort camDevice)
        {
            char[] cmd = new char[] { (char)0xaa, (char)(0x06 | cameraAddr), (char)0x08, (char)(PIC_PKT_LEN & 0xff), (char)((PIC_PKT_LEN >> 8) & 0xff), (char)0 };
            byte[] resp = new byte[1024];
            bool cmdRet = false;
            int ds2r = 6;

            while (true)
            {
                clearBuffer(camDevice);
                sendCmd(camDevice,cmd, 6);

                cmdRet = readReply(camDevice, resp, ref ds2r);
                if (!cmdRet && ds2r < 6) continue;

                if (resp[0] == 0xaa && resp[1] == (0x0e | cameraAddr) && resp[2] == 0x06 && resp[4] == 0 && resp[5] == 0) break;
            }

            cmdRet = false;
            cmd[1] = (char)(0x05 | cameraAddr);
            cmd[2] = (char)0;
            cmd[3] = (char)0;
            cmd[4] = (char)0;
            cmd[5] = (char)0;
            while (true)
            {
                clearBuffer(camDevice);
                sendCmd(camDevice, cmd, 6);

                cmdRet = readReply(camDevice,resp, ref ds2r);
                if (!cmdRet && ds2r < 6) continue;

                if (resp[0] == 0xaa && resp[1] == (0x0e | cameraAddr) && resp[2] == 0x05 && resp[4] == 0 && resp[5] == 0) break;
            }
            cmdRet = false;
            cmd[1] = (char)(0x04 | cameraAddr);
            cmd[2] = (char)0x1;
            while (true)
            {
                clearBuffer(camDevice);
                sendCmd(camDevice, cmd, 6);

                cmdRet = readReply(camDevice, resp, ref ds2r);
                if (!cmdRet && ds2r < 6) continue;

                if (resp[0] == 0xaa && resp[1] == (0x0e | cameraAddr) && resp[2] == 0x04 && resp[4] == 0 && resp[5] == 0)
                {
                    if (ds2r == 12)
                    {
                        if (resp[6] == 0xaa && resp[7] == (0x0a | cameraAddr) && resp[8] == 0x01)
                        {
                            picTotalLen = (ulong)((resp[9]) | (resp[10] << 8) | (resp[11] << 16));
                            break;
                        }
                    }
                }
            }

        }

        //Initialize camera
        static void CameraInit(SerialPort camDevice)
        {
            char[] cmd = new char[] { (char)0xaa,(char) (0x0d | cameraAddr), (char)0x00, (char)0x00, (char)0x00, (char)0x00 };
            byte[] resp = new byte[1024];
            bool cmdRet = false;
            int dS2R = 6;

            while (true)
            {
                clearBuffer(camDevice);
                sendCmd(camDevice, cmd, 6);

                cmdRet = readReply(camDevice, resp, ref dS2R);
                if (!cmdRet && dS2R < 6) continue;

                if (resp[0] == 0xaa && resp[1] == (0x0e | cameraAddr) && resp[2] == 0x0d && resp[4] == 0 && resp[5] == 0)
                {
                    if (dS2R == 12)
                    {
                        if (resp[6] == 0xaa && resp[7] == (0x0d | cameraAddr) && resp[8] == 0 && resp[9] == 0 && resp[10] == 0 && resp[11] == 0)
                            break;
                    }
                }
            }
            cmd[1] = (char)(0x0e | cameraAddr);
            cmd[2] = (char)0x0d;
            sendCmd(camDevice, cmd, 6);
        }

        //Record image and save it on jpg file
        static void GetImage(SerialPort camDevice)
        {
            var volume = new VolumeInfo("SD");
            uint pktCnt = (uint)((picTotalLen) / (PIC_PKT_LEN - 6));
            const int BUFFER_SIZE = 1024;
            byte[] buffer = new byte[BUFFER_SIZE];
            string path = "";
            string picName = "";
            uint counter = 0;
            uint cnt = 0;

            if ((picTotalLen % (PIC_PKT_LEN - 6)) != 0) pktCnt += 1;

            char[] cmd = new char[] { (char)0xaa, (char)(0x0e | cameraAddr), (char)0x00, (char)0x00, (char)0x00, (char)0x00 };
            byte[] pkt = new byte[picTotalLen];//PIC_PKT_LEN

            if (camDevice == Camera1)
            {
                picName = "picLeft.jpg";
            }
            else if (camDevice == Camera2)
            {
                picName = "picRight.jpg";
            }

            path = Path.Combine("SD", picName);

                for (uint i = 0; i < pktCnt; i++)
                {
                    cmd[4] = (char)(i & 0xff);
                    cmd[5] = (char)((i >> 8) & 0xff);

                    int retry_cnt = 0;
                retry:
                    System.Threading.Thread.Sleep(10);
                    clearBuffer(camDevice);

                    sendCmd(camDevice, cmd, 6);

                    if (camDevice == Camera1)
                        cnt = (uint)dataSize1; 
                    else if (camDevice == Camera2)
                        cnt = (uint)dataSize2;

                    if (cnt > 3)
                    {
                        byte sum = 0;

                        if (camDevice == Camera1)
                        {
                            for (int y = 0; y < cnt - 2; y++)
                            {
                                sum += messageFromCam1[y];
                            }

                            if (sum != messageFromCam1[cnt - 2])
                            {
                                if (++retry_cnt < 100) goto retry;
                                else break;
                            }
                            else
                            {
                                for (int z = 4; z < cnt - 6; z++)
                                {
                                    pkt[counter++] = messageFromCam1[z];
                                }
                            }
                        }
                    else if (camDevice == Camera2)
                    {
                        for (int y = 0; y < cnt - 2; y++)
                        {
                            sum += messageFromCam2[y];
                        }

                        if (sum != messageFromCam2[cnt - 2])
                        {
                            if (++retry_cnt < 100) goto retry;
                            else break;
                        }
                        else
                        {
                            for (int z = 4; z < cnt - 6; z++)
                            {
                                pkt[counter++] = messageFromCam2[z];
                            }
                        }
                    }
                }
                else
                    {
                        goto retry;
                    }

                }

                byte[] mypkt = new byte[counter];
                for (int z = 0; z < counter; z++)
                {
                    mypkt[z] = pkt[z];
                }

                // write some text to a file
                File.WriteAllBytes(path, mypkt);

                // Must call flush to write immediately. Otherwise, there's no guarantee 
                // as to when the file is written. 
                volume.FlushAll();

                cmd[4] = (char)0xf0;
                cmd[5] = (char)0xf0;
                sendCmd(camDevice, cmd, 6);
            
            }


    //UART event function
    static void SerialDataReceived(object sender, SerialDataReceivedEventArgs e)
    {
        if ((e.EventType == SerialData.Chars) && (sender == Camera1))
        {
            const int BUFFER_SIZE = 1024;
            byte[] buffer = new byte[BUFFER_SIZE];

            int amount = ((SerialPort)sender).Read(buffer, 0, BUFFER_SIZE);
            if (amount > 0)
            {
                for (int index = 0; index < amount; index++)
                {
                    messageFromCam1[index] = buffer[index];
                }
                dataRead1 = true;
                dataSize1 = amount;
            }
        }
        else if ((e.EventType == SerialData.Chars) && (sender == Camera2))
        {
            const int BUFFER_SIZE = 1024;
            byte[] buffer = new byte[BUFFER_SIZE];

            int amount = ((SerialPort)sender).Read(buffer, 0, BUFFER_SIZE);
            if (amount > 0)
            {
                for (int index = 0; index < amount; index++)
                {
                    messageFromCam1[index] = buffer[index];
                }
                dataRead1 = true;
                dataSize1 = amount;
            }
        }

    }


    public static void Main()
    {
        //initialize communication timeout 
        Camera2.WriteTimeout = 500;
        Camera2.ReadTimeout = 500;
        Camera1.WriteTimeout = 500;
        Camera1.ReadTimeout = 500;

        //Set event callback function
        Camera2.DataReceived += SerialDataReceived;
        Camera1.DataReceived += SerialDataReceived;

        //Open UART communication port
        Camera2.Open();
        Camera1.Open();

        //Initialize camera
        CameraInit(Camera2);
        CameraInit(Camera1);

        while (true)
        {
                //Snap image when board button pressed
            if (_button.Read())
            {
                //Record image from camera 1
                Set4Capture(Camera1);
                Snap(Camera1);
                GetImage(Camera1);

                //Record image from camera 2
                Set4Capture(Camera2);
                Snap(Camera2);
                GetImage(Camera2);
            }
        }

    }


}
}

Credits

Irak Mayer

Irak Mayer

18 projects • 10 followers
Elizabeth Vicarte

Elizabeth Vicarte

13 projects • 7 followers
Stephanie Vicarte

Stephanie Vicarte

14 projects • 12 followers

Comments