Laser Radar Learning Notes (iii) feature extraction _ LiDAR

The information that lidar obtains is the distance information with the surrounding object, and it is widely used in the field of mobile robot, especially autonomous mobile robot, so let's start with the autonomous navigation of mobile robot.

Mobile robot navigation means that the mobile robot relies on the sensor to realize the collision-free motion from the starting position to the target position according to the criterion of time, shortest path or minimum energy consumption in a specific environment.

The traditional navigation problem of mobile robot consists of three main elements: map creation, location and motion control, and solve three basic problems through three main factors: where am I? Where I'm going. How to go.

The purpose of robot positioning is to answer "Where am I?" "This basic question.

Laser radar after the acquisition of data through certain operations can accurately calculate the current absolute position or relative position, then you can know where the robot, of course, sometimes map creation and positioning is synchronized, such as slam algorithm (simultaneous localization and mapping), can be viewed as an exploratory issue at this time.

Said so much, but is to say that the laser radar is how useful, how powerful, and the completion of these functions should be the first to obtain the characteristics of LIDAR data, this is back to the topic of feature extraction.

Feature extraction is mainly divided into two steps: Region segmentation and feature extraction. The regional segmentation stage mainly completes the classification and identification of feature patterns, that is, determining the types of patterns, such as lines, arcs, and so on, and determining the set of laser data points in the region and region which belong to the feature pattern. The feature extraction stage mainly completes various characteristic pattern parameters and extracts the feature points.

One, the region divides

For each frame distance data, the laser scanning point is divided into different blocks first. If the distance of two consecutive scan points is less than one threshold, the two scan points belong to the same block. If the distance of two consecutive scan points is greater than a threshold, the data frame is separated from this place. Finally, a frame distance data is divided into several blocks. The segmented block is represented by Ri (I=1,..., Q, where Q is the number of chunks), each containing a NI point. Because the distribution of the scan point is not uniform, usually, the scanning point density is larger than that of the sensor, but the density of the scanning point away from the sensor is smaller. Therefore, the adaptive variable threshold segmentation method is applied to the distance data segmentation. For example, when a scan point from the sensor center distance of D, the segmentation threshold selection is D, when the scan point from the sensor center distance of 3D, the threshold selection is 3d. In addition, other linear or non-linear functions can be used to define the adaptive segmentation threshold value. In a word, different segmentation thresholds are chosen at different scanning points, so that the segmentation blocks of distance data can be better consistent with the real environment characteristic model. If the laser's effective ranging distance is 10 meters and the angle resolution is 0.25 degrees, the minimum distance between adjacent scan points is: 2x10mxsin (0.125°) =0.0436m. Depending on the value, you can set the appropriate separator threshold.

UTM-30LX effective distance of 60m angle resolution 0.25 degrees, but generally for indoor applications will not exceed 10m

A, calculate the distance between the two adjacent points DJ
B, judge the relationship between DJ and Threshold Delta

If the DJ is larger than the Min Delta, then the point (x,y) is the dividing point of the two regions, and the selection of the threshold is generally based on the dynamic threshold value.

c, (optional) to determine the number of data points in each region, if a region contains a number of positions less than three, then the region is considered a noise area, discard these noise points

Laser Radar Region segmentation effect, different regions with different color segmentation (the same color does not mean in the same area, but the color is limited, several colors in the recycling)

Second, feature extraction

In the data scanned by LIDAR, several important features are: Tearing point (Breakpoint), corner Point (Corner), straight line, arc and so on.

Area segmentation has actually found the tearing point in the data. A polyline can also be regarded as a feature, which is a feature of a straight line plus a corner point.

As a key feature of the line in many papers are the key to extract, in view of the line is universal, then corner detection is also an unavoidable problem. Then we first extract the corner point and break all the polylines into straight and corner points.

1. Corner detection

Assuming that there is a polyline with only a single corner point, then we can use the multiple-deformation fitting method to determine the position of the corner point. First, the region within the point of the proposed synthesis of a straight line, and then find the farthest point from the line, if the distance is greater than a threshold, it can be considered a polyline, and that point is the dividing point of the line, no is a straight line.

When a region contains multiple corner points, it is necessary to use the iterative or recursive way, constantly looking for the corner--> split into two paragraphs, loop, until each area does not exist corner points.

The way the polygon fits determines whether there are corner points, and where the corners are

