This is Rover

Post your EV3 projects, project ideas, etc here!

Moderators: roger, gloomyandy, skoehler

steveiswicked
Novice
Posts: 35
Joined: Sun Feb 06, 2011 11:22 pm
Contact:

This is Rover

Postby steveiswicked » Sat Nov 04, 2017 11:18 pm

Hi,
I've spent some time away from robots, but I saw some cool stuff with OpenCV. I experimented with code by Andy and Lawrie on Lejos news. Also I found Mark Crosbie's code on face detection. I bought a cheap webcam and when the Lejos code worked I was blown away. So I built Rover. Rover can see using OpenCV. He has jaws that can pick up a Duplo ball. Rover began life with very simply with skeleton structure of wheels and a webcam but the webcam field of view was very limited. I bought a "wide angle" camera, the depth of field was better but I still couldn't get a view of the floor and the room at the same time.
I wasn't going to gamble on more cameras, so I rebuilt Rover so that he can control camera elevation, and rover has become a bit of a beast. I've tested all the mechanics using RMI. Rover can grab and lift a ball, he can maneuver, and he can detect a face using openCV.

https://drive.google.com/file/d/0Bwtv1a ... sp=sharing

My Idea is that Rover will wait until he detects a "face", go recover a ball from somewhere, take the ball to the "face", drop the ball and "encourage" a game of fetch, track the ball as it is rolled away and then search/fetch it back to the/a "face". All this seemed would have seemed impossible only a few years ago. I really appreciate the commitment and dedication of the Lejos team for making my new project feasible.

Steve

User avatar
gloomyandy
leJOS Team Member
Posts: 5992
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: This is Rover

Postby gloomyandy » Sun Nov 05, 2017 9:12 am

Nice, rather than get a different webcam, you might want to hunt around and see if you can find a lens that can change the field of view of what you have?
leJOS news https://lejosnews.wordpress.com/

steveiswicked
Novice
Posts: 35
Joined: Sun Feb 06, 2011 11:22 pm
Contact:

Re: This is Rover

Postby steveiswicked » Thu Nov 09, 2017 10:48 pm

