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

  void overlayPoints(pcl::PointCloud<pcl::PointXYZ> detector_points, tf::Transform &transform, cv_bridge::CvImagePtr& image)
  {  
    // Overlay calibration points on the image
    pcl::PointCloud<pcl::PointXYZ> transformed_detector_points;
    
    pcl_ros::transformPointCloud(detector_points, transformed_detector_points, transform);
    
    int font_face = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
    double font_scale = 1;
    int thickness = 2;
    int radius = 5;
   
    for (unsigned int i=0; i < transformed_detector_points.size(); i++)
    {
      pcl::PointXYZ pt = transformed_detector_points[i];
      cv::Point3d pt_cv(pt.x, pt.y, pt.z);
      cv::Point2d uv;
      uv = cam_model_.project3dToPixel(pt_cv);

      cv::circle(image->image, uv, radius, CV_RGB(255,0,0), -1);
      cv::Size text_size;
      int baseline = 0;
      std::stringstream out;
      out << i+1;
      
      text_size = cv::getTextSize(out.str(), font_face, font_scale, thickness, &baseline);
                            
      cv::Point origin = cvPoint(uv.x - text_size.width / 2,
                               uv.y - radius - baseline/* - thickness*/);
      cv::putText(image->image, out.str(), origin, font_face, font_scale, CV_RGB(255,0,0), thickness);
    }
  }

 void FindObjectOnPlane::generateStartPoints(
   const cv::Point2f& point_2d,
   const image_geometry::PinholeCameraModel& model,
   const pcl::ModelCoefficients::Ptr& coefficients,
   std::vector<cv::Point3f>& search_points_3d,
   std::vector<cv::Point2f>& search_points_2d)
 {
   NODELET_INFO("generateStartPoints");
   jsk_recognition_utils::Plane::Ptr plane
     (new jsk_recognition_utils::Plane(coefficients->values));
   cv::Point3d ray = model.projectPixelTo3dRay(point_2d);
   Eigen::Vector3f projected_point = rayPlaneInteersect(ray, plane);
   const double resolution_step = 0.01;
   const int resolution = 5;
   search_points_3d.clear();
   search_points_2d.clear();
   for (int i = - resolution; i < resolution; ++i) {
     for (int j = - resolution; j < resolution; ++j) {
       double x_diff = resolution_step * i;
       double y_diff = resolution_step * j;
       Eigen::Vector3f moved_point = projected_point + Eigen::Vector3f(x_diff, y_diff, 0);
       Eigen::Vector3f projected_moved_point;
       plane->project(moved_point, projected_moved_point);
       cv::Point3f projected_moved_point_cv(projected_moved_point[0],
                                            projected_moved_point[1],
                                            projected_moved_point[2]);
       search_points_3d.push_back(cv::Point3f(projected_moved_point_cv));
       cv::Point2d p2d = model.project3dToPixel(projected_moved_point_cv);
       search_points_2d.push_back(p2d);
     }
   }
 }

