Example #1
0
 /**Static version (all in one)
  */
 Board BoardDetector::detect ( const cv::Mat &Image, const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters ) {
   BoardDetector BD;
   BD.setParams ( bc,cp,markerSizeMeters );
   BD.detect ( Image );
   return BD.getDetectedBoard();
 }
int main(int argc,char **argv)
{
    try
    {
        if (  readArguments (argc,argv)==false) return 0;
//parse arguments
        TheBoardConfig.readFromFile(TheBoardConfigFile);
        //read from camera or from  file
        if (TheInputVideo=="live") {
            TheVideoCapturer.open(0);
            waitTime=10;
        }
        else TheVideoCapturer.open(TheInputVideo);
        //check video is open
        if (!TheVideoCapturer.isOpened()) {
            cerr<<"Could not open video"<<endl;
            return -1;

        }

        //read first image to get the dimensions
        TheVideoCapturer>>TheInputImage;

        //Open outputvideo
        if ( TheOutVideoFilePath!="")
            VWriter.open(TheOutVideoFilePath,CV_FOURCC('M','J','P','G'),15,TheInputImage.size());

        //read camera parameters if passed
        if (TheIntrinsicFile!="") {
            TheCameraParameters.readFromXMLFile(TheIntrinsicFile);
            TheCameraParameters.resize(TheInputImage.size());
        }

        //Create gui

        cv::namedWindow("thres",1);
        cv::namedWindow("in",1);
	TheBoardDetector.setParams(TheBoardConfig,TheCameraParameters,TheMarkerSize);
	TheBoardDetector.getMarkerDetector().getThresholdParams( ThresParam1,ThresParam2);
	TheBoardDetector.getMarkerDetector().enableErosion(true);//for chessboards
        iThresParam1=ThresParam1;
        iThresParam2=ThresParam2;
        cv::createTrackbar("ThresParam1", "in",&iThresParam1, 13, cvTackBarEvents);
        cv::createTrackbar("ThresParam2", "in",&iThresParam2, 13, cvTackBarEvents);
        char key=0;
        int index=0;
        //capture until press ESC or until the end of the video
        while ( key!=27 && TheVideoCapturer.grab())
        {
            TheVideoCapturer.retrieve( TheInputImage);
            TheInputImage.copyTo(TheInputImageCopy);
            index++; //number of images captured
            double tick = (double)getTickCount();//for checking the speed
            //Detection of the board
            float probDetect=TheBoardDetector.detect(TheInputImage);
            //chekc the speed by calculating the mean speed of all iterations
            AvrgTime.first+=((double)getTickCount()-tick)/getTickFrequency();
            AvrgTime.second++;
            cout<<"Time detection="<<1000*AvrgTime.first/AvrgTime.second<<" milliseconds"<<endl;
            //print marker borders
            for (unsigned int i=0;i<TheBoardDetector.getDetectedMarkers().size();i++)
                TheBoardDetector.getDetectedMarkers()[i].draw(TheInputImageCopy,Scalar(0,0,255),1);

            //print board
             if (TheCameraParameters.isValid()) {
                if ( probDetect>0.2)   {
                    CvDrawingUtils::draw3dAxis( TheInputImageCopy,TheBoardDetector.getDetectedBoard(),TheCameraParameters);
                    //draw3dBoardCube( TheInputImageCopy,TheBoardDetected,TheIntriscCameraMatrix,TheDistorsionCameraParams);
                }
            }
            //DONE! Easy, right?

            //show input with augmented information and  the thresholded image
            cv::imshow("in",TheInputImageCopy);
            cv::imshow("thres",TheBoardDetector.getMarkerDetector().getThresholdedImage());
            //write to video if required
            if (  TheOutVideoFilePath!="") {
                //create a beautiful compiosed image showing the thresholded
                //first create a small version of the thresholded image
                cv::Mat smallThres;
                cv::resize( TheBoardDetector.getMarkerDetector().getThresholdedImage(),smallThres,cvSize(TheInputImageCopy.cols/3,TheInputImageCopy.rows/3));
                cv::Mat small3C;
                cv::cvtColor(smallThres,small3C,CV_GRAY2BGR);
                cv::Mat roi=TheInputImageCopy(cv::Rect(0,0,TheInputImageCopy.cols/3,TheInputImageCopy.rows/3));
                small3C.copyTo(roi);
                VWriter<<TheInputImageCopy;
// 			 cv::imshow("TheInputImageCopy",TheInputImageCopy);

            }

            key=cv::waitKey(waitTime);//wait for key to be pressed
            processKey(key);
        }


    } catch (std::exception &ex)

    {
        cout<<"Exception :"<<ex.what()<<endl;
    }

}
		void image_callback(const sensor_msgs::ImageConstPtr& msg)
		{
            static tf::TransformBroadcaster br;
            
			if(!cam_info_received) return;

			cv_bridge::CvImagePtr cv_ptr;
			try
			{
				cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
				inImage = cv_ptr->image;
				resultImg = cv_ptr->image.clone();

				//detection results will go into "markers"
				markers.clear();
				//Ok, let's detect
				mDetector.detect(inImage, markers, camParam, marker_size, false);
				//Detection of the board
				float probDetect=the_board_detector.detect(markers, the_board_config, the_board_detected, camParam, marker_size);
				if (probDetect > 0.0)
				{
					foundBoard = true; //added///////////////////////
					tf::Transform transform = ar_sys::getTf(the_board_detected.Rvec, the_board_detected.Tvec);

					tf::StampedTransform stampedTransform(transform, msg->header.stamp, msg->header.frame_id, board_frame);
                    
                    			if (publish_tf) 
						br.sendTransform(stampedTransform);

					geometry_msgs::PoseStamped poseMsg;
					tf::poseTFToMsg(transform, poseMsg.pose);
					poseMsg.header.frame_id = msg->header.frame_id;
					poseMsg.header.stamp = msg->header.stamp;
					pose_pub.publish(poseMsg);

					geometry_msgs::TransformStamped transformMsg;
					tf::transformStampedTFToMsg(stampedTransform, transformMsg);
					transform_pub.publish(transformMsg);

					geometry_msgs::Vector3Stamped positionMsg;
					positionMsg.header = transformMsg.header;
					positionMsg.vector = transformMsg.transform.translation;
					position_pub.publish(positionMsg);
					
					std_msgs::Float32 boardSizeMsg;
					boardSizeMsg.data=the_board_detected[0].ssize;
					boardSize_pub.publish(boardSizeMsg);				
					
									
				}
				//ADDED////////////////////////////////////////////////////////////////////////////
				if(rvec_pub.getNumSubscribers() > 0 && foundBoard)
				{
					cv_bridge::CvImage rvecMsg;
					rvecMsg.header.frame_id = msg->header.frame_id;
					rvecMsg.header.stamp = msg->header.stamp;
					rvecMsg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
					rvecMsg.image = the_board_detected[0].Rvec;
					rvec_pub.publish(rvecMsg.toImageMsg());
				}
	
				if(tvec_pub.getNumSubscribers() > 0 && foundBoard)
				{
					cv_bridge::CvImage tvecMsg;
					tvecMsg.header.frame_id = msg->header.frame_id;
					tvecMsg.header.stamp = msg->header.stamp;
					tvecMsg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
					tvecMsg.image = the_board_detected[0].Tvec;
					tvec_pub.publish(tvecMsg.toImageMsg());
				}
				///////////////////////////////////////////////////////////////////////////////
				
				//for each marker, draw info and its boundaries in the image
				for(size_t i=0; draw_markers && i < markers.size(); ++i)
				{
					
					markers[i].draw(resultImg,cv::Scalar(0,0,255),2);
				}


				if(camParam.isValid() && marker_size != -1)
				{
					//draw a 3d cube in each marker if there is 3d info
					for(size_t i=0; i<markers.size(); ++i)
					{
						if (draw_markers_cube) CvDrawingUtils::draw3dCube(resultImg, markers[i], camParam);
						if (draw_markers_axis) CvDrawingUtils::draw3dAxis(resultImg, markers[i], camParam);
					}
					//draw board axis
					if (probDetect > 0.0)
					{ 
						CvDrawingUtils::draw3dAxis(resultImg, the_board_detected, camParam);
						
					}
				}

				if(image_pub.getNumSubscribers() > 0)
				{
					//show input with augmented information
					cv_bridge::CvImage out_msg;
					out_msg.header.frame_id = msg->header.frame_id;
					out_msg.header.stamp = msg->header.stamp;
					out_msg.encoding = sensor_msgs::image_encodings::RGB8;
					out_msg.image = resultImg;
					image_pub.publish(out_msg.toImageMsg());
				}

				if(debug_pub.getNumSubscribers() > 0)
				{
					//show also the internal image resulting from the threshold operation
					cv_bridge::CvImage debug_msg;
					debug_msg.header.frame_id = msg->header.frame_id;
					debug_msg.header.stamp = msg->header.stamp;
					debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
					debug_msg.image = mDetector.getThresholdedImage();
					debug_pub.publish(debug_msg.toImageMsg());
				}
			}
			catch (cv_bridge::Exception& e)
			{
				ROS_ERROR("cv_bridge exception: %s", e.what());
				return;
			}
		}
		void image_callback(const sensor_msgs::ImageConstPtr& msg)
		{
			if(!cam_info_received) return;

			cv_bridge::CvImagePtr cv_ptr;
			try
			{
				cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
				inImage = cv_ptr->image;
				resultImg = cv_ptr->image.clone();

				//detection results will go into "markers"
				markers.clear();

				//Ok, let's detect
				double min_size = boards[0].marker_size;
				for (int board_index = 1; board_index < boards.size(); board_index++)
					if (min_size > boards[board_index].marker_size) min_size = boards[board_index].marker_size;
				mDetector.detect(inImage, markers, camParam, min_size, false);


				for (int board_index = 0; board_index < boards.size(); board_index++)
				{
					Board board_detected;

					//Detection of the board
					float probDetect = the_board_detector.detect(markers, boards[board_index].config, board_detected, camParam, boards[board_index].marker_size);
					if (probDetect > 0.0)
					{
						tf::Transform transform = ar_sys::getTf(board_detected.Rvec, board_detected.Tvec);

						tf::StampedTransform stampedTransform(transform, ros::Time::now(), "apollon_cam_right", boards[board_index].name +"_right");

						//_tfBraodcaster.sendTransform(stampedTransform);		// from phillip

						/*geometry_msgs::PoseStamped poseMsg;
						tf::poseTFToMsg(transform, poseMsg.pose);
						poseMsg.header.frame_id = msg->header.frame_id;
						poseMsg.header.stamp = msg->header.stamp;
						pose_pub.publish(poseMsg);*/

						geometry_msgs::TransformStamped transformMsg;
						tf::transformStampedTFToMsg(stampedTransform, transformMsg);
						transform_pub.publish(transformMsg);

						/*geometry_msgs::Vector3Stamped positionMsg;
						positionMsg.header = transformMsg.header;
						positionMsg.vector = transformMsg.transform.translation;
						position_pub.publish(positionMsg);*/

						if(camParam.isValid())
						{
							//draw board axis
							CvDrawingUtils::draw3dAxis(resultImg, board_detected, camParam);
						}
					}
				}

				//for each marker, draw info and its boundaries in the image
				for(size_t i=0; draw_markers && i < markers.size(); ++i)
				{
					markers[i].draw(resultImg,cv::Scalar(0,0,255),2);
				}

				if(camParam.isValid())
				{
					//draw a 3d cube in each marker if there is 3d info
					for(size_t i=0; i<markers.size(); ++i)
					{
						if (draw_markers_cube) CvDrawingUtils::draw3dCube(resultImg, markers[i], camParam);
						if (draw_markers_axis) CvDrawingUtils::draw3dAxis(resultImg, markers[i], camParam);
					}
				}

				if(image_pub.getNumSubscribers() > 0)
				{
					//show input with augmented information
					cv_bridge::CvImage out_msg;
					out_msg.header.frame_id = msg->header.frame_id;
					out_msg.header.stamp = msg->header.stamp;
					out_msg.encoding = sensor_msgs::image_encodings::RGB8;
					out_msg.image = resultImg;
					image_pub.publish(out_msg.toImageMsg());
				}

				if(debug_pub.getNumSubscribers() > 0)
				{
					//show also the internal image resulting from the threshold operation
					cv_bridge::CvImage debug_msg;
					debug_msg.header.frame_id = msg->header.frame_id;
					debug_msg.header.stamp = msg->header.stamp;
					debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
					debug_msg.image = mDetector.getThresholdedImage();
					debug_pub.publish(debug_msg.toImageMsg());
				}
			}
			catch (cv_bridge::Exception& e)
			{
				ROS_ERROR("cv_bridge exception: %s", e.what());
				return;
			}
		}
