diff --git a/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp b/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp index 019229c46..f05ad6a57 100644 --- a/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp @@ -33,6 +33,7 @@ #endif #include +#include #include @@ -43,9 +44,13 @@ namespace fs = std::filesystem; namespace sara = DO::Sara; -#if 0 struct ReprojectionError { + static constexpr auto ResidualDimension = 2; + static constexpr auto IntrinsicParameterCount = 4; + static constexpr auto ExtrinsicParameterCount = 6; + static constexpr auto PointDimension = 3; + ReprojectionError(double observed_x, double observed_y) : observed_x{observed_x} , observed_y{observed_y} @@ -53,36 +58,32 @@ struct ReprojectionError } template - bool operator()(const T* const camera, // (1) camera parameters to optimize. - const T* const point, // (2) 3D points to optimize + bool operator()(const T* const extrinsics, // (1) extrinsic camera parameters + const T* const intrinsics, //(2) extrinsic camera parameters + const T* const point, // (2) 3D points T* residuals) const { T p[3]; - auto camera_view = CameraModelView{camera}; - // Rotate the point. - ceres::AngleAxisRotatePoint(camera_view.angle_axis(), point, p); + ceres::AngleAxisRotatePoint(extrinsics, point, p); // Translate the point. - p[0] += camera_view.t()[0]; - p[1] += camera_view.t()[1]; - p[2] += camera_view.t()[2]; + const auto t = extrinsics + 3; + p[0] += t[0]; + p[1] += t[1]; + p[2] += t[2]; // Normalized camera coordinates. T xp = p[0] / p[2]; T yp = p[1] / p[2]; - // Apply second and fourth order order radial distortion. - const auto& l1 = camera_view.l1(); - const auto& l2 = camera_view.l2(); - const auto r2 = xp * xp + yp * yp; - const auto distortion = T(1) + r2 * (l1 + l2 * r2); - xp *= distortion; - yp *= distortion; - - // Apply the internal camera matrix. - const auto predicted_x = camera_view.fx() * xp + camera_view.x0(); - const auto predicted_y = camera_view.fy() * yp + camera_view.y0(); + // Apply the internal parameters. + const auto& fx = intrinsics[0]; + const auto& fy = intrinsics[1]; + const auto& u0 = intrinsics[2]; + const auto& v0 = intrinsics[3]; + const auto predicted_x = fx * xp + u0; + const auto predicted_y = fy * yp + v0; residuals[0] = predicted_x - T(observed_x); residuals[1] = predicted_y - T(observed_y); @@ -93,15 +94,16 @@ struct ReprojectionError static ceres::CostFunction* create(const double observed_x, const double observed_y) { - constexpr auto NumParams = 6 /* camera parameters */ + 3 /* points */; - return new ceres::AutoDiffCostFunction{ - new ReprojectionError{observed_x, observed_y}}; + return new ceres::AutoDiffCostFunction< + ReprojectionError, ResidualDimension, // + ExtrinsicParameterCount, IntrinsicParameterCount, PointDimension>{ + new ReprojectionError{observed_x, observed_y} // + }; } double observed_x; double observed_y; }; -#endif GRAPHICS_MAIN() @@ -264,20 +266,134 @@ GRAPHICS_MAIN() point_cloud_generator.grow_point_cloud(tracks, images[1], pose_edge, pinhole_camera); -#if 0 + // Filter the feature tracks by NMS. + auto tracks_filtered = std::vector{}; + for (const auto& track : tracks) + { + // Does a track have a 3D point? If not, discard it. + const auto p = point_cloud_generator.scene_point(track.front()); + if (p == std::nullopt) + continue; + + // Filter the feature track by NMS: there should be only 1 feature per + // image. + auto track_filtered = point_cloud_generator // + .filter_by_non_max_suppression(track); + + tracks_filtered.emplace_back(std::move(track_filtered)); + } + + // Collect the BA data. + const auto num_scene_points = static_cast(tracks_filtered.size()); + auto num_image_points = 0; + for (const auto& track : tracks_filtered) + num_image_points += static_cast(track.size()); + + auto ba_data = sara::BundleAdjustmentData{}; + static constexpr auto num_views = 2; + static constexpr auto num_intrinsics = 4; // fx, fy, u0, v0 + static constexpr auto num_extrinsics = 6; + ba_data.resize(num_image_points, num_scene_points, num_views, num_intrinsics, + num_extrinsics); + + SARA_LOGI(logger, "Populating the BA observation/image point data..."); + auto o = 0; // observation index. + for (auto t = std::size_t{}; t < tracks_filtered.size(); ++t) + { + const auto& track = tracks_filtered[t]; + for (const auto& u : track) + { + const Eigen::Vector2d pixel_coords = point_cloud_generator // + .pixel_coords(u) + .cast(); + ba_data.observations(o, 0) = pixel_coords.x(); + ba_data.observations(o, 1) = pixel_coords.y(); + + ba_data.point_indices[o] = static_cast(t); + ba_data.camera_indices[o] = + static_cast(feature_tracker._feature_graph[u].pose_vertex); + + ++o; + } + } + + SARA_LOGI(logger, "Populating the BA (3D) point data..."); + for (auto t = std::size_t{}; t < tracks_filtered.size(); ++t) + { + // Retrieve the scene point. + const auto& track = tracks_filtered[t]; + const auto scene_point = point_cloud_generator.scene_point(track.front()); + + // Store. + const auto tt = static_cast(t); + ba_data.point_coords[tt].vector() = scene_point->coords(); + } + + SARA_LOGI(logger, "Populating the BA camera parameter data..."); + auto extrinsics_params = ba_data.extrinsics.matrix(); + auto intrinsics_params = ba_data.intrinsics.matrix(); + for (auto v = 0; pose_graph.num_vertices(); ++v) + { + // Angle axis vector. + auto extrinsics_v = extrinsics_params.row(v); + auto intrinsics_v = intrinsics_params.row(v); + + // The original data. + const auto& pose_v = pose_graph[v].pose; + const auto aaxis_v = Eigen::AngleAxisd{pose_v.q}; + const Eigen::Vector3d aaxis_v_3d = aaxis_v.angle() * aaxis_v.axis(); + // Initialize the absolute rotation. + extrinsics_v.head(3) = aaxis_v_3d.transpose(); + // Initialize the absolute translation. + extrinsics_v.tail(3) = pose_v.t.transpose(); + + // Initialize the internal camera parameters. + intrinsics_v(0) = K[v](0, 0); // fx + intrinsics_v(1) = K[v](1, 1); // fy + intrinsics_v(2) = K[v](0, 2); // u0 + intrinsics_v(3) = K[v](1, 2); // v0 + } + // Solve the bundle adjustment problem with Ceres. - auto problem = ceres::Problem{}; - for (int i = 0; i < ba_problem.observations.size(0); ++i) + auto ba_problem = ceres::Problem{}; + for (auto i = 0; i < num_image_points; ++i) { - auto cost_fn = ReprojectionError::create(ba_problem.observations(i, 0), - ba_problem.observations(i, 1)); - - problem.AddResidualBlock(cost_fn, nullptr /* squared loss */, - ba_problem.camera_parameters.data() + - ba_problem.camera_indices[i] * - CameraModelView::dof(), - ba_problem.points_abs_coords_3d.data() + - ba_problem.point_indices[i] * 3); + // Create a cost residual function. + const auto cost_fn = ReprojectionError::create(ba_data.observations(i, 0), + ba_data.observations(i, 1)); + + const auto camera_idx = ba_data.camera_indices[i]; + // Locate the parameter data. + const auto extrinsics_ptr = + extrinsics_params.data() + + camera_idx * ReprojectionError::ExtrinsicParameterCount; + const auto intrinsics_ptr = + intrinsics_params.data() + + camera_idx * ReprojectionError::IntrinsicParameterCount; + const auto scene_point_ptr = + ba_data.point_coords.data() + + ba_data.point_indices[i] * ReprojectionError::PointDimension; + + ba_problem.AddResidualBlock(cost_fn, nullptr /* squared loss */, // + extrinsics_ptr, intrinsics_ptr, + scene_point_ptr); + } + + // Freeze all the intrinsic parameters during the optimization. + for (auto v = 0; v < 2; ++v) + { + const auto intrinsics_ptr = intrinsics_params.data() + + v * ReprojectionError::IntrinsicParameterCount; + ba_problem.SetParameterBlockConstant(intrinsics_ptr); + } + + // Freeze the first absolute pose parameters. + { + const auto camera_idx = 0; + const auto extrinsics_ptr = + extrinsics_params.data() + + camera_idx * ReprojectionError::ExtrinsicParameterCount; + ba_problem.SetParameterBlockConstant(extrinsics_ptr); } auto options = ceres::Solver::Options{}; @@ -285,17 +401,13 @@ GRAPHICS_MAIN() options.minimizer_progress_to_stdout = true; auto summary = ceres::Solver::Summary{}; - ceres::Solve(options, &problem, &summary); + ceres::Solve(options, &ba_problem, &summary); std::cout << summary.BriefReport() << std::endl; SARA_LOGI(logger, "Check the SfM..."); SARA_DEBUG << "camera_parameters =\n" - << ba_problem.camera_parameters.matrix() << std::endl; - SARA_DEBUG << "points =\n" - << ba_problem.points_abs_coords_3d.matrix() << std::endl; - - // TODO: check the point reprojection errors. -#endif + << ba_data.extrinsics.matrix() << std::endl; + SARA_DEBUG << "points =\n" << ba_data.point_coords.matrix() << std::endl; return 0; } diff --git a/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp b/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp index b9e45bb6a..71284e929 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp @@ -12,6 +12,7 @@ #pragma once #include +#include #include @@ -24,243 +25,72 @@ namespace DO::Sara { * @{ */ - //! @brief Camera model view class. - template - struct CameraModelView - { - auto angle_axis() const -> const T* - { - return parameters; - } - auto t() const -> const T* - { - return parameters + 3; - } - auto fx() const -> const T& - { - return parameters[6]; - } - auto fy() const -> const T& - { - return parameters[7]; - } - auto x0() const -> const T& - { - return parameters[8]; - } - auto y0() const -> const T& - { - return parameters[9]; - } - auto l1() const -> const T& - { - return parameters[10]; - } - auto l2() const -> const T& - { - return parameters[11]; - } - static auto dof() - { - return 12; - }; - - const T* parameters; - }; - -#if 0 - //! @brief Observation reference class. - struct ObservationRef - { - FeatureGID gid; - int camera_id; - int point_id; - }; - //! @brief Bundle adjustment class. - struct BundleAdjustmentProblem + struct BundleAdjustmentData { - //! @brief observation = 2D points in images. + //! @brief An observation is a 2D image point. Tensor_ observations; - //! @brief the corresponding 3D point index for observation 'o' + //! @brief the corresponding 3D point index for the observation 'o' std::vector point_indices; - //! @brief the corresponding 3D camera index for observation 'o' + //! @brief the corresponding 3D camera index for the observation 'o' std::vector camera_indices; + + //! @brief The parameter is the set of camera parameters and the set of 3D + //! point coordinates. + std::vector parameters; //! @{ - //! @brief camera parameters + 3D point coordinates. + //! @brief The number of parameters in details. int num_cameras; + int num_intrinsics; + int num_extrinsics; int num_points; - int camera_dof; - std::vector parameters; - TensorView_ points_abs_coords_3d; - TensorView_ camera_parameters; //! @} - - auto resize(int num_observations, // - int num_points_, int num_cameras_, int camera_dof_) + //! @brief Convenient parameter data views. + TensorView_ intrinsics; + TensorView_ extrinsics; + TensorView_ point_coords; + + auto resize(const int num_image_points, const int num_scene_points, + const int num_views, const int num_intrinsic_params, + const int num_extrinsic_params) { - SARA_DEBUG << "Resizing data..." << std::endl; - observations = Tensor_{{num_observations, 2}}; - point_indices = std::vector(num_observations); - camera_indices = std::vector(num_observations); - - num_points = num_points_; - num_cameras = num_cameras_; - camera_dof = camera_dof_; - const auto num_parameters = num_cameras * camera_dof + num_points * 3; + auto& logger = Logger::get(); + + SARA_LOGI(logger, "Allocating memory for observation data..."); + observations = Tensor_{{num_image_points, 2}}; + point_indices = std::vector(num_image_points); + camera_indices = std::vector(num_image_points); + + SARA_LOGI(logger, "Allocating memory for parameter data..."); + // Store the number of parameters. + num_cameras = num_views; + num_intrinsics = num_intrinsic_params; + num_extrinsics = num_extrinsic_params; + num_points = num_scene_points; + // Allocate. + const auto num_parameters = + num_cameras * (num_intrinsics + num_extrinsics) + num_points * 3; parameters = std::vector(num_parameters); - auto points_abs_coords_3d_new = TensorView_{ - parameters.data() + camera_dof * num_cameras, {num_points, 3}}; - auto camera_parameters_new = - TensorView_{parameters.data(), {num_cameras, camera_dof}}; - - points_abs_coords_3d.swap(points_abs_coords_3d_new); - camera_parameters.swap(camera_parameters_new); - } - - auto populate_observations( - const std::vector& obs_refs, - const std::vector>& keypoints) -> void - { - SARA_DEBUG << "Populating observations..." << std::endl; - const auto num_observations = observations.size(0); - for (int i = 0; i < num_observations; ++i) - { - const auto& ref = obs_refs[i]; - - // Easy things first. - point_indices[i] = ref.point_id; - camera_indices[i] = ref.camera_id; - - // Initialize the 2D observations. - const auto& image_id = ref.gid.image_id; - const auto& local_id = ref.gid.local_id; - const double x = features(keypoints[image_id])[local_id].x(); - const double y = features(keypoints[image_id])[local_id].y(); - observations(i, 0) = x; - observations(i, 1) = y; - } - } - - auto populate_3d_points_from_two_view_geometry( - const std::set>& feature_tracks, - const std::multimap& match_index, - const TwoViewGeometry& two_view_geometry) -> void - { - SARA_DEBUG << "Populating 3D points..." << std::endl; - - auto points_view = points_abs_coords_3d.colmajor_view().matrix(); - for (auto [t, track] = std::make_pair(0, feature_tracks.begin()); - track != feature_tracks.end(); ++t, ++track) - { - const auto p = match_index.find(*track->begin())->second.m; - const auto point_p = two_view_geometry.X.col(p); - points_view.col(t) = point_p.hnormalized(); -# if DEBUG - SARA_DEBUG << "Point[" << t << "] = " // - << points_view.col(t).transpose() << std::endl; -# endif - } - } - - auto populate_camera_parameters(const TwoViewGeometry& two_view_geometry) - -> void - { - SARA_DEBUG << "Populating camera parameters..." << std::endl; - - auto cam_matrix = camera_parameters.matrix(); - // Angle axis vector. - auto cam_0 = cam_matrix.row(0); - auto angle_axis_0 = Eigen::AngleAxisd{two_view_geometry.C1.R}; - auto aaxis0 = angle_axis_0.angle() * angle_axis_0.axis(); - cam_0.segment(0, 3) = aaxis0.transpose(); - // translation. - const auto& t0 = two_view_geometry.C1.t; - cam_0.segment(3, 3) = t0; - // Internal parameters. - SARA_DEBUG << "K1 =\n" << two_view_geometry.C1.K << std::endl; - cam_0(6) = two_view_geometry.C1.K(0, 0); // fx - cam_0(7) = two_view_geometry.C1.K(1, 1); // fy - cam_0(8) = two_view_geometry.C1.K(0, 2); // x0 - cam_0(9) = two_view_geometry.C1.K(1, 2); // y0 - cam_0(10) = 1.0; // l1 - cam_0(11) = 1.0; // l2 - - // Angle axis vector. - auto cam_1 = cam_matrix.row(1); - auto angle_axis_1 = Eigen::AngleAxisd{two_view_geometry.C2.R}; - auto aaxis1 = angle_axis_1.angle() * angle_axis_1.axis(); - cam_1.segment(0, 3) = aaxis1.transpose(); - // translation. - const auto& t1 = two_view_geometry.C2.t; - cam_1.segment(3, 3) = t1; - // Internal parameters. - SARA_DEBUG << "K2 =\n" << two_view_geometry.C2.K << std::endl; - cam_1(6) = two_view_geometry.C2.K(0, 0); // fx - cam_1(7) = two_view_geometry.C2.K(1, 1); // fy - cam_1(8) = two_view_geometry.C2.K(0, 2); // x0 - cam_1(9) = two_view_geometry.C2.K(1, 2); // y0 - cam_1(10) = 1.0; // l1 - cam_1(11) = 1.0; // l2 - - SARA_DEBUG << "cam_matrix =\n" << cam_matrix << std::endl; - } - - auto populate_data_from_two_view_geometry( - const std::set>& feature_tracks, - const std::vector>& keypoints, - const std::multimap& match_index, - const TwoViewGeometry& two_view_geometry) -> void - { - const auto num_points = static_cast(feature_tracks.size()); - SARA_CHECK(num_points); - - auto num_observations_per_points = std::vector(num_points); - std::transform( - std::begin(feature_tracks), std::end(feature_tracks), - std::begin(num_observations_per_points), - [](const auto& track) { return static_cast(track.size()); }); - - const auto num_observations = - std::accumulate(std::begin(num_observations_per_points), - std::end(num_observations_per_points), 0); - SARA_CHECK(num_observations); - - // 3. Count the number of cameras, which should be equal to the number of - // images. - auto image_ids = std::set{}; - for (const auto& track : feature_tracks) - for (const auto& f : track) - image_ids.insert(f.image_id); - const auto num_cameras = static_cast(image_ids.size()); - SARA_CHECK(num_cameras); - - const auto camera_dof_ = CameraModelView::dof(); - const auto num_parameters = camera_dof_ * num_cameras + 3 * num_points; - SARA_CHECK(num_parameters); - - // 4. Transform the data for convenience. - auto obs_refs = std::vector{}; - { - obs_refs.reserve(num_observations); - for (auto [point_id, track] = std::make_pair(0, feature_tracks.begin()); - track != feature_tracks.end(); ++point_id, ++track) - { - for (const auto& f : *track) - obs_refs.push_back({f, f.image_id, point_id}); - } - } - - resize(num_observations, num_points, num_cameras, camera_dof_); - populate_observations(obs_refs, keypoints); - populate_3d_points_from_two_view_geometry(feature_tracks, match_index, - two_view_geometry); - populate_camera_parameters(two_view_geometry); + // Update the memory views. + auto intrinsics_new = TensorView_{ + parameters.data(), // + {num_cameras, num_extrinsics} // + }; + auto extrinsics_new = TensorView_{ + parameters.data() + num_cameras * num_intrinsics, // + {num_cameras, num_extrinsics} // + }; + auto point_coords_new = TensorView_{ + parameters.data() + num_cameras * (num_extrinsics + num_intrinsics), + {num_points, 3}}; + + intrinsics.swap(intrinsics_new); + extrinsics.swap(extrinsics_new); + point_coords.swap(point_coords_new); } }; -#endif //! @} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 4171b9ab0..531e7738f 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -66,6 +66,14 @@ namespace DO::Sara { return feature(u).center(); } + auto scene_point(const FeatureVertex u) const -> std::optional + { + const auto it = _from_vertex_to_scene_point_index.find(u); + if (it == _from_vertex_to_scene_point_index.end()) + return std::nullopt; + return _point_cloud[it->second]; + } + public: /* helper query methods */ auto list_scene_point_indices(const FeatureTrack&) const -> std::vector; @@ -128,7 +136,7 @@ namespace DO::Sara { const v2::PinholeCamera& camera) -> void; auto write_point_cloud(const std::vector& ftracks, - const std::filesystem::path& out_csv) const -> void; + const std::filesystem::path& out_csv) const -> void; private: /* data members */ const CameraPoseGraph& _pose_graph;