C++ BoardConfiguration类代码示例

int main(int argc,char **argv)
{
	try
	{
		if(argc<3) {cerr<<"Usage: image  boardConfig.yml [cameraParams.yml] [markerSize]  [outImage]"<<endl;exit(0);}
		aruco::CameraParameters CamParam;
		MarkerDetector MDetector;
		vector<aruco::Marker> Markers;
		float MarkerSize=-1;
		BoardConfiguration TheBoardConfig;
		BoardDetector TheBoardDetector;
		Board TheBoardDetected;
		
		cv::Mat InImage=cv::imread(argv[1]);
		TheBoardConfig.readFromFile(argv[2]);
		if (argc>=4) {
		  CamParam.readFromXMLFile(argv[3]);
		  //resizes the parameters to fit the size of the input image
		  CamParam.resize( InImage.size());
		}

		if (argc>=5) 
		  MarkerSize=atof(argv[4]);
		
		cv::namedWindow("in",1);
		MDetector.detect(InImage,Markers);//detect markers without computing R and T information
		//Detection of the board
		float probDetect=TheBoardDetector.detect( Markers, TheBoardConfig,TheBoardDetected, CamParam,MarkerSize);
		
		//for each marker, draw info and its boundaries in the image
		for(unsigned int i=0;i<Markers.size();i++){
			cout<<Markers[i]<<endl;
			Markers[i].draw(InImage,Scalar(0,0,255),2);
		}

		//draw a 3d cube in each marker if there is 3d info
		if (  CamParam.isValid()){
 		  for(unsigned int i=0;i<Markers.size();i++){
 		    CvDrawingUtils::draw3dCube(InImage,Markers[i],CamParam);
 		    CvDrawingUtils::draw3dAxis(InImage,Markers[i],CamParam);
 		  }
		  CvDrawingUtils::draw3dAxis(InImage,TheBoardDetected,CamParam);
		  cout<<TheBoardDetected.Rvec<<" "<<TheBoardDetected.Tvec<<endl;
		}
		//draw board axis
		
		//show input with augmented information
		cv::imshow("in",InImage);
		cv::waitKey(0);//wait for key to be pressed
		if(argc>=6) cv::imwrite(argv[5],InImage);
		
	}catch(std::exception &ex)

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

}