Good Call! I got a Victsing "3 in 1" Phone camera lens set which comes with a decent quality "~180 degree" lens (though I'd say it was ~130). I widened the aperture of my original camera lens housing with an emery board and it fits nicely. Both depth of field and field of view are much improved. My first hardware hack :D ! Thanks

steveiswicked
Novice
Posts: 35
Joined: Sun Feb 06, 2011 11:22 pm
Contact:

Re: This is Rover

Postby steveiswicked » Wed Nov 22, 2017 9:11 am

Well, I think I might have been naive here. I've got face detection working in its own thread feeding coordinates to behaviors in an arbitrator object. It's nice seeing the robot turn and look back at me, but it's really not performant enough, I have to be close and well lit for it to work at all, and I have to turn off the webcam stream to the pc for it to be reasonably fast enough. I think I will modify my "person detection" to be based on movement and some basic assumptions about the environment.

User avatar
gloomyandy
leJOS Team Member
Posts: 5992
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: This is Rover

Postby gloomyandy » Wed Nov 22, 2017 5:45 pm

Yep they all sound like typical robot vision problems! Robotics in the real world is not an easy problem to solve! This is especially true with low cost hardware. But the fun part is that with a "toy" you are recreating things that were cutting edge research only a few years ago (and which had the same problems you are running into!). Have fun!
leJOS news https://lejosnews.wordpress.com/

steveiswicked
Novice
Posts: 35
Joined: Sun Feb 06, 2011 11:22 pm
Contact:

Re: This is Rover

Postby steveiswicked » Thu Jan 25, 2018 12:38 am

I've been experimenting with OpenCV and decided that the best option is to offload the image analysis to the desktop. I'm embarrassed to say that working out how to transfer the image data from the brick and read it into the desktop application took me an extraordinary amount of effort. The code's not elegant but reasonably straightforward, I think it might be worth sharing. The VidStream code on the brick loads the openCV library, initialises the camera, and opens a server socket on port 8080, it then starts a loop to capture a video frame as a 120*160 matrix of 3 bytes each byte each representing value of Red Green and Blue. The image buffer is JPG encoded and written to a variable length byte array. The integer length of this image byte array is then written into a four byte array of bytes. The length byte array and the image byte array are then written to the socket output stream. The code achieves a frame rate of about 10fps. The main latency seems to be capturing the frame from the camera, different encoding schemes .png .jp2 dont make much difference to the fps. Even skipping both encoding and the writing the image to the stream don't seem to improve the frame rate.

Code: Select all

import java.net.ServerSocket;
import java.net.Socket;
import java.nio.ByteBuffer;
import lejos.hardware.Button;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfByte;
import org.opencv.highgui.Highgui;
import org.opencv.highgui.VideoCapture;

public class VidStream6 {
   public static void main(String[] args) throws Exception {
      System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
      MatOfByte buf = new MatOfByte();
      VideoCapture vid = new VideoCapture(0);
      Mat mat = new Mat(120, 160, CvType.CV_8UC3);
   
      vid.set(Highgui.CV_CAP_PROP_FRAME_WIDTH, 160);
      vid.set(Highgui.CV_CAP_PROP_FRAME_HEIGHT, 120);

      vid.open(0);
      System.out.println("Camera open");

      ServerSocket ss = new ServerSocket(8080);
      Socket sock = ss.accept();
      System.out.println("Socket connected");

      long stime = System.currentTimeMillis();
      int cnt = 0;
      while (Button.ESCAPE.isUp()) {
         vid.read(mat);
         if (!mat.empty()) {

            Highgui.imencode(".jpeg", mat, buf);
            byte[] imageBytes = buf.toArray();
            byte[] bytes = ByteBuffer.allocate(4).putInt(imageBytes.length).array();
            sock.getOutputStream().write(bytes);
            sock.getOutputStream().write(imageBytes);

            if (cnt++ >= 100) {
               long stop = System.currentTimeMillis();
               System.out.println("Frame rate: " + (cnt * 1000 / (stop - stime)));
               cnt = 0;
               stime = stop;
            }
         } else
            System.out.println("No picture");
      }
      sock.close();
      ss.close();
   }

}


The desktop Roboball client code attaches to the server socket on the brick to read the length of the image array and read and decode the appropriate number of bytes to convert them back into an OpenCV "Mat". Im using OpenCV 3.3.1 on the desktop. This code is based on the tutorial http://cell0907.blogspot.co.uk/2013/07/tracking-ball-with-javaopencv.html (where you will find the "Panel" extention for JPanel class that will write the Mat to a java bufferedImage). My version is tuned to find a red ball and overlay the image with a pink circle, which it does with mixed results, since the code is not really "tracking" it's just image processing each frame in isolation. The code opens four windows that show the original image, the conversion to HSV, a grayscale image, and the resulting threshold mask that's used to find the ball. It's a start, I'm now working on where to put the robot AI and communication protocol, since I'd like the robot to be as autonomous as possible.

If anyone can tell me how to improve the image capture frame rate, I'd be very grateful.

Code: Select all

package myocv;

import java.io.DataInputStream;
import java.io.IOException;
import java.net.Socket;
import java.util.ArrayList;
import java.util.List;

import javax.swing.JFrame;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfByte;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;

public class RoboBall {
   public static void main(String arg[]) throws IOException {
      // Load the native library.
      System.out.println("Welcome to OpenCV " + Core.VERSION);
      System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
      // It is better to group all frames together so cut and paste to
      // create more frames is easier
      JFrame frame1 = new JFrame("Camera");
      frame1.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
      frame1.setSize(200, 160);
      frame1.setBounds(0, 50, frame1.getWidth(), frame1.getHeight());
      Panel panel1 = new Panel();
      frame1.setContentPane(panel1);
      frame1.setVisible(true);
      JFrame frame2 = new JFrame("HSV");
      frame2.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
      frame2.setSize(200, 160);
      frame2.setBounds(300, 50, frame2.getWidth(),frame2.getHeight());
      Panel panel2 = new Panel();
      frame2.setContentPane(panel2);
      frame2.setVisible(true);
      JFrame frame3 = new JFrame("S,V Distance");
      frame3.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
      frame3.setSize(200, 160);
      frame3.setBounds(600, 50, frame3.getWidth(),frame3.getHeight());
      Panel panel3 = new Panel();
      frame3.setContentPane(panel3);
      frame3.setVisible(true);
      JFrame frame4 = new JFrame("Threshold");
      frame4.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
      frame4.setSize(200, 160);
      frame4.setBounds(900, 50, frame3.getWidth(), frame3.getHeight());
      Panel panel4 = new Panel();
      frame4.setContentPane(panel4);
      frame4.setVisible(true);

      Mat hsv_image = new Mat();
      Mat thresholded = new Mat();
      Mat thresholded2 = new Mat();
      Mat roboImage = new Mat(120, 160, CvType.CV_8UC3);
      // open the image stream
      Socket clientSocket = new Socket("192.168.1.86", 8080);
      DataInputStream in = new DataInputStream(clientSocket.getInputStream());

      // -- 2. Read a frame from the socket to help initialise windows
      MatOfByte k = new MatOfByte(imgData(in));
      roboImage = Imgcodecs.imdecode(k, Imgcodecs.CV_LOAD_IMAGE_UNCHANGED);

      frame1.setSize(roboImage.width() + 40, roboImage.height() + 60);
      frame2.setSize(roboImage.width() + 40, roboImage.height() + 60);
      frame3.setSize(roboImage.width() + 40, roboImage.height() + 60);
      frame4.setSize(roboImage.width() + 40, roboImage.height() + 60);

      Mat array255 = new Mat(roboImage.height(), roboImage.width(), CvType.CV_8UC1);
      array255.setTo(new Scalar(255));

      Mat distance = new Mat(roboImage.height(), roboImage.width(), CvType.CV_8UC1);

      List<Mat> lhsv = new ArrayList<Mat>(3);

      Mat circles = new Mat(); // No need (and don't know how) to initialize it.
      
      // The function later will do it... (to a 1*N*CV_32FC3)
      /*
       * Scalar hsv_min = new Scalar(0, 50, 50, 0); Scalar hsv_max = new
       * Scalar(6, 255, 255, 0); Scalar hsv_min2 = new Scalar(175, 50, 50, 0);
       * Scalar hsv_max2 = new Scalar(179, 255, 255, 0);
       */
      Scalar hsv_min = new Scalar(0, 100, 100, 0);
      Scalar hsv_max = new Scalar(10, 255, 255, 0);
      Scalar hsv_min2 = new Scalar(95, 65, 100, 0);
      Scalar hsv_max2 = new Scalar(183, 255, 255, 0);

      double[] cData = new double[3];

      while (true) {
         // read video from socket
         k = new MatOfByte(imgData(in));
         roboImage = Imgcodecs.imdecode(k, Imgcodecs.CV_LOAD_IMAGE_UNCHANGED);
//         System.out.println("orig kimage image " + roboImage.toString());

         if (!roboImage.empty()) {
            roboImage.convertTo(distance, CvType.CV_8UC1);
//            System.out.println("distance image " + distance.toString());
            // One way to select a range of colors by Hue
            Imgproc.cvtColor(roboImage, hsv_image, Imgproc.COLOR_BGR2HSV);
            Core.inRange(hsv_image, hsv_min, hsv_max, thresholded);
            Core.inRange(hsv_image, hsv_min2, hsv_max2, thresholded2);
            Core.bitwise_or(thresholded, thresholded2, thresholded);
            // Notice that the thresholds don't really work as a
            // "distance"
            // Ideally we would like to cut the image by hue and then pick just the area where S combined V are largest.
            // Strictly speaking, this would be something like sqrt((255-S)^2+(255-V)^2)>Range
            // But if we want to be "faster" we can do just (255-S)+(255-V)>Range
            // Or otherwise 510-S-V>Range Anyhow, we do the following... Will see how fast it goes...
            
            Core.split(hsv_image, lhsv); // We get 3 2D one channel Mats
            Mat S = lhsv.get(1);
            Mat V = lhsv.get(2);
            Core.subtract(array255, S, S);
            Core.subtract(array255, V, V);
            S.convertTo(S, CvType.CV_32F);
            V.convertTo(V, CvType.CV_32F);
            Core.magnitude(S, V, distance);
            
            Core.inRange(distance, new Scalar(0.0), new Scalar(170.0), thresholded2);
            Core.bitwise_and(thresholded, thresholded2, thresholded);
            // Apply the Hough Transform to find the circles
            Imgproc.GaussianBlur(thresholded, thresholded, new Size(9, 9), 0, 0);
//            Imgproc.erode(thresholded, thresholded, new Mat());
//            Imgproc.dilate(thresholded, thresholded, new Mat());
//            Imgproc.dilate(thresholded, thresholded, new Mat());

            Imgproc.HoughCircles(thresholded, circles, Imgproc.CV_HOUGH_GRADIENT, 2, thresholded.height() / 4, 500, 50, 1, 0);
            
            // Imgproc.Canny(thresholded, thresholded, 500, 250);
            // -- 4. Add some info to the image
            Imgproc.line(roboImage, new Point(75, 50), new Point(102, 100), new Scalar(255, 10, 20)/* CV_BGR(100,10,10) */, 2);
            Imgproc.circle(roboImage, new Point(105, 105), 10, new Scalar(255, 10, 20), 2);
            cData = roboImage.get(105, 105);
            Imgproc.putText(roboImage, String.format("(" + String.valueOf(cData[0]) + "," + String.valueOf(cData[1]) + "," + String.valueOf(cData[2]) + ")"),
                  new Point(30, 30), 1 , 1.0, new Scalar(255, 10, 20, 255), 1);
            
            // int cols = circles.cols();
            int rows = circles.rows();
            int elemSize = (int) circles.elemSize(); // Returns 12 (3 * 4 bytes in a float)
            float[] data2 = new float[rows * elemSize / 4];
            if (data2.length > 0) {
               circles.get(0, 0, data2); // Points to the first element and reads the whole thing into data2
               for (int i = 0; i < data2.length; i = i + 3) {
                  Point center = new Point(data2[i], data2[i + 1]);
                  // Core.ellipse( this, center, new Size( rect.width*0.5,  rect.height*0.5), 0, 0, 360, new Scalar( 255, 0, 255 ), 4, 8, 0 );
                  Imgproc.ellipse(roboImage, center,
                        new Size((double) data2[i + 2], (double) data2[i + 2]), 0, 0, 360,
                        new Scalar(255, 10, 255), 3, 8, 0);
               }
            }
            
            
            Imgproc.line(hsv_image, new Point(75, 50), new Point(102, 100), new Scalar(255, 10, 20)/* CV_BGR(100,10,10) */, 2);
            Imgproc.circle(hsv_image, new Point(105, 105), 10, new Scalar(255, 10, 20), 2);
            cData = hsv_image.get(105, 105);
            Imgproc.putText(hsv_image, String.format("(" + String.valueOf(cData[0]) + "," + String.valueOf(cData[1]) + "," + String.valueOf(cData[2]) + ")"),
                  new Point(30, 30), 1 , 1.0, new Scalar(255, 10, 20, 255), 1);
            
            
            distance.convertTo(distance, CvType.CV_8UC1);
            Imgproc.line(distance, new Point(75, 50), new Point(102, 100), new Scalar(100)/* CV_BGR(100,10,10) */, 2);
            Imgproc.circle(distance, new Point(105, 105), 10, new Scalar(10), 2);
            cData = (double[]) distance.get(105, 105);
         
            Imgproc.putText(distance, String.format("(" + String.valueOf(cData[0]) + ")"), new Point(30, 30), 1 , 1.0,
                  new Scalar(10), 1);
            
            // -- 5. Display the images
            panel1.setimagewithMat(roboImage);
            panel2.setimagewithMat(hsv_image);
            // panel2.setimagewithMat(S);
            panel3.setimagewithMat(distance);
            panel4.setimagewithMat(thresholded);
            frame1.repaint();
            frame2.repaint();
            frame3.repaint();
            frame4.repaint();
         } else {
            System.out.println(" --(!) No captured frame -- Break!");
            break;
         }

      }
      return;
   }

   public static byte[] imgData(DataInputStream dIS) throws IOException {
      int length = dIS.readInt();
      byte[] data = new byte[length];

      int pos = 0;
      while (pos < data.length) {
         int n = dIS.read(data, pos, data.length - pos);
         if (n < 0)
            break;
         pos += n;
      }

      return data;

   }

}


User avatar
gloomyandy
leJOS Team Member
Posts: 5992
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: This is Rover

Postby gloomyandy » Thu Jan 25, 2018 1:21 am

If you have a webcam that can produce mjpeg images directly for higher resolutions and you do not want/need to process the image on the EV3 then you can send the compressed images to the PC and avoid having to decode them on the EV3. This allows for faster frame rates and higher resolutions, see the following link for a discussion on how to do this...
viewtopic.php?f=18&t=8337&hilit=mjpeg

Oh and before you ask, I'm sorry but I don't have any code for converting the mjpeg stream into an openCV mat, but that should be relatively straight forward.
leJOS news https://lejosnews.wordpress.com/


Return to “EV3 Projects”

Who is online

Users browsing this forum: No registered users and 1 guest