/**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
}
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();
}
