checkerboard_detector2.cpp

Go to the documentation of this file.

// Software License Agreement (BSD License)
// Copyright (c) 2008, Willow Garage, Inc.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above copyright
//     notice, this list of conditions and the following disclaimer in the
//     documentation and/or other materials provided with the distribution.
//   * The name of the author may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// author: Rosen Diankov
#include <cstdio>
#include <vector>
#include <sstream>
#include <ros/ros.h>

#include <boost/thread/mutex.hpp>
#include <boost/format.hpp>
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "cv_bridge/CvBridge.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/Image.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "checkerboard_detector2/ObjectDetection.h"
#include "checkerboard_detector2/Detect.h"
#include "checkerboard_detector2/Set.h"
#include "math.h"
#include <image_transport/image_transport.h>
#include <tf/transform_broadcaster.h>

#include <sys/timeb.h>    // ftime(), struct timeb
#include <sys/time.h>

#define sqr(a) ((a)*(a))

using namespace std;
using namespace ros;
using namespace boost;

class CheckerboardDetector
{
    template <typename T>
    static vector<T> tokenizevector(const string& s)
    {
        stringstream ss(s);
        return vector<T>((istream_iterator<T>(ss)), istream_iterator<T>());
    }

public:
    struct CHECKERBOARD
    {
        CvSize griddims; 
        vector<Vector> grid3d;
        vector<CvPoint2D32f> corners;
        TransformMatrix tlocaltrans;
        TransformMatrix previous_pose;
        double cellwidth;
        double cellheight;
    };

    checkerboard_detector2::ObjectDetection _objdetmsg;
    sensor_msgs::CvBridge _cvbridge;
    sensor_msgs::CameraInfo _camInfoMsg;

    ros::Subscriber camInfoSubscriber;
    image_transport::Subscriber  imageSubscriber;
//    ros::Publisher _pubDetection;
    ros::Publisher _pubPoseArray;
    std::vector<ros::Publisher> _pubPose;

    ros::ServiceServer service_;

    string frame_id; // tf frame id

    int display, verbose;
    vector<CHECKERBOARD> vcheckers; // grid points for every checkerboard
    vector< string > vstrtypes; // type names for every grid point
    map<string,int> maptypes;
    ros::Time lasttime;
    CvMat *intrinsic_matrix; // intrinsic matrices
    boost::mutex mutexcalib;
    IplImage* frame;

    ros::NodeHandle _node_private;
    ros::NodeHandle _node_pub;

    bool _active;
    tf::TransformBroadcaster br_;

    // Constructor
    CheckerboardDetector() : intrinsic_matrix(NULL), frame(NULL), _node_private("~"), _active(true)
    {
        _node_private.param("display", display, 0);
        _node_private.param("verbose", verbose, 1);
        _node_private.param("start_active", _active, _active);
        
        char str[32];
        int index = 0;

        while(1) {
            int dimx, dimy;
            double fRectSize[2];
            string type;

            sprintf(str,"grid%d_size_x",index);
            if( !_node_private.getParam(str,dimx) )
                break;
            if (dimx < 3) {
                ROS_ERROR("Param: %s must be greater than 2",str);
                return;
            }

            sprintf(str,"grid%d_size_y",index);
            if( !_node_private.getParam(str,dimy) )
                break;
            if (dimy < 3) {
                ROS_ERROR("Param: %s must be greater than 2",str);
                return;
            }

            sprintf(str,"rect%d_size_x",index);
            if( !_node_private.getParam(str,fRectSize[0]) )
                break;

            sprintf(str,"rect%d_size_y",index);
            if( !_node_private.getParam(str,fRectSize[1]) )
                break;

            sprintf(str,"type%d",index);
            if( !_node_private.getParam(str,type) ) {
                sprintf(str,"checker%dx%d", dimx, dimy);
                type = str;
            }

            string strtranslation,strrotation;
            sprintf(str,"translation%d",index);
            _node_private.param(str,strtranslation,string());

            vector<float> vtranslation = tokenizevector<float>(strtranslation);
            sprintf(str,"rotation%d",index);
            _node_private.param(str,strrotation,string());

            vector<float> vrotation = tokenizevector<float>(strrotation);

            CHECKERBOARD cb;
            cb.griddims = cvSize(dimx,dimy);
            cb.cellwidth = fRectSize[0];
            cb.cellheight = fRectSize[0];

            cb.grid3d.resize(dimx*dimy);
            int j=0;
            for(int y=0; y<dimy; ++y)
                for(int x=0; x<dimx; ++x)
                    cb.grid3d[j++] = Vector(x*fRectSize[0], y*fRectSize[1], 0);

            if( vtranslation.size() == 3 )
                cb.tlocaltrans.trans = 
                    Vector(vtranslation[0],vtranslation[1],vtranslation[2]);

            if( vrotation.size() == 9 )  {
                for(int k = 0; k < 3; ++k) {
                    cb.tlocaltrans.m[4*k+0] = vrotation[3*k+0];
                    cb.tlocaltrans.m[4*k+1] = vrotation[3*k+1];
                    cb.tlocaltrans.m[4*k+2] = vrotation[3*k+2];
                }
            }

            vcheckers.push_back(cb);
            vstrtypes.push_back(type);
            maptypes[vstrtypes.back()] = index;
            index++;
        }

        _node_private.param("frame_id", frame_id,string(""));

        if( maptypes.size() == 0 ) {
            ROS_ERROR("no checkerboards to detect");
            return;
        }

        if( display ) {
            cvNamedWindow("Checkerboard Detector", CV_WINDOW_AUTOSIZE);
            cvStartWindowThread();
        }

        lasttime = ros::Time::now();
//        _pubDetection = _node_pub.advertise<checkerboard_detector2::ObjectDetection>("ObjectDetection",1);
        _pubPoseArray = _node_pub.advertise<geometry_msgs::PoseArray>("pose_array",1);

        cout <<"vcheckers.size()=="<<vcheckers.size()<<endl;
        for(size_t i=0;i<vcheckers.size();i++) {
                _pubPose.push_back( _node_pub.advertise<geometry_msgs::PoseStamped>( boost::str(format("pose%d")%i),1) );
        }


        service_ =_node_pub.advertiseService("set_detect_checkerboard", &CheckerboardDetector::serviceCallback, this);
        if(_active) this->activate(true);
    }

