Real-time Distance Measurement Using Single Image

2018-03-11
Taha Emara

Opencv Java Computer Vision

Introduction

Distance measurement between a robot and the object is needed to control the action of the robot such as grabbing an object or even avoiding obstacles. There are many methods to estimate the distance such as ultrasonic ranging, laser ranging, and vision based ranging. Vision based techniques have the merit of its low cost, so in this post, we will learn a method for distance measurement between a pinhole camera and a circle marker and implement it.

Requirements:

Webcam, I used ELP Sony IMX322 Sensor Mini Usb Camera Module HD 1080P(2.8-12mm lens with housing).
Paper with marker (blue circle with 1 cm radius) and stick it to any rigid surface.
Tape rule to measure distance.
Java Development Kit (JDK), you can get it from here.
Netbeans or any Java IDEs, you can get it from here.
Javacv, Java wrapper to Opencv, FFmpeg, and many more. It is included in pom file.
Source Code, you can get it from https://github.com/tahaemara/real-time-distance-measurement.
If you don't know how to run a maven project with netbeans, see this video https://www.youtube.com/watch?v=CDkdy3BwIqs.

The Idea of this method

The pinhole (monocular) camera generates a one-to-one relationship between the object and the image. Using this principle, we can deduce a relationship between known parameters: focal length (f), radius of marker in the image plane (r), and radios of marker in the object plane (R) and unknown parameter, distance from the camera to the object(d).

Using the principle of Similar Triangles, we can obtain the formulas as follows:

\begin{equation} \label{eq:equ1} \frac{f}{d}=\frac{r}{R} \end{equation} \begin{equation} \label{eq:equ2} f=d\times\frac{r}{R} \text{ pixels } \end{equation} \begin{equation} \label{eq:equ3} d=f\times\frac{R}{r} \text{ cm } \end{equation}

Calibration to find focal length

OpenCV can be used to estimate the focal length after taking 10 to 20 photos of checkerboard pattern with your camera like here, the result will be intrinsic parameters: focal length and optical center and extrinsic parameters: rotation and translation vectors of the camera.
But we will use another scenario as follow:

Print a blue circle on a paper with radius 1 cm and stick it to a rigid object like a carton box. We use color circle to make the segmentation of marker easy, in case of using a black marker, we get a lot of noise.
Adjust the distance from camera to object (d) to a specific value.
The radius of the circle (R) is known1 cm.
The radius of the marker in image plane (r) can be obtained after segmenting the circle and get its radius.
Substitute these parameters to the equation \eqref{eq:equ2} to get the focal length (f) in pixels.

Calibration Code

We will use Calibration class to estimate the focal length in pixels and then use it in our real-time app to measure the distance, as follow:

Grab a frame from the camera and convert it to IplImage and Mat types.

FrameGrabber grabber = FrameGrabber.createDefault(0);
grabber.start();
Frame frame = grabber.grab();
System.out.println("\nFrame size " + frame.imageWidth + " x " + frame.imageHeight);
IplImage grabbedImage = converter.convert(frame);
Mat img = converter.convertToMat(frame);
displayImage(grabbedImage, "Grabbed Image");

Resize image to make processing faster, and then invert the image to HSV color space because it separates image intensity from the color information, this improves the segmentation based on the color.

/*Resize Image (comment this section if you want to use default resolution)*/
Size size = new Size(640, 640);
resize(img, img, size);
grabbedImage = new IplImage(img);

/*Invert to HSV color space*/
Mat imghsv = new Mat();
cvtColor(img, imghsv, COLOR_BGR2HSV);

Convert image to grayscale and then binarise it.

//*Convert image to grayscale*/
IplImage gray = cvCreateImage(cvGetSize(grabbedImage), IPL_DEPTH_8U, 1);
cvCvtColor(grabbedImage, gray, CV_BGR2GRAY);

