C++ PinholeCameraModel::fromCameraInfo方法代码示例

 void camerainfoCb(const sensor_msgs::CameraInfoConstPtr& info_msg)
 {
     ROS_INFO("infocallback :shutting down camerainfosub");
     cam_model_.fromCameraInfo(info_msg);
     camera_topic = info_msg->header.frame_id;
     camerainfosub_.shutdown();
 }

void cam_info_cb(const sensor_msgs::CameraInfo::ConstPtr& msg)
{
	if( cam_model_.fromCameraInfo(msg) )
	{
		got_cam_info_ = true;
		ROS_INFO("[bk_skeletal_tracker] Got RGB camera info.");
	} else {
		ROS_ERROR("[bk_skeletal_tracker] Couldn't read camera info.");
	}
}

 void infoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)
 {
   if (calibrated)
     return;
   cam_model_.fromCameraInfo(info_msg);
   pattern_detector_.setCameraMatrices(cam_model_.intrinsicMatrix(), cam_model_.distortionCoeffs());
   
   calibrated = true;
   image_sub_ = nh_.subscribe("/camera/rgb/image_mono", 1, &CalibrateKinectCheckerboard::imageCallback, this);
   
   ROS_INFO("[calibrate] Got image info!");
 }

void doOverlay(const sensor_msgs::ImageConstPtr& img_msg,
               const sensor_msgs::CameraInfoConstPtr& info_msg) {

    // convert camera image into opencv
    cam_model.fromCameraInfo(info_msg);
    cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::RGB8);

    double alpha_mult;
    ros::param::param<double>("~alpha_mult", alpha_mult, 0.5);

    uint8_t r, g, b;
    if(aligned_pc) {
        if(!tf_list->waitForTransform(img_msg->header.frame_id, "/base_link",
                                     img_msg->header.stamp, ros::Duration(3)))
            return;
        tf::StampedTransform transform;
        tf_list->lookupTransform(img_msg->header.frame_id, "/base_link", 
                                img_msg->header.stamp, transform);
        PCRGB::Ptr tf_pc(new PCRGB());
        pcl_ros::transformPointCloud<PRGB>(*aligned_pc, *tf_pc, transform);
        for(uint32_t i=0;i<tf_pc->size();i++) {
            cv::Point3d proj_pt_cv(tf_pc->points[i].x, tf_pc->points[i].y, 
                                   tf_pc->points[i].z);
            cv::Point pt2d = cam_model.project3dToPixel(proj_pt_cv);
            extractRGB(tf_pc->points[i].rgb, r, g, b);
            if(pt2d.x >= 0 && pt2d.y >= 0 && 
               pt2d.x < cv_img->image.rows && pt2d.y < cv_img->image.cols) {
                double old_r = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0];
                double old_g = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1];
                double old_b = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2];
                cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0] = 
                    (uint8_t) min(alpha_mult*old_r+r, 255.0);
                cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1] = 
                    (uint8_t) min(alpha_mult*old_g+g, 255.0);
                cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2] = 
                    (uint8_t) min(alpha_mult*old_b+b, 255.0);
            }
        }
    }
    
    overlay_pub.publish(cv_img->toImageMsg());
}

