ar_multi.cpp

Go to the documentation of this file.

/*
 *  Multi Marker Pose Estimation using ARToolkit
 *  Copyright (C) 2010, CCNY Robotics Lab
 *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
 *  William Morris <morris@ee.ccny.cuny.edu>
 *  Gautier Dumonteil <gautier.dumonteil@gmail.com>
 *  http://robotics.ccny.cuny.edu
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "ar_pose/ar_multi.h"
#include "ar_pose/object.h"

using namespace cv;
using namespace ar_pose;

int main (int argc, char **argv)
{
  ros::init (argc, argv, "ar_single");
  ros::NodeHandle n;
  ar_pose::ARSinglePublisher ar_single (n);
  
  dynamic_reconfigure::Server<ARSinglePublisherConfig> server;
  dynamic_reconfigure::Server<ARSinglePublisherConfig>::CallbackType f;

  f = boost::bind(&(ARSinglePublisher::callback), _1, _2);
  server.setCallback(f);
  
  
  ros::spin ();
  return 0;
}

namespace ar_pose
{
  
  int ARSinglePublisher::window_size_ = 80;
  double ARSinglePublisher::k_ = 0.06;
  bool ARSinglePublisher::enable_thresholded_ = false;
  bool ARSinglePublisher::enable_hist_ = false;
  int ARSinglePublisher::threshold_ = 100;
  bool ARSinglePublisher::enable_local_thresh_=false;
  
  void ARSinglePublisher::callback(ARSinglePublisherConfig &config, uint32_t level) {
  ROS_INFO("Reconfigure Request: %d %d %d %.2f %d", 
            config.window_size, 
            config.enable_hist, 
            config.enable_thresholded, 
            config.k,
            config.original_thresh,
            config.enable_local_thresh);
       window_size_ = config.window_size;
       enable_hist_ = config.enable_hist; 
       enable_thresholded_ = config.enable_thresholded;
       k_ = config.k;
       threshold_ = config.original_thresh; 
       enable_local_thresh_=config.enable_local_thresh;
  }
  
  IplImage * ARSinglePublisher::drawHistogram(IplImage *img)
  {
    int numBins = 256;
    float range[] = {0, 255};
    float *ranges[] = { range };
    float scaleX = 2, scaleY = 2;
    const int width = 3;

    CvHistogram *hist = cvCreateHist(1, &numBins, CV_HIST_ARRAY, ranges, 1);
    cvClearHist(hist);

    IplImage *im_gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
    cvCvtColor(img, im_gray, CV_RGB2GRAY);

    cvCalcHist(&im_gray, hist, 0, 0);
    IplImage *imgHistGray = cvCreateImage(cvSize(256*scaleX, 64*scaleY), 8, 3);
    cvZero(imgHistGray);

    float histMax = 0;
    cvGetMinMaxHistValue(hist, 0, &histMax, 0, 0);

    for(int i=0;i<255;i++)
    {
      float histValue = cvQueryHistValue_1D(hist, i);
      float nextValue = cvQueryHistValue_1D(hist, i+1);

      CvPoint pt1 = cvPoint(i*scaleX, 64*scaleY);
      CvPoint pt2 = cvPoint(i*scaleX+scaleX, 64*scaleY);
      CvPoint pt3 = cvPoint(i*scaleX+scaleX, (64-nextValue*64/histMax)*scaleY);
      CvPoint pt4 = cvPoint(i*scaleX, (64-histValue*64/histMax)*scaleY);

      int numPts = 5;
      CvPoint pts[] = {pt1, pt2, pt3, pt4, pt1};

      cvFillConvexPoly(imgHistGray, pts, numPts, CV_RGB(255,255,255));
    }
    /*
    cvcv(imgHistGray,cvPoint(thresh*scaleX, 0), 
    cvPoint(thresh*scaleX, 64*scaleY),
    CV_RGB(255,0,0),width);
      */

    cvReleaseImage(&im_gray);
    cvReleaseHist(&hist);

    return imgHistGray;
  }
  
  void ARSinglePublisher::localThreshold(Mat& img, Mat& dst, double k, int window)
  {
    Mat mean(img.size(), CV_32F);
    Mat deviation(img.size(), CV_32F);
    Mat aux(img.size(), CV_32F);
    Mat thresh(img.size(), CV_8U);
    boxFilter(img, mean, CV_32F, Size(window, window));
    subtract(img, mean, deviation, noArray(), CV_32F);
    subtract(Scalar(255), deviation, aux, noArray(), CV_32F);
    divide(deviation, aux, aux, 1, CV_32F);
    subtract(aux, Scalar(1), aux, noArray(), CV_32F);
    multiply(Scalar(k), aux, aux, 1, CV_32F);
    add(Scalar(1), aux, aux, noArray(), CV_32F);
    multiply(mean, aux, thresh, 1, CV_8U);
    dst = img > thresh;
  }
  
  ARSinglePublisher::ARSinglePublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
  {
    std::string local_path;
    std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
          std::string default_path = "data/object_4x4";
    ros::NodeHandle n_param ("~");
    XmlRpc::XmlRpcValue xml_marker_center;

    // **** get parameters

    if (!n_param.getParam ("publish_tf", publishTf_))
      publishTf_ = true;
    ROS_INFO ("\tPublish transforms: %d", publishTf_);

    if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
      publishVisualMarkers_ = true;
    ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);

    if (!n_param.getParam ("threshold", threshold_))