    // Destructor
    virtual ~CheckerboardDetector()
    {
        if( frame )
            cvReleaseImage(&frame);
        if( this->intrinsic_matrix )
            cvReleaseMat(&this->intrinsic_matrix);
        this->camInfoSubscriber.shutdown();
        this->imageSubscriber.shutdown();
    }

    // Camera info callback
    void caminfo_cb(const sensor_msgs::CameraInfoConstPtr &msg)
    {
        boost::mutex::scoped_lock lock(this->mutexcalib);

        this->_camInfoMsg = *msg;

        // only get the camera info once <- this is dumb
        //this->camInfoSubscriber.shutdown();
        //this->camInfoSubscriber2.shutdown();
    }


    // Image data callback
    void image_cb(const sensor_msgs::ImageConstPtr &msg)
    {
        boost::mutex::scoped_lock lock(this->mutexcalib);
        if( Detect(_objdetmsg,*msg,this->_camInfoMsg) ) {
                _objdetmsg.header = msg->header;
//            _pubDetection.publish(_objdetmsg);
            geometry_msgs::PoseArray array;
            array.header = _objdetmsg.header;
            array.poses.resize(vcheckers.size());
            for(size_t i=0;i<_objdetmsg.objects.size();i++) {
                array.poses[_objdetmsg.objects[i].id] = _objdetmsg.objects[i].pose;
            }
            _pubPoseArray.publish(array);
            vector<tf::StampedTransform> transforms;
            for(size_t i=0;i<_objdetmsg.objects.size();i++) {
                geometry_msgs::PoseStamped pose;
                pose.header = _objdetmsg.header;
                pose.pose = _objdetmsg.objects[i].pose;
                _pubPose[_objdetmsg.objects[i].id].publish( pose );

              tf::Stamped<tf::Transform> trnsfm;
              poseStampedMsgToTF (pose, trnsfm);
              char cb_frame_id[100];
              sprintf(cb_frame_id, "/checkerboard_%lu", i);

              transforms.push_back(tf::StampedTransform(trnsfm, trnsfm.stamp_, trnsfm.frame_id_, cb_frame_id));
            }
            br_.sendTransform(transforms);
        }
    }

    bool detect_cb(checkerboard_detector2::Detect::Request& req, checkerboard_detector2::Detect::Response& res)
    {
        if(!_active)
          return true;
        else 
          return Detect(res.object_detection,req.image,req.camera_info);
    }

