FSPoint FSVision::detectLaserLine( cv::Mat &laserOff, cv::Mat &laserOn, unsigned int threshold ) { unsigned int cols = laserOff.cols; unsigned int rows = laserOff.rows; cv::Mat laserLine = subLaser2(laserOff, laserOn); std::vector<cv::Vec4i> lines; double deltaRho = 1; double deltaTheta = M_PI/2; int minVote = 20; double minLength = 50; double maxGap = 10; cv::Mat laserLineBW( cols, rows, CV_8U, cv::Scalar(0) ); cv::cvtColor(laserLine, laserLineBW, CV_RGB2GRAY); //convert to grayscale /*cv::namedWindow("Detected Lines with HoughP"); cv::imshow("Detected Lines with HoughP",laserLineBW); cv::waitKey(0);*/ //cvDestroyWindow("Detected Lines with HoughP"); cv::HoughLinesP( laserLineBW, lines, deltaRho, deltaTheta, minVote, minLength, maxGap ); //should at least detect the laser line qDebug() << "detected"<<lines.size()<<"lines"; if(lines.size()==0){ qDebug("Did not detect any laser line, did you select a SerialPort form the menu?"); FSPoint p = FSMakePoint(0.0,0.0,0.0); return(p); } //assert(lines.size()>0); //for(int i=0;i<lines.size();i++){ cv::Point p1; cv::Point p2; for(unsigned int i=0;i<lines.size();i++){ qDebug() << "drawing line "<<lines[i][0]<<lines[i][1]<<lines[i][2]<<lines[i][3]; //int i = 0; p1.x = lines[i][0]; p1.y = lines[i][1]; p2.x = lines[i][2]; p2.y = lines[i][3]; cv::line(laserLine, p1, p2, CV_RGB( 255,0,0 ),1); //draw laser line } /*cv::namedWindow("Detected Lines with HoughP"); cv::imshow("Detected Lines with HoughP",laserLine); cv::waitKey(0); cvDestroyWindow("Detected Lines with HoughP");*/ FSPoint p = convertCvPointToFSPoint(p1); return p; }
FSPoint FSVision::detectLaserLine( cv::Mat &laserOff, cv::Mat &laserOn, unsigned int threshold, cv::Point *mP ) { IsFindLine=false; unsigned int cols = laserOff.cols; unsigned int rows = laserOff.rows; cv::Mat laserLine = subLaser2(laserOff, laserOn,1); std::vector<cv::Vec4i> lines; double deltaRho = 1; double deltaTheta = M_PI/2; int minVote = 20; double minLength = 50; double maxGap = 10; cv::Mat laserLineBW( cols, rows, CV_8U, cv::Scalar(0) ); cv::cvtColor(laserLine, laserLineBW, CV_RGB2GRAY); //convert to grayscale cv::Mat mask( cols/2, rows/2-100, CV_8U, cv::Scalar(0) ); cv::Rect rect(0, 0,cols/2, rows/2 - 100); laserLineBW(rect).copyTo(mask); //cv::imshow("extractedlaserLine3",mask); //cv::waitKey(100); // cv::namedWindow("Detected Lines with HoughP"); //cvDestroyWindow("Detected Lines with HoughP"); cv::HoughLinesP( mask, lines, deltaRho, deltaTheta, minVote, minLength, maxGap ); cv::putText(mask,"",cv::Point(10,10),6,6,cv::Scalar(255,0,0),1,8,false); cv::imshow("extractedlaserLine2",mask); //cv::waitKey(0); //should at least detect the laser line qDebug() << "detected"<<lines.size()<<"lines"; if(lines.size()==0) { qDebug("Did not detect any laser line, did you select a SerialPort form the menu?"); FSPoint p = FSMakePoint(0.0,0.0,0.0); mP[0].x =0; mP[0].y =0; return(p); } //assert(lines.size()>0); //for(int i=0;i<lines.size();i++){ cv::Point p1; cv::Point p2; for(int i=0; i<lines.size(); i++) { qDebug() << "drawing line "<<lines[i][0]<<lines[i][1]<<lines[i][2]<<lines[i][3]; //int i = 0; p1.x = lines[i][0]; p1.y = lines[i][1]; p2.x = lines[i][2]; p2.y = lines[i][3]; cv::line(laserLine, p1, p2, CV_RGB( 255,0,0 ),1); //draw laser line IsFindLine=true; } mP[0].x =p1.x; mP[0].y =p1.y; /*cv::namedWindow("Detected Lines with HoughP"); cv::imshow("Detected Lines with HoughP",laserLine); cv::waitKey(100); cvDestroyWindow("Detected Lines with HoughP"); */ FSPoint p = convertCvPointToFSPoint(p1); return p; }
void FSVision::putPointsFromFrameToCloud( cv::Mat &laserOff, cv::Mat &laserOn, int dpiVertical, //step between vertical points double lowerLimit, int JD) //remove points below this limit { //qDebug() << "putPointsFromFrameToCloud"; //the following lines are just to make to code more readable FSModel* model = FSController::getInstance()->model; FSLaser* laser = FSController::getInstance()->laser; FSTurntable* turntable = FSController::getInstance()->turntable; FSWebCam* webcam = FSController::getInstance()->webcam; //extract laser line from the two images //cv::Mat laserLine = subLaser(laserOff,laserOn,threshold); cv::Mat laserLine = subLaser2(laserOff,laserOn,JD); // cv::imshow("extractedlaserLine2",laserLine); // cv::waitKey(300) ; //calculate position of laser in cv frame //position of the laser line on the back plane in frame/image coordinates FSPoint fsLaserLinePosition = laser->getLaserPointPosition(); //position of the laser line on the back plane in world coordinates CvPoint cvLaserLinePosition = convertFSPointToCvPoint(fsLaserLinePosition); double laserPos = cvLaserLinePosition.x; //const over all y //laserLine is result of subLaser2, is in RGB unsigned int cols = laserLine.cols; unsigned int rows = laserLine.rows; //create new image in black&white cv::Mat bwImage( cols,rows,CV_8U,cv::Scalar(100) ); //qDebug("still works here"); //convert from rgb to b&w cv::cvtColor(laserLine, bwImage, CV_RGB2GRAY); //convert to grayscale cv::imshow("extractedlaserLine2",bwImage); cv::waitKey(1) ; //now iterating from top to bottom over bwLaserLine frame //no bear outside of these limits :) cutting of top and bottom of frame for(int y = UPPER_ANALYZING_FRAME_LIMIT; y < bwImage.rows-(LOWER_ANALYZING_FRAME_LIMIT); y+=dpiVertical ) { //qDebug() << "checking point at line " << y << laserPos+ANALYZING_LASER_OFFSET; //ANALYZING_LASER_OFFSET is the offset where we stop looking for a reflected laser, cos we might catch the non reflected //now iteratinf from right to left over bwLaserLine frame for(int x = bwImage.cols-1; x >= laserPos+ANALYZING_LASER_OFFSET; x -= 1) { //qDebug() << "Pixel value: " << bwImage.at<uchar>(y,x); if(bwImage.at<uchar>(y,x)==255) { //check if white=laser-reflection //qDebug() << "found point at x=" << x; //if (row[x] > 200){ //we have a white point in the grayscale image, so one edge laser line found //no we should continue to look for the other edge and then take the middle of those two points //to take the width of the laser line into account //position of the reflected laser line on the image coord CvPoint cvNewPoint; cvNewPoint.x = x; cvNewPoint.y = y; //convert to world coordinates withouth depth FSPoint fsNewPoint = FSVision::convertCvPointToFSPoint(cvNewPoint); FSLine l1 = computeLineFromPoints(webcam->getPosition(), fsNewPoint); FSLine l2 = computeLineFromPoints(laser->getPosition(), laser->getLaserPointPosition()); FSPoint i = computeIntersectionOfLines(l1, l2); fsNewPoint.x = i.x; fsNewPoint.z = i.z; //At this point we know the depth=z. Now we need to consider the scaling depending on the depth. //First we move our point to a camera centered cartesion system. fsNewPoint.y -= (webcam->getPosition()).y; fsNewPoint.y *= ((webcam->getPosition()).z - fsNewPoint.z)/(webcam->getPosition()).z; //Redo the translation to the box centered cartesion system. fsNewPoint.y += (webcam->getPosition()).y; //get color from picture without laser FSUChar r = laserOff.at<cv::Vec3b>(y,x)[2]; FSUChar g = laserOff.at<cv::Vec3b>(y,x)[1]; FSUChar b = laserOff.at<cv::Vec3b>(y,x)[0]; fsNewPoint.color = FSMakeColor(r, g, b); //turning new point according to current angle of turntable //translate coordinate system to the middle of the turntable fsNewPoint.z -= TURNTABLE_POS_Z; //7cm radius of turntbale plus 5mm offset from back plane FSPoint alphaDelta = turntable->getRotation(); double alphaOld = (float)atan(fsNewPoint.z/fsNewPoint.x); double alphaNew = alphaOld+alphaDelta.y*(M_PI/180.0f); double hypotenuse = (float)sqrt(fsNewPoint.x*fsNewPoint.x + fsNewPoint.z*fsNewPoint.z); if(fsNewPoint.z < 0 && fsNewPoint.x < 0) { alphaNew += M_PI; } else if(fsNewPoint.z > 0 && fsNewPoint.x < 0) { alphaNew -= M_PI; } fsNewPoint.z = (float)sin(alphaNew)*hypotenuse; fsNewPoint.x = (float)cos(alphaNew)*hypotenuse; if(fsNewPoint.y>lowerLimit+0.5 && hypotenuse < 7) { //eliminate points from the grounds, that are not part of the model //qDebug("adding point"); model->addPointToPointCloud(fsNewPoint); } break; } } } }