/*Binarising Image*/
IplImage binimg = cvCreateImage(cvGetSize(gray), IPL_DEPTH_8U, 1);
cvThreshold(gray, binimg, 0, 255, THRESH_BINARY + CV_THRESH_OTSU);
imwrite("binarise.jpg", new Mat(binimg));

Segment the blue circle by checking every circle in the image such that, if the color of its center belongs to the blue HSV range using method (isPixelBlue), this is the marker, and then compute its radius and then substitute it in the equation \eqref{eq:equ2} to get the focal length in pixels.

 /*Find countour */
CvMemStorage storage = cvCreateMemStorage(0);
CvSeq contours = new CvSeq();
cvFindContours(binimg, storage, contours, Loader.sizeof(CvContour.class), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0, 0));
CvSeq ptr = new CvSeq();
Mat m = new Mat(grabbedImage);
for (ptr = contours; ptr != null; ptr = ptr.h_next()) {
    /*Find Enclosing Circles*/
    Point2f center = new Point2f();
    FloatPointer radius = new FloatPointer(1f);
    opencv_imgproc.minEnclosingCircle(new Mat(ptr), center, radius);
    /*Check, if the color of the center picel is blue, it is our marker*/
    if (isPixelBlue(imghsv, (int) center.x(), (int) center.y())) {
        double area = opencv_imgproc.contourArea(new Mat(ptr), true);
        if (area > 100) {
            opencv_imgproc.cvCircle(grabbedImage, new CvPoint((int) center.x(), (int) center.y()), (int) radius.get(0),
                    CV_RGB(0, 255, 0), 3, 0, 0);
            opencv_imgproc.putText(m, area + "", new Point((int) center.x(), (int) center.y()), 0, 2.0, new Scalar(0, 0, 0, 0));//print result above every digit
            float rad = radius.get(0);
            System.out.println("Radius " + rad);
            double focallen = (DISTANCE_TO_OBJECT * rad) / RADIUS_OF_MARKER;
            System.out.println("Focal lenght is " + focallen + " Pixels");
        }
    }
}//End for  countors

Copy the result (focal length) and assign the variable FOCAL_LENGTH in DistanceMeasurement class.

Grabbed image and the output after segmentation of marker.

Measurement of distance in real time

The code in DistanceMeasurement class is the same as Calibration class except we add a while loop to make it in real-time and modify the logic code for computing the distance. After segmenting the marker, we compute its radius in the image plane and substitute along with -known- the focal length and the radius of the marker (1cm) in equation \eqref{eq:equ3} to get the estimated distance between the camera and object in cm.

 /*Find countour */
CvMemStorage storage = cvCreateMemStorage(0);
CvSeq contours = new CvSeq();
cvFindContours(binimg, storage, contours, Loader.sizeof(CvContour.class), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0, 0));
CvSeq ptr = new CvSeq();
Mat m = new Mat(grabbedImage);
for (ptr = contours; ptr != null; ptr = ptr.h_next()) {
    /*Find Enclosing Circles*/
    Point2f center = new Point2f();
    FloatPointer radius = new FloatPointer(1f);
    opencv_imgproc.minEnclosingCircle(new Mat(ptr), center, radius);
    /*Check, if the color of the center picel is blue, it is our marker*/
    if (isPixelBlue(imghsv, (int) center.x(), (int) center.y())) {
        double area = contourArea(new Mat(ptr), true);
        if (area > 100) {
            cvCircle(grabbedImage, new CvPoint((int) center.x(), (int) center.y()), (int) radius.get(0),
                    CV_RGB(0, 255, 0), 3, 0, 0);
            float rad = radius.get(0);
            System.out.println("Radius is " + rad);
            double distance = (FOCAL_LENGTH * RADIUS_OF_MARKER) / rad;
            System.out.println("Distance in cm " + distance);
            putText(m, "Distance is : " + distance + " cm", new Point(200, 100), 0, 0.5, new Scalar(0, 255, 0, 0));
        }
    }
}//End for  countors
canvas.showImage(converter.convert(m));
Thread.sleep(150);
}//End if