diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.cpp index ef9a22ae6..c97254ae8 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.cpp @@ -147,7 +147,7 @@ auto BundleAdjuster::populate_camera_params( const Eigen::Vector3d aaxis_v_3d = aaxis_v.angle() * aaxis_v.axis(); // Initialize the absolute rotation. extrinsics_v << aaxis_v_3d.transpose(), pose_v.t.transpose(); - SARA_LOGD(logger, "Populating extrinsics[{}]=\n{}", v, extrinsics_v.eval()); + SARA_LOGT(logger, "Populating extrinsics[{}]=\n{}", v, extrinsics_v.eval()); // Initialize the internal camera parameters. const auto& K = calibration_matrices[v]; @@ -155,7 +155,7 @@ auto BundleAdjuster::populate_camera_params( intrinsics_v(1) = K(1, 1); // fy intrinsics_v(2) = K(0, 2); // u0 intrinsics_v(3) = K(1, 2); // v0 - SARA_LOGD(logger, "Populating intrinsics[{}]=\n{}", v, intrinsics_v.eval()); + SARA_LOGT(logger, "Populating intrinsics[{}]=\n{}", v, intrinsics_v.eval()); } } diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index afd0e87ee..574287e0a 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -390,12 +390,12 @@ auto PointCloudGenerator::grow_point_cloud( auto scene_point_value = ScenePoint{coords, color}; _point_cloud.emplace_back(std::move(scene_point_value)); - // Recall that a match is a pair of feature vertex. - const auto& [x, y] = fmatches[j]; - // Assign a scene point index to the two feature vertices. - _from_vertex_to_scene_point_index[x] = scene_point_index; - _from_vertex_to_scene_point_index[y] = scene_point_index; + const auto& ftrack = ftracks_without_scene_point[j]; + for (const auto& v : ftrack) + _from_vertex_to_scene_point_index[v] = scene_point_index; + + // Increment the scene point index. ++scene_point_index; } diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp index e4af6c8cd..853e52a09 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp @@ -401,15 +401,9 @@ auto OdometryPipeline::adjust_bundles() -> void const auto& ftrack = ftracks_filtered[t]; const auto idx = _point_cloud_generator->scene_point_index(ftrack.front()); if (idx == std::nullopt) -#ifdef BUG_DEBUG_ME throw std::runtime_error{fmt::format( "Error: the feature vertex {} must have a scene point index!", ftrack.front())}; -#else - SARA_LOGE(logger, - "Error: the feature vertex {} must have a scene point index!", - ftrack.front()); -#endif if (*idx >= _point_cloud.size()) throw std::runtime_error{