Esempio n. 1
0
void cvTackBarEvents(int pos,void*) {
	if (i_threshold_param1 < 3) 
		i_threshold_param1 = 3;
	if (i_threshold_param1 % 2 != 1) 
		i_threshold_param1++;
	if (i_threshold_param2 < 1) 
		i_threshold_param2 = 1;

	threshold_param1 = i_threshold_param1;
	threshold_param2 = i_threshold_param2;
	marker_dector.setThresholdParams(threshold_param1, threshold_param2);

	//recompute
	marker_dector.detect(input_image, markers, camera_params);
	input_image.copyTo(input_image_copy);

	for(unsigned int i=0; i < markers.size(); i++)
		markers[i].draw(input_image, Scalar(0, 0, 255), 2);

	//draw a 3d cube in each marker if there is 3d info
	if (camera_params.isValid())
		for(unsigned int i=0; i < markers.size(); i++)
			aruco::CvDrawingUtils::draw3dCube(input_image_copy, markers[i], camera_params);

	cv::imshow("in", input_image_copy);
	cv::imshow("thres", marker_dector.getThresholdedImage());
}
Esempio n. 2
0
/*!
 *
 */
static void cvTrackBarEvents(int ,void*)
{
  if (iThresParam1<3)
    iThresParam1=3;
  if (iThresParam1%2!=1)
    iThresParam1++;
  if (dThresParam2<1)
    dThresParam2=1;

  dThresParam1=iThresParam1;
  dThresParam2=iThresParam2;
  boardDetector.getMarkerDetector().setThresholdParams(dThresParam1,dThresParam2);

  //recompute
  //Detection of the board
  float probDetect=boardDetector.detect( inpImg);
  inpImg.copyTo(inpImgCpy);
  if (params.isValid() && probDetect>0.2)
    aruco::CvDrawingUtils::draw3dAxis(inpImgCpy,boardDetector.getDetectedBoard(),params);


  cv::imshow("in",inpImgCpy);
  cv::imshow("thres",boardDetector.getMarkerDetector().getThresholdedImage());
}
Esempio n. 3
0
  void image_callback(const sensor_msgs::ImageConstPtr& msg)
  {
    //        double ticksBefore = cv::getTickCount();
    static tf::TransformBroadcaster br;
    if(cam_info_received)
    {
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
        inImage = cv_ptr->image;

        //detection results will go into "markers"
        markers.clear();
        //Ok, let's detect
        mDetector.detect(inImage, markers, camParam, marker_size, false);
        //for each marker, draw info and its boundaries in the image
        for(size_t i=0; i<markers.size(); ++i)
        {
          // only publishing the selected marker
          if(markers[i].id == marker_id)
          {
            tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
            tf::StampedTransform stampedTransform(transform, ros::Time::now(),
                                                  parent_name, child_name);
            br.sendTransform(stampedTransform);
            geometry_msgs::PoseStamped poseMsg;
            tf::poseTFToMsg(transform, poseMsg.pose);
            poseMsg.header.frame_id = parent_name;
            poseMsg.header.stamp = ros::Time::now();
            pose_pub.publish(poseMsg);
          }
          // but drawing all the detected markers
          markers[i].draw(inImage,Scalar(0,0,255),2);
        }

        //draw a 3d cube in each marker if there is 3d info
        if(camParam.isValid() && marker_size!=-1)
        {
          for(size_t i=0; i<markers.size(); ++i)
          {
            //CvDrawingUtils::draw3dCube(inImage, markers[i], camParam);
            CvDrawingUtils::draw3dAxis(inImage, markers[i], camParam);
          }
        }

        if(image_pub.getNumSubscribers() > 0)
        {
          //show input with augmented information
          cv_bridge::CvImage out_msg;
          out_msg.header.stamp = ros::Time::now();
          out_msg.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
          out_msg.image = inImage;
          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.stamp = ros::Time::now();
          debug_msg.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
          debug_msg.image = mDetector.getThresholdedImage();
          debug_pub.publish(debug_msg.toImageMsg());
        }

        //            ROS_INFO("runtime: %f ms",
        //                     1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
      }
      catch (cv_bridge::Exception& e)
      {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
      }
    }
  }
