keypoint_map::keypoint_map(cv::Mat & rgb, cv::Mat & depth,
Eigen::Affine3f & transform) {
init_feature_detector(fd, de, dm);
intrinsics << 525.0, 525.0, 319.5, 239.5;
std::vector<cv::KeyPoint> keypoints;
compute_features(rgb, depth, intrinsics, fd, de, keypoints, keypoints3d,
descriptors);
for (int keypoint_id = 0; keypoint_id < keypoints.size(); keypoint_id++) {
observation o;
o.cam_id = 0;
o.point_id = keypoint_id;
o.coord << keypoints[keypoint_id].pt.x, keypoints[keypoint_id].pt.y;
observations.push_back(o);
weights.push_back(1.0f);
keypoints3d[keypoint_id].getVector3fMap() = transform
* keypoints3d[keypoint_id].getVector3fMap();
}
camera_positions.push_back(transform);
rgb_imgs.push_back(rgb);
depth_imgs.push_back(depth);
}
reduce_measurement_g2o::reduce_measurement_g2o(reduce_measurement_g2o& rb,
tbb::split) :
size(rb.size), frames(rb.frames) {
init_feature_detector();
icp.setMaxCorrespondenceDistance(0.5);
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);
}
reduce_measurement_g2o::reduce_measurement_g2o(
const tbb::concurrent_vector<color_keyframe::Ptr> & frames, int size) :
size(size), frames(frames) {
init_feature_detector();
icp.setMaxCorrespondenceDistance(0.5);
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);
}
keypoint_map::keypoint_map(const std::string & dir_name) {
init_feature_detector(fd, de, dm);
intrinsics << 525.0, 525.0, 319.5, 239.5;
pcl::io::loadPCDFile(dir_name + "/keypoints3d.pcd", keypoints3d);
cv::FileStorage fs(dir_name + "/descriptors.yml", cv::FileStorage::READ);
fs["descriptors"] >> descriptors;
fs["weights"] >> weights;
cv::FileNode obs = fs["observations"];
for (cv::FileNodeIterator it = obs.begin(); it != obs.end(); ++it) {
observation o;
*it >> o;
observations.push_back(o);
}
cv::FileNode cam_pos = fs["camera_positions"];
for (cv::FileNodeIterator it = cam_pos.begin(); it != cam_pos.end(); ++it) {
Eigen::Affine3f pos;
int i = 0;
for (cv::FileNodeIterator it2 = (*it).begin(); it2 != (*it).end();
++it2) {
int u = i / 4;
int v = i % 4;
*it2 >> pos.matrix().coeffRef(u, v);
i++;
}
camera_positions.push_back(pos);
}
fs.release();
for (size_t i = 0; i < camera_positions.size(); i++) {
cv::Mat rgb = cv::imread(
dir_name + "/rgb/" + boost::lexical_cast<std::string>(i)
+ ".png", CV_LOAD_IMAGE_UNCHANGED);
cv::Mat depth = cv::imread(
dir_name + "/depth/" + boost::lexical_cast<std::string>(i)
+ ".png", CV_LOAD_IMAGE_UNCHANGED);
rgb_imgs.push_back(rgb);
depth_imgs.push_back(depth);
}
}