void projectPoints(const image_geometry::PinholeCameraModel &cam_model,
                   const cv::Point3d &points3D,
                   cv::Point2d *points2D)
{
  *points2D = cam_model.project3dToPixel(points3D);
//   *points2D = cam_model.rectifyPoint(*points2D);
}

 bool objectSrv(jsk_smart_gui::point2screenpoint::Request &req,
                jsk_smart_gui::point2screenpoint::Response &res)
 {
     ROS_INFO("3dtopixel request:x=%lf,y=%lf,z=%lf",req.point.point.x,req.point.point.y,req.point.point.z);
     geometry_msgs::PointStamped point_transformed;
     tf_listener_.transformPoint(camera_topic, req.point, point_transformed);
     cv::Point3d xyz;
     cv::Point2d uv_rect;
     xyz.x = point_transformed.point.x;
     xyz.y = point_transformed.point.y;
     xyz.z = point_transformed.point.z;
     uv_rect = cam_model_.project3dToPixel(xyz);
     res.x=uv_rect.x;
     res.y=uv_rect.y;
     return true;
 }

 cv::Point2d FindObjectOnPlane::getUyEnd(
   const cv::Point2d& ux_start,
   const cv::Point2d& ux_end,
   const image_geometry::PinholeCameraModel& model,
   const jsk_recognition_utils::Plane::Ptr& plane)
 {
   cv::Point3d start_ray = model.projectPixelTo3dRay(ux_start);
   cv::Point3d end_ray = model.projectPixelTo3dRay(ux_end);
   Eigen::Vector3f ux_start_3d = rayPlaneInteersect(start_ray, plane);
   Eigen::Vector3f ux_end_3d = rayPlaneInteersect(end_ray, plane);
   Eigen::Vector3f ux_3d = ux_end_3d - ux_start_3d;
   Eigen::Vector3f normal = plane->getNormal();
   Eigen::Vector3f uy_3d = normal.cross(ux_3d).normalized();
   Eigen::Vector3f uy_end_3d = ux_start_3d + uy_3d;
   cv::Point2d uy_end = model.project3dToPixel(cv::Point3d(uy_end_3d[0],
                                                           uy_end_3d[1],
                                                           uy_end_3d[2]));
   return uy_end;
 }

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

	inline void
	filter_depth_projection(	const image_geometry::PinholeCameraModel &camera_model,
								const pcl::PointCloud<PointT> &in,
								pcl::PointCloud<PointT> &out,
								int rows,
								int cols,
								int k_neighbors = 2,
								float threshold = 0.3)
	{
		std::vector<std::vector<bool> > hit( cols, std::vector<bool>(rows));
		std::vector<int> points2d_indices;
		pcl::PointCloud<pcl::PointXY> points2d;
		pcl::PointCloud<PointT> z_filtred;

#ifdef DEBUG
		std::cout << "in points: "<< in.size() << " width: " << cols << " height: " << rows << "\n";
#endif

		project2d::Points2d<PointT> point_map;
		point_map.init(camera_model, in, rows, cols);
		point_map.get_points(z_filtred, 25);

		// Project points into image space
		for(unsigned int i=0; i < z_filtred.size();  ++i){
			const PointT* pt = &z_filtred.points.at(i);
			//if( pt->z > 1)
			{ // min distance from camera 1m

				cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));

				if( between<int>(0, point_image.x, cols )
					&& between<int>( 0, point_image.y, rows )
				)
				{
					// Point allready at this position?
					if(!hit[point_image.x][point_image.y]){
						hit[point_image.x][point_image.y] = true;

						pcl::PointXY p_image;
						p_image.x = point_image.x;
						p_image.y = point_image.y;

						points2d.push_back(p_image);
						points2d_indices.push_back(i);
					}
					else{
#ifdef DEBUG
						std::cout << "[" << point_image.x << "][" << point_image.y << "] already inserted " << pt << "\n";
#endif

					}

				}
			}
		}

#ifdef DEBUG
		std::cout << "Z_filtred: " << z_filtred.size() << " projected 2d points: "<< points2d.size() << " indices: " << points2d_indices.size() << "\n";
#endif

		pcl::search::KdTree<pcl::PointXY> tree_n;
		tree_n.setInputCloud(points2d.makeShared());
		tree_n.setEpsilon(0.5);

		for(unsigned int i=0; i < points2d.size(); ++i){
			std::vector<int> k_indices;
			std::vector<float> square_distance;

			//tree_n.nearestKSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
			tree_n.radiusSearch(points2d.at(i), k_neighbors, k_indices, square_distance);

			look_up_indices(points2d_indices, k_indices);

			float distance_value;
			if(is_edge_threshold(z_filtred, points2d_indices.at(i), k_indices, square_distance, distance_value, threshold)){
				out.push_back(z_filtred.points.at(points2d_indices.at(i)));
				out.at(out.size()-1).intensity = sqrt(distance_value);
			}
		}

#ifdef DEBUG
		std::cout << "out 2d points: "<< out.size() << "\n";