Esempio n. 4
0
    void image_callback(const sensor_msgs::ImageConstPtr& msg)
    {
        static tf::TransformBroadcaster br;
        if(cam_info_received)
        {
            cv_bridge::CvImagePtr cv_ptr;
            try
            {
                cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
                inImage = cv_ptr->image;

                //detection results will go into "markers"
                markers.clear();
                //Ok, let's detect
                mDetector.detect(inImage, markers, camParam, marker_size, false);
                //for each marker, draw info and its boundaries in the image
                for(size_t i=0; i<markers.size(); ++i)
                {
                    // only publishing the selected marker
                    if(markers[i].id == marker_id)
                    {
                        tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
                        tf::StampedTransform cameraToReference;
                        cameraToReference.setIdentity();

                        if ( reference_frame != camera_frame )
                        {
                            getTransform(reference_frame,
                                         camera_frame,
                                         cameraToReference);
                        }

                        transform =
                            static_cast<tf::Transform>(cameraToReference)
                            * static_cast<tf::Transform>(rightToLeft)
                            * transform;

                        tf::StampedTransform stampedTransform(transform, ros::Time::now(),
                                                              reference_frame, marker_frame);
                        br.sendTransform(stampedTransform);
                        geometry_msgs::PoseStamped poseMsg;
                        tf::poseTFToMsg(transform, poseMsg.pose);
                        poseMsg.header.frame_id = reference_frame;
                        poseMsg.header.stamp = ros::Time::now();
                        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);
                    }
                    // but drawing all the detected markers
                    markers[i].draw(inImage,cv::Scalar(0,0,255),2);
                }

                //draw a 3d cube in each marker if there is 3d info
                if(camParam.isValid() && marker_size!=-1)
                {
                    for(size_t i=0; i<markers.size(); ++i)
                    {
                        CvDrawingUtils::draw3dAxis(inImage, markers[i], camParam);
                    }
                }

                if(image_pub.getNumSubscribers() > 0)
                {
                    //show input with augmented information
                    cv_bridge::CvImage out_msg;
                    out_msg.header.stamp = ros::Time::now();
                    out_msg.encoding = sensor_msgs::image_encodings::RGB8;
                    out_msg.image = inImage;
                    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.stamp = ros::Time::now();
                    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;
            }
        }
    }
Esempio n. 5
0
/*!
 *
 */
