| 66 | void render_currentpose(); |
| 67 | |
| 68 | bool read_pose(fstream& file) { |
| 69 | int count = 0; |
| 70 | bool good = true; |
| 71 | double para[16]; |
| 72 | while (good) { |
| 73 | count++; |
| 74 | for (int i = 0; i < 8 && good; i++) { |
| 75 | good = good && file >> para[i]; |
| 76 | } |
| 77 | if (good) { |
| 78 | Eigen::Vector3d request_position; |
| 79 | Eigen::Quaterniond request_pose; |
| 80 | request_position.x() = para[1]; |
| 81 | request_position.y() = para[2]; |
| 82 | request_position.z() = para[3]; |
| 83 | request_pose.w() = para[4]; |
| 84 | request_pose.x() = para[5]; |
| 85 | request_pose.y() = para[6]; |
| 86 | request_pose.z() = para[7]; |
| 87 | |
| 88 | PoseInfo info; |
| 89 | info.time = para[0]; |
| 90 | info.pose = Matrix4d::Identity(); |
| 91 | info.pose.block<3, 3>(0, 0) = request_pose.toRotationMatrix(); |
| 92 | info.pose(0, 3) = para[1]; |
| 93 | info.pose(1, 3) = para[2]; |
| 94 | info.pose(2, 3) = para[3]; |
| 95 | gt_pose_vect.push_back(info); |
| 96 | } |
| 97 | } |
| 98 | printf("we have %d poses.\n", count); |
| 99 | return true; |
| 100 | } |
| 101 | |
| 102 | vector<cv::Point3f> pts_3; |
| 103 | vector<cv::Point2f> pts_2; |