void FeatureTracker::image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
  //need pose data for each picture, need to publish a camera pose
  ros::Time acquisition_time = msg->header.stamp;
  geometry_msgs::PoseStamped basePose;
  geometry_msgs::PoseStamped mapPose;
  basePose.pose.orientation.w=1.0;
  ros::Duration timeout(3);
  basePose.header.frame_id="/base_link";
  mapPose.header.frame_id="/map";
  
  try {
    tf_listener_.waitForTransform("/camera_1_link", "/map", acquisition_time, timeout);
    tf_listener_.transformPose("/map", acquisition_time,basePose,"/camera_1_link",mapPose);
    printf("pose #%d %f %f %f\n",pic_number++,mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation));
  }
  catch (tf::TransformException& ex) {
    ROS_WARN("[map_maker] TF exception:\n%s", ex.what());
    printf("[map_maker] TF exception:\n%s", ex.what());
    return;
  }
  cam_model.fromCameraInfo(info_msg);
  
  
  
  
  // printf("callback called\n");
  try
  {
    // if you want to work with color images, change from mono8 to bgr8
    if(image_rect==NULL){
      image_rect = cvCloneImage(bridge.imgMsgToCv(msg, "mono8"));
      last_image= cvCloneImage(bridge.imgMsgToCv(msg, "mono8"));
      pyrA=cvCreateImage(cvSize(last_image->width+8,last_image->height/3.0), IPL_DEPTH_32F, 1);
      pyrB=cvCloneImage(pyrA);
      //  printf("cloned image\n");
    }
    else{
      //save the last image
      cvCopy(image_rect,last_image);
      cvCopy(bridge.imgMsgToCv(msg, "mono8"),image_rect);
      // printf("copied image\n");
    }
    if(output_image==NULL){
      output_image =cvCloneImage(image_rect);
    }
    if(eigImage==NULL){
      eigImage =cvCloneImage(image_rect);
    }
    if(tempImage==NULL){
      tempImage =cvCloneImage(image_rect);
    }
  }
  catch (sensor_msgs::CvBridgeException& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());
    return;
  }
  
  if(image_rect!=NULL) {
    cvCopy(image_rect,output_image);
    
    printf("got image\n");
    
    track_features(mapPose);
    
    //draw features on the image
    for(int i=0;i<last_feature_count;i++){
      CvPoint center=cvPoint((int)features[i].x,(int)features[i].y);
      cvCircle(output_image,center,10,cvScalar(150),2);
      
      char strbuf [10];
      
      int n=sprintf(strbuf,"%d",current_feature_id[ i] );
      std::string text=std::string(strbuf,n);
      
      CvFont font;
      
      cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,1,1);
      
      cvPutText(output_image,text.c_str(),cvPoint(center.x,center.y+20),&font,cvScalar(255));
      
      
      cv::Point3d tempRay;
      cv::Point2d tempPoint=cv::Point2d(features[i]);
      cam_model.projectPixelTo3dRay(tempPoint,tempRay);
  //    printf("%f x  %f y  %f z\n",tempRay.x,tempRay.y,tempRay.z);
    }
    
  //  featureList[0].print();
    
    //determine error gradient
    
    int min_features=10;
    
  //  printf("ypr %f %f %f\n",yaw,pitch,roll);
    
    cv::Point3d error_sum=calc_error(min_features,0, 0, 0);
    printf("total error is : %f\n",error_sum.x);
    
    for(int i=0;i<featureList.size();i++){
//.........这里部分代码省略.........

   void imageCb2(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
      
      MySecond.clear();
      Ccount++;

      cam_model_right.fromCameraInfo(info_msg);

      cv_bridge::CvImagePtr cv_ptr;
      try
      {
         cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
      }
      catch (cv_bridge::Exception& e)
      {
         ROS_ERROR("cv_bridge exception: %s", e.what());
         return;
      }

      if (Ccount == 0)
         return;

      Mat image, gray, temp, mask;
      image = cv_ptr->image;

      mask = image.clone();

      int niters = 1;
      dilate(mask, temp, Mat(), Point(-1, -1), niters);
      erode(temp, temp, Mat(), Point(-1, -1), niters * 2);
      dilate(temp, temp, Mat(), Point(-1, -1), niters);

      threshold(temp, temp, 200, 255, CV_THRESH_BINARY);

      for (int i = 0; i <= 5; i++) {
         for (int j = 1000; j < temp.cols; j++) {
            temp.at<uchar>(i, j) = 0;
         }
      }

      vector<vector<Point> > contours1, contours2;
      vector<Vec4i> hierarchy;
      findContours(temp, contours2, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

      int cmin = 0;
      int cmax = 100;

      for (int i = 0; i < contours2.size(); i++) {
         if (contours2[i].size() < cmin || contours2[i].size() > cmax) {
            continue;
         }
         else
            contours1.push_back(contours2[i]);
      }


      image = Scalar(255, 255, 255);

      std::vector<std::vector<cv::Point> > ::iterator itc;
      itc = contours1.begin();

      MyPoint TTEMP;
      while (itc != contours1.end()) {

         cv::Moments mom = cv::moments(cv::Mat(*itc++));
#ifdef DEBUG
         cv::circle(image, cv::Point(mom.m10 / mom.m00, mom.m01 / mom.m00), 2, cv::Scalar(0), 2);
#endif
         TTEMP.x = mom.m10 / mom.m00;
         TTEMP.y = mom.m01 / mom.m00;

         if (TTEMP.x == 0 && TTEMP.y == 0)
            return;

         MySecond.push_back(TTEMP);
      }

#ifdef DEBUG
      cv::imshow(OPENCV_WINDOW2, cv_ptr->image);
      cv::waitKey(3);
#endif
   }

   void imageCb(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
      MyFirst.clear();
      Ccount1++;
      cv_bridge::CvImageConstPtr cv_ptr;
      double cur = ros::Time::now().toSec();

      // lpf_x.set_cutoff_freq(0);
      // lpf_x.get_lpf(10);
      try
      {
         cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
      }
      catch (cv_bridge::Exception& e)
      {
         ROS_ERROR("cv_bridge exception: %s", e.what());
         return;
      }

      cam_model_left.fromCameraInfo(info_msg);
      // cout << "dist " << info_msg->D[0] << " " << info_msg->D[1] << " " << info_msg->D[2] << " " <<  endl;

      Mat image, gray, temp, mask;
      image = cv_ptr->image;

      mask = image.clone();

      int niters = 1;
      dilate(mask, temp, Mat(), Point(-1, -1), niters);
      erode(temp, temp, Mat(), Point(-1, -1), niters * 2);
      dilate(temp, temp, Mat(), Point(-1, -1), niters);

      threshold(temp, temp, 200, 255, CV_THRESH_BINARY);

      for (int i = 0; i <= 5; i++) {
         for (int j = 1000; j < temp.cols; j++) {
            temp.at<uchar>(i, j) = 0;
         }
      }

      vector<vector<Point> > contours1, contours2;
      vector<Vec4i> hierarchy;
      findContours(temp, contours2, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);


      int cmin = 0;
      int cmax = 100;


      for (int i = 0; i < contours2.size(); i++) {
         if (contours2[i].size() < cmin || contours2[i].size() > cmax) {
            continue;
         }
         else
            contours1.push_back(contours2[i]);
      }

      image = Scalar(255, 255, 255);

      std::vector<std::vector<cv::Point> > ::iterator itc;
      itc = contours1.begin();

      MyPoint TTEMP;
      while (itc != contours1.end()) {

         cv::Moments mom = cv::moments(cv::Mat(*itc++));
#ifdef DEBUG
         cv::circle(image, cv::Point(mom.m10 / mom.m00, mom.m01 / mom.m00), 2, cv::Scalar(0), 2);
#endif
         TTEMP.x = mom.m10 / mom.m00;
         TTEMP.y = mom.m01 / mom.m00;

         MyFirst.push_back(TTEMP);
      }


#ifdef DEBUG
      cv::imshow(OPENCV_WINDOW, cv_ptr->image);
      //Rect rect(MyFirst[0].x - 50, MyFirst[0].y- 50, 100, 100);
      //Mat subimage = cv_ptr->image(rect);
      //cv::imshow(OPENCV_WINDOW, subimage);
      cv::waitKey(3);
#endif

      vector<My3DPoint> myvec;
      My3DPoint myTempVec;

#ifdef DEBUG
      //cout << Ccount << " " << MyFirst[0].y << " " << MySecond[0].y << endl;
#endif
      // cout << "size : " << MyFirst.size() << "," << MySecond.size() << endl;

      if (MyFirst.size() && MySecond.size()) {
         // cout << MyFirst[0].x << "," << MyFirst[0].y << endl;
         cv::Point2d left_pt(MyFirst[0].x, MyFirst[0].y);
         cv::Point2d right_pt(MySecond[0].x, MySecond[0].y);
         cam_model_left.rectifyPoint(left_pt);

            Matrix<float, 3, 1> Z;
            Matrix<float, 6, 1> RES_KALMAN;
            Z<< 
//.........这里部分代码省略.........

void depthCallback(const sensor_msgs::ImageConstPtr& original_image, const sensor_msgs::CameraInfoConstPtr& info){

    if(need_for_wall==2){
        need_for_wall = 0;
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(original_image, enc::TYPE_16UC1);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
            return;
        }
        cam_model.fromCameraInfo(info);
        Mat depth_image = cv_ptr->image;

        float x = 0;
        float y = 360;
        float wall_height = 0;
        geometry_msgs::PointStamped xy_point_stamped, pipe_point_stamped;
        walls_detection::Walls walls_all_message;
        for(int i = 100; i<600; i+=110){
            wall_height = 0;
            y = 360;
            x = i;
            unsigned short wall_depth;
            while(wall_height < 0.05 && y > 0){
                    cv::Point2d uv_point(x,y);
                    unsigned short wall_depth;
                    wall_depth = depth_image.at<unsigned short>(y, x);
                    cv::Point3d xy_point;
                    xy_point = cam_model.projectPixelTo3dRay(uv_point);
                    xy_point = xy_point * wall_depth;

                    xy_point_stamped.header.frame_id = "/camera_rgb_optical_frame";
                    xy_point_stamped.header.stamp = ros::Time::now();

                    xy_point_stamped.point.x = 0.001*xy_point.x;
                    xy_point_stamped.point.y = 0.001*xy_point.y;
                    xy_point_stamped.point.z = 0.001*xy_point.z;

                    try {
                            tf_listener->waitForTransform("/pipe_link", "/camera_rgb_optical_frame", ros::Time::now(), ros::Duration(10.0) );
                            tf_listener->transformPoint("/pipe_link", xy_point_stamped, pipe_point_stamped);
                    }
                    catch (tf::TransformException ex) {
                            ROS_ERROR("%s",ex.what());
                    }

                wall_height = pipe_point_stamped.point.z;
                y = y - 120;
            }
            std::cout<<"Sciana nr "<< i <<" - x: "<< pipe_point_stamped.point.x <<", y: "<< pipe_point_stamped.point.y <<", z: "<< pipe_point_stamped.point.z <<"\n";
            if(wall_height < 0.05){
                std::cout << "Nie ma sciany\n";
                pipe_point_stamped.point.x = 0;
                pipe_point_stamped.point.y = 0;
                pipe_point_stamped.point.z = 0;
            }
            switch(i){
                case 100:
                walls_all_message.wall1 = pipe_point_stamped;
                break;

                case 210:
                walls_all_message.wall2 = pipe_point_stamped;
                break;

                case 320:
                walls_all_message.wall3 = pipe_point_stamped;
                break;

                case 430:
                walls_all_message.wall4 = pipe_point_stamped;
                break;

                case 540:
                walls_all_message.wall5 = pipe_point_stamped;
                break;

            }

	}
    walls_all.publish(walls_all_message);
    }
}

void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& infoMsg) 
{
	cameraModel.fromCameraInfo(infoMsg);
}

void depthCallback(const sensor_msgs::ImageConstPtr& original_image, const sensor_msgs::CameraInfoConstPtr& info)
{
    if(is_robot_running==0 && balls_written==1 && ball_chosen==0){
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(original_image, sensor_msgs::image_encodings::TYPE_16UC1);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
            return;
        }

        cam_model.fromCameraInfo(info);
        Mat depth_image = cv_ptr->image;

        if(balls_written == 1){
            for( size_t i = 0; i < circles_all.size(); i++ ){
                if (pilki[i]!=0){
                    cv::Point2d uv_point(circles_all[i][0], circles_all[i][1]);
                    unsigned short ball_depth;
                    ball_depth = depth_image.at<unsigned short>(circles_all[i][1], circles_all[i][0])+20;
                    cv::Point3d xy_point;
                    xy_point = cam_model.projectPixelTo3dRay(uv_point);
                    xy_point = xy_point * ball_depth;

                    geometry_msgs::PointStamped xy_point_stamped, odom_point_stamped;

                    xy_point_stamped.header.frame_id = "/camera_rgb_optical_frame";
                    xy_point_stamped.header.stamp = ros::Time::now();

                    xy_point_stamped.point.x = 0.001*xy_point.x;
                    xy_point_stamped.point.y = 0.001*xy_point.y;
                    xy_point_stamped.point.z = 0.001*xy_point.z;

                    try {
                        tf_listener->waitForTransform("/base_link", "/camera_rgb_optical_frame", ros::Time::now(), ros::Duration(10.0) );
                        tf_listener->transformPoint("/base_link", xy_point_stamped, odom_point_stamped);
                    }
                    catch (tf::TransformException ex) {
                        ROS_ERROR("%s",ex.what());
                    }

                    if(odom_point_stamped.point.z > 0.05 || odom_point_stamped.point.z < 0.001){
			std::cout << "Srodek pilki na wysokosci: " << odom_point_stamped.point.z << ".\n"; 
                        std::cout << "Skondensowane pilki: to nie pilka, jest za wysoko albo za nisko! \n";
                        pilki[i]=0;
                    }
                }
            }

            depth_written = 1;
            std::cout << "Policzylem wysokosc dla skondensowanych pilek \n";
            int closest_ball = choose_closest_ball();

            if(closest_ball == -1){
                std::cout << "NIE WYBRANO ZADNEJ PILKI \n";
                send_no_balls();
            }

            if(closest_ball != -1){
                int wybrana_x = circles_all[closest_ball][0];
                int wybrana_y = circles_all[closest_ball][1];
                int wybrana_z = circles_all[closest_ball][2];

                ball_chosen = 1;
                std::cout << "Wybrana jedna pilka \n";

                last_circle[0] = wybrana_x;
                last_circle[1] = wybrana_y;
                last_circle[2] = wybrana_z;

                wybrana.x = last_circle[0];
                wybrana.y = last_circle[1];
                wybrana.z = last_circle[2];

                draw_balls = 1;

                cv::Point2d uv_point(wybrana.x, wybrana.y);
                unsigned short ball_depth;

                ball_depth = depth_image.at<unsigned short>(wybrana.y,wybrana.x)+20;

                geometry_msgs::Point message_selected;
                cv::Point3d xy_point;
                xy_point = cam_model.projectPixelTo3dRay(uv_point);
                xy_point = xy_point * ball_depth;

                geometry_msgs::PointStamped xy_point_stamped, odom_point_stamped;

                xy_point_stamped.header.frame_id = "/camera_rgb_optical_frame";
                xy_point_stamped.header.stamp = ros::Time::now();

                xy_point_stamped.point.x = 0.001*xy_point.x;
                xy_point_stamped.point.y = 0.001*xy_point.y;
                xy_point_stamped.point.z = 0.001*xy_point.z;

                try {
                    tf_listener->waitForTransform("/odom", "/camera_rgb_optical_frame", ros::Time::now(), ros::Duration(10.0) );
//.........这里部分代码省略.........

	void infoCb(const sensor_msgs::CameraInfo& msg)
	{
		model.fromCameraInfo(msg);
		//SHUTDOWN LATER #HACK
	}