    bool Detect(checkerboard_detector2::ObjectDetection& objdetmsg, const sensor_msgs::Image& imagemsg, const sensor_msgs::CameraInfo& camInfoMsg)
    {
        if( this->intrinsic_matrix == NULL )
            this->intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);

        for(int i = 0; i < 3; ++i)
            for(int j = 0; j < 3; ++j)
                this->intrinsic_matrix->data.fl[3*i+j] = camInfoMsg.P[4*i+j];

        if( !_cvbridge.fromImage(imagemsg, "mono8") ) {
            ROS_ERROR("failed to get image");
            return false;
        }

        IplImage *pimggray = _cvbridge.toIpl();
        if( display ) {
            // copy the raw image
            if( frame != NULL && (frame->width != (int)imagemsg.width || frame->height != (int)imagemsg.height) ) {
                cvReleaseImage(&frame);
                frame = NULL;
            }

            if( frame == NULL ) 
                frame = cvCreateImage(cvSize(imagemsg.width,imagemsg.height),IPL_DEPTH_8U, 3);

            cvCvtColor(pimggray,frame,CV_GRAY2RGB);
        }

        vector<checkerboard_detector2::Object6DPose> vobjects;

//#pragma omp parallel for schedule(dynamic,1)
        for(int i = 0; i < (int)vcheckers.size(); ++i) {
            CHECKERBOARD& cb = vcheckers[i];
            int ncorners;
            checkerboard_detector2::Object6DPose objpose;

            // do until no more checkerboards detected
            while(1) {
                cb.corners.resize(200);
                int allfound = cvFindChessboardCorners( pimggray, cb.griddims, &cb.corners[0], &ncorners,
                                                        CV_CALIB_CB_ADAPTIVE_THRESH );
                cb.corners.resize(ncorners);

                //cvDrawChessboardCorners(pimgGray, itbox->second.griddims, &corners[0], ncorners, allfound);
                //cvSaveImage("temp.jpg", pimgGray);

                if(!allfound || ncorners != (int)cb.grid3d.size())
                    break;

                // remove any corners that are close to the border
                const int borderthresh = 30;
        
                for(int j = 0; j < ncorners; ++j) {
                    int x = cb.corners[j].x;
                    int y = cb.corners[j].y;
                    if( x < borderthresh || x > pimggray->width-borderthresh ||
                        y < borderthresh || y > pimggray->height-borderthresh ) 
                    {
                        allfound = 0;
                        break;
                    }
                }

                // mark out the image
                CvPoint upperleft, lowerright;
                upperleft.x = lowerright.x = cb.corners[0].x; 
                upperleft.y = lowerright.y = cb.corners[0].y; 
                for(int j = 1; j < (int)cb.corners.size(); ++j) {
                    if( upperleft.x > cb.corners[j].x ) upperleft.x = cb.corners[j].x;
                    if( upperleft.y > cb.corners[j].y ) upperleft.y = cb.corners[j].y;
                    if( lowerright.x < cb.corners[j].x ) lowerright.x = cb.corners[j].x;
                    if( lowerright.y < cb.corners[j].y ) lowerright.y = cb.corners[j].y;
                }

                if( allfound ) {
                    cvFindCornerSubPix(pimggray, &cb.corners[0], cb.corners.size(), cvSize(5,5),cvSize(-1,-1),
                                       cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
                    CHECKERBOARD cb_copy = cb;
                    objpose.pose = FindTransformation(cb.corners, cb.grid3d, cb.tlocaltrans,cb_copy);

                    // now check for proximity of other previously markers, outliers are bad for tracking
                        double mindist2 =  sqr(cb.cellwidth*cb.griddims.width) + sqr(cb.cellheight*cb.griddims.height);
                    for(int j = 0; j < (int)vcheckers.size(); ++j) {
                        if(j != i) {
                                double dist2 = sqr(objpose.pose.position.x - vcheckers[j].previous_pose.trans.x) +
                                                          sqr(objpose.pose.position.y - vcheckers[j].previous_pose.trans.y) +
                                                          sqr(objpose.pose.position.z - vcheckers[j].previous_pose.trans.z);
                                if(dist2 < mindist2) {
                                        cout << "skipping marker, because "<<dist2<<" < "<<mindist2<<endl;
                                        allfound = false; // ignore this detection, too close to another previously seen marker
                                }
                        }
                    }
                    if(allfound) {
                        cb = cb_copy;
                    }
                }

//#pragma omp critical
                {
                    if( allfound ) {
                        vobjects.push_back(objpose);
                        vobjects.back().type = vstrtypes[i];
                        vobjects.back().id = i;
                    }

                    cvRectangle(pimggray, upperleft, lowerright, CV_RGB(0,0,0),CV_FILLED);
                }
            }

            //cvSaveImage("temp.jpg", pimggray);
        }

        objdetmsg.objects = vobjects;
        objdetmsg.header.stamp = imagemsg.header.stamp;
        if( frame_id.size() > 0 )
            objdetmsg.header.frame_id = frame_id;
        else
            objdetmsg.header.frame_id = imagemsg.header.frame_id;

        if( verbose > 0 )
            ROS_INFO("checkerboard: image: %ux%u (size=%u), num: %u, total: %.3fs",imagemsg.width,imagemsg.height,
                     (unsigned int)imagemsg.data.size(), (unsigned int)objdetmsg.objects.size(),
                     (float)(ros::Time::now()-lasttime).toSec());
        lasttime = ros::Time::now();

        if( display ) {
            // draw each found checkerboard
            for(size_t i = 0; i < vobjects.size(); ++i) {
                int itype = maptypes[vobjects[i].type];
                CHECKERBOARD& cb = vcheckers[itype];
                Transform tglobal;
                tglobal.trans = Vector(vobjects[i].pose.position.x,vobjects[i].pose.position.y,vobjects[i].pose.position.z);
                tglobal.rot = Vector(vobjects[i].pose.orientation.w,vobjects[i].pose.orientation.x,vobjects[i].pose.orientation.y, vobjects[i].pose.orientation.z);
                Transform tlocal = tglobal * cb.tlocaltrans.inverse();

                CvPoint X[4];

                Vector vaxes[4];
                vaxes[0] = Vector(0,0,0);
                vaxes[1] = Vector(0.05f,0,0);
                vaxes[2] = Vector(0,0.05f,0);
                vaxes[3] = Vector(0,0,0.05f);

                for(int i = 0; i < 4; ++i) {
                    Vector p = tglobal*vaxes[i];
                    dReal fx = p.x*camInfoMsg.P[0] + p.y*camInfoMsg.P[1] + p.z*camInfoMsg.P[2] + camInfoMsg.P[3];
                    dReal fy = p.x*camInfoMsg.P[4] + p.y*camInfoMsg.P[5] + p.z*camInfoMsg.P[6] + camInfoMsg.P[7];
                    dReal fz = p.x*camInfoMsg.P[8] + p.y*camInfoMsg.P[9] + p.z*camInfoMsg.P[10] + camInfoMsg.P[11];
                    X[i].x = (int)(fx/fz);
                    X[i].y = (int)(fy/fz);
                }

                // draw three lines
                CvScalar col0 = CV_RGB(255,0,(64*itype)%256);
                CvScalar col1 = CV_RGB(0,255,(64*itype)%256);
                CvScalar col2 = CV_RGB((64*itype)%256,(64*itype)%256,255);
                cvLine(frame, X[0], X[1], col0, 1);
                cvLine(frame, X[0], X[2], col1, 1);
                cvLine(frame, X[0], X[3], col2, 1);

                // draw all the points
                for(size_t i = 0; i < cb.grid3d.size(); ++i) {
                    Vector p = tlocal * cb.grid3d[i];
                    dReal fx = p.x*camInfoMsg.P[0] + p.y*camInfoMsg.P[1] + p.z*camInfoMsg.P[2] + camInfoMsg.P[3];
                    dReal fy = p.x*camInfoMsg.P[4] + p.y*camInfoMsg.P[5] + p.z*camInfoMsg.P[6] + camInfoMsg.P[7];
                    dReal fz = p.x*camInfoMsg.P[8] + p.y*camInfoMsg.P[9] + p.z*camInfoMsg.P[10] + camInfoMsg.P[11];
                    int x = (int)(fx/fz);
                    int y = (int)(fy/fz);
                    cvCircle(frame, cvPoint(x,y), 6, CV_RGB(0,0,0), 2);
                    cvCircle(frame, cvPoint(x,y), 2, CV_RGB(0,0,0), 2);
                    cvCircle(frame, cvPoint(x,y), 4, CV_RGB(128,128,64*itype), 3);
                }

                cvCircle(frame, X[0], 3, CV_RGB(255,255,128), 3);
            }

            cvShowImage("Checkerboard Detector",frame);
        }

        return true;
    }


