image_geometry project3d to pixel
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:
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_);
}
}