ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

image_geometry project3d to pixel

asked 2011-02-24 19:35:01 -0500

KoenBuys gravatar image

I'm getting multiple projections of 3d points to the same pixel which don't coincide with the 3D ray but parallel with the image plane. This can be seen here:

image description

I'm unable to find an error in my code or in the re-projection function (CODE API)

This is my code fragment in which I calculate it:

void calculateVoxels (void)
{
 Point2d uv;
 Point3d pt_cv;
 int width, height;
 double vox_number = 0;

 width = cam_model_.reducedResolution().width;
 height = cam_model_.reducedResolution().height;

 cloud_.width    = WORLD_RESOLUTION;
 cloud_.height   = 1;
 cloud_.is_dense = false;
 cloud_.points.resize (WORLD_RESOLUTION);
 ///Only calculate if the image and the camera info are received.
 if(image_rec_ & cam_info_rec_){
 double x, y , z;
 int a=0;
 int b=0;
 int c=0;
 for(x = WORLD_BEGIN_X; x < WORLD_MAX_X; a++){
  b = 0;
  for(y = WORLD_BEGIN_Y; y < WORLD_MAX_Y; b++){
    c = 0;
    for(z = WORLD_BEGIN_Z; z < WORLD_MAX_Z; c++){
      pt_cv = Point3d(x, y, z);

      ///WARNING!: (u,v) in rectified pixel coordinates!
      uv = cam_model_.project3dToPixel(pt_cv);

      ///@todo : check this, might be inverse
      ///Only if it falls on the image plane
      if((uv.x < height) && (uv.y < width) && (uv.x >=0) && (uv.y >= 0)){

        if(current_image_.at<int>(uv) != 0){
          world_[a][b][c] = 255;           ///Start indexing from 0,0,0 in the array!

          ///Also create the pointcloud
          cloud_.points[vox_number].x = x;
          cloud_.points[vox_number].y = y;
          cloud_.points[vox_number].z = z;
          vox_number ++;
        }
        else
          world_[a][b][c] = 0;
      }
      z= z + WORLD_STEP_Z;
    }
    y= y + WORLD_STEP_Y;
  }
  x= x + WORLD_STEP_X;
}

calc_voxels_ = true; 
///Trigger a update of the OpenGL world
if(do_opengl_){
  glutPostRedisplay();
}

///Save the pointcloud
cloud_.width = vox_number;            ///minimize the size of the pointcloud
cloud_.points.resize (vox_number);    ///maximal  vox_number points written
if(save_cloud_){
  pcl::io::savePCDFileASCII (PCL_PCD_SAVE, cloud_);
  ROS_INFO ("Saved %d data points to %s", (int)cloud_.points.size (), PCL_PCD_SAVE);
}

cloud_.header.frame_id = "world";
cloud_.header.stamp = ros::Time::now ();
pub_.publish(cloud_);

}
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2011-03-01 05:09:25 -0500

KoenBuys gravatar image

The cam_info_rec_ variable is only true if a correct camera_info callback happened. Furthermore all WORLD_x variables are defines and varying them gives the correct response (except for my problem). I verified the projection routine from image_geometry and found no bugs (except for the limited implementation (see my question on multiple camera set-up )).

edit flag offensive delete link more
0

answered 2011-03-01 03:31:48 -0500

raphael favier gravatar image

updated 2011-03-01 03:33:39 -0500

Hey Koen,

a few basic checks :

  • are you confident with the correctness of your different WORLD_X constants?
  • is your camera model initialized correctly? Via a call to fromCameraInfo(info_msg) for instance. (see code line 43 of the tf to image tutorial)

Hope it helps

Raph

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2011-02-24 19:35:01 -0500

Seen: 1,260 times

Last updated: Mar 01 '11