| 150 | } |
| 151 | |
| 152 | void load_keyframes_pose_pcd( |
| 153 | const std::string &pose_file, std::vector<int> &index_vec, |
| 154 | std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d>> &poses_vec, |
| 155 | std::vector<double> ×_vec) { |
| 156 | pcl::PointCloud<PointPose>::Ptr CloudPosePtr( |
| 157 | new pcl::PointCloud<PointPose>()); |
| 158 | pcl::io::loadPCDFile<PointPose>(pose_file, *CloudPosePtr); |
| 159 | |
| 160 | for (int i = 0; i < CloudPosePtr->points.size(); ++i) { |
| 161 | PointPose p = CloudPosePtr->points[i]; |
| 162 | Eigen::Vector3d temp_xyz(p.x, p.y, p.z); |
| 163 | Eigen::Quaterniond temp_q(p.qw, p.qx, p.qy, p.qz); |
| 164 | std::pair<Eigen::Vector3d, Eigen::Matrix3d> single_pose; |
| 165 | single_pose.first = temp_xyz; |
| 166 | single_pose.second = temp_q.toRotationMatrix(); |
| 167 | poses_vec.push_back(single_pose); |
| 168 | |
| 169 | index_vec.push_back(p.intensity); |
| 170 | times_vec.push_back(p.t - 0.1); |
| 171 | } |
| 172 | } |
| 173 | |
| 174 | std::vector<int> |
| 175 | findCorrespondingFrames(const std::vector<double> &key_frame_times_vec, |