bool reduce_measurement_g2o::find_transform(const color_keyframe::Ptr & fi,
const color_keyframe::Ptr & fj, Sophus::SE3f & t) {
std::vector<cv::KeyPoint> keypoints_i, keypoints_j;
pcl::PointCloud<pcl::PointXYZ> keypoints3d_i, keypoints3d_j;
cv::Mat descriptors_i, descriptors_j;
compute_features(fi->get_i(0), fi->get_d(0), fi->get_intrinsics(0), fd, de,
keypoints_i, keypoints3d_i, descriptors_i);
compute_features(fj->get_i(0), fj->get_d(0), fj->get_intrinsics(0), fd, de,
keypoints_j, keypoints3d_j, descriptors_j);
std::vector<cv::DMatch> matches, matches_filtered;
dm->match(descriptors_j, descriptors_i, matches);
Eigen::Affine3f transform;
std::vector<bool> inliers;
bool res = estimate_transform_ransac(keypoints3d_j, keypoints3d_i, matches,
100, 0.03 * 0.03, 20, transform, inliers);
t = Sophus::SE3f(transform.rotation(), transform.translation());
return res;
}
bool keypoint_map::merge_keypoint_map(const keypoint_map & other,
int min_num_inliers) {
/*
std::vector<std::vector<cv::DMatch> > all_matches2;
std::vector<cv::DMatch> matches;
dm->knnMatch(
other.descriptors, descriptors, all_matches2, 2);
for (size_t i = 0; i < all_matches2.size(); ++i) {
double ratio = all_matches2[i][0].distance
/ all_matches2[i][1].distance;
if (ratio < 0.6) {
matches.push_back(all_matches2[i][0]);
}
}
*/
std::vector<cv::DMatch> matches;
dm->match(other.descriptors, descriptors, matches);
Eigen::Affine3f transform;
std::vector<bool> inliers;
bool res = estimate_transform_ransac(other.keypoints3d, keypoints3d,
matches, 3000, 0.03 * 0.03, min_num_inliers, transform, inliers);
if (!res)
return false;
std::map<int, int> unique_matches;
for (size_t i = 0; i < matches.size(); i++) {
if (unique_matches.find(matches[i].trainIdx) == unique_matches.end()) {
unique_matches[matches[i].trainIdx] = i;
} else {
if (matches[unique_matches[matches[i].trainIdx]].distance
> matches[i].distance) {
inliers[unique_matches[matches[i].trainIdx]] = false;
unique_matches[matches[i].trainIdx] = i;
} else {
inliers[i] = false;
}
}
}
size_t current_camera_positions_size = camera_positions.size();
for (size_t i = 0; i < other.camera_positions.size(); i++) {
camera_positions.push_back(
transform.cast<float>() * other.camera_positions[i]);
camera_positions[i].makeAffine();
rgb_imgs.push_back(other.rgb_imgs[i]);
depth_imgs.push_back(other.depth_imgs[i]);
}
for (size_t i = 0; i < matches.size(); i++) {
if (inliers[i]) {
/*descriptors.row(matches[i].trainIdx) = (descriptors.row(
matches[i].trainIdx) * weights[matches[i].trainIdx]
+ other.descriptors.row(matches[i].queryIdx)
* other.weights[matches[i].queryIdx])
/ (weights[matches[i].trainIdx]
+ other.weights[matches[i].queryIdx]);
*/
weights[matches[i].trainIdx] += other.weights[matches[i].queryIdx];
for (size_t j = 0; j < other.observations.size(); j++) {
if (other.observations[j].point_id == matches[i].queryIdx) {
observation o = other.observations[j];
o.point_id = matches[i].trainIdx;
o.cam_id += current_camera_positions_size;
observations.push_back(o);
}
}
} else {
pcl::PointXYZ p;
p.getVector3fMap() = transform
* other.keypoints3d[matches[i].queryIdx].getVector3fMap();
keypoints3d.push_back(p);
cv::vconcat(descriptors, other.descriptors.row(matches[i].queryIdx),
descriptors);
weights.push_back(other.weights[matches[i].queryIdx]);
for (size_t j = 0; j < other.observations.size(); j++) {
if (other.observations[j].point_id == matches[i].queryIdx) {
observation o = other.observations[j];
o.point_id = keypoints3d.size() - 1;
o.cam_id += current_camera_positions_size;
observations.push_back(o);
}
}
}
}
return true;
}
