| 47 | }; |
| 48 | |
| 49 | std::unordered_map<int64_t, py::array_t<double>> pose_graph_solve( |
| 50 | std::unordered_map<int64_t, py::array_t<double>> camera_poses, |
| 51 | std::vector<std::tuple<int64_t, int64_t, py::array_t<double>, py::array_t<double>, py::array_t<double>>> relative_pose_constraints, |
| 52 | std::unordered_map<int64_t, bool> constant_pose_index, |
| 53 | int64_t max_iteration_num) { |
| 54 | ceres::Problem problem; |
| 55 | ceres::Solver::Options options; |
| 56 | ceres::Solver::Summary summary; |
| 57 | std::map<int64_t, std::array<double, 6>> camera_parameters; |
| 58 | int64_t min_cam_indx = std::numeric_limits<int64_t>::max(); |
| 59 | |
| 60 | for (const auto& [cam_idx, cam_pose] : camera_poses) { |
| 61 | std::array<double, 6> camera_parameter; |
| 62 | auto cam_pose_buf = cam_pose.unchecked<2>(); |
| 63 | Eigen::Matrix4d cam_pose_eigen; |
| 64 | for (ssize_t i = 0; i < 4; ++i) |
| 65 | for (ssize_t j = 0; j < 4; ++j) |
| 66 | cam_pose_eigen(i, j) = cam_pose_buf(i, j); |
| 67 | |
| 68 | Eigen::Matrix3d R = cam_pose_eigen.block<3, 3>(0, 0); |
| 69 | Eigen::Vector3d t = cam_pose_eigen.block<3, 1>(0, 3); |
| 70 | |
| 71 | camera_parameter[0] = t[0]; |
| 72 | camera_parameter[1] = t[1]; |
| 73 | camera_parameter[2] = t[2]; |
| 74 | ceres::RotationMatrixToAngleAxis(R.data(), camera_parameter.data() + 3); |
| 75 | camera_parameters[cam_idx] = camera_parameter; |
| 76 | } |
| 77 | |
| 78 | for (const auto& [cam_idx_i, cam_idx_j, relative_pose_j_i, translation_weight, rotation_weight] : relative_pose_constraints) { |
| 79 | auto relative_pose_j_i_buf = relative_pose_j_i.unchecked<2>(); |
| 80 | Eigen::Matrix4d relative_pose_j_i_eigen; |
| 81 | for (ssize_t i = 0; i < 4; ++i) |
| 82 | for (ssize_t j = 0; j < 4; ++j) |
| 83 | relative_pose_j_i_eigen(i, j) = relative_pose_j_i_buf(i, j); |
| 84 | auto translation_weight_buf = translation_weight.unchecked<1>(); |
| 85 | Eigen::Vector3d translation_weight_eigen; |
| 86 | for (ssize_t i = 0; i < 3; ++i) |
| 87 | translation_weight_eigen[i] = translation_weight_buf(i); |
| 88 | auto rotation_weight_buf = rotation_weight.unchecked<1>(); |
| 89 | Eigen::Vector3d rotation_weight_eigen; |
| 90 | for (ssize_t i = 0; i < 3; ++i) |
| 91 | rotation_weight_eigen[i] = rotation_weight_buf(i); |
| 92 | ceres::CostFunction* relative_pose_error = new ceres::AutoDiffCostFunction<RelativePoseError, 6, 6, 6>(new RelativePoseError(relative_pose_j_i_eigen, translation_weight_eigen, rotation_weight_eigen)); |
| 93 | problem.AddResidualBlock(relative_pose_error, nullptr, camera_parameters.at(cam_idx_i).data(), camera_parameters.at(cam_idx_j).data()); |
| 94 | } |
| 95 | |
| 96 | for (const auto& [cam_idx, is_constant] : constant_pose_index) { |
| 97 | if (is_constant) { |
| 98 | if (!problem.HasParameterBlock(camera_parameters.at(cam_idx).data())) { |
| 99 | problem.AddParameterBlock(camera_parameters.at(cam_idx).data(), 6); |
| 100 | } |
| 101 | problem.SetParameterBlockConstant(camera_parameters.at(cam_idx).data()); |
| 102 | } |
| 103 | } |
| 104 | |
| 105 | options.linear_solver_type = ceres::SPARSE_SCHUR; |
| 106 | options.minimizer_progress_to_stdout = false; |
no outgoing calls