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(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;
}
}
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;
}
}