int main(int argc,char **argv)
{
  readArguments(argc,argv);

  aruco::BoardConfiguration boardConf;
  boardConf.readFromFile(boC->sval[0]);

  cv::VideoCapture vcapture;

  //read from camera or from  file
  if (strcmp(inp->sval[0], "live")==0)
  {
    vcapture.open(0);
    vcapture.set(CV_CAP_PROP_FRAME_WIDTH,  wid->ival[0]);
    vcapture.set(CV_CAP_PROP_FRAME_HEIGHT, hei->ival[0]);
    int val = CV_FOURCC('M', 'P', 'E', 'G');
    vcapture.set(CV_CAP_PROP_FOURCC, val);
  }
  else
    vcapture.open(inp->sval[0]);

  //check video is open
  if (!vcapture.isOpened())
  {
    std::cerr<<"Could not open video"<<std::endl;
    return -1;
  }

  //read first image to get the dimensions
  vcapture>>inpImg;

  //Open outputvideo
  cv::VideoWriter vWriter;
  if (out->count)
    vWriter.open(out->sval[0], CV_FOURCC('M','J','P','G'), fps->ival[0], inpImg.size());

  //read camera parameters if passed
  if (intr->count)
  {
    params.readFromXMLFile(intr->sval[0]);
    params.resize(inpImg.size());
  }

  //Create gui
  cv::namedWindow("thres", CV_GUI_NORMAL | CV_WINDOW_AUTOSIZE);
  cv::namedWindow("in", CV_GUI_NORMAL | CV_WINDOW_AUTOSIZE);
  cvMoveWindow("thres", 0, 0);
  cvMoveWindow("in", inpImg.cols + 5, 0);
  boardDetector.setParams(boardConf,params,msiz->dval[0]);
  boardDetector.getMarkerDetector().getThresholdParams( dThresParam1,dThresParam2);
//  boardDetector.getMarkerDetector().enableErosion(true);//for chessboards
  iThresParam1=dThresParam1;
  iThresParam2=dThresParam2;
  cv::createTrackbar("ThresParam1", "in",&iThresParam1, 13, cvTrackBarEvents);
  cv::createTrackbar("ThresParam2", "in",&iThresParam2, 13, cvTrackBarEvents);
  int index=0;

  ///////////////
  // Main Lopp //
  ///////////////
  while(appRunnin)
  {
    inpImg.copyTo(inpImgCpy);
    index++;                                    //number of images captured
    double tick = static_cast<double>(cv::getTickCount());//for checking the speed
    float probDetect=boardDetector.detect(inpImg); //Detection of the board
    tick=(static_cast<double>(cv::getTickCount())-tick)/cv::getTickFrequency();
    std::cout<<"Time detection="<<1000*tick<<" milliseconds"<<std::endl;
    //print marker borders
    for (unsigned int i=0; i<boardDetector.getDetectedMarkers().size(); i++)
      boardDetector.getDetectedMarkers()[i].draw(inpImgCpy,cv::Scalar(0,0,255),1);

    //print board
    if (params.isValid())
    {
      if ( probDetect>thre->dval[0])
      {
        aruco::CvDrawingUtils::draw3dAxis( inpImgCpy,boardDetector.getDetectedBoard(),params);
      }
    }
    //DONE! Easy, right?

    //show input with augmented information and the thresholded image
    cv::imshow("in",inpImgCpy);
    cv::imshow("thres",boardDetector.getMarkerDetector().getThresholdedImage());

    // write to video if desired
    if (out->count)
    {
      //create a beautiful composed image showing the thresholded
      //first create a small version of the thresholded image
      cv::Mat smallThres;
      cv::resize(boardDetector.getMarkerDetector().getThresholdedImage(),smallThres,
                 cv::Size(inpImgCpy.cols/3,inpImgCpy.rows/3));
      cv::Mat small3C;
      cv::cvtColor(smallThres,small3C,CV_GRAY2BGR);
      cv::Mat roi=inpImgCpy(cv::Rect(0,0,inpImgCpy.cols/3,inpImgCpy.rows/3));
      small3C.copyTo(roi);
      vWriter<<inpImgCpy;
    }

    processKey(cv::waitKey(waitTime));//wait for key to be pressed
    appRunnin &= vcapture.grab();
    vcapture.retrieve(inpImg);
  }

  arg_freetable(argtable,sizeof(argtable)/sizeof(argtable[0]));
  return EXIT_SUCCESS;
}
  void image_callback(const sensor_msgs::ImageConstPtr& msg)
  {
    static tf::TransformBroadcaster br;

    
    if(cam_info_received)
    {
      ros::Time curr_stamp(ros::Time::now());
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
        inImage = cv_ptr->image;

        //detection results will go into "markers"
        markers.clear();
        //Ok, let's detect

        mDetector.detect(inImage, markers, camParam, marker_size, false);
		
		//ROS_INFO("x: %0.3f y: %0.3f z: %0.3f", markers[0].Tvec.at<float>(0,0), markers[0].Tvec.at<float>(0,1), markers[0].Tvec.at<float>(0,2));	
		
        //for each marker, draw info and its boundaries in the image
        for(size_t i=0; i<markers.size(); ++i)
        {
          // only publishing the selected marker

          //----------------------add all pose messages of markers detected --------------------------------------
          //if(markers[i].id == marker_id)


          if(markers[i].id == 26 || markers[i].id == 35 || markers[i].id == 58 || markers[i].id == 163 || markers[i].id == 640 || markers[i].id == 512 || markers[i].id == 43 || markers[i].id == 291 || markers[i].id == 355)

          {
		  // by Weiwei
			//ROS_INFO("x: %0.3f y: %0.3f z: %0.3f", markers[i].Tvec.at<float>(0,0), markers[i].Tvec.at<float>(0,1), markers[i].Tvec.at<float>(0,2));		
			// 
			
            tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
			
			

			
            tf::StampedTransform cameraToReference;
            cameraToReference.setIdentity();

            if ( reference_frame != camera_frame )
            {
              getTransform(reference_frame,
                           camera_frame,
                           cameraToReference);
            }

            transform = 
              static_cast<tf::Transform>(cameraToReference) 
              * static_cast<tf::Transform>(rightToLeft) 
              * transform;

            tf::StampedTransform stampedTransform(transform, curr_stamp,
                                                  reference_frame, marker_frame);
            br.sendTransform(stampedTransform);
            geometry_msgs::PoseStamped poseMsg;
			
            tf::poseTFToMsg(transform, poseMsg.pose);
            poseMsg.header.frame_id = reference_frame;
            poseMsg.header.stamp = curr_stamp;


            double aruco_roll_, aruco_pitch_, aruco_yaw_;
            tf::Quaternion aruco_quat_;
            tf::quaternionMsgToTF(poseMsg.pose.orientation, aruco_quat_);
            tf::Matrix3x3(aruco_quat_).getRPY(aruco_roll_, aruco_pitch_, aruco_yaw_);

            //ROS_INFO("April Tag RPY: [%0.3f, %0.3f, %0.3f]", aruco_roll_*180/3.1415926, aruco_pitch_*180/3.1415926, aruco_yaw_*180/3.1415926);
//-------------------------unified coordinate systems of pose----------------------------
            aruco_yaw_ = aruco_yaw_*180/3.141592;
            printf("Original [x, y, yaw] = [%0.3f, %0.3f, %0.3f]\n", poseMsg.pose.position.x, poseMsg.pose.position.y, aruco_yaw_);

            if (markers[i].id == 26 || markers[i].id == 58)
            {
              float PI = 3.1415926;
              float ang = PI*3/4;
              float x_0 = -0.045;//-0.015;
              float y_0 = -0.015;//0.045;
              

              poseMsg.pose.position.x = x_0 + poseMsg.pose.position.x;// + poseMsg.pose.position.x * cos(-ang) - poseMsg.pose.position.y * sin(-ang); 
              poseMsg.pose.position.y = y_0 + poseMsg.pose.position.y;// + poseMsg.pose.position.x * sin(-ang) + poseMsg.pose.position.y * cos(-ang);

              //printf("[x, y] = [%0.3f, %0.3f]\n",poseMsg.pose.position.x, poseMsg.pose.position.y);

              aruco_yaw_ = aruco_yaw_ + ang*180/PI; 
              printf("[x, y, yaw] = [%0.3f, %0.3f, %0.3f]\n",poseMsg.pose.position.x, poseMsg.pose.position.y, aruco_yaw_);

              //printf("-----------unify the coordinate system ---------------------\n"); 
            }

            //printf("------------------------------------------------------------------\n-----------------aruco_yaw_ = %0.3f\n", aruco_yaw_);
            //printf("------------------------------------------------------------------\n");

            double temp_x = poseMsg.pose.position.x;
            double temp_y = poseMsg.pose.position.y;

            poseMsg.pose.position.x = -temp_y;
            poseMsg.pose.position.y = -temp_x;

            tf::Quaternion quat = tf::createQuaternionFromRPY(aruco_roll_, aruco_pitch_, aruco_yaw_);
            
            poseMsg.pose.orientation.x = quat.x();
            poseMsg.pose.orientation.y = quat.y();
            poseMsg.pose.orientation.z = quat.z();
            poseMsg.pose.orientation.w = quat.w();   

            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);
          }
          // but drawing all the detected markers
          markers[i].draw(inImage,cv::Scalar(0,0,255),2);
        }

        //draw a 3d cube in each marker if there is 3d info
        if(camParam.isValid() && marker_size!=-1)
        {
          for(size_t i=0; i<markers.size(); ++i)
          {
            CvDrawingUtils::draw3dAxis(inImage, markers[i], camParam);
          }
        }

        if(image_pub.getNumSubscribers() > 0)
        {
          //show input with augmented information
          cv_bridge::CvImage out_msg;
          out_msg.header.stamp = curr_stamp;
          out_msg.encoding = sensor_msgs::image_encodings::RGB8;
          out_msg.image = inImage;
          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.stamp = curr_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;
      }
    }
  }