    // FindTransformation
    geometry_msgs::Pose FindTransformation(const vector<CvPoint2D32f> &imgpts, const vector<Vector> &objpts, const Transform& tlocal, CHECKERBOARD &cb)
    {
        CvMat *objpoints = cvCreateMat(3,objpts.size(),CV_32FC1);
        for(size_t i=0; i<objpts.size(); ++i) {
            cvSetReal2D(objpoints, 0,i, objpts[i].x);
            cvSetReal2D(objpoints, 1,i, objpts[i].y);
            cvSetReal2D(objpoints, 2,i, objpts[i].z);
        }

        geometry_msgs::Pose pose;
        Transform tchecker;
        assert(sizeof(tchecker.trans.x)==sizeof(float));
        float fR3[3];
        CvMat R3, T3;
        assert(sizeof(pose.position.x) == sizeof(double));
        cvInitMatHeader(&R3, 3, 1, CV_32FC1, fR3);
        cvInitMatHeader(&T3, 3, 1, CV_32FC1, &tchecker.trans.x);

        float kc[4] = {0};
        CvMat kcmat;
        cvInitMatHeader(&kcmat,1,4,CV_32FC1,kc);

        CvMat img_points;
        cvInitMatHeader(&img_points, 1,imgpts.size(), CV_32FC2, const_cast<CvPoint2D32f*>(&imgpts[0]));

        cvFindExtrinsicCameraParams2(objpoints, &img_points, this->intrinsic_matrix, &kcmat, &R3, &T3);
        cvReleaseMat(&objpoints);

        double fang = sqrt(fR3[0]*fR3[0] + fR3[1]*fR3[1] + fR3[2]*fR3[2]);
        if( fang >= 1e-6 ) {
            double fmult = sin(fang/2)/fang;
            tchecker.rot = Vector(cos(fang/2),fR3[0]*fmult, fR3[1]*fmult, fR3[2]*fmult);
        }

        // move pose to center of marker
        Transform center(Vector(1,0,0,0),Vector(
                        cb.cellwidth*(cb.griddims.width-1)/2.0,
                        cb.cellheight*(cb.griddims.height-1)/2.0,
                        0));

        // disambiguate by tracking..
        Transform rot180(Vector(1,0,0,0),Vector(0,0,0));
        rot180.rotfromaxisangle(Vector(0,0,1),(float)M_PI);

        Transform tglobal1 = tchecker*center*tlocal;
        Transform tglobal2 = tchecker*center*rot180*tlocal;
        TransformMatrix diff1(cb.previous_pose.inverse() * tglobal1);
        TransformMatrix diff2(cb.previous_pose.inverse() * tglobal2);

        Transform tglobal;
        if(diff1.m[0]>=0)
                tglobal = tglobal1;
        else
                tglobal = tglobal2;

        cb.previous_pose = tglobal;

        pose.position.x = tglobal.trans.x;
        pose.position.y = tglobal.trans.y;
        pose.position.z = tglobal.trans.z;
        pose.orientation.x = tglobal.rot.y;
        pose.orientation.y = tglobal.rot.z;
        pose.orientation.z = tglobal.rot.w;
        pose.orientation.w = tglobal.rot.x;
        return pose;
    }

