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;
}
}
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();
}
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;
for (size_t i = 0; i < pointsThatPassTest.size(); i++) {
objPoints_filtered.push_back(objPoints[pointsThatPassTest[i]]);
imagePoints_filtered.push_back(imagePoints[pointsThatPassTest[i]]);
}
cv::solvePnP(objPoints_filtered, imagePoints_filtered, 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 (_setYPerpendicular)
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;
}
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;
}
}
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;
}