#endif
	}

	inline void
	hit_same_point(	const image_geometry::PinholeCameraModel &camera_model,
								const pcl::PointCloud<PointT> &in,
								pcl::PointCloud<PointT> &out,
								int rows,
								int cols,
								float z_threshold = 0.3)
	{
		std::vector<std::vector <std::vector<PointT> > > hit( cols, std::vector< std::vector<PointT> >(rows, std::vector<PointT>()));

		// Project points into image space
		for(unsigned int i=0; i < in.points.size();  ++i){
			const PointT* pt = &in.points.at(i);
			if( pt->z > 1) { // min distance from camera 1m

				cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));

				if( between<int>(0, point_image.x, cols )
					&& between<int>( 0, point_image.y, rows )
				)
				{
					// Sort all points into image
					{
						hit[point_image.x][point_image.y].push_back(in.points.at(i));
					}

				}
			}
		}
		assert(out.empty());
		for(int x = 0; x < hit.size(); ++x ){
			for(int y = 0; y < hit[0].size(); ++y){
				if(hit[x][y].size()>1){
					PointT min_z = hit[x][y][0];
					float max_z = min_z.z;
					for(int p = 1; p < hit[x][y].size(); ++p){
					// find min and max z
						max_z = MAX(max_z, hit[x][y][p].z);
#ifdef DEBUG
						std::cout << hit[x][y].size() << "\t";
#endif

						if(hit[x][y][p].z < min_z.z)
						{
							min_z = hit[x][y][p];
						}
					}
#ifdef DEBUG
					std::cout << min_z.z << "\t" << max_z << "\t";
#endif
					if(max_z - min_z.z > z_threshold){
#ifdef DEBUG
						std::cout << min_z << std::endl;
#endif
						out.push_back(min_z);
					}
				}
			}
		}
#ifdef DEBUG
		std::cout << "hit_same_point in: "<< in.size()  << "\t out: " << out.size() << "\n";
#endif
	}

	inline void
	remove_cluster_2d(	const image_geometry::PinholeCameraModel &camera_model,
								const pcl::PointCloud<PointT> &in,
								pcl::PointCloud<PointT> &out,
								int rows,
								int cols,
								int k_neighbors = 4,
								int border = 25)
	{
		std::vector<int> points2d_indices;
		pcl::PointCloud<pcl::PointXY> points2d;

#ifdef DEBUG
		std::cout << "in points: "<< in.size() << "\n";
#endif

		// Project points into image space
		for(unsigned int i=0; i < in.points.size();  ++i){
			const PointT* pt = &in.points.at(i);
			if( pt->z > 1) { // min distance from camera 1m

				cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));

				if( between<int>(0+border, point_image.x, cols-border )
					&& between<int>( 0+border, point_image.y, rows-border )
				)
				{
					pcl::PointXY p_image;
					p_image.x = point_image.x;
					p_image.y = point_image.y;

					points2d.push_back(p_image);
					points2d_indices.push_back(i);
				}
			}
		}

#ifdef DEBUG
		std::cout << "projected 2d points: "<< points2d.size() << " indices: " << points2d_indices.size() << "\n";
#endif

		pcl::search::KdTree<pcl::PointXY> tree_n;
		tree_n.setInputCloud(points2d.makeShared());
		tree_n.setEpsilon(0.1);

		for(unsigned int i=0; i < points2d.size(); ++i){
			std::vector<int> k_indices;
			std::vector<float> square_distance;

			//tree_n.nearestKSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
			tree_n.radiusSearch(points2d.at(i), k_neighbors, k_indices, square_distance);

			if(k_indices.empty()) continue; // Dont add points without neighbors

			look_up_indices(points2d_indices, k_indices);

			float edginess = 0;
			if(is_edge_z(in, points2d_indices.at(i), k_indices, square_distance, edginess, 0.1)){
				out.push_back(in.points.at(points2d_indices.at(i)));
				out.at(out.size()-1).intensity = edginess;
			}
		}

#ifdef DEBUG
		std::cout << "out 2d points: "<< out.size() << "\n";
#endif
	}