Convert depthInMillimters to pointcloud


#1

Hi,

I’m having trouble converting depthInMillimeters to a pointcloud. The x/y/z values seem to be inaccurate by some constant factor but I can’t figure it out. The code is similar to others I’ve seen, and this is on an iPad 5th generation. I don’t think the model matters since I am only interested in depth information. Is this code correct? If not, how can I improve it?

PointCloudT::Ptr toPointCloud(DepthFrame const & frame,
                              GLKMatrix4 const & cameraPose) {
  PointCloudT::Ptr cloud(new PointCloudT());

#define QVGA_COLS 320
#define QVGA_ROWS 240
#define QVGA_F_X 305.73
#define QVGA_F_Y 305.62
#define QVGA_C_X 159.69
#define QVGA_C_Y 119.86

  auto _fx = QVGA_F_X/QVGA_COLS*frame.width;
  auto _fy = QVGA_F_X/QVGA_ROWS*frame.height;
  auto _cx = QVGA_C_X/QVGA_COLS*frame.width;
  auto _cy = QVGA_C_Y/QVGA_ROWS*frame.height;

  for(int row = 0; row < frame.height; ++row) {
    for(int col = 0; col < frame.width; ++col) {
      int idx = row*frame.width + col;
      auto depth = frame.depthInMillimeters[idx]*0.001f;
      if(isnan(depth)) continue;
      PointT point;
      point.x = depth * ((float)col - _cx) / _fx;
      point.y = depth * (_cy - (float)row) / _fy;
      point.z = depth;
      cloud->push_back(point);
    }
  }

  return cloud;
}