| 119 | } |
| 120 | |
| 121 | void solve_pnp() { |
| 122 | // translation : |
| 123 | // 0.994976 -0.0431638 0.0903361 -0.0338185 |
| 124 | // 0.0444475 0.998937 -0.012246 0.0541652 |
| 125 | // -0.0897114 0.0161996 0.995836 0.0384018 |
| 126 | // 0 0 0 1 |
| 127 | printf("we have %d pair points.\n", pts_3.size()); |
| 128 | if (pts_3.size() < 5 || pts_2.size() < 5) { |
| 129 | return; |
| 130 | } |
| 131 | if (pts_3.size() != pts_2.size()) { |
| 132 | printf("error, not equal!\n"); |
| 133 | return; |
| 134 | } |
| 135 | |
| 136 | cv::Mat r, rvec, t; |
| 137 | cv::solvePnP(pts_3, pts_2, cv_K, cv::Mat::zeros(4, 1, CV_32FC1), rvec, t); |
| 138 | cv::Rodrigues(rvec, r); |
| 139 | Matrix3d R_ref; |
| 140 | for (int i = 0; i < 3; i++) |
| 141 | for (int j = 0; j < 3; j++) { |
| 142 | R_ref(i, j) = r.at<double>(i, j); |
| 143 | } |
| 144 | Matrix4d pnp_result = Matrix4d::Identity(); |
| 145 | pnp_result.block<3, 3>(0, 0) = R_ref; |
| 146 | pnp_result(0, 3) = t.at<double>(0, 0); |
| 147 | pnp_result(1, 3) = t.at<double>(1, 0); |
| 148 | pnp_result(2, 3) = t.at<double>(2, 0); |
| 149 | |
| 150 | vicon2leica = pnp_result.inverse(); |
| 151 | cout << "translation : " << endl << pnp_result << endl; |
| 152 | } |
| 153 | |
| 154 | void image_pose_callback(const sensor_msgs::ImageConstPtr& image_input, |
| 155 | const geometry_msgs::TransformStampedConstPtr& pose_input) { |
no test coverage detected