Esempio n. 7
0
void image_callback(const sensor_msgs::ImageConstPtr& msg)
{
  double ticksBefore = cv::getTickCount();
  static tf::TransformBroadcaster br;
  if(cam_info_received)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
      inImage = cv_ptr->image;

      if(normalizeImageIllumination)
      {
        cv::Mat inImageNorm;
        pal_vision_util::dctNormalization(inImage, inImageNorm, dctComponentsToRemove);
        inImage = inImageNorm;
      }

      //detection results will go into "markers"
      markers.clear();
      //Ok, let's detect
      mDetector.detect(inImage, markers, camParam, marker_size);
      //for each marker, draw info and its boundaries in the image
      for(unsigned int i=0; i<markers.size(); ++i)
      {
        // only publishing the selected marker
        if ( markers[i].id == marker_id1 )
        {
          tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
          br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
                                                parent_name, child_name1));
          geometry_msgs::Pose poseMsg;
          tf::poseTFToMsg(transform, poseMsg);
          pose_pub1.publish(poseMsg);
        }
        else if ( markers[i].id == marker_id2 )
        {
          tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
          br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
                                                parent_name, child_name2));
          geometry_msgs::Pose poseMsg;
          tf::poseTFToMsg(transform, poseMsg);
          pose_pub2.publish(poseMsg);
        }

        // but drawing all the detected markers
        markers[i].draw(inImage,Scalar(0,0,255),2);
      }

      //paint a circle in the center of the image
      cv::circle(inImage, cv::Point(inImage.cols/2, inImage.rows/2), 4, cv::Scalar(0,255,0), 1);

      if ( markers.size() == 2 )
      {
        float x[2], y[2], u[2], v[2];
        for (unsigned int i = 0; i < 2; ++i)
        {
          ROS_DEBUG_STREAM("Marker(" << i << ") at camera coordinates = ("
                           << markers[i].Tvec.at<float>(0,0) << ", "
                           << markers[i].Tvec.at<float>(1,0) << ", "
                           << markers[i].Tvec.at<float>(2,0));
          //normalized coordinates of the marker
          x[i] = markers[i].Tvec.at<float>(0,0)/markers[i].Tvec.at<float>(2,0);
          y[i] = markers[i].Tvec.at<float>(1,0)/markers[i].Tvec.at<float>(2,0);
          //undistorted pixel
          u[i] = x[i]*camParam.CameraMatrix.at<float>(0,0) +
              camParam.CameraMatrix.at<float>(0,2);
          v[i] = y[i]*camParam.CameraMatrix.at<float>(1,1) +
              camParam.CameraMatrix.at<float>(1,2);
        }

        ROS_DEBUG_STREAM("Mid point between the two markers in the image = ("
                         << (x[0]+x[1])/2 << ", " << (y[0]+y[1])/2 << ")");

        //              //paint a circle in the mid point of the normalized coordinates of both markers
        //              cv::circle(inImage,
        //                         cv::Point((u[0]+u[1])/2, (v[0]+v[1])/2),
        //                         3, cv::Scalar(0,0,255), CV_FILLED);


        //compute the midpoint in 3D:
        float midPoint3D[3]; //3D point
        for (unsigned int i = 0; i < 3; ++i )
          midPoint3D[i] = ( markers[0].Tvec.at<float>(i,0) +
                            markers[1].Tvec.at<float>(i,0) ) / 2;
        //now project the 3D mid point to normalized coordinates
        float midPointNormalized[2];
        midPointNormalized[0] = midPoint3D[0]/midPoint3D[2]; //x
        midPointNormalized[1] = midPoint3D[1]/midPoint3D[2]; //y
        u[0] = midPointNormalized[0]*camParam.CameraMatrix.at<float>(0,0) +
            camParam.CameraMatrix.at<float>(0,2);
        v[0] = midPointNormalized[1]*camParam.CameraMatrix.at<float>(1,1) +
            camParam.CameraMatrix.at<float>(1,2);

        ROS_DEBUG_STREAM("3D Mid point between the two markers in undistorted pixel coordinates = ("
                         << u[0] << ", " << v[0] << ")");

        //paint a circle in the mid point of the normalized coordinates of both markers
        cv::circle(inImage,
                   cv::Point(u[0], v[0]),
                   3, cv::Scalar(0,0,255), CV_FILLED);

      }

      //draw a 3d cube in each marker if there is 3d info
      if(camParam.isValid() && marker_size!=-1)
      {
        for(unsigned int i=0; i<markers.size(); ++i)
        {
          CvDrawingUtils::draw3dCube(inImage, markers[i], camParam);
        }
      }

      if(image_pub.getNumSubscribers() > 0)
      {
        //show input with augmented information
        cv_bridge::CvImage out_msg;
        out_msg.header.stamp = ros::Time::now();
        out_msg.encoding = sensor_msgs::image_encodings::RGB8;
        out_msg.image = inImage;
        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.stamp = ros::Time::now();
        debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
        debug_msg.image = mDetector.getThresholdedImage();
        debug_pub.publish(debug_msg.toImageMsg());
      }

      ROS_DEBUG("runtime: %f ms",
                1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
  }
}
  void image_callback(const sensor_msgs::ImageConstPtr& msg)
  {
    static tf::TransformBroadcaster br;
    if(cam_info_received_)
    {
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
        inImage_ = cv_ptr->image;

        //clear out previous detection results
        markers_.clear();
        marker_msg_->markers.clear();

        //Ok, let's detect
        mDetector_.detect(inImage_, markers_, camParam_, marker_size_, false);
        marker_msg_->markers.resize(markers_.size());

        ros::Time curr_stamp(ros::Time::now());
        marker_msg_->header.stamp = curr_stamp;
        marker_msg_->header.seq++;

        //get the current transform from the camera frame to output ref frame
        tf::StampedTransform cameraToReference;
        cameraToReference.setIdentity();

        if ( reference_frame_ != camera_frame_ )
        {
          getTransform(reference_frame_,
              camera_frame_,
              cameraToReference);
        }

        //Now find the transform for each detected marker
        for(size_t i=0; i < markers_.size(); ++i)
        {
          tf::Transform transform = aruco_ros::arucoMarker2Tf(markers_[i]);
          transform = static_cast<tf::Transform>(cameraToReference) * transform;

          //Maybe change this to broadcast separate transforms for each marker?
          //tf::StampedTransform stampedTransform(transform, curr_stamp,
              //reference_frame_, marker_frame_);
          //br.sendTransform(stampedTransform);

          aruco_msgs::Marker & marker_i = marker_msg_->markers.at(i);
          tf::poseTFToMsg(transform, marker_i.pose.pose);
          marker_i.header.frame_id = reference_frame_;
          marker_i.header.stamp = curr_stamp;
          marker_i.id = markers_.at(i).id;
          marker_i.confidence = 1.0;

          markers_[i].draw(inImage_,cv::Scalar(0,0,255),2);
        }

        //publish marker array
        if (marker_msg_->markers.size() > 0)
          marker_pub_.publish(marker_msg_);

        //draw a 3d cube in each marker if there is 3d info
        if(camParam_.isValid() && marker_size_!=-1)
        {
          for(size_t i=0; i<markers_.size(); ++i)
            aruco::CvDrawingUtils::draw3dAxis(inImage_, markers_[i], camParam_);
        }

        if(image_pub_.getNumSubscribers() > 0)
        {
          //show input with augmented information
          cv_bridge::CvImage out_msg;
          out_msg.header.stamp = curr_stamp;
          out_msg.encoding = sensor_msgs::image_encodings::RGB8;
          out_msg.image = inImage_;
          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.stamp = curr_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());
      }
    }
  }
		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;
			}
		}