    void activate(bool active, string image_topic = std::string(), string camera_info_topic = std::string()){
      if(active){
        image_transport::ImageTransport it(_node_pub);
        if(image_topic.empty() || camera_info_topic.empty())
        {
          this->camInfoSubscriber = _node_pub.subscribe("camera_info", 1, &CheckerboardDetector::caminfo_cb, this);
          this->imageSubscriber = it.subscribe("image_rect", 1, &CheckerboardDetector::image_cb,this);
        }
        else{
          this->camInfoSubscriber = _node_pub.subscribe(camera_info_topic, 1, &CheckerboardDetector::caminfo_cb, this);
          this->imageSubscriber = it.subscribe(image_topic, 1, &CheckerboardDetector::image_cb,this);
        }
      }
      else{
        this->camInfoSubscriber.shutdown();
        this->imageSubscriber.shutdown();
      }
      _active = active;
    }

    bool serviceCallback(checkerboard_detector2::Set::Request  &req,
                         checkerboard_detector2::Set::Response &res)
    {
      this->activate(req.active, req.image_topic, req.camera_info_topic);
      return true;
    }


};

// MAIN
int main(int argc, char **argv)
{
    ros::init(argc,argv,"checkerboard_detector");
    if( !ros::master::check() )
        return 1;
  
    CheckerboardDetector cd;
    ros::spin();

    return 0;
}