caltech_lanes: /home/laradmin/lar/perception/road/caltech

00001 
00008 #ifndef LANEDETECTOR_HH_
00009 #define LANEDETECTOR_HH_
00010 
00011 #include "mcv.hh"
00012 #include "InversePerspectiveMapping.hh"
00013 
00014 namespace LaneDetector
00015 {
00016 
00017 //Debug global variable
00018 extern int DEBUG_LINES;
00019 
00021 typedef enum LineType_ {
00022   LINE_HORIZONTAL = 0,
00023   LINE_VERTICAL = 1
00024 } LineType;
00025 
00027 typedef enum LineColor_ {
00028   LINE_COLOR_NONE,
00029   LINE_COLOR_YELLOW,
00030   LINE_COLOR_WHITE
00031 } LineColor;
00032 
00034 typedef struct Line
00035 {
00037   FLOAT_POINT2D startPoint;
00039   FLOAT_POINT2D endPoint;
00041   LineColor color;
00043   float score;
00044 } Line;
00045 
00047 typedef struct Spline
00048 {
00050   int degree;
00052   CvPoint2D32f points[4];
00054   LineColor color;
00056   float score;
00057 } Spline;
00058 
00061 typedef struct LineState_
00062 {
00064   vector<Spline> ipmSplines;
00065 
00067   vector<CvRect> ipmBoxes;
00068 } LineState;
00069 
00070 typedef enum CheckSplineStatus_
00071 {
00072   ShortSpline = 0x1,
00073   //spline is curved i.e. form control points
00074   CurvedSpline = 0x2,
00075   //spline is curved i.e. overall (thetaDiff)
00076   CurvedSplineTheta = 0x4,
00077   HorizontalSpline = 0x8
00078 } CheckSplineStatus;
00079 
00080 #define GROUPING_TYPE_HV_LINES 0
00081 #define GROUPING_TYPE_HOUGH_LINES 1
00082 
00084 typedef struct LaneDetectorConf
00085 {
00087   FLOAT ipmWidth;
00089   FLOAT ipmHeight;
00091   int ipmLeft;
00093   int ipmRight;
00095   int ipmTop;
00097   int ipmBottom;
00099   int ipmInterpolation;
00100 
00102   FLOAT lineWidth;
00104   FLOAT lineHeight;
00106   unsigned char kernelWidth;
00107   unsigned char kernelHeight;
00109   FLOAT lowerQuantile;
00111   bool localMaxima;
00113   unsigned char groupingType;
00116   bool binarize;
00117   //unsigned char topClip;
00119   FLOAT detectionThreshold;
00121   bool smoothScores;
00123   float rMin, rMax, rStep;
00125   float thetaMin, thetaMax, thetaStep;
00128   float ipmVpPortion;
00130   bool getEndPoints;
00132   bool group;
00134   float groupThreshold;
00136   bool ransac;
00138   int ransacLineNumSamples;
00139   int ransacLineNumIterations;
00140   int ransacLineNumGoodFit;
00141   float ransacLineThreshold;
00142   float ransacLineScoreThreshold;
00143   bool ransacLineBinarize;
00145   int ransacLineWindow;
00147   int ransacSplineNumSamples;
00148   int ransacSplineNumIterations;
00149   int ransacSplineNumGoodFit;
00150   float ransacSplineThreshold;
00151   float ransacSplineScoreThreshold;
00152   bool ransacSplineBinarize;
00153   int ransacSplineWindow;
00155   int ransacSplineDegree;
00157   bool ransacSpline;
00158   bool ransacLine;
00159 
00161   float ransacSplineStep;
00162 
00164   float overlapThreshold;
00165 
00168   float localizeAngleThreshold;
00170   int localizeNumLinePixels;
00171 
00174   float extendAngleThreshold;
00177   float extendMeanDirAngleThreshold;
00179   int extendLinePixelsTangent;
00181   int extendLinePixelsNormal;
00184   float extendContThreshold;
00186   int extendDeviationThreshold;
00188   int extendRectTop;
00190   int extendRectBottom;
00191 
00194   float extendIPMAngleThreshold;
00197   float extendIPMMeanDirAngleThreshold;
00199   int extendIPMLinePixelsTangent;
00201   int extendIPMLinePixelsNormal;
00204   float extendIPMContThreshold;
00206   int extendIPMDeviationThreshold;
00208   int extendIPMRectTop;
00210   int extendIPMRectBottom;
00211 
00212 
00214   int splineScoreJitter;
00216   float splineScoreLengthRatio;
00218   float splineScoreAngleRatio;
00220   float splineScoreStep;
00221 
00223   int splineTrackingNumAbsentFrames;
00225   int splineTrackingNumSeenFrames;
00226 
00228   float mergeSplineThetaThreshold;
00230   float mergeSplineRThreshold;
00232   float mergeSplineMeanThetaThreshold;
00234   float mergeSplineMeanRThreshold;
00236   float mergeSplineCentroidThreshold;
00237 
00239   int lineTrackingNumAbsentFrames;
00241   int lineTrackingNumSeenFrames;
00242 
00244   float mergeLineThetaThreshold;
00246   float mergeLineRThreshold;
00247 
00249   int numStrips;
00250 
00252   bool checkSplines;
00254   float checkSplinesCurvenessThreshold;
00256   float checkSplinesLengthThreshold;
00258   float checkSplinesThetaDiffThreshold;
00260   float checkSplinesThetaThreshold;
00261 
00263   bool checkIPMSplines;
00265   float checkIPMSplinesCurvenessThreshold;
00267   float checkIPMSplinesLengthThreshold;
00269   float checkIPMSplinesThetaDiffThreshold;
00271   float checkIPMSplinesThetaThreshold;
00272 
00274   float finalSplineScoreThreshold;
00275 
00277   bool useGroundPlane;
00278 
00280   bool checkColor;
00282   int checkColorWindow;
00284   int checkColorNumBins;
00286   float checkColorNumYellowMin;
00288   float checkColorRGMin;
00290   float checkColorRGMax;
00292   float checkColorGBMin;
00294   float checkColorRBMin;
00296   float checkColorRBFThreshold;
00298   bool checkColorRBF;
00299 
00301   bool ipmWindowClear;
00303   int ipmWindowLeft;
00305   int ipmWindowRight;
00306 
00308   bool checkLaneWidth;
00310   float checkLaneWidthMean;
00312   float checkLaneWidthStd;
00313 } LaneDetectorConf;
00314 
00315 //function definitions
00325 void mcvGetGaussianKernel(CvMat *kernel, unsigned char w, FLOAT sigma);
00326 
00327 
00337 void mcvGet2DerivativeGaussianKernel(CvMat *kernel, unsigned char w,
00338                                      FLOAT sigma);
00339 
00340 
00358 #define FILTER_LINE_HORIZONTAL 0
00359 #define FILTER_LINE_VERTICAL 1
00360 void mcvFilterLines(const CvMat *inImage, CvMat *outImage, unsigned char wx=2,
00361                     unsigned char wy=2, FLOAT sigmax=1, FLOAT sigmay=1,
00362                     LineType lineType=LINE_HORIZONTAL);
00363 
00364 
00378 #define HV_LINES_HORIZONTAL 0
00379 #define HV_LINES_VERTICAL   1
00380 void mcvGetHVLines(const CvMat *inImage, vector<Line> *lines,
00381                    vector<FLOAT> *lineScores, LineType lineType=LINE_HORIZONTAL,
00382                    FLOAT linePixelWidth=1., bool binarize=false,
00383                    bool localMaxima=false, FLOAT detectionThreshold=1.,
00384                    bool smoothScores=true);
00385 
00391 void mcvBinarizeImage(CvMat *inImage);
00392 
00402 void mcvGetVectorMax(const CvMat *inVector, double *max, int *maxLoc,
00403                      int ignore=0);
00404 
00412 FLOAT mcvGetQuantile(const CvMat *mat, FLOAT qtile);
00413 
00423 void mcvThresholdLower(const CvMat *inMat, CvMat *outMat, FLOAT threshold);
00424 
00438 void mcvGetStopLines(const CvMat *inImage, vector<Line> *stopLines,
00439                      vector<float> *lineScores, const CameraInfo *cameraInfo,
00440                      LaneDetectorConf *stopLineConf);
00441 
00452 void mcvLines2Mat(const vector<Line> *lines, CvMat *mat);
00453 
00463 void mcvMat2Lines(const CvMat *mat, vector<Line> *lines);
00464 
00472 void mcvIntersectLineWithBB(const Line *inLine, const CvSize bbox,
00473                             Line *outLine);
00474 
00483 bool mcvIsPointInside(FLOAT_POINT2D point, CvSize bbox);
00484 
00491 void mcvMatInt2Float(const CvMat *inMat, CvMat *outMat);
00492 
00501 void mcvDrawLine(CvMat *image, Line line, CvScalar color=CV_RGB(0,0,0),
00502                  int width=1);
00503 
00512 void mcvDrawRectangle (CvMat *image, CvRect rect,
00513                        CvScalar color=CV_RGB(255,0,0), int width=1);
00514 
00522 void mcvInitLaneDetectorConf(char * const fileName,
00523                             LaneDetectorConf *laneDetectorConf);
00524 
00525 void SHOW_LINE(const Line line, char str[]="Line:");
00526 void SHOW_SPLINE(const Spline spline, char str[]="Spline");
00527 
00537 double mcvGetLocalMaxSubPixel(double val1, double val2, double val3);
00538 
00539 
00561 void mcvGetHoughTransformLines(const CvMat *inImage, vector <Line> *lines,
00562                                vector <FLOAT> *lineScores,
00563                                FLOAT rMin, FLOAT rMax, FLOAT rStep,
00564                                FLOAT thetaMin, FLOAT thetaMax,
00565                                FLOAT thetaStep, bool binarize, bool localMaxima,
00566                                FLOAT detectionThreshold, bool smoothScores,
00567                                bool group, FLOAT groupTthreshold);
00568 
00579 void mcvIntersectLineRThetaWithBB(FLOAT r, FLOAT theta, const CvSize bbox,
00580                                   Line *outLine);
00581 
00592 void mcvGetMatLocalMax(const CvMat *inMat, vector<double> &localMaxima,
00593                        vector<CvPoint> &localMaximaLoc, double threshold=0.0);
00594 
00605 void mcvGetMatMax(const CvMat *inMat, vector<double> &maxima,
00606                   vector<CvPoint> &maximaLoc, double threshold);
00607 
00615 void mcvGetVectorLocalMax(const CvMat *inVec, vector<double> &localMaxima,
00616                           vector<int> &localMaximaLoc);
00617 
00625 //void mcvGetLinePixels(const Line &line, vector<int> &x, vector<int> &y)
00626 CvMat * mcvGetLinePixels(const Line &line);
00627 
00637 //void mcvGetLinePixels(const Line &line, vector<int> &x, vector<int> &y);
00638 
00648 void mcvGetLineExtent(const CvMat *im, const Line &inLine, Line &outLine);
00649 
00660 void mcvLineXY2RTheta(const Line &line, float &r, float &theta);
00661 
00662 
00669 bool mcvIsPointInside(FLOAT_POINT2D &point, const Line &rect);
00670 
00682 void mcvIntersectLineRThetaWithRect(FLOAT r, FLOAT theta, const Line &rect,
00683                                     Line &outLine);
00684 
00685 
00701 void mcvGetLanes(const CvMat *inImage, const CvMat* clrImage,
00702                  vector<Line> *lanes, vector<FLOAT> *lineScores,
00703                  vector<Spline> *splines, vector<float> *splineScores,
00704                  CameraInfo *cameraInfo, LaneDetectorConf *stopLineConf,
00705                  LineState* state = NULL);
00706 
00707 
00721 void mcvGetLines(const CvMat* image, LineType lineType, vector<Line> &lines,
00722                  vector<float> &lineScores, vector<Spline> &splines,
00723                  vector<float> &splineScores, LaneDetectorConf *lineConf,
00724                  LineState *state);
00725 
00743 void mcvPostprocessLines(const CvMat* image, const CvMat* clrImage,
00744                          const CvMat* rawipm, const CvMat* fipm,
00745                          vector<Line> &lines, vector<float> &lineScores,
00746                          vector<Spline> &splines, vector<float> &splineScores,
00747                          LaneDetectorConf *lineConf, LineState *state,
00748                          IPMInfo &ipmInfo, CameraInfo &cameraInfo);
00749 
00758 CvMat* mcvGetNonZeroPoints(const CvMat *inMat, bool floatMat);
00759 
00777 void mcvFitRansacLine(const CvMat *image, int numSamples, int numIterations,
00778                       float threshold, float scoreThreshold, int numGoodFit,
00779                       bool getEndPoints, LineType lineType,
00780                       Line *lineXY, float *lineRTheta, float *lineScore);
00781 
00794 void mcvFitRobustLine(const CvMat *points, float *lineRTheta, float *lineAbc);
00795 
00803 void mcvGroupLines(vector<Line> &lines, vector<float> &lineScores,
00804                    float groupThreshold, CvSize bbox);
00805 
00806 
00816 void mcvGetRansacLines(const CvMat *im, vector<Line> &lines,
00817                        vector<float> &lineScores, LaneDetectorConf *lineConf,
00818                        LineType lineType);
00819 
00820 
00828 void  mcvSetMat(CvMat *inMat, CvRect mask, double val);
00829 
00839 void mcvSplinesImIPM2Im(vector<Spline> &splines, IPMInfo &ipmInfo,
00840                         CameraInfo &cameraInfo, CvSize imSize);
00841 
00851 void mcvLinesImIPM2Im(vector<Line> &lines, IPMInfo &ipmInfo,
00852                       CameraInfo &cameraInfo, CvSize imSize);
00853 
00861 void mcvDrawSpline(CvMat *image, Spline spline, CvScalar color, int width);
00862 
00863 
00887 void mcvFitRansacSpline(const CvMat *image, int numSamples, int numIterations,
00888                         float threshold, float scoreThreshold, int numGoodFit,
00889                         int splineDegree, float h, Spline *spline,
00890                         float *splineScore, int splineScoreJitter,
00891                         float splineScoreLengthRatio,
00892                         float splineScoreAngleRatio, float splineScoreStep,
00893                         vector<Spline> *prevSplines = NULL);
00894 
00906 void mcvGetRansacSplines(const CvMat *im, vector<Line> &lines,
00907                          vector<float> &lineScores, LaneDetectorConf *lineConf,
00908                          LineType lineType, vector<Spline> &splines,
00909                          vector<float> &splineScores, LineState* state);
00910 
00921 CvMat* mcvGetBezierSplinePixels(Spline &spline, float h, CvSize box,
00922                                 bool extendSpline=false);
00923 
00924 
00932 CvMat* mcvEvalBezierSpline(const Spline &spline, float h, CvMat *tangents=NULL);
00933 
00940 Spline mcvFitBezierSpline(CvMat *points, int degree);
00941 
00949 void mcvSortPoints(const CvMat *inPoints, CvMat *outPoints,
00950                    int dim, int dir);
00951 
00961 void mcvSampleWeighted(const CvMat *cumSum, int numSamples,
00962                        CvMat *randInd, CvRNG *rng);
00963 
00964 
00971 void mcvCumSum(const CvMat *inMat, CvMat *outMat);
00972 
00984 void mcvLocalizePoints(const CvMat *im, const CvMat *inPoints,
00985                        CvMat *outPoints, int numLinePixels,
00986                        float angleThreshold);
00987 
01002 float mcvGetLinePeak(const CvMat *im, const Line &line,
01003                      vector<CvPoint2D32f> &peaks, vector<float> &peakVals,
01004                      bool positivePeak=true, bool smoothPeaks=true);
01005 
01021 int mcvChooseBestPeak(const vector<CvPoint2D32f> &peaks,
01022                       const vector<float> &peakVals, CvPoint2D32f &peak,
01023                       float &peakVal, float contThreshold,
01024                       const CvPoint2D32f &tangent,
01025                       const CvPoint2D32f &prevPoint, float angleThreshold);
01026 
01043 CvMat*  mcvExtendPoints(const CvMat *im, const CvMat *inPoints,
01044                         float angleThreshold, float meanDirAngleThreshold,
01045                         int linePixelsTangent, int linePixelsNormal,
01046                         float contThreshold, int deviationThreshold,
01047                         CvRect bbox, bool smoothPeaks=true);
01048 
01058 Line mcvGetExtendedNormalLine(CvPoint2D32f &curPoint, CvPoint2D32f &tangent,
01059                               int linePixelsTangent, int linePixelsNormal,
01060                               CvPoint2D32f &nextPoint);
01061 
01072 bool mcvIsValidPeak(const CvPoint2D32f &peak, const CvPoint2D32f &tangent,
01073                     const CvPoint2D32f &prevPoint, float angleThreshold);
01074 
01075 
01076 
01084 void mcvGroupBoundingBoxes(vector<CvRect> &boxes, LineType type,
01085                            float groupThreshold);
01086 
01087 
01092 CvPoint2D32f mcvNormalizeVector(const CvPoint2D32f &v);
01093 
01098 CvPoint2D32f mcvNormalizeVector(const CvPoint &v);
01099 
01100 
01106 CvPoint2D32f mcvNormalizeVector(float x, float y);
01107 
01108 
01122 float mcvGetSplineScore(const CvMat* image, Spline& spline, float h,
01123                         int  jitterVal, float lengthRatio,
01124                         float angleRatio);
01125 
01134 vector<int> mcvGetJitterVector(int maxJitter);
01135 
01142 CvPoint2D32f mcvAddVector(CvPoint2D32f v1, CvPoint2D32f v2);
01143 
01152 CvPoint2D32f  mcvGetPointsMeanVector(const CvMat *points, bool forward = true);
01153 
01154 
01170 bool mcvCheckMergeSplines(const Spline& sp1, const Spline& sp2,
01171                           float thetaThreshold, float rThreshold,
01172                           float MeanThetaThreshold, float MeanRThreshold,
01173                           float centroidThreshold);
01174 
01175 
01193 void mcvGetSplineFeatures(const Spline& spline, CvPoint2D32f* centroid=0,
01194                           float* theta=0, float* r=0, float* length=0,
01195                           float* meanTheta=0, float* meanR=0,
01196                           float* curveness=0);
01197 
01215 void mcvGetPointsFeatures(const CvMat* points,
01216                           CvPoint2D32f* centroid=0,
01217                           float* theta=0, float* r=0,
01218                           float* length=0, float* meanTheta=0,
01219                           float* meanR=0, float* curveness=0);
01220 
01221 
01229 CvPoint2D32f  mcvSubtractVector(const CvPoint2D32f& v1, const CvPoint2D32f& v2);
01230 
01237 float  mcvGetVectorNorm(const CvPoint2D32f& v);
01238 
01239 
01250 bool mcvCheckMergeLines(const Line& line1, const Line& line2,
01251                         float thetaThreshold, float rThreshold);
01252 
01253 
01262 Spline mcvLineXY2Spline(const Line& line, int degree);
01263 
01270 bool mcvIsPointInside(FLOAT_POINT2D &point, const CvRect &rect);
01271 
01272 
01282 void mcvDrawText(CvMat *image, char* str, CvPoint point,
01283                  float size=.25f, CvScalar color=CV_RGB(1,1,1));
01284 
01297 int mcvCheckSpline(const Spline &spline, float curvenessThreshold,
01298                    float lengthThreshold, float thetaDiffThreshold,
01299                    float thetaThreshold);
01300 
01301 
01310 int mcvCheckPoints(const CvMat* points);
01311 
01319 float mcvGetLineAngle(const Line& line);
01320 
01326 void mcvGroupSplines(vector<Spline> &splines, vector<float> &scores);
01327 
01334 CvPoint2D32f mcvMultiplyVector(CvPoint2D32f v, float s);
01335 
01336 
01352 LineColor mcvGetPointsColor(const CvMat* im, const CvMat* points,
01353                             int window, float numYellowMin,
01354                             float rgMin, float rgMax,
01355                             float gbMin, float rbMin,
01356                             bool rbf, float rbfThreshold);
01357 
01365 void mcvGetSplinesBoundingBoxes(const vector<Spline> &splines, LineType type,
01366                                 CvSize size, vector<CvRect> &boxes);
01367 
01368 
01376 void mcvGetLinesBoundingBoxes(const vector<Line> &lines, LineType type,
01377                               CvSize size, vector<CvRect> &boxes);
01378 
01379 
01388 void mcvCheckLaneWidth(vector<Line> &lines, vector<float> &scores,
01389                        float wMu, float wSigma);
01390 
01391 } // namespace LaneDetector
01392 
01393 #endif /*LANEDETECTOR_HH_*/
/home/laradmin/lar/perception/road/caltech_lanes/src/LaneDetector.hh