//      threshold_ = 100;
    ROS_INFO ("\tThreshold: %d", threshold_);
        
        //modifications to allow path list from outside the package
        n_param.param ("marker_pattern_list", local_path, default_path);
        if (local_path.compare(0,5,"data/") == 0){
          //according to previous implementations, check if first 5 chars equal "data/"
          sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
        }
        else{
          //for new implementations, can pass a path outside the package_path
          sprintf (pattern_filename_, "%s", local_path.c_str ());
        }
        ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
        
    // **** subscribe

    ROS_INFO ("Subscribing to info topic");
    sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARSinglePublisher::camInfoCallback, this);
    getCamInfo_ = false;

    // **** advertsie 

    thresholded_ = it_.advertise("thresholded", 1);
    histogram_ = it_.advertise("histogram", 1);
    arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_marker", 0);
    if(publishVisualMarkers_)
    {
                rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
         }
  }

  ARSinglePublisher::~ARSinglePublisher (void)
  {
    //cvReleaseImage(&capture_); //Don't know why but crash when release the image
    arVideoCapStop ();
    arVideoClose ();
  }

  void ARSinglePublisher::camInfoCallback (const sensor_msgs::CameraInfoConstPtr & cam_info)
  {
    if (!getCamInfo_)
    {
      cam_info_ = (*cam_info);

      cam_param_.xsize = cam_info_.width;
      cam_param_.ysize = cam_info_.height;

      cam_param_.mat[0][0] = cam_info_.P[0];
      cam_param_.mat[1][0] = cam_info_.P[4];
      cam_param_.mat[2][0] = cam_info_.P[8];
      cam_param_.mat[0][1] = cam_info_.P[1];
      cam_param_.mat[1][1] = cam_info_.P[5];
      cam_param_.mat[2][1] = cam_info_.P[9];
      cam_param_.mat[0][2] = cam_info_.P[2];
      cam_param_.mat[1][2] = cam_info_.P[6];
      cam_param_.mat[2][2] = cam_info_.P[10];
      cam_param_.mat[0][3] = cam_info_.P[3];
      cam_param_.mat[1][3] = cam_info_.P[7];
      cam_param_.mat[2][3] = cam_info_.P[11];

      cam_param_.dist_factor[0] = cam_info_.K[2];       // x0 = cX from openCV calibration
      cam_param_.dist_factor[1] = cam_info_.K[5];       // y0 = cY from openCV calibration
      cam_param_.dist_factor[2] = -100*cam_info_.D[0];  // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
      cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
      
      arInit ();

      ROS_INFO ("Subscribing to image topic");
      cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARSinglePublisher::getTransformationCallback, this);
      getCamInfo_ = true;
    }
  }

  void ARSinglePublisher::arInit ()
  {
    arInitCparam (&cam_param_);
    ROS_INFO ("*** Camera Parameter ***");
    arParamDisp (&cam_param_);

    // load in the object data - trained markers and associated bitmap files
    if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
      ROS_BREAK ();
    ROS_DEBUG ("Objectfile num = %d", objectnum);

    sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
    capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
  }

  void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k, j, l;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the 
     * ARToolKit configure option (see config.h).*/
    try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
    }
    