Esempio n. 11
0
 void Marker::calculateExtrinsics(float markerSize,const aruco::CameraParameters &CP)throw(cv::Exception)
 {
     if (!CP.isValid()) throw cv::Exception(9004,"!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
     calculateExtrinsics( markerSize,CP.CameraMatrix,CP.Distorsion);
 }
Esempio n. 12
0
int main() {
	
	

	// 打开摄像头
	video_capture.open(0);
	if (!video_capture.isOpened()){
		std::cerr << "Could not open video" << endl;
		return -1;
	}

	// 获取第一张图像,用于这设置参数
	video_capture >> input_image;

	// 读取摄像机参数
	camera_params.readFromXMLFile("camera.yml");
	camera_params.resize(input_image.size());

	// 注册窗口
	cv::namedWindow("thres",1);
	cv::namedWindow("in",1);

	marker_dector.getThresholdParams(threshold_param1, threshold_param2);
	i_threshold_param1 = threshold_param1;
	i_threshold_param2 = threshold_param2;

	cv::createTrackbar("ThresParam1", "in",&i_threshold_param1, 13, cvTackBarEvents);
	cv::createTrackbar("ThresParam2", "in",&i_threshold_param2, 13, cvTackBarEvents);

	char key=0;
	int index=0;

	//capture until press ESC or until the end of the video
	while( key!=27 && video_capture.grab())
	{
		//copy image
		video_capture.retrieve(input_image);
		
		//number of images captured
		index++; 

		//for checking the speed
		double tick = (double)cv::getTickCount();
		//Detection of markers in the image passed
		marker_dector.detect(input_image, markers, camera_params, marker_size);
		//chekc the speed by calculating the mean speed of all iterations
		average_time.first += ((double)cv::getTickCount() - tick) / cv::getTickFrequency();
		average_time.second++;
		std::cout << "Time detection=" << 1000 * average_time.first / average_time.second << " milliseconds" << endl;

		//print marker info and draw the markers in image
		input_image.copyTo(input_image_copy);
		for(unsigned int i = 0; i < markers.size(); i++){
			std::cout << markers[i] << std::endl;
			markers[i].draw(input_image, Scalar(0, 0, 255), 2);
		}

		//draw a 3d cube in each marker if there is 3d info
		if ( camera_params.isValid())
			for(unsigned int i = 0; i < markers.size(); i++){
				aruco::CvDrawingUtils::draw3dCube(input_image, markers[i], camera_params);
				aruco::CvDrawingUtils::draw3dAxis(input_image, markers[i], camera_params);
			}

			//DONE! Easy, right?
			cout << endl << endl << endl;
			//show input with augmented information and  the thresholded image
			cv::imshow("in", input_image);
			cv::imshow("thres", marker_dector.getThresholdedImage());

			key=cv::waitKey(0);//wait for key to be pressed
	}
	return 0;
}