OpenCV and ROS: by a Raspberry Pi rover project

OpenCV and ROS: by a Raspberry Pi rover project


This is based on a rover project by using ROS as framework and OpenCV as image processing SDK. The rover runs by following a track which is detected from image sequences via Picamera. Hough transform and contour detection are the ways to get the track, then the direction of detected track is converted to rover direction. You can find the codes in my git repository.
In my rover test project, the rover has to follow a white track, about 1.8-inch as width, made by duct-tape on concrete floor. This article focus on the ways to extract and detect tracks as well as how you incorporate ROS image message and OpenCV image format. As to convert the track information to motor direction, it is covered in my other post regarding Raspberry Pi rover.
The track is detected from the image captured by Pi camera via ROS usb_cam package. ROS cv_bridge converts the image data to OpenCV image format, and two different methods are used to find tracks, one is Hough transform, the other is contour detection. Hough transform will find slope of the edges of the track, so I can get their direction.
In above Hough-transform example, thick-green lines are found edges, the program will depends on the direction of these two edges to adjust rover's direction.
Finding contours can get the boundary of the track, then calculate the mass-center of this contour, and convert this mass-center to direction.
2   5
Left example: the found contour is near left side of the image, so the rover will turn left. Right example: the found contour keeps in the middle of the image, so the rover will keep straight.
My PiCamera is set to serve image in 320x240 RGB, 30fps. Pi Camera can provide HD image, however, limited to RPi2 processing power, 320x240 is the choice. I'll explain my implementations with diagrams and C++ codes. 

ROS image to OpenCV image

ROS is the framework used in this project, usb_cam is the ROS package to acquire image from Pi Camera. The input data from usb_cam is raw RGB, you need to convert it to OpenCV format to work with OpenCV API.
int32_t CNavigatorEngineWithImageSource::ProcessImageData(const sensor_msgs::ImageConstPtr img, bool bDisplayImage)
	int nRet = 0;
        // uses cv_bridge to convert ROS image to OpenCV IplImage
	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::BGR8);
	// that is the IplImage to work
	IplImage WorkingImage = cv_ptr->image;

	// if you need cv::Mat, 
        cv::Mat workingCVMat;
        workingCMat = cv_ptr->image;

Hough Transform

The captured OpenCV image passes Gaussian-blur filter, Canny filter and Hough filter. The result are sets of lines in slope-intersection form (y = mx + b), a simple RANSAC is applied to these lines to find left and right edges of the track. With these edges, I can know the direction of the track, then change rover direction accordingly.
Here are the C++ codes of this method.
#define ROIRATIO (4)

m_ROIFrameSize.width = pFrame->width;
m_ROIFrameSize.height = pFrame->height / ROIRATIO;

m_ROI = cvRect(0, m_FrameSize.height * (ROIRATIO - 1) / ROIRATIO, m_ROIFrameSize.width, m_ROIFrameSize.height);
// we're interested only in road below horizont - so crop top image portion off
    crop(pFrame, m_pWorkingImage, m_ROI);
    cvCvtColor(m_pWorkingImage, m_pGreyImage, CV_BGR2GRAY); // convert to grayscale

// Perform a Gaussian blur ( Convolving with 9x9 Gaussian) & detect edges
    cvSmooth(m_pGreyImage, m_pGreyImage, CV_GAUSSIAN, 9, 9);
// finding edges
    CvScalar mu, sigma;
    cvAvgSdv(m_pGreyImage, &mu, &sigma);
    cvCanny(m_pGreyImage, m_pGreyImage, mu.val[0] - sigma.val[0], mu.val[0] + sigma.val[0], 3);