//     ROS_WARN_STREAM("********** trying to compute local threshold\n");
    IplImage *hist = NULL;
    if (enable_hist_)
       hist = drawHistogram(capture_);
    IplImage *im_gray = cvCreateImage(cvGetSize(capture_),IPL_DEPTH_8U,1);
    cvCvtColor(capture_, im_gray, CV_BGR2GRAY);  
    Mat gray(im_gray);
    Mat thresholded(gray.size(), CV_8U);
    
    
    if (enable_local_thresh_){
    localThreshold(gray, thresholded, k_, 
                  (window_size_ % 2 == 0 ? window_size_ + 1: window_size_));
    }
    else {
    threshold(gray, thresholded, threshold_, 255, THRESH_BINARY);
    }
//     int erosion_size = 1;   
//     cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,
//                       cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), 
//                       cv::Point(erosion_size, erosion_size) );
//     dilate(thresholded, thresholded, element);
//     erode(thresholded, thresholded, element);
    
    
    
//     ROS_WARN_STREAM("********* successfully computed local threshold\n");
    
    //cvConvertImage(capture,capture,CV_CVTIMG_FLIP);
    //dataPtr = (ARUint8 *) capture_->imageData;
    
    // copy thresholded image into data
    dataPtr = (ARUint8*) malloc(sizeof(ARUint8)*capture_->width*capture_->height*3);
    int cnt = 0;
    for (int y = 0; y < capture_->height; y++) {
      for (int x = 0; x < capture_->width; x++) {
        for (int j = 0; j < 3; j++) {
          dataPtr[cnt++] = thresholded.at<uchar>(y,x);
        }
      }
    }

    // detect the markers in the video frame
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      argCleanup ();
      ROS_BREAK ();
    }

    arPoseMarkers_.markers.clear ();
    // check for known patterns
    for (i = 0; i < objectnum; i++)
    {
      k = -1;
      l = -1;
      for (j = 0; j < marker_num; j++)
      {
        if (object[i].id == marker_info[j].id)
        {
          if (k == -1)
            k = j;
          else                  // make sure you have the best pattern (highest confidence factor)
          if (marker_info[k].cf < marker_info[j].cf) {
            l = k;
            k = j;
          }
        }
      }
      if (k == -1)
      {
        object[i].visible = 0;
        continue;
      }
      
      if (l != -1 && marker_info[k].cf < 2 * marker_info[l].cf)
      {
        object[i].visible = 0;
        continue;
      }

      // calculate the transform for each marker
      if (object[i].visible == 0)
      {
        arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      else
      {
        arGetTransMatCont (&marker_info[k], object[i].trans,
                           object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      object[i].visible = 1;
      
      

      double arQuat[4], arPos[3];

      //arUtilMatInv (object[i].trans, cam_trans);
      arUtilMat2QuatPos (object[i].trans, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];

      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" Object num %i ------------------------------------------------", i);
      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

      ar_pose::ARMarker ar_pose_marker;
      ar_pose_marker.header.frame_id = image_msg->header.frame_id;
      
      ar_pose_marker.header.stamp = image_msg->header.stamp;
      ar_pose_marker.id = object[i].id;

      ar_pose_marker.pose.pose.position.x = pos[0];
      ar_pose_marker.pose.pose.position.y = pos[1];
      ar_pose_marker.pose.pose.position.z = pos[2];

      ar_pose_marker.pose.pose.orientation.x = quat[0];
      ar_pose_marker.pose.pose.orientation.y = quat[1];
      ar_pose_marker.pose.pose.orientation.z = quat[2];
      ar_pose_marker.pose.pose.orientation.w = quat[3];

      ar_pose_marker.confidence = round(marker_info->cf * 100);
      arPoseMarkers_.markers.push_back (ar_pose_marker);

      // **** publish transform between camera and marker

      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      btVector3 origin (pos[0], pos[1], pos[2]);
      btTransform t (rotation, origin);

      if (publishTf_)
      {
                        tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[i].name);
                        broadcaster_.sendTransform(camToMarker);
      }

      // **** publish visual marker

      if (publishVisualMarkers_)
      {
        btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        btTransform m (btQuaternion::getIdentity (), markerOrigin);
        btTransform markerPose = t * m; // marker pose in the camera frame

        tf::poseTFToMsg (markerPose, rvizMarker_.pose);

        rvizMarker_.header.frame_id = image_msg->header.frame_id;
        rvizMarker_.header.stamp = image_msg->header.stamp;
        rvizMarker_.id = object[i].id;

        rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.ns = "basic_shapes";
        rvizMarker_.type = visualization_msgs::Marker::CUBE;
        rvizMarker_.action = visualization_msgs::Marker::ADD;
        switch (i)
        {
          case 0:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 1.0f;
            rvizMarker_.color.a = 1.0;
            break;
          case 1:
            rvizMarker_.color.r = 1.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
            break;
          default:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 1.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
        }
        rvizMarker_.lifetime = ros::Duration (1);

        rvizMarkerPub_.publish (rvizMarker_);
        ROS_DEBUG ("Published visual marker");
      }
    }
    
 
    
//     ROS_WARN_STREAM("************* Trying to convert to bridge\n");
    if (enable_thresholded_) {
      IplImage threshipl = thresholded;
      IplImage* iplThresh = &threshipl; 
      IplImage* color = cvCreateImage(thresholded.size(), IPL_DEPTH_8U, 3);
      cvCvtColor(iplThresh, color, CV_GRAY2BGR);
      
      int colorR = 255/(objectnum+1);
      for (k = 0; k < marker_num; k++)
      {
        if (marker_info[k].cf<1 && marker_info[k].cf>0){
         const int width = 3;
        cvLine(color,cvPoint(marker_info[k].vertex[0][0],marker_info[k].vertex[0][1]),
        cvPoint(marker_info[k].vertex[1][0],marker_info[k].vertex[1][1])
          ,CV_RGB(colorR*marker_info[k].id, 255-colorR*marker_info[k].id, 50),width);
        cvLine(color,cvPoint(marker_info[k].vertex[1][0],marker_info[k].vertex[1][1]),
        cvPoint(marker_info[k].vertex[2][0],marker_info[k].vertex[2][1])
          ,CV_RGB(colorR*marker_info[k].id, 255-colorR*marker_info[k].id, 50),width);
        cvLine(color,cvPoint(marker_info[k].vertex[2][0],marker_info[k].vertex[2][1]),
        cvPoint(marker_info[k].vertex[3][0],marker_info[k].vertex[3][1])
          ,CV_RGB(colorR*marker_info[k].id, 255-colorR*marker_info[k].id, 50),width);
        cvLine(color,cvPoint(marker_info[k].vertex[3][0],marker_info[k].vertex[3][1]),
        cvPoint(marker_info[k].vertex[0][0],marker_info[k].vertex[0][1])
          ,CV_RGB(colorR*marker_info[k].id, 255-colorR*marker_info[k].id, 50),width);
        }
      }
    
      sensor_msgs::ImagePtr threshPtr = sensor_msgs::CvBridge::cvToImgMsg(color, "passthrough");
//       threshPtr.header.frame_id = image_msg.frame_id;
//       threshPtr.header.stamp = image_msg.stamp;
      threshPtr->header=image_msg->header;
      
      thresholded_.publish(threshPtr);
      
      cvReleaseImage(&color);
      //cvReleaseImage(&iplThresh);
    }
    
    if (enable_hist_) {
      sensor_msgs::ImagePtr histPtr = sensor_msgs::CvBridge::cvToImgMsg(hist, "passthrough");
      histPtr->header=image_msg->header;
      histogram_.publish(histPtr);
    }
//     ROS_WARN_STREAM("************* Sucessfully converted to bridge\n");
   
     cvReleaseImage(&im_gray);
     cvReleaseImage(&hist);
    free(dataPtr);
    
//     ROS_WARN_STREAM("************ successfully cleaned up\n");
    arMarkerPub_.publish (arPoseMarkers_);
    ROS_DEBUG ("Published ar_multi markers");
  }


}                               // end namespace ar_pose