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