void MainWindow::on_pushButton_2_clicked()
{


    std::string casen = "clutter";
    bool saveimages = true;
    global::doDraw = true;
    std::string reportPath = "/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/";
    //std::string filename =  "Report_" + utils::currentDateTime();

    //if (ui->UseDefaultglobal::image_imp->isChecked()){
    if (!global::image_rgb.data){
        //global::image_rgb = cv::imread("/Users/benedicte/Dropbox/kings/thesis/images/checkers7.jpg");
        //global::image_rgb = cv::imread("/Users/benedicte/Dropbox/kings/thesis/report/boards/green2.jpg");
        //global::image_rgb = cv::imread("/Users/benedicte/Dropbox/kings/thesis/report/pieces/red.jpg");
        //global::image_rgb = cv::imread("/Users/benedicte/Dropbox/kings/thesis/report/casestudy/casestudy.jpg");
        global::image_rgb = cv::imread("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/"+casen+".jpg");

        if (global::image_rgb.data){
            cvutils::rotate(global::image_rgb,global::image_rgb,-90);
        } else {
            throw std::invalid_argument("Image could not be read");
        }
    }


    // Set up global image variables
     Preprocess prep;
     Settings::PreprocessSettings settings;
     settings.gaussianBlurSigma = 3;
     settings.gaussianBlurSize = cv::Size(3,3);

     // print image channels
     if (saveimages){
         cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/image_r.png", global::image_r);
         cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/image_g.png", global::image_g);
         cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/image_b.png", global::image_b);
     }
    // chessboard detector
    Lines houghlines;

    Board board; // container for the detected board
    bool tryAgain = true;
    bool boardDetected = false;


    while (tryAgain){
        prep.detectLines(settings);
        prep.getLines(houghlines);
        BoardDetector cbd = BoardDetector(houghlines);
        prep.showCanny();
        prep.showHoughlines();

        if (saveimages){
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/rgb.png", global::image_rgb);
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/gray.png", global::image_gray);
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/normalized.png", global::image_norm);
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/resized.png", global::image);
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/canny.png", prep.getCanny());
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/hough.png", prep.getHough());
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/blurred.png", prep.getBlurred());
            cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/hough_mod.png", global::image_hough_mod);
        }

        try{
            std::cout << "Trying to detect board with blur sigma " << settings.gaussianBlurSigma << std::endl;

            boardDetected = cbd.detect(board, &reportPath);


        } catch(std::exception &e){
        }

        if (boardDetected){
            tryAgain = false;
        } else {
            if (settings.gaussianBlurSigma > 1)
                settings.gaussianBlurSigma -= 1;
            if (settings.gaussianBlurSigma == 1){
                settings.gaussianBlurSize = cv::Size(3,3);
            }
            if (settings.gaussianBlurSize == cv::Size(3,3) && settings.gaussianBlurSigma == 1){
                settings.gaussianBlurSize = cv::Size(1,1);
            }
            if (settings.gaussianBlurSigma == 1 && settings.gaussianBlurSize == cv::Size(1,1) && settings.cannyLow > 4){
                settings.cannyLow -= 4;
            }
            if (settings.gaussianBlurSigma == 1 && settings.gaussianBlurSize == cv::Size(1,1) && settings.cannyLow <= 4){
                std::cout << "I give up" << std::endl;
            }
        }
    }


    // save images


    if (saveimages){
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/rgb.png", global::image_rgb);
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/gray.png", global::image_gray);
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/normalized.png", global::image_norm);
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/resized.png", global::image);
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/canny.png", prep.getCanny());
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/hough.png", prep.getHough());
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/blurred.png", prep.getBlurred());
    }
    if (saveimages){

        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/hough_mod.png", global::image_hough_mod);
        board.write("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/final.png");
    }


    board.draw();
    cv::destroyAllWindows();
    board.detectPieces();

    if (global::doDraw) board.drawWithPieces();
    board.writeImgWithPiecesToGlobal();
    if (saveimages){
        cv::imwrite("/Users/benedicte/Dropbox/kings/thesis/report/"+casen+"/pieces.png", global::image_pieces);
    }
    // Initial state to feed minimax
    cv::destroyAllWindows();

    State state = board.initState();

    std::cout << "FINISHED" << std::endl;
    // WRITE REPORTS
}
Example #6
0
void vDrawScene() {
    if (TheResizedImage.rows == 0) // prevent from going on until the image is initialized
        return;
    /// clear
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    /// draw image in the buffer
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, TheGlWindowSize.width, 0, TheGlWindowSize.height, -1.0, 1.0);
    glViewport(0, 0, TheGlWindowSize.width, TheGlWindowSize.height);
    glDisable(GL_TEXTURE_2D);
    glPixelZoom(1, -1);
    glRasterPos3f(0, TheGlWindowSize.height - 0.5, -1.0);
    glDrawPixels(TheGlWindowSize.width, TheGlWindowSize.height, GL_RGB, GL_UNSIGNED_BYTE,
                 TheResizedImage.ptr(0));
    /// Set the appropriate projection matrix so that rendering is done in a enrvironment
    // like the real camera (without distorsion)
    glMatrixMode(GL_PROJECTION);
    double proj_matrix[16];
    TheCameraParams.glGetProjectionMatrix(TheInputImage.size(), TheGlWindowSize, proj_matrix, 0.05, 10);
    glLoadIdentity();
    glLoadMatrixd(proj_matrix);
    glLineWidth(2);
    // now, for each marker,
    double modelview_matrix[16];

    /*    for (unsigned int m=0;m<TheMarkers.size();m++)
        {
            TheMarkers[m].glGetModelViewMatrix(modelview_matrix);
            glMatrixMode(GL_MODELVIEW);
            glLoadIdentity();
            glLoadMatrixd(modelview_matrix);
    // 		axis(TheMarkerSize);
            glColor3f(1,0.4,0.4);
            glTranslatef(0, TheMarkerSize/2,0);
            glPushMatrix();
            glutWireCube( TheMarkerSize );

            glPopMatrix();
        }*/
    // If the board is detected with enough probability
    if (TheBoardDetected.second > 0.3) {
        TheBoardDetected.first.glGetModelViewMatrix(modelview_matrix);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        glLoadMatrixd(modelview_matrix);
        glColor3f(0, 1, 0);
        axis(TheMarkerSize);
        if (TheBoardDetector.isYPerpendicular())
            glTranslatef(0, TheMarkerSize / 2, 0);
        else
            glTranslatef(0, 0, TheMarkerSize / 2);
        glPushMatrix();
        glutWireCube(TheMarkerSize);
        glPopMatrix();
    }

    glutSwapBuffers();
}