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