Esempio n. 1
0
void
StereoDisparity::getXyz(OdometryFrame * odom_frame)
{
  int num_levels = odom_frame->getNumLevels();
  for(int level_num=0; level_num < num_levels; ++level_num) {
    PyramidLevel* level = odom_frame->getLevel(level_num);
    int num_kp = level->getNumKeypoints();
    //std::cout << level_num << " | number of keypoints: " << num_kp << "\n";

    for(int kp_ind=0; kp_ind < num_kp; ++kp_ind) {
      KeypointData* kpdata(level->getKeypointData(kp_ind));

      int u = (int)(kpdata->rect_base_uv(0)+0.5);
      int v = (int)(kpdata->rect_base_uv(1)+0.5);

      kpdata->disparity = _disparity_data[(int) v*_width + (int) u];
      if (kpdata->disparity == MIN_DISPARITY){ // disp ==0 if no disparity available given
        kpdata->disparity = NAN;
        kpdata->has_depth = false;
        kpdata->xyzw = Eigen::Vector4d(NAN, NAN, NAN, NAN);
        kpdata->xyz = Eigen::Vector3d(NAN, NAN, NAN);
      } else {
        kpdata->has_depth = true;
        kpdata->xyz = getXyzValues(u, v, kpdata->disparity);
        kpdata->xyzw.head<3>() = kpdata->xyz;
        kpdata->xyzw.w() = 1;
      }
    }
  }
}
Esempio n. 2
0
void
DepthImage::getXyz(OdometryFrame * frame) 
{
  int num_levels = frame->getNumLevels();
  for(int level_num=0; level_num < num_levels; ++level_num) {
    PyramidLevel* level = frame->getLevel(level_num);

    int num_kp = level->getNumKeypoints();
    for(int kp_ind=0; kp_ind < num_kp; ++kp_ind) {

      KeypointData* kpdata(level->getKeypointData(kp_ind));

      int u = (int)(kpdata->rect_base_uv(0)+0.5);
      int v = (int)(kpdata->rect_base_uv(1)+0.5);

      kpdata->disparity = NAN;

      float z = _depth_data[rgbToDepthIndex(u, v)];
      if(isnan(z)) {
        kpdata->has_depth = false;
        kpdata->xyzw = Eigen::Vector4d(NAN, NAN, NAN, NAN);
        kpdata->xyz = Eigen::Vector3d(NAN, NAN, NAN);
      } else {
        kpdata->has_depth = true;
        kpdata->xyz = z * _rays->col(v * _rgb_width + u);
        kpdata->xyzw.head<3>() = kpdata->xyz;
        kpdata->xyzw.w() = 1;
      }
    }
  }
}