float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix, Mat distCoeff,
                            float markerSizeMeters) throw(cv::Exception) {
    if (BConf.size() == 0)
        throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty", __FILE__, __LINE__);
    if (BConf[0].size() < 2)
        throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty 2", __FILE__, __LINE__);
    // compute the size of the markers in meters, which is used for some routines(mostly drawing)
    float ssize;
    if (BConf.mInfoType == BoardConfiguration::PIX && markerSizeMeters > 0)
        ssize = markerSizeMeters;
    else if (BConf.mInfoType == BoardConfiguration::METERS) {
        ssize = cv::norm(BConf[0][0] - BConf[0][1]);
    }

    // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
    Bdetected.clear();
    /// find among detected markers these that belong to the board configuration
    for (unsigned int i = 0; i < detectedMarkers.size(); i++) {
        int idx = BConf.getIndexOfMarkerId(detectedMarkers[i].id);
        if (idx != -1) {
            Bdetected.push_back(detectedMarkers[i]);
            Bdetected.back().ssize = ssize;
        }
    }
    // copy configuration
    Bdetected.conf = BConf;
    //

    bool hasEnoughInfoForRTvecCalculation = false;
    if (Bdetected.size() >= 1) {
        if (camMatrix.rows != 0) {
            if (markerSizeMeters > 0 && BConf.mInfoType == BoardConfiguration::PIX)
                hasEnoughInfoForRTvecCalculation = true;
            else if (BConf.mInfoType == BoardConfiguration::METERS)
                hasEnoughInfoForRTvecCalculation = true;
        }
    }

    // calculate extrinsic if there is information for that
    if (hasEnoughInfoForRTvecCalculation) {

        // calculate the size of the markers in meters if expressed in pixels
        double marker_meter_per_pix = 0;
        if (BConf.mInfoType == BoardConfiguration::PIX)
            marker_meter_per_pix = markerSizeMeters / cv::norm(BConf[0][0] - BConf[0][1]);
        else
            marker_meter_per_pix = 1; // to avoind interferring the process below

        // now, create the matrices for finding the extrinsics
        vector< cv::Point3f > objPoints;
        vector< cv::Point2f > imagePoints;
        for (size_t i = 0; i < Bdetected.size(); i++) {
            int idx = Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
            assert(idx != -1);
            for (int p = 0; p < 4; p++) {
                imagePoints.push_back(Bdetected[i][p]);
                const aruco::MarkerInfo &Minfo = Bdetected.conf.getMarkerInfo(Bdetected[i].id);
                objPoints.push_back(Minfo[p] * marker_meter_per_pix);
                //  		cout<<objPoints.back()<<endl;
            }
        }
        if (distCoeff.total() == 0)
            distCoeff = cv::Mat::zeros(1, 4, CV_32FC1);

        // 	    for(size_t i=0;i< imagePoints.size();i++){
        // 		cout<<objPoints[i]<<" "<<imagePoints[i]<<endl;
        // 	    }
        // 	    cout<<"cam="<<camMatrix<<" "<<distCoeff<<endl;
        cv::Mat rvec, tvec;
        cv::solvePnP(objPoints, imagePoints, camMatrix, distCoeff, rvec, tvec);
        rvec.convertTo(Bdetected.Rvec, CV_32FC1);
        tvec.convertTo(Bdetected.Tvec, CV_32FC1);
        //             cout<<rvec<< " "<<tvec<<" _setYPerpendicular="<<_setYPerpendicular<<endl;

        {
            vector< cv::Point2f > reprojected;
            cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);
            double errSum = 0;
            // check now the reprojection error and
            for (size_t i = 0; i < reprojected.size(); i++) {
                errSum += cv::norm(reprojected[i] - imagePoints[i]);
            }
            //                  cout<<"AAA RE="<<errSum/double ( reprojected.size() ) <<endl;
        }
        // now, do a refinement and remove points whose reprojection error is above a threshold, then repeat calculation with the rest
        if (repj_err_thres > 0) {
            vector< cv::Point2f > reprojected;
            cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);

            vector< int > pointsThatPassTest; // indices
            // check now the reprojection error and
            for (size_t i = 0; i < reprojected.size(); i++) {
                float err = cv::norm(reprojected[i] - imagePoints[i]);
                if (err < repj_err_thres)
                    pointsThatPassTest.push_back(i);
            }
            // cerr<<"Number of points after reprjection test "<<pointsThatPassTest.size() <<"/"<<objPoints.size() <<endl;
            // copy these data to another vectors and repeat
            vector< cv::Point3f > objPoints_filtered;
            vector< cv::Point2f > imagePoints_filtered;
//.........这里部分代码省略.........

float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const  BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters ) throw ( cv::Exception )
{
    if (BConf.size()==0) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty",__FILE__,__LINE__);
    if (BConf[0].size()<2) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty 2",__FILE__,__LINE__);
    //compute the size of the markers in meters, which is used for some routines(mostly drawing)
    float ssize;
    if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters;
    else if ( BConf.mInfoType==BoardConfiguration::METERS )
    {
        ssize=cv::norm ( BConf[0][0]-BConf[0][1] );
    }

    // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
    Bdetected.clear();
    ///find among detected markers these that belong to the board configuration
    for ( unsigned int i=0;i<detectedMarkers.size();i++ )
    {
        int idx=BConf.getIndexOfMarkerId(detectedMarkers[i].id);
        if (idx!=-1) {
            Bdetected.push_back ( detectedMarkers[i] );
            Bdetected.back().ssize=ssize;
        }
    }
    //copy configuration
    Bdetected.conf=BConf;
//

    bool hasEnoughInfoForRTvecCalculation=false;
    if ( Bdetected.size() >=1 )
    {
        if ( camMatrix.rows!=0 )
        {
            if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=true;
            else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=true;
        }
    }

//calculate extrinsic if there is information for that
    if ( hasEnoughInfoForRTvecCalculation )
    {

        //calculate the size of the markers in meters if expressed in pixels
        double marker_meter_per_pix=0;
        if ( BConf.mInfoType==BoardConfiguration::PIX ) marker_meter_per_pix=markerSizeMeters /  cv::norm ( BConf[0][0]-BConf[0][1] );
        else marker_meter_per_pix=1;//to avoind interferring the process below

        // now, create the matrices for finding the extrinsics
        Mat objPoints ( 4*Bdetected.size(),3,CV_32FC1 );
        Mat imagePoints ( 4*Bdetected.size(),2,CV_32FC1 );
        //size in meters of inter-marker distance

        for ( size_t i=0;i<Bdetected.size();i++ )
        {
            int idx=Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
            assert(idx!=-1);
            for ( int p=0;p<4;p++ )
            {
                imagePoints.at<float> ( ( i*4 ) +p,0 ) =Bdetected[i][p].x;
                imagePoints.at<float> ( ( i*4 ) +p,1 ) =Bdetected[i][p].y;
                const aruco::MarkerInfo &Minfo=Bdetected.conf.getMarkerInfo( Bdetected[i].id);

                objPoints.at<float> ( ( i*4 ) +p,0 ) = Minfo[p].x*marker_meter_per_pix;
                objPoints.at<float> ( ( i*4 ) +p,1 ) = Minfo[p].y*marker_meter_per_pix;
                objPoints.at<float> ( ( i*4 ) +p,2 ) = Minfo[p].z*marker_meter_per_pix;
// 		cout<<objPoints.at<float>( (i*4)+p,0)<<" "<<objPoints.at<float>( (i*4)+p,1)<<" "<<objPoints.at<float>( (i*4)+p,2)<<endl;
            }
        }
        if (distCoeff.total()==0) distCoeff=cv::Mat::zeros(1,4,CV_32FC1 );

        cv::Mat rvec,tvec;
        cv::solvePnP(objPoints,imagePoints,camMatrix,distCoeff,rvec,tvec );
        rvec.convertTo(Bdetected.Rvec,CV_32FC1);
        tvec.convertTo(Bdetected.Tvec,CV_32FC1);
        //now, rotate 90 deg in X so that Y axis points up
        if (_setYPerperdicular)
            rotateXAxis ( Bdetected.Rvec );
//         cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
//         cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
    }

    float prob=float( Bdetected.size() ) /double ( Bdetected.conf.size() );
    return prob;
}

		ArSysSingleBoard()
			: cam_info_received(false),
			foundBoard(false),
			nh("~"),
			it(nh)
		{
			image_sub = it.subscribe("/image", 1, &ArSysSingleBoard::image_callback, this);
			cam_info_sub = nh.subscribe("/camera_info", 1, &ArSysSingleBoard::cam_info_callback, this);

			image_pub = it.advertise("result", 1);
			debug_pub = it.advertise("debug", 1);
			//Added/////////////////////////////////////////////////////////////////
			rvec_pub = it.advertise("rvec",1);
			tvec_pub = it.advertise("tvec",1);
			boardSize_pub = nh.advertise<std_msgs::Float32>("boardSize", 100);
			///////////////////////////////////////////////////////////////////
			pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
			transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
			position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
			
	
			nh.param<double>("marker_size", marker_size, 0.05);
			nh.param<std::string>("board_config", board_config, "boardConfiguration.yml");
			nh.param<std::string>("board_frame", board_frame, "");
			nh.param<bool>("image_is_rectified", useRectifiedImages, true);
			nh.param<bool>("draw_markers", draw_markers, false);
			nh.param<bool>("draw_markers_cube", draw_markers_cube, false);
			nh.param<bool>("draw_markers_axis", draw_markers_axis, false);
            		nh.param<bool>("publish_tf", publish_tf, false);

			the_board_config.readFromFile(board_config.c_str());

			ROS_INFO("Modified ArSys node started with marker size of %f m and board configuration: %s",
					 marker_size, board_config.c_str());
		}