To fit a polygon: Points: Point N on Profile--number of contour points Eps--fitting precision//return value: If the contour segment needs to be segmented, return the index of the segment point in the Contour Point column, otherwise, the return 0 means that there is no need to fragment//This is the entire algorithm computational complexity One of the biggest places to improve the efficiency of the program, the distance from point to line calculation is improved: a straight line in a polygon fit is determined by the point in the Point column. To calculate the distance from the point to the line,//use the coordinate system to rotate the straight line to the x axis, so that the distance from the point to the line is the point// In the absolute value of Y after the rotation of the coordinates//at the same time, the coordinate rotation matrix is fixed in this operation, only one calculation, no need for multiple square or trigonometric computations int Openradar::P olycontourfit (int* X, int* Y, int n, float Eps  
                     //According to the contour point, use polygons to fit the contour point {double dis = sqrt (double) ((X[0]-x[n-1]) * (X[0]-x[n-1)) +
    ((Y[0]-y[n-1]) ((y[0)-y[n-1)));
    Double Costheta = (x[n-1]-x[0])/dis;
    Double Sintheta =-(y[n-1]-y[0])/dis;
    Double Maxdis = 0;
    int i;
    int maxdisind =-1;
    Double Dbdis;  for (i = 1; i < n-1; i++) {//coordinate rotation, seek the distance from the point to the X axis after rotation Dbdis = ABS ((y[i)-y[0]) * Costheta + (x[i)
        -x[0]) * Sintheta);
            if (Dbdis > Maxdis) {maxdis = Dbdis;
        Maxdisind = i; } if (Maxdis > Eps) {returnMaxdisind;
        cout << "Line 1:" << Endl; cout << "Start:" << points[0].x << "" << points[0].y << "---" << point
        s[maxdisind].x << "" << points[maxdisind].y << Endl; cout << "angle:" <<180 * atan2 (POINTS[0].Y-POINTS[MAXDISIND].Y, points[0].x-points[maxdisind].x)
        /3.1415926;
        cout << "Line 2:" << Endl; cout << "Start:" << points[maxdisind].x << "<< points[maxdisind].y <<"---
        "<< points[n-1].x <<" "<< points[n-1].y << Endl; cout << "angle:" << 180 * atan2 (POINTS[N-1].Y-POINTS[MAXDISIND].Y, Points[n-1].x-points[maxdis
    ind].x)/3.1415926;
    }//else{//cout << "line 1:" << Endl; cout << "Start:" << points[0].x << "" << poinTS[0].Y << "---" << points[n-1].x << "<< points[n-1].y << Endl; cout << "angle:" <<180 * atan2 (POINTS[N-1].Y-POINTS[0].Y, points[n-1].x-points[0].x)/3.1415

    926;
return 0; }

The above can only detect a single corner of the polyline, any corner of the polyline using a recursive way, want to speed up the transformation of their own way to achieve the iteration.

Splits a polyline into multiple segments int openradar::breakpolyline (vector<int>& Breakedradarrho, Vector<doubl   
                             e>& Breakedradartheta, vector<int>& Sepradarrho,
    Vector<double>&sepradartheta) {int rho = 0;
    Double theta = 0.0;
    int x[1200] = {0};
    int y[1200] = {0};
    int rhocopy[1200] = {0};
    Double thetacopy[1200] = {0};
    int pointcnt = 0;
    int linecnt = 0;
    int N = 0;
    Sepradarrho.clear ();
    Sepradartheta.clear ();

    Corners.clear ();
   Iterate over multiple iterations, splitting all polylines into straight segments vector<int>cornerindex;
   int cornercnt = 0;
    int tempindex = 0;
        for (int i = 0; i < static_cast<int> (Breakedradarrho.size ()); i++) {rho = breakedradarrho.at (i);

        theta = breakedradartheta.at (i); The IF (Rho < 0) {if (pointcnt > 200)//number of points that are less than directly discards the {cornerindex.clear (
                ); Cornercnt = Findcorners (cornerindex,x,y,0,pointcnt,200);
                    if (cornerindex.size () = = 0) {for (int k = 0; k < pointcnt;k++)
                        {Sepradarrho.push_back (rhocopy[k]);
                    Sepradartheta.push_back (Thetacopy[k]);
                    } sepradarrho.push_back (-1);
                    Sepradartheta.push_back (1000.0);
                linecnt++;
                    }else {tempindex = 0;
                        for (int k = 0; k < pointcnt;k++) {Sepradarrho.push_back (rhocopy[k]);
                        Sepradartheta.push_back (Thetacopy[k]);
                            if (k = = cornerindex.at (tempindex)) {Sepradarrho.push_back (-1);
                            Sepradartheta.push_back (1000.0);
         linecnt++;                   if (Tempindex < static_cast<int> (Cornerindex.size ())-1) {
                            tempindex++;
                    }} sepradarrho.push_back (-1);
                    Sepradartheta.push_back (1000.0);
                linecnt++;
            } pointcnt = 0;
        Continue
        } x[pointcnt] = static_cast<int> (Rho*cos (theta));
        Y[POINTCNT] = static_cast<int> (Rho*sin (theta));
        RHOCOPY[POINTCNT] = Rho;
        THETACOPY[POINTCNT] = theta;
    pointcnt++;
    }//cout<< "linecnt:" <<lineCnt<<endl;
return linecnt; }

2, Straight line fitting

If a region does not exist corner points, and the point data is relatively large, then generally is a straight line, (not absolute, the line of the method of determining the blog after writing)

The principle of line fitting is simple, in fact, it is a least square method, or in order to improve the precision of fitting, the weighted least two multiplication is used, and the weighted least squares is adopted here.

The original data figure Blue Dot is the radar position

Fitting straight line and corner point graph, the coarse point is the corner point, the thick solid line is the line that fits out

Since only a line with multiple points is fitted and the upper part of the line is drawn, the actual number of points is smaller than the linear error, which is not the key feature.

SOURCE Download: http://download.csdn.net/detail/renshengrumenglibing/5076599