// do Hough transform to find lines

    nHoughLines = 0;
    nHoughThreshold = m_ROIFrameSize.height * 0.4;

    pLines = cvHoughLines2(m_pGreyImage, m_pHoughStorage, CV_HOUGH_PROBABILISTIC,
                                        rho, theta, nHoughThreshold,
                                        m_ROIFrameSize.height * 0.4,

// here, we prefer some lines which are not horizontal and within some distance, done by RANSAC
    ProcessLanes(pLines, m_pGreyImage, m_pWorkingImage, m_bShowLine, 0, 0);
The major issues are the noises in the image. With a well-controlled environment, the track over concrete floor under even light sources, it is always easy to find correct track. With noises introduced (shadows, or uneven light sources), Hough transform could get extra edges not belonging to tracks. Also, in this implementation, I tried not to use too many "magic numbers", but with some statistical values instead. For example, the threshold for Canny filter and Hough transform. For max line gap in Hough transform, that still depends on my test environment, which you may need to tune in your own cases.
In order to eliminate false edges, I use RANSAC to apply extra rules to find expected edges of the track. Those rules will filter out horizontal lines, or if two lines are not parallel enough, or if two lines got big length different. Some of the parameters are hard-coded for the nature of the input image dimension (320 x 240.) After RANSAC, the result is quite promising. Here the codes of RANSAC rules. 
virtual GRANSAC::VPFloat ComputeDistanceMeasure(std::shared_ptr<GRANSAC::AbstractParameter> inParam)
    std::shared_ptr<yisys_roswheels::MyLine2D> ExtLine2D = std::dynamic_pointer_cast<MyLine2D>(inParam);

    // distance is theta between two lines with some pre-condition
    // two line with similar length
    // lengthdiff: will be normalized to [0..1]
    float lengthdiff = fabs(ExtLine2D->m_fLength - m_Params.m_fLength) / m_Params.m_fLength;
    // fAngleDiff: [-90 .. 90]
    float fDist = fAngleDiff / 90 * 7 + lengthdiff * 3;// weight as 0.7/0.3
    float fLineDistance = 0;

    // if two lines are not from the same "line band (track)" or
    // if two lines has big length difference, or
    // if input line is almost horizontal
    if (fAngleDiff > 30 || (lengthdiff >= 0.7) || fabs(ExtLine2D->m_fAngle) > 80)
            fDist += _MAX_DIST; // _MAX_DIST=100, that is to make it a fail case

    if (fDist < _MAX_DIST)
        float fY = (ExtLine2D->m_Point1.y + ExtLine2D->m_Point2.y + m_Params.m_Point1.y + m_Params.m_Point2.y) / 4;

        float fX1 = ExtLine2D->FindX(fY);
        float fX2 = m_Params.FindX(fY);
        float fD = fX2 - fX1;

        fLineDistance = sqrt((fD * fD));
        // my input image is 320x240, so check if the distance in this range
        if (fLineDistance < 40 || fLineDistance > 160)
            // to eliminate two close parallel lines. or if two lines are too far-away
            fDist += _MAX_DIST;
    return fDist;

Results by Hough transform

With window light as noises, RANSAC can still find the correct track.

Finding Contours

Finding contours is the method inspired by similar projects using light-sensor to detect the position of the track then adjust rover direction. The idea is to find the possible boundary of the track, get the mass-center of this boundary, then decide the rover direction according to the difference between mass-center of the track and image center.
finding contour

 By the nature of this method, it only needs a small portion of the image to find the track. Still, in case image with lots of noises, the detected contour get bigger error. So, it is better to adjust camera to capture the required portion only, which can eliminate noises, also reduce processing time.
I don't want to introduce lots of "magic numbers" in this case, the only one is the filter out noised contours by check its area if bigger than 1/6 of the image size.

#define ROIRATIO 4

m_nHeight = InputImage.rows;
m_nWidth = InputImage.cols;

int nROILeft = 0;
int nROIWidth = m_nWidth;
int nROITop = m_nHeight * (ROIRATIO - 1) / ROIRATIO;
int nROIHeight = m_nHeight / ROIRATIO;

cv::Rect roi(nROILeft, nROITop, nROIWidth, nROIHeight);
// we only get lower part of the input image
// convert to grey-scale
cv::cvtColor(roiImg, roiImg, CV_BGR2GRAY);
// blur to remove some noise
cv::blur(roiImg, roiImg, cv::Size(5, 5), cv::Point(-1,-1));
// separate track from background as possible as it can
cv::threshold(roiImg, roiImg, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
// find countours
cv::findContours(roiImg, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0));

int nMaxAreaContourIndex = -1;
float fMaxArea = 0.f;
// find contour with max area
for (size_t s = 0; s < contours.size(); s++)
	float fArea = cv::contourArea(contours[s]);
	if (fArea > fMaxArea)
		nMaxAreaContourIndex = s;
		fMaxArea = fArea;
// use this contour if it is over some creteria
// and find its mass-center
if (fMaxArea > ContourAreaThreshold &amp;& nMaxAreaContourIndex >= 0)
	cv::Moments mu;
	mu = cv::moments(contours[nMaxAreaContourIndex], false);
	// point in center (x only)
	cv::Point2f center(mu.m10 / mu.m00, mu.m01 / mu.m00); 
	nCenterX = (int32_t)center.x + nROILeft;
	nCenterY = (int32_t)center.y + nROITop;

Results by finding contours

As a simple rover project, these two methods are very easy to implement, which is a good hello-world project to me. With well-controlled environment, these two methods can give acceptable result. There are several to-dos can improve the result, however, with limited processing power of Raspberry Pi 2, that is not feasible. The possible solution I will try is to train some track cases in neural network in other powerful machine, then feed the data to RPi2 to detect tracks later. 
Last modified on Wednesday, 27 January 2016 10:58

© Copyright 2007- Yi Systems, Inc. All rights reserved.

5009 Asteria St, Torrance, CA, 90503, U.S.A. | +1-310-561-7237 |