int main(int argc, char** argv) {
    try {
        if (readArguments(argc, argv) == false)
            return 0;

        // read board configuration
        TheBoardConfig.readFromFile(TheBoardConfigFile);

        // Open video input source
        if (TheInputVideo == "") // read from camera
            TheVideoCapturer.open(0);
        else
            TheVideoCapturer.open(TheInputVideo);
        if (!TheVideoCapturer.isOpened()) {
            cerr << "Could not open video" << endl;
            return -1;
        }

        // read first image
        TheVideoCapturer >> TheInputImage;
        // read camera paramters if passed
        TheCameraParams.readFromXMLFile(TheIntrinsicFile);
        TheCameraParams.resize(TheInputImage.size());

        TheBoardDetector.getMarkerDetector().setThresholdParams(25, 7);

        glutInit(&argc, argv);
        glutInitWindowPosition(0, 0);
        glutInitWindowSize(TheInputImage.size().width, TheInputImage.size().height);
        glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
        glutCreateWindow("ArUco");
        glutDisplayFunc(vDrawScene);
        glutIdleFunc(vIdle);
        glutReshapeFunc(vResize);
        glutMouseFunc(vMouse);
        glClearColor(0.0, 0.0, 0.0, 1.0);
        glClearDepth(1.0);

        // these two are necesary for the mask effect
        glEnable(GL_ALPHA_TEST);
        glAlphaFunc(GL_GREATER, 0.5);

        TheGlWindowSize = TheInputImage.size();
        vResize(TheGlWindowSize.width, TheGlWindowSize.height);
        glutMainLoop();

    } catch (std::exception& ex)

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

int main(int argc,char **argv)
{
    try
    {
        if (readArguments (argc,argv)==false) return 0;

        //read board configuration
        TheBoardConfig.readFromFile(TheBoardConfigFile);

        //Open video input source
        if (TheInputVideo=="live")  //read from camera
            TheVideoCapturer.open(0);
        else TheVideoCapturer.open(TheInputVideo);
        if (!TheVideoCapturer.isOpened())
        {
            cerr<<"Could not open video"<<endl;
            return -1;

        }

        //read first image
        TheVideoCapturer>>TheInputImage;
        //read camera paramters if passed
        TheCameraParams.readFromXMLFile(TheIntrinsicFile);
        TheCameraParams.resize( TheInputImage.size());

		//init glut information and init
        glutInit(&argc, argv);
        glutInitWindowPosition( 0, 0);
        glutInitWindowSize(TheInputImage.size().width,TheInputImage.size().height);
        glutInitDisplayMode( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE );
        glutCreateWindow( "AruCo" );
        glutDisplayFunc( vDrawScene );
        glutIdleFunc( vIdle );
        glutReshapeFunc( vResize );
        glutMouseFunc(vMouse);
        glClearColor( 0.0, 0.0, 0.0, 1.0 );
        glClearDepth( 1.0 );
        TheGlWindowSize=TheInputImage.size();
        vResize(TheGlWindowSize.width,TheGlWindowSize.height);
        glutMainLoop();

    } catch (std::exception &ex)

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

}

/*!
 *  
 */
int main(int argc,char **argv)
{
  readArguments (argc,argv);

  //read board configuration
  boardConfig.readFromFile(bcon->sval[0]);

  //Open video input source
  if (inp->count==0 || strcmp(inp->sval[0], "live")==0)
  {
    //read from camera
    vCapturer.open(0);

    vCapturer.set(CV_CAP_PROP_FRAME_WIDTH, wid->ival[0]);
    vCapturer.set(CV_CAP_PROP_FRAME_HEIGHT, hei->ival[0]);
    int val = CV_FOURCC('M', 'P', 'E', 'G');
    vCapturer.set(CV_CAP_PROP_FOURCC, val);
  }
  else
    vCapturer.open(inp->sval[0]);

  if (!vCapturer.isOpened())
  {
    std::cerr<<"Could not open video"<<std::endl;
    return -1;
  }

  //read first image
  vCapturer>>inImg;
  //read camera paramters if passed
  camParams.readFromXMLFile(ints->sval[0]);
  camParams.resize( inImg.size());

  glutInit(&argc, argv);
  glutInitWindowPosition( 0, 0);
  glutInitWindowSize(inImg.size().width,inImg.size().height);
  glutInitDisplayMode( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE );
  glutCreateWindow( "AruCo" );
  glutDisplayFunc( vDrawScene );
  glutIdleFunc( vIdle );
  glutReshapeFunc( vResize );
  glutMouseFunc(vMouse);
  glClearColor( 0.0, 0.0, 0.0, 1.0 );
  glClearDepth( 1.0 );
  glSize=inImg.size();
  vResize(glSize.width,glSize.height);
  glutMainLoop();
}

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;
    }
//.........这里部分代码省略.........