This is a stub. You will fill in the Ceres logic.
| 49 | |
| 50 | // This is a stub. You will fill in the Ceres logic. |
| 51 | py::tuple ba_solve( |
| 52 | CameraPoses camera_poses, |
| 53 | Point3Ds point_3ds, |
| 54 | Observations observations, |
| 55 | py::array_t<double> K, |
| 56 | ConstantPoseIndex constant_pose_index, |
| 57 | RelativePoseConstraints relative_pose_constraints |
| 58 | ) { |
| 59 | ceres::Problem problem; |
| 60 | ceres::Solver::Options options; |
| 61 | ceres::Solver::Summary summary; |
| 62 | std::map<int64_t, std::array<double, 6>> camera_parameters; |
| 63 | int64_t min_cam_indx = std::numeric_limits<int64_t>::max(); |
| 64 | for (const auto& [cam_idx, cam_pose] : camera_poses) { |
| 65 | std::array<double, 6> camera_parameter; |
| 66 | auto cam_pose_buf = cam_pose.unchecked<2>(); |
| 67 | Eigen::Matrix4d cam_pose_eigen; |
| 68 | for (ssize_t i = 0; i < 4; ++i) |
| 69 | for (ssize_t j = 0; j < 4; ++j) |
| 70 | cam_pose_eigen(i, j) = cam_pose_buf(i, j); |
| 71 | |
| 72 | Eigen::Matrix3d R = cam_pose_eigen.block<3, 3>(0, 0); |
| 73 | Eigen::Vector3d t = cam_pose_eigen.block<3, 1>(0, 3); |
| 74 | |
| 75 | camera_parameter[0] = t[0]; |
| 76 | camera_parameter[1] = t[1]; |
| 77 | camera_parameter[2] = t[2]; |
| 78 | ceres::RotationMatrixToAngleAxis(R.data(), camera_parameter.data() + 3); |
| 79 | camera_parameters[cam_idx] = camera_parameter; |
| 80 | } |
| 81 | |
| 82 | std::map<int64_t, std::array<double, 3>> point_parameters; |
| 83 | for (const auto& [pt_idx, pt_3d] : point_3ds) { |
| 84 | auto pt_3d_buf = pt_3d.unchecked<1>(); |
| 85 | std::array<double, 3> point_parameter; |
| 86 | point_parameter[0] = pt_3d_buf(0); |
| 87 | point_parameter[1] = pt_3d_buf(1); |
| 88 | point_parameter[2] = pt_3d_buf(2); |
| 89 | point_parameters[pt_idx] = point_parameter; |
| 90 | } |
| 91 | |
| 92 | ceres::LossFunction* loss_function = new ceres::HuberLoss(2.0); |
| 93 | auto K_buf = K.unchecked<2>(); |
| 94 | Eigen::Matrix3d K_eigen; |
| 95 | for (ssize_t i = 0; i < 3; ++i) |
| 96 | for (ssize_t j = 0; j < 3; ++j) |
| 97 | K_eigen(i, j) = K_buf(i, j); |
| 98 | // observations is a list of tuples (cam_idx, pt_idx, 2x1) |
| 99 | for (const auto& [cam_idx, pt_idx, obs] : observations) { |
| 100 | auto obs_buf = obs.unchecked<1>(); |
| 101 | Eigen::Vector2d observed_keypoint(obs_buf(0), obs_buf(1)); |
| 102 | ceres::CostFunction* reprojection_error = new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6, 3>(new ReprojectionError(observed_keypoint, K_eigen)); |
| 103 | problem.AddResidualBlock(reprojection_error, loss_function, camera_parameters.at(cam_idx).data(), point_parameters.at(pt_idx).data()); |
| 104 | } |
| 105 | |
| 106 | // constant_pose_index maybe None |
| 107 | for (const auto& [cam_idx, is_constant] : constant_pose_index) { |
| 108 | if (is_constant) { |
no outgoing calls
no test coverage detected