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;
}
}
}
}
void
StereoDepth::refineXyz(FeatureMatch * matches,
int num_matches,
OdometryFrame * odom_frame)
{
for (int m_ind = 0; m_ind < num_matches; ++m_ind) {
FeatureMatch& match = matches[m_ind];
if (match.status != MATCH_NEEDS_DEPTH_REFINEMENT) {
continue;
}
const KeypointData * left_kpdata = match.target_keypoint;
int level_num = match.ref_keypoint->pyramid_level;
int kp_ind = left_kpdata->keypoint_index;
// TODO this is brittle. We are relying too much on what
// external code does with the keypoints in the odom_frame
// (ie that keypoints without depth are deleted).
Eigen::Vector2d left_uv(match.refined_target_keypoint.kp.u,
match.refined_target_keypoint.kp.v);
Eigen::Vector2d right_uv(_matched_right_keypoints_per_level[level_num][kp_ind].first,
_matched_right_keypoints_per_level[level_num][kp_ind].second);
PyramidLevel* left_level = odom_frame->getLevel(level_num);
PyramidLevel* right_level = _right_frame->getLevel(level_num);
Eigen::Vector2d refined_right_uv;
float delta_sse;
refineFeatureMatch(left_level, right_level, left_uv, right_uv,
&refined_right_uv, &delta_sse);
double ds = (right_uv - refined_right_uv).norm();
// scale refined and unrefined up to base level dimensions
Eigen::Vector2d base_refined_right_uv = refined_right_uv * (1 << level_num);
// get rectified and undistorted position
Eigen::Vector2d rect_base_refined_right_uv;
_right_frame->rectify(base_refined_right_uv, &rect_base_refined_right_uv);
Eigen::Vector2d diff = match.refined_target_keypoint.rect_base_uv - rect_base_refined_right_uv;
double disparity = diff(0);
if (disparity < MIN_DISPARITY || disparity > _max_disparity ||
ds > _max_refinement_displacement ||
fabs(diff(1)) > _max_dist_epipolar_line*(1 << level_num)) {
match.status = MATCH_REFINEMENT_FAILED;
match.inlier = false;
match.refined_target_keypoint.xyzw = Eigen::Vector4d(NAN, NAN, NAN, NAN);
match.refined_target_keypoint.xyz = Eigen::Vector3d(NAN, NAN, NAN);
match.refined_target_keypoint.disparity = NAN;
continue;
}
KeypointData& kpdata(match.refined_target_keypoint);
Eigen::Vector4d uvd1(kpdata.rect_base_uv(0), kpdata.rect_base_uv(1), disparity, 1);
kpdata.xyzw = (*_uvd1_to_xyz) * uvd1;
kpdata.xyz = kpdata.xyzw.head<3>() / kpdata.xyzw.w();
kpdata.disparity = disparity;
match.status = MATCH_OK;
}
}
