├── .clang-format ├── .github └── workflows │ └── main.yml ├── .gitignore ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── cmake ├── addTest.cmake ├── modules │ ├── FindSuiteSparse.cmake │ ├── ceres.cmake │ ├── cnpy.cmake │ ├── eigen.cmake │ ├── gbenchmark.cmake │ ├── gflags.cmake │ ├── glog.cmake │ ├── gtest.cmake │ ├── opencv.cmake │ ├── opengv.cmake │ ├── pangolin.cmake │ ├── pybind11.cmake │ ├── sophus.cmake │ └── yaml-cpp.cmake ├── options.cmake └── tools.cmake ├── code_guildeline.md ├── dependencies.cmake ├── docker ├── run_local_CI.sh ├── run_test_coverage.sh └── ubuntu │ ├── .dockerignore │ └── Dockerfile ├── docs ├── Doxyfile.in ├── development_notes.md ├── extending_dsopp.md ├── pydsopp_utilities.md └── running_on_own_dataset.md ├── format.sh ├── pydsopp ├── CMakeLists.txt ├── features │ ├── CMakeLists.txt │ ├── include │ │ └── pydsopp │ │ │ └── features │ │ │ └── extract_tracking_features.hpp │ └── src │ │ └── extract_tracking_features.cpp ├── pydsopp.cpp ├── storage │ └── track_storage.py ├── track │ ├── __init__.py │ ├── camera_calibration.py │ ├── export │ │ ├── colmap_track_export.py │ │ └── json_track_export.py │ ├── frame.py │ └── track.py ├── transformations │ ├── __init__.py │ ├── __sophus_import.py │ ├── motion.py │ └── utils.py └── utils │ ├── extract_images.py │ ├── nerf_exporter.py │ ├── point_cloud_exporter.py │ ├── track2colmap.py │ └── track2json.py ├── requirements.txt ├── src ├── CMakeLists.txt ├── agent │ ├── CMakeLists.txt │ ├── include │ │ └── agent │ │ │ ├── agent.hpp │ │ │ └── agent_settings.hpp │ └── src │ │ ├── agent.cpp │ │ └── agent_settings.cpp ├── application │ ├── CMakeLists.txt │ ├── dsopp_main.cpp │ ├── tools │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── application │ │ │ │ └── tools │ │ │ │ └── read_tracks.hpp │ │ └── src │ │ │ └── read_tracks.cpp │ ├── track2trajectory.cpp │ └── viewer_main.cpp ├── common │ ├── CMakeLists.txt │ ├── constexpr_tools │ │ ├── CMakeLists.txt │ │ └── include │ │ │ └── common │ │ │ └── constexpr_tools │ │ │ └── constexpr_tools.hpp │ ├── fabric_tools │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── common │ │ │ │ └── fabric_tools │ │ │ │ └── parameters.hpp │ │ └── src │ │ │ └── parameters.cpp │ ├── file_tools │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── common │ │ │ │ └── file_tools │ │ │ │ ├── camera_frame_times.hpp │ │ │ │ ├── parsing.hpp │ │ │ │ └── read_tum_poses.hpp │ │ └── src │ │ │ ├── camera_frame_times.cpp │ │ │ ├── parsing.cpp │ │ │ └── read_tum_poses.cpp │ ├── image_tools │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── common │ │ │ │ └── image_tools │ │ │ │ └── conversion.hpp │ │ └── src │ │ │ └── conversion.cpp │ ├── include │ │ └── common │ │ │ └── settings.hpp │ ├── patch │ │ ├── CMakeLists.txt │ │ └── include │ │ │ └── common │ │ │ └── patch │ │ │ └── patch.hpp │ ├── pattern │ │ ├── CMakeLists.txt │ │ └── include │ │ │ └── common │ │ │ └── pattern │ │ │ └── pattern.hpp │ ├── semantics │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── semantics │ │ │ │ ├── semantic_filter.hpp │ │ │ │ └── semantic_legend.hpp │ │ └── src │ │ │ ├── semantic_filter.cpp │ │ │ └── semantic_legend.cpp │ ├── time │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── common │ │ │ │ └── time │ │ │ │ └── time.hpp │ │ └── src │ │ │ └── time.cpp │ └── timestamp_storage │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── common │ │ │ └── timestamp_storage │ │ │ └── timestamp_storage.hpp │ │ └── src │ │ └── timestamp_storage.cpp ├── dsopp │ ├── CMakeLists.txt │ ├── include │ │ └── dsopp │ │ │ ├── config_loader.hpp │ │ │ └── dsopp.hpp │ └── src │ │ ├── config_loader.cpp │ │ └── dsopp.cpp ├── energy │ ├── CMakeLists.txt │ ├── camera_model │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── energy │ │ │ │ └── camera_model │ │ │ │ ├── camera_model_base.hpp │ │ │ │ ├── distroted_camera.hpp │ │ │ │ ├── fisheye │ │ │ │ ├── atan_camera.hpp │ │ │ │ ├── division_model.hpp │ │ │ │ └── tum_fov_model.hpp │ │ │ │ ├── pinhole │ │ │ │ ├── ios_camera_model.hpp │ │ │ │ ├── pinhole_camera.hpp │ │ │ │ └── simple_radial.hpp │ │ │ │ └── polynomial_solver.hpp │ │ └── src │ │ │ ├── atan_camera.cpp │ │ │ ├── camera_model_base.cpp │ │ │ ├── ios_camera_model.cpp │ │ │ ├── pinhole_camera.cpp │ │ │ └── simple_radial.cpp │ ├── epipolar_geometry │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── energy │ │ │ │ └── epipolar_geometry │ │ │ │ ├── epipolar_line.hpp │ │ │ │ ├── epipolar_line_builder.hpp │ │ │ │ ├── epipolar_line_builder_pinhole_se3.hpp │ │ │ │ ├── essential_matrix.hpp │ │ │ │ ├── general_epipolar_line_triangulator.hpp │ │ │ │ ├── se3_epipolar_line_triangulator.hpp │ │ │ │ └── triangulation.hpp │ │ └── src │ │ │ ├── epipolar_line.cpp │ │ │ ├── epipolar_line_builder.cpp │ │ │ ├── essential_matrix.cpp │ │ │ └── se3_epipolar_line_triangulator.cpp │ ├── motion │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── energy │ │ │ │ └── motion │ │ │ │ ├── local_parameterization_se3.hpp │ │ │ │ ├── motion.hpp │ │ │ │ └── se3_motion.hpp │ │ └── src │ │ │ ├── motion.cpp │ │ │ └── se3_motion.cpp │ ├── n_point_solvers │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── energy │ │ │ │ └── n_point_solvers │ │ │ │ └── pure_rotation_estimator.hpp │ │ └── src │ │ │ └── pure_rotation_estimator.cpp │ ├── problems │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── energy │ │ │ │ ├── levenberg_marquardt_algorithm │ │ │ │ └── levenberg_marquardt_algorithm.hpp │ │ │ │ ├── normal_linear_system.hpp │ │ │ │ └── problems │ │ │ │ ├── cost_functors │ │ │ │ └── motion_priors.hpp │ │ │ │ ├── depth_map.hpp │ │ │ │ ├── geometric_bundle_adjustment │ │ │ │ ├── ceres_geometric_bundle_adjustment.hpp │ │ │ │ └── local_frame.hpp │ │ │ │ ├── photometric_bundle_adjustment │ │ │ │ ├── ceres_photometric_bundle_adjustment.hpp │ │ │ │ ├── eigen_photometric_bundle_adjustment.hpp │ │ │ │ ├── frame_parameterization.hpp │ │ │ │ ├── photometric_bundle_adjustment.hpp │ │ │ │ ├── trust_region_photometric_bundle_adjustment_options.hpp │ │ │ │ └── void_photometric_bundle_adjustment.hpp │ │ │ │ ├── pose_alignment │ │ │ │ ├── eigen_pose_alignment.hpp │ │ │ │ ├── pose_alignment.hpp │ │ │ │ └── precalculated_pose_alignment.hpp │ │ │ │ └── so3xs2_refinement.hpp │ │ ├── internal │ │ │ ├── energy │ │ │ │ └── problems │ │ │ │ │ ├── cost_functors │ │ │ │ │ ├── affine_brightness_regularization_cost_functor.hpp │ │ │ │ │ ├── bundle_adjustment_geometric_cost_functor.hpp │ │ │ │ │ ├── bundle_adjustment_photometric_cost_functor.hpp │ │ │ │ │ ├── bundle_adjustment_photometric_cost_functor_analytic.hpp │ │ │ │ │ └── sampson_distance_cost.hpp │ │ │ │ │ ├── local_parameterization_s2.hpp │ │ │ │ │ └── photometric_bundle_adjustment │ │ │ │ │ ├── bundle_adjustment_photometric_evaluation_callback.hpp │ │ │ │ │ ├── ceres_photometric_bundle_adjustment_add_blocks.hpp │ │ │ │ │ ├── covariance_matrices_of_relative_poses.hpp │ │ │ │ │ ├── eigen_photometric_bundle_adjustment_problem.hpp │ │ │ │ │ ├── evaluate_jacobians.hpp │ │ │ │ │ ├── first_estimate_jacobians.hpp │ │ │ │ │ ├── hessian_block_evaluation.hpp │ │ │ │ │ ├── local_frame.hpp │ │ │ │ │ └── state_priors.hpp │ │ │ └── matrix_accumulator.hpp │ │ └── src │ │ │ ├── bundle_adjustment_photometric_evaluation_callback.cpp │ │ │ ├── ceres_photometric_bundle_adjustment.cpp │ │ │ ├── eigen_photometric_bundle_adjustment.cpp │ │ │ ├── eigen_pose_alignment.cpp │ │ │ ├── geometric_bundle_adjustment │ │ │ ├── ceres_geometric_bundle_adjustment_solver.cpp │ │ │ └── local_frame.cpp │ │ │ ├── normal_linear_system.cpp │ │ │ ├── photometric_bundle_adjustment.cpp │ │ │ ├── precalculated_pose_alignment.cpp │ │ │ ├── so3xs2_refinement.cpp │ │ │ └── void_photometric_bundle_adjustment.cpp │ └── projector │ │ ├── CMakeLists.txt │ │ └── include │ │ └── energy │ │ └── projector │ │ ├── camera_projector.hpp │ │ └── camera_reproject.hpp ├── feature_based_slam │ ├── CMakeLists.txt │ ├── features │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── feature_based_slam │ │ │ │ └── features │ │ │ │ ├── correspondence.hpp │ │ │ │ ├── correspondences_finder.hpp │ │ │ │ ├── distinct_feature.hpp │ │ │ │ ├── distinct_features_extractor.hpp │ │ │ │ ├── distinct_features_extractor_orb.hpp │ │ │ │ ├── distinct_features_frame.hpp │ │ │ │ ├── optical_flow.hpp │ │ │ │ └── orb.hpp │ │ └── src │ │ │ ├── correspondences_finder.cpp │ │ │ ├── distinct_feature.cpp │ │ │ ├── distinct_features_extractor_orb.cpp │ │ │ ├── distinct_features_frame.cpp │ │ │ ├── optical_flow.cpp │ │ │ └── orb.cpp │ ├── initialization_strategy │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── feature_based_slam │ │ │ │ └── initialization_strategy │ │ │ │ ├── frequency_initializer_keyframe_strategy.hpp │ │ │ │ ├── initializer_keyframe_strategy.hpp │ │ │ │ └── wait_for_movement_keyframe_strategy.hpp │ │ └── src │ │ │ ├── frequency_initializer_keyframe_strategy.cpp │ │ │ └── wait_for_movement_keyframe_strategy.cpp │ ├── track │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── feature_based_slam │ │ │ │ └── track │ │ │ │ ├── frame.hpp │ │ │ │ ├── landmark.hpp │ │ │ │ ├── landmarks.hpp │ │ │ │ └── track.hpp │ │ └── src │ │ │ ├── frame.cpp │ │ │ ├── landmark.cpp │ │ │ └── landmarks.cpp │ └── tracker │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── feature_based_slam │ │ │ └── tracker │ │ │ ├── fabric.hpp │ │ │ ├── initializer.hpp │ │ │ ├── monocular_initializer.hpp │ │ │ ├── monocular_tracker.hpp │ │ │ ├── tracker.hpp │ │ │ └── void_initializer.hpp │ │ ├── internal │ │ └── feature_based_slam │ │ │ └── tracker │ │ │ ├── add_new_frame.hpp │ │ │ ├── add_new_landmarks.hpp │ │ │ ├── add_new_projections_to_landmarks.hpp │ │ │ ├── autocalibration_selector.hpp │ │ │ ├── estimate_se3_pnp.hpp │ │ │ ├── estimate_se3_pnp_autocalibration.hpp │ │ │ ├── estimate_so3_inlier_count.hpp │ │ │ ├── estimate_so3xs2.hpp │ │ │ ├── estimate_so3xs2_autocalibration.hpp │ │ │ ├── feature_frame_from_landmarks.hpp │ │ │ ├── initialize_poses.hpp │ │ │ ├── landmark_feature_frame.hpp │ │ │ ├── refine_track.hpp │ │ │ ├── remove_outliers.hpp │ │ │ └── triangulate_points.hpp │ │ └── src │ │ ├── add_new_frame.cpp │ │ ├── add_new_landmarks.cpp │ │ ├── add_new_projections_to_landmarks.cpp │ │ ├── estimate_se3_pnp.cpp │ │ ├── estimate_so3_inlier_count.cpp │ │ ├── estimate_so3xs2.cpp │ │ ├── fabric.cpp │ │ ├── feature_frame_from_landmarks.cpp │ │ ├── initialize_poses.cpp │ │ ├── monocular_initializer.cpp │ │ ├── monocular_tracker.cpp │ │ ├── refine_track.cpp │ │ ├── remove_outliers.cpp │ │ ├── tracker.cpp │ │ └── triangulate_points.cpp ├── features │ ├── CMakeLists.txt │ ├── include │ │ └── features │ │ │ └── camera │ │ │ ├── camera_features.hpp │ │ │ ├── ceres_grid.hpp │ │ │ ├── eigen_tracking_features_extractor.hpp │ │ │ ├── frame_embedding_extractor.hpp │ │ │ ├── pattern_patch.hpp │ │ │ ├── photometrically_corrected_image.hpp │ │ │ ├── pixel_data_frame.hpp │ │ │ ├── pixel_data_frame_extractor.hpp │ │ │ ├── pixel_map.hpp │ │ │ ├── sobel_tracking_features_extractor.hpp │ │ │ ├── tracking_feature.hpp │ │ │ ├── tracking_features_extractor.hpp │ │ │ └── tracking_features_frame.hpp │ ├── internal │ │ └── features │ │ │ └── camera │ │ │ ├── calculate_pixelinfo.hpp │ │ │ ├── downscale_image.hpp │ │ │ └── extraction_tools.hpp │ └── src │ │ ├── calculate_pixelinfo.cpp │ │ ├── camera_features.cpp │ │ ├── eigen_tracking_features_extractor.cpp │ │ ├── extraction_tools.cpp │ │ ├── identity_extractor.cpp │ │ ├── pattern_patch.cpp │ │ ├── photometrically_corrected_image.cpp │ │ ├── pixel_data_frame.cpp │ │ ├── pixel_data_frame_extractor.cpp │ │ ├── pixel_map.cpp │ │ ├── sobel_tracking_features_extractor.cpp │ │ ├── tracking_feature.cpp │ │ └── tracking_features_frame.cpp ├── marginalization │ ├── CMakeLists.txt │ ├── include │ │ └── marginalization │ │ │ ├── fabric.hpp │ │ │ ├── frame_marginalization_strategy.hpp │ │ │ ├── maximum_size_frame_marginalization_strategy.hpp │ │ │ └── sparse_frame_marginalization_strategy.hpp │ └── src │ │ ├── fabric.cpp │ │ ├── frame_marginalization_strategy.cpp │ │ ├── maximum_size_frame_marginalization_strategy.cpp │ │ └── sparse_frame_marginalization_strategy.cpp ├── measures │ ├── CMakeLists.txt │ └── include │ │ └── measures │ │ ├── similarity_measure.hpp │ │ └── similarity_measure_ssd.hpp ├── output │ ├── CMakeLists.txt │ ├── output_interfaces │ │ ├── CMakeLists.txt │ │ └── include │ │ │ └── output_interfaces │ │ │ ├── camera_output_interfaces.hpp │ │ │ ├── image_output_interface.hpp │ │ │ ├── text_output_interface.hpp │ │ │ └── track_output_interface.hpp │ ├── persistent │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── output │ │ │ │ └── persistent │ │ │ │ └── protobuf_exporter.hpp │ │ └── src │ │ │ └── protobuf_exporter.cpp │ └── visualizer │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── visualizer │ │ │ ├── output_parameters.hpp │ │ │ └── visualizer.hpp │ │ ├── internal │ │ └── visualizer │ │ │ ├── buffer_storage.hpp │ │ │ ├── local_frame.hpp │ │ │ ├── local_odometry_frame.hpp │ │ │ ├── local_odometry_track.hpp │ │ │ ├── local_track.hpp │ │ │ ├── local_tracking_frame.hpp │ │ │ ├── points_storage.hpp │ │ │ ├── renderable.hpp │ │ │ ├── visualizer_image_output_interface.hpp │ │ │ ├── visualizer_output_interface.hpp │ │ │ ├── visualizer_text_output_interface.hpp │ │ │ └── visualizer_track_output_interface.hpp │ │ └── src │ │ ├── local_frame.cpp │ │ ├── local_odometry_frame.cpp │ │ ├── local_odometry_track.cpp │ │ ├── local_track.cpp │ │ ├── local_tracking_frame.cpp │ │ ├── visualizer.cpp │ │ ├── visualizer_image_output_interface.cpp │ │ ├── visualizer_text_output_interface.cpp │ │ └── visualizer_track_output_interface.cpp ├── ransac │ ├── CMakeLists.txt │ └── include │ │ └── ransac │ │ ├── random_sequence_generator.hpp │ │ └── ransac.hpp ├── sanity_checker │ ├── CMakeLists.txt │ ├── include │ │ └── sanity_checker │ │ │ ├── fabric.hpp │ │ │ ├── sanity_check_status.hpp │ │ │ └── sanity_checker.hpp │ └── src │ │ └── fabric.cpp ├── sensors │ ├── CMakeLists.txt │ ├── camera │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── sensors │ │ │ │ └── camera │ │ │ │ └── camera.hpp │ │ └── src │ │ │ └── camera.cpp │ ├── camera_calibration │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── sensors │ │ │ │ └── camera_calibration │ │ │ │ ├── camera_calibration.hpp │ │ │ │ ├── camera_settings.hpp │ │ │ │ ├── fabric.hpp │ │ │ │ ├── mask │ │ │ │ └── camera_mask.hpp │ │ │ │ └── undistorter │ │ │ │ └── undistorter.hpp │ │ └── src │ │ │ ├── camera_calibration.cpp │ │ │ ├── camera_mask.cpp │ │ │ ├── camera_settings.cpp │ │ │ ├── fabric.cpp │ │ │ └── undistorter.cpp │ ├── camera_providers │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── sensors │ │ │ │ └── camera_providers │ │ │ │ ├── camera_data_frame.hpp │ │ │ │ ├── camera_provider.hpp │ │ │ │ ├── fabric.hpp │ │ │ │ ├── image_folder_provider.hpp │ │ │ │ ├── image_video_provider.hpp │ │ │ │ └── npy_folder_provider.hpp │ │ └── src │ │ │ ├── camera_data_frame.cpp │ │ │ ├── fabric.cpp │ │ │ ├── image_folder_provider.cpp │ │ │ ├── image_video_provider.cpp │ │ │ └── npy_folder_provider.cpp │ ├── camera_transformers │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── sensors │ │ │ │ └── camera_transformers │ │ │ │ ├── camera_image_crop.hpp │ │ │ │ ├── camera_resizer.hpp │ │ │ │ ├── camera_transformer.hpp │ │ │ │ ├── fabric.hpp │ │ │ │ └── image_cropper.hpp │ │ └── src │ │ │ ├── camera_resizer.cpp │ │ │ ├── camera_transformer.cpp │ │ │ ├── fabric.cpp │ │ │ └── image_cropper.cpp │ ├── sensor │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── sensor │ │ │ │ ├── data_frame.hpp │ │ │ │ ├── data_provider.hpp │ │ │ │ ├── sensor.hpp │ │ │ │ ├── synchronized_frame.hpp │ │ │ │ └── utilities.hpp │ │ └── src │ │ │ └── synchronized_frame.cpp │ ├── sensors │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── sensors │ │ │ │ └── sensors.hpp │ │ └── src │ │ │ └── sensors.cpp │ └── sensors_builder │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── sensors │ │ │ └── sensors_builder │ │ │ └── camera_fabric.hpp │ │ └── src │ │ └── camera_fabric.cpp ├── storage │ ├── CMakeLists.txt │ ├── include │ │ └── storage │ │ │ └── track_storage.hpp │ ├── proto │ │ ├── CMakeLists.txt │ │ ├── agent_settings.proto │ │ ├── camera_settings.proto │ │ ├── connection.proto │ │ ├── ecef_poses.proto │ │ ├── frame.proto │ │ ├── gnss_frame.proto │ │ ├── gnss_track.proto │ │ ├── landmark.proto │ │ ├── odometry_track.proto │ │ ├── sanity_check_results.proto │ │ ├── sanity_check_status.proto │ │ └── semantic_legend.proto │ └── src │ │ └── track_storage.cpp ├── synchronizer │ ├── CMakeLists.txt │ ├── include │ │ └── synchronizer │ │ │ ├── fabric.hpp │ │ │ ├── master_sensor_synchronizer.hpp │ │ │ ├── no_synchronization.hpp │ │ │ └── synchronizer.hpp │ └── src │ │ ├── fabric.cpp │ │ ├── master_sensor_synchronizer.cpp │ │ └── no_synchronization.cpp ├── track │ ├── CMakeLists.txt │ ├── connections │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── track │ │ │ │ └── connections │ │ │ │ ├── connections_container.hpp │ │ │ │ └── frame_connection.hpp │ │ └── src │ │ │ ├── connections_container.cpp │ │ │ └── frame_connection.cpp │ ├── export │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── track │ │ │ │ └── export │ │ │ │ ├── timestamp_output.hpp │ │ │ │ ├── track2tum_exporter.hpp │ │ │ │ └── transformations_output.hpp │ │ └── src │ │ │ ├── timestamp_output.cpp │ │ │ ├── track2tum_exporter.cpp │ │ │ └── transformations_output.cpp │ ├── frames │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── track │ │ │ │ └── frames │ │ │ │ ├── active_keyframe.hpp │ │ │ │ ├── frame.hpp │ │ │ │ ├── keyframe.hpp │ │ │ │ ├── points_storage.hpp │ │ │ │ ├── slam_internal_tracking_frame.hpp │ │ │ │ └── tracking_frame.hpp │ │ └── src │ │ │ ├── active_keyframe.cpp │ │ │ ├── frame.cpp │ │ │ ├── keyframe.cpp │ │ │ ├── slam_internal_tracking_frame.cpp │ │ │ └── tracking_frame.cpp │ ├── landmarks │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── track │ │ │ │ └── landmarks │ │ │ │ ├── active_tracking_landmark.hpp │ │ │ │ ├── immature_tracking_landmark.hpp │ │ │ │ ├── tracking_landmark.hpp │ │ │ │ ├── tracking_landmark_base.hpp │ │ │ │ └── tracking_landmark_with_patch.hpp │ │ └── src │ │ │ ├── active_tracking_landmark.cpp │ │ │ ├── immature_tracking_landmark.cpp │ │ │ ├── tracking_landmark.cpp │ │ │ ├── tracking_landmark_base.cpp │ │ │ └── tracking_landmark_with_patch.cpp │ └── track │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── track │ │ │ ├── active_odometry_track.hpp │ │ │ ├── active_track.hpp │ │ │ ├── odometry_track.hpp │ │ │ ├── odometry_track_base.hpp │ │ │ ├── track.hpp │ │ │ └── track_base.hpp │ │ └── src │ │ ├── active_odometry_track.cpp │ │ ├── active_track.cpp │ │ ├── odometry_track.cpp │ │ ├── odometry_track_base.cpp │ │ ├── track.cpp │ │ └── track_base.cpp └── tracker │ ├── CMakeLists.txt │ ├── depth_estimators │ ├── CMakeLists.txt │ ├── include │ │ └── tracker │ │ │ └── depth_estimators │ │ │ ├── depth_estimation.hpp │ │ │ └── depth_estimator.hpp │ └── src │ │ └── depth_estimation.cpp │ ├── keyframe_strategy │ ├── CMakeLists.txt │ ├── include │ │ └── tracker │ │ │ └── keyframe_strategy │ │ │ ├── fabric.hpp │ │ │ ├── frequency_keyframe_strategy.hpp │ │ │ ├── mean_square_optical_flow_and_rmse_keyframe_strategy.hpp │ │ │ └── tracker_keyframe_strategy.hpp │ └── src │ │ ├── fabric.cpp │ │ ├── frequency_keyframe_strategy.cpp │ │ ├── mean_square_optical_flow_and_rmse_keyframe_strategy.cpp │ │ └── tracker_keyframe_strategy.cpp │ ├── landmarks_activator │ ├── CMakeLists.txt │ ├── include │ │ └── tracker │ │ │ └── landmarks_activator │ │ │ └── landmarks_activator.hpp │ └── src │ │ └── landmarks_activator.cpp │ └── tracker │ ├── CMakeLists.txt │ ├── include │ └── tracker │ │ ├── fabric.hpp │ │ ├── monocular │ │ └── monocular_tracker.hpp │ │ └── tracker.hpp │ ├── internal │ └── tracker │ │ ├── build_features.hpp │ │ └── create_depth_maps.hpp │ └── src │ ├── create_depth_maps.cpp │ ├── fabric.cpp │ └── monocular_tracker.cpp ├── test ├── .gitignore ├── CMakeLists.txt ├── download_test_data.sh ├── local_track_performance_test.sh ├── mock │ └── mock_camera_provider.hpp ├── performance │ ├── CMakeLists.txt │ ├── application │ │ ├── run_mega_performance_test.py │ │ └── run_track_performance.sh │ ├── benchmarks │ │ ├── CMakeLists.txt │ │ ├── energy │ │ │ ├── CMakeLists.txt │ │ │ ├── photometric_bundle_adjustment_benchmark.cpp │ │ │ └── pose_alignment_benchmark.cpp │ │ ├── features │ │ │ ├── CMakeLists.txt │ │ │ └── feature_frame_benchmark.cpp │ │ ├── reprojector │ │ │ ├── CMakeLists.txt │ │ │ └── reprojector_benchmark.cpp │ │ └── tracker │ │ │ ├── CMakeLists.txt │ │ │ └── depth_estimation_benchmark.cpp │ ├── features │ │ ├── CMakeLists.txt │ │ ├── run_btf_extractor.cpp │ │ └── run_gn_net_extractor.cpp │ ├── initializer │ │ ├── CMakeLists.txt │ │ └── calculate_monocular_initializer_deviations.cpp │ ├── output │ │ ├── CMakeLists.txt │ │ └── visualizer │ │ │ ├── CMakeLists.txt │ │ │ └── calculate_average_visualizer_fps.cpp │ └── tracker │ │ ├── CMakeLists.txt │ │ └── test_depth_estimation_accuracy │ │ ├── CMakeLists.txt │ │ └── test_depth_estimation_accuracy.cpp ├── pydsopp │ ├── CMakeLists.txt │ ├── features │ │ ├── CMakeLists.txt │ │ └── test_extract_tracking_features.py │ ├── python_reader │ │ ├── CMakeLists.txt │ │ ├── compare_two_tracks.cpp │ │ ├── read_and_save.py │ │ ├── save_random_track.cpp │ │ └── test_python_reader.sh │ └── test_json_exporter.sh ├── test │ ├── CMakeLists.txt │ ├── application │ │ ├── CMakeLists.txt │ │ └── test_config_loader.cpp │ ├── energy │ │ ├── CMakeLists.txt │ │ ├── camera_model │ │ │ ├── CMakeLists.txt │ │ │ ├── generic_camera_test.hpp │ │ │ ├── test_atan_camera_model.cpp │ │ │ ├── test_division_camera_model.cpp │ │ │ ├── test_ios_camera_model.cpp │ │ │ ├── test_pinhole_camera_model.cpp │ │ │ ├── test_simple_radial.cpp │ │ │ └── test_tum_fov_model.cpp │ │ ├── epipolar_geometry │ │ │ ├── CMakeLists.txt │ │ │ ├── generalized_epipolar_line.cpp │ │ │ ├── test_essential_matrix.cpp │ │ │ └── triangulation.cpp │ │ ├── motion │ │ │ ├── CMakeLists.txt │ │ │ └── se3_motion.cpp │ │ ├── problems │ │ │ ├── CMakeLists.txt │ │ │ ├── test_affine_brightness.cpp │ │ │ ├── test_analytical_diff.cpp │ │ │ ├── test_ceres_camera_optimization.cpp │ │ │ ├── test_ceres_pose_alignment.cpp │ │ │ ├── test_ceres_pose_alignment_rotated.cpp │ │ │ ├── test_geometric_bundle_adjustment.cpp │ │ │ ├── test_incremental_solver.cpp │ │ │ ├── test_linear_system.cpp │ │ │ ├── test_local_parameterization_s2.cpp │ │ │ └── test_photometric_bundle_adjustment.cpp │ │ └── projector │ │ │ ├── CMakeLists.txt │ │ │ ├── test_projector.cpp │ │ │ └── test_reprojects.cpp │ ├── feature_based_slam │ │ ├── CMakeLists.txt │ │ ├── test_fbs_feature_matcher.cpp │ │ └── test_fbs_tracker.cpp │ ├── features │ │ ├── CMakeLists.txt │ │ ├── test_dxdy_accelerated.cpp │ │ └── test_tracking_features_extractor.cpp │ ├── marginalization │ │ ├── CMakeLists.txt │ │ └── test_marginalization.cpp │ ├── output │ │ ├── CMakeLists.txt │ │ └── visualizer │ │ │ └── test_visualizer.cpp │ ├── sensors │ │ ├── CMakeLists.txt │ │ ├── camera │ │ │ ├── calibration │ │ │ │ ├── test_camera_settings.cpp │ │ │ │ ├── test_get_camera_model.cpp │ │ │ │ └── undistorter.cpp │ │ │ ├── providers │ │ │ │ ├── test_image_folder_provider.cpp │ │ │ │ ├── test_image_video_provider.cpp │ │ │ │ └── test_npy_folder_provider.cpp │ │ │ ├── test_camera.cpp │ │ │ └── test_camera_mask.cpp │ │ └── camera_transformers │ │ │ └── test_camera_transformers.cpp │ ├── track │ │ ├── CMakeLists.txt │ │ ├── test_track_frames.cpp │ │ └── test_track_storage.cpp │ └── tracker │ │ ├── CMakeLists.txt │ │ ├── keyframe_strategy │ │ ├── CMakeLists.txt │ │ └── test_frequency_keyframe_strategy.cpp │ │ ├── landmarks │ │ ├── CMakeLists.txt │ │ ├── test_depth_estimation.cpp │ │ └── test_landmarks_activator.cpp │ │ └── reference_frame │ │ ├── CMakeLists.txt │ │ └── test_reference_frame_depth_map.cpp ├── test_data │ └── tummono │ │ ├── .gitignore │ │ ├── README.md │ │ ├── dense.yaml │ │ ├── download_data.sh │ │ ├── extreme.yaml │ │ ├── fast.yaml │ │ ├── mono.yaml │ │ └── standart.yaml └── tools │ ├── CMakeLists.txt │ ├── evaluation │ ├── associate.py │ └── evaluate_ate.py │ ├── include │ └── test │ │ └── tools │ │ ├── compare_track.hpp │ │ ├── depth_gt.hpp │ │ ├── random_track.hpp │ │ ├── savitzky_golay_filter.hpp │ │ ├── solver_test_data.hpp │ │ ├── transformations_equality.hpp │ │ └── tum_gt.hpp │ ├── src │ ├── compare_track.cpp │ ├── depth_gt.cpp │ ├── random_track.cpp │ ├── savitzky_golay_filter.cpp │ ├── solver_test_data.cpp │ └── tum_gt.cpp │ └── tests │ └── test_track2tum_exporter.cpp └── tools └── run_clang_format.py /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: CI 4 | 5 | # Controls when the workflow will run 6 | on: 7 | # Triggers the workflow on push or pull request events but only for the main branch 8 | push: 9 | branches: [ main ] 10 | pull_request: 11 | branches: [ main ] 12 | 13 | # Allows you to run this workflow manually from the Actions tab 14 | workflow_dispatch: 15 | 16 | jobs: 17 | build-gcc-test: 18 | runs-on: ubuntu-latest 19 | steps: 20 | - name: checkout 21 | uses: actions/checkout@v3 22 | - name: Build image 23 | run: docker buildx build --tag dsopp-latest --build-arg USER_ID=$(id -u) --build-arg GROUP_ID=$(id -g) -f $GITHUB_WORKSPACE/docker/ubuntu/Dockerfile . 24 | - name: Download test data 25 | run: docker run -v "$GITHUB_WORKSPACE:/app" -i dsopp-latest bash -c "cd app/test; sh download_test_data.sh" 26 | - name: Build 27 | run: docker run -v "$GITHUB_WORKSPACE:/app" -i dsopp-latest bash -c "cd app; cmake . -DCMAKE_BUILD_TYPE=Release -DCOMPILE_HIDDEN_CODE=OFF -DLTO_OPTIMIZATION=ON -Bbuild && cmake --build build -- -j4 && cd build && ctest" -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeCache.txt 2 | CMakeFiles 3 | CMakeScripts 4 | Makefile 5 | cmake_install.cmake 6 | 7 | build 8 | docker_gcc_build 9 | docker_clang_build 10 | 11 | cmake-build-* 12 | 13 | .* 14 | 15 | compile_commands.json 16 | *.sw? 17 | 18 | __pycache__ 19 | *.pyc 20 | *.bin 21 | 22 | 23 | *.zip 24 | 2011_09_30 25 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | ## Contributing guidelines 2 | 3 | Thank you for your interest in contributing to DSO++. 4 | 5 | Please read through this tiny document before submitting any issues or pull requests. 6 | 7 | If you have noticed a bug or came up with a new feature or algorithm, please check [DSOPP issue tracker](https://github.com/RoadlyInc/DSOPP/issues) or [DSOPP pull request page](https://github.com/RoadlyInc/DSOPP/pulls) to make sure somebody else hasn't already reported an issue or created pull request. 8 | 9 | If you want to contribute, please follow [DSOPP Code Guidlines](https://github.com/RoadlyInc/DSOPP/blob/main/code_guildeline.md). It helps to keep code readable and pull requests easier to look through. 10 | -------------------------------------------------------------------------------- /cmake/addTest.cmake: -------------------------------------------------------------------------------- 1 | # Wrapper to simplify test 2 | function(dsopp_add_test test_name test_source) 3 | add_executable(${test_name} ${test_source}) 4 | target_link_libraries(${test_name} gtest gtest_main) 5 | target_link_libraries(${test_name} ${ARGN}) 6 | add_test(${test_name} ${test_name}) 7 | endfunction() 8 | 9 | # Wrapper to simplify test and coverage 10 | function(dsopp_add_test_and_coverage test_name test_source) 11 | dsopp_add_test(${test_name} ${test_source} ${ARGN}) 12 | set_tests_properties(${test_name} PROPERTIES LABELS "Coverage") 13 | endfunction() 14 | -------------------------------------------------------------------------------- /cmake/modules/cnpy.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://github.com/rogersce/cnpy.git) 2 | set(VERSION 4e8810b1a8637695171ed346ce68f6984e585ef4) 3 | set(cnpy_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/cnpy_external) 4 | set(cnpy_LIBS ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/cnpy_external-build/libcnpy${CMAKE_STATIC_LIBRARY_SUFFIX}) 5 | 6 | find_package(ZLIB REQUIRED) 7 | if (NOT EXISTS ${cnpy_LIBS}) 8 | externalproject_add(cnpy_external 9 | GIT_REPOSITORY ${URL} 10 | GIT_TAG ${VERSION} 11 | CMAKE_ARGS 12 | -DCMAKE_BUILD_TYPE=Release 13 | -DCMAKE_INSTALL_PREFIX=./install 14 | INSTALL_COMMAND "" 15 | TEST_COMMAND "" 16 | PREFIX 3rd_party 17 | EXCLUDE_FROM_ALL 1 18 | ) 19 | endif() 20 | add_library(cnpy INTERFACE IMPORTED GLOBAL) 21 | add_dependencies(cnpy cnpy_external) 22 | file(MAKE_DIRECTORY ${cnpy_INCLUDE_DIR}) 23 | target_include_directories(cnpy INTERFACE ${cnpy_INCLUDE_DIR}) 24 | target_link_libraries(cnpy INTERFACE ${ZLIB_LIBRARIES}) 25 | target_link_libraries(cnpy INTERFACE ${cnpy_LIBS} ${ZLIB_LIBRARIES}) 26 | -------------------------------------------------------------------------------- /cmake/modules/eigen.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://gitlab.com/libeigen/eigen.git) 2 | set(VERSION 1f4c0311cda3403999b702c996898af5707973a9) 3 | 4 | set(eigen_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/eigen_external-build/install/include/eigen3) 5 | get_filename_component(eigen_cmake "${eigen_INCLUDE_DIR}/../../share/eigen3/cmake" ABSOLUTE) 6 | if (NOT EXISTS ${eigen_cmake}) 7 | externalproject_add(eigen_external 8 | GIT_REPOSITORY ${URL} 9 | GIT_TAG ${VERSION} 10 | CMAKE_ARGS 11 | -DCMAKE_BUILD_TYPE=Release 12 | -DCMAKE_INSTALL_PREFIX=./install 13 | TEST_COMMAND "" 14 | PREFIX 3rd_party 15 | EXCLUDE_FROM_ALL 1 16 | ) 17 | file(MAKE_DIRECTORY ${eigen_INCLUDE_DIR}) 18 | endif() 19 | 20 | add_library(eigen INTERFACE IMPORTED GLOBAL) 21 | add_dependencies(eigen eigen_external) 22 | 23 | target_include_directories(eigen INTERFACE ${eigen_INCLUDE_DIR}) 24 | -------------------------------------------------------------------------------- /cmake/modules/glog.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://github.com/google/glog.git) 2 | set(VERSION 0a2e5931bd5ff22fd3bf8999eb8ce776f159cda6) 3 | set(glog_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/glog_external-build/install/include) 4 | set(glog_LIBS ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/glog_external-build/install/lib/libglog${CMAKE_STATIC_LIBRARY_SUFFIX}) 5 | if (NOT EXISTS ${glog_LIBS}) 6 | externalproject_add(glog_external 7 | DEPENDS gflags 8 | GIT_REPOSITORY ${URL} 9 | GIT_TAG ${VERSION} 10 | CMAKE_ARGS 11 | -DCMAKE_BUILD_TYPE=Release 12 | -DCMAKE_CXX_STANDARD=20 13 | -DCMAKE_CXX_STANDARD_REQUIRED=ON 14 | -DCMAKE_CXX_FLAGS=-march=native 15 | -DCMAKE_INSTALL_PREFIX=./install 16 | -DBUILD_TESTING=OFF 17 | -DWITH_GFLAGS=OFF 18 | -DWITH_UNWIND=OFF 19 | TEST_COMMAND "" 20 | PREFIX 3rd_party 21 | EXCLUDE_FROM_ALL 1 22 | ) 23 | endif() 24 | 25 | add_library(glog INTERFACE IMPORTED GLOBAL) 26 | add_dependencies(glog glog_external) 27 | file(MAKE_DIRECTORY ${glog_INCLUDE_DIR}) 28 | target_include_directories(glog INTERFACE ${glog_INCLUDE_DIR}) 29 | set_property(GLOBAL PROPERTY glog_path ${glog_LIBS}) 30 | target_link_libraries(glog INTERFACE ${glog_LIBS} gflags ${GFLAGS_LIBRARIES}) 31 | -------------------------------------------------------------------------------- /cmake/modules/opengv.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://github.com/laurentkneip/opengv.git) 2 | set(VERSION 306a54e6c6b94e2048f820cdf77ef5281d4b48ad) 3 | 4 | set(opengv_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/opengv_external/include) 5 | set(opengv_LIBS ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/opengv_external-build/lib/libopengv${CMAKE_STATIC_LIBRARY_SUFFIX}) 6 | 7 | get_target_property(eigen_INCLUDE_DIRS eigen INTERFACE_INCLUDE_DIRECTORIES) 8 | 9 | if (NOT EXISTS ${opengv_LIBS}) 10 | externalproject_add(opengv_external 11 | DEPENDS eigen 12 | GIT_REPOSITORY ${URL} 13 | GIT_TAG ${VERSION} 14 | CMAKE_ARGS 15 | -DCMAKE_BUILD_TYPE=Release 16 | -DEIGEN_INCLUDE_DIR:PATH=${eigen_INCLUDE_DIRS} 17 | INSTALL_COMMAND "" 18 | TEST_COMMAND "" 19 | PREFIX 3rd_party 20 | EXCLUDE_FROM_ALL 1 21 | ) 22 | endif() 23 | add_library(opengv INTERFACE IMPORTED GLOBAL) 24 | add_dependencies(opengv opengv_external) 25 | file(MAKE_DIRECTORY ${opengv_INCLUDE_DIR}) 26 | target_include_directories(opengv INTERFACE ${opengv_INCLUDE_DIR} ${eigen_INCLUDE_DIR}) 27 | target_link_libraries(opengv INTERFACE ${opengv_LIBS} ) 28 | -------------------------------------------------------------------------------- /cmake/modules/pybind11.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://github.com/pybind/pybind11.git) 2 | set(VERSION v2.7.1) 3 | 4 | include(FetchContent) 5 | FetchContent_Declare(pybind11 6 | GIT_REPOSITORY ${URL} 7 | GIT_TAG ${VERSION}) 8 | 9 | FetchContent_GetProperties(pybind11) 10 | if(NOT pybind11_POPULATED) 11 | FetchContent_Populate(pybind11) 12 | add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR}) 13 | endif() 14 | 15 | add_library(pybind11 INTERFACE IMPORTED GLOBAL) 16 | target_include_directories(pybind11 INTERFACE ${pybind11_INCLUDE_DIRS}) 17 | -------------------------------------------------------------------------------- /cmake/modules/sophus.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://github.com/strasdat/Sophus.git) 2 | set(VERSION 593db47500ea1a2de5f0e6579c86147991509c59) 3 | 4 | set(sophus_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/sophus_external) 5 | 6 | get_target_property(eigen_INCLUDE_DIRS eigen INTERFACE_INCLUDE_DIRECTORIES) 7 | get_filename_component(eigen_cmake "${eigen_INCLUDE_DIRS}/../../share/eigen3/cmake" ABSOLUTE) 8 | 9 | 10 | if (NOT EXISTS ${sophus_INCLUDE_DIR}/sophus) 11 | externalproject_add(sophus_external 12 | DEPENDS eigen 13 | GIT_REPOSITORY ${URL} 14 | GIT_TAG ${VERSION} 15 | CMAKE_ARGS 16 | -DCMAKE_BUILD_TYPE=Release 17 | -DBUILD_TESTS=OFF 18 | -DEigen3_DIR=${eigen_cmake} 19 | CONFIGURE_COMMAND "" 20 | BUILD_COMMAND "" 21 | INSTALL_COMMAND "" 22 | TEST_COMMAND "" 23 | PREFIX 3rd_party 24 | EXCLUDE_FROM_ALL 1 25 | ) 26 | file(MAKE_DIRECTORY ${sophus_INCLUDE_DIR}) 27 | endif() 28 | 29 | add_library(sophus INTERFACE IMPORTED GLOBAL) 30 | add_dependencies(sophus sophus_external) 31 | 32 | target_include_directories(sophus INTERFACE ${sophus_INCLUDE_DIR} ${eigen_INCLUDE_DIR}) 33 | -------------------------------------------------------------------------------- /cmake/modules/yaml-cpp.cmake: -------------------------------------------------------------------------------- 1 | set(URL https://github.com/jbeder/yaml-cpp.git) 2 | set(VERSION 08aa252611843b93c4f98959fe89c81b872224ae) 3 | 4 | set(yaml-cpp_INCLUDE_DIR 5 | ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/yaml-cpp_external/include) 6 | set(yaml-cpp_LIBS 7 | ${CMAKE_CURRENT_BINARY_DIR}/3rd_party/src/yaml-cpp_external-build/libyaml-cpp${CMAKE_STATIC_LIBRARY_SUFFIX} 8 | ) 9 | 10 | file(MAKE_DIRECTORY ${yaml-cpp_INCLUDE_DIR}) 11 | 12 | 13 | ExternalProject_Add( 14 | yaml-cpp_external 15 | GIT_REPOSITORY ${URL} 16 | GIT_TAG ${VERSION} 17 | INSTALL_COMMAND "" 18 | CMAKE_ARGS -DCMAKE_BUILD_TYPE=Release 19 | TEST_COMMAND "" 20 | PREFIX 3rd_party 21 | EXCLUDE_FROM_ALL 1) 22 | 23 | add_library(yaml-cpp INTERFACE IMPORTED GLOBAL) 24 | add_dependencies(yaml-cpp yaml-cpp_external) 25 | target_include_directories(yaml-cpp INTERFACE ${yaml-cpp_INCLUDE_DIR}) 26 | target_link_libraries(yaml-cpp INTERFACE ${yaml-cpp_LIBS}) 27 | -------------------------------------------------------------------------------- /cmake/options.cmake: -------------------------------------------------------------------------------- 1 | option(BUILD_DOC "Build documentation" OFF) 2 | option(CHECK_FORMAT "check clang and cmake format" ON) 3 | option(COMPILE_HIDDEN_CODE "Compile hidden code" OFF) 4 | option(LTO_OPTIMIZATION "Enable LTO optimization" ON) 5 | 6 | option(VISUALIZATION "Enable visualization" ON) 7 | option(USE_FLOAT "Use float arithmetics" OFF) 8 | 9 | option(TEST_COVERAGE "Build for test coverage" OFF) 10 | -------------------------------------------------------------------------------- /code_guildeline.md: -------------------------------------------------------------------------------- 1 | # Code Guidelines 2 | 3 | We are using [Google C++ Style Guide] 4 | 5 | All files including CMake, python, c++ headers/sources should be formatted. There is `format.sh` script to help you with it. 6 | Run it from project's root as follows: 7 | 8 | ``` 9 | bash fromat.sh 10 | ``` 11 | 12 | ### Additional 13 | 14 | #### Template class casts: 15 | 16 | * Use ``CastT`` alias to cast templated motion/calibration types 17 | 18 | ``` 19 | using MotionCasted = typename Motion::template CastT; 20 | using ModelCasted = typename Model::template CastT; 21 | ``` 22 | 23 | ### Pull Request limits 24 | 25 | We have soft (300 lines changes) and hard (500 lines changes) limits for pull requests size. 26 | 27 | [Google C++ Style Guide]: https://google.github.io/styleguide/cppguide.html 28 | 29 | -------------------------------------------------------------------------------- /dependencies.cmake: -------------------------------------------------------------------------------- 1 | include(ExternalProject) 2 | 3 | include(cmake/modules/eigen.cmake) 4 | include(cmake/modules/sophus.cmake) 5 | include(cmake/modules/yaml-cpp.cmake) 6 | include(cmake/modules/gflags.cmake) 7 | include(cmake/modules/glog.cmake) 8 | include(cmake/modules/gtest.cmake) 9 | include(cmake/modules/opengv.cmake) 10 | if(VISUALIZATION) 11 | include(cmake/modules/pangolin.cmake) 12 | endif() 13 | include(cmake/modules/pybind11.cmake) 14 | include(cmake/modules/cnpy.cmake) 15 | include(cmake/modules/ceres.cmake) 16 | include(cmake/modules/opencv.cmake) 17 | include(cmake/modules/gbenchmark.cmake) 18 | -------------------------------------------------------------------------------- /docker/ubuntu/.dockerignore: -------------------------------------------------------------------------------- 1 | * 2 | -------------------------------------------------------------------------------- /docs/pydsopp_utilities.md: -------------------------------------------------------------------------------- 1 | `pydsopp` offers an easy way to process `track.bin` from `dsopp_main` 2 | 3 | ### `nerf_exporter.py` 4 | 5 | Export `track.bin` format to [Nvidia instant-ngp](https://github.com/NVlabs/instant-ngp) nerf format 6 | 7 | From the build folder: 8 | 9 | ``` 10 | python3 -m pydsopp.utils.nerf_exporter --track track_bin_path --output output_directory 11 | ``` 12 | 13 | ### `track2colmap.py` 14 | 15 | Export `track.bin` to colmap `txt` track format 16 | 17 | From build folder: 18 | 19 | ``` 20 | python3 -m pydsopp.utils.track2colmap --track track_bin_path --output output_directory 21 | ``` 22 | 23 | Note: `save_images` should be set to `on` in `mono.yaml` to save undistorted images too 24 | 25 | ### `point_cloud_exporter.py` 26 | 27 | Sparse point cloud exporter 28 | 29 | From the build folder: 30 | 31 | ``` 32 | python3 -m pydsopp.utils.point_cloud_exporter --track track_bin_path --coord_system odometry --color_scheme image_colors --file_format xyz --output output_cloud_path.xyz 33 | ``` 34 | 35 | Note: that `--color_schame image_colors` would work only if `save_images` was set to `on` in `mono.yaml`, otherwise use `--color_scheme semantic` 36 | -------------------------------------------------------------------------------- /format.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/sh 2 | find {src/,test/,pydsopp/} -name "*.?pp" | xargs clang-format-10 --style=file -i 3 | find {src/,test/,pydsopp/} -name "CMakeLists.txt" | xargs cmake-format -i 4 | cmake-format -i CMakeLists.txt 5 | yapf -i -r test/pydsopp 6 | yapf -i -r pydsopp 7 | -------------------------------------------------------------------------------- /pydsopp/features/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(features_wrapper src/extract_tracking_features.cpp) 2 | 3 | target_include_directories( 4 | features_wrapper PUBLIC $ 5 | $) 6 | 7 | target_link_libraries(features_wrapper PUBLIC settings pybind11) 8 | target_link_libraries(features_wrapper PRIVATE features) 9 | -------------------------------------------------------------------------------- /pydsopp/features/include/pydsopp/features/extract_tracking_features.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_PYDSOPP_FEATURES_EXTRACT_TRACKING_FEATURES_HPP 2 | #define DSOPP_PYDSOPP_FEATURES_EXTRACT_TRACKING_FEATURES_HPP 3 | 4 | #include 5 | 6 | #include "common/settings.hpp" 7 | 8 | namespace dsopp::pydsopp::features { 9 | std::vector> extract_tracking_features( 10 | const pybind11::array_t &image_array); 11 | } 12 | 13 | #endif // DSOPP_PYDSOPP_FEATURES_EXTRACT_TRACKING_FEATURES_HPP 14 | -------------------------------------------------------------------------------- /pydsopp/pydsopp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "pydsopp/features/extract_tracking_features.hpp" 5 | 6 | PYBIND11_MODULE(pydsopp, pydsopp) { 7 | pydsopp.doc() = "DSOPP Python binding"; 8 | 9 | auto features = pydsopp.def_submodule("features", "Features"); 10 | features.def("extract_tracking_features", &dsopp::pydsopp::features::extract_tracking_features, 11 | "Extracts tracking features from BGR image", pybind11::arg("image")); 12 | } 13 | -------------------------------------------------------------------------------- /pydsopp/track/__init__.py: -------------------------------------------------------------------------------- 1 | from .frame import Landmark 2 | from .frame import Frame 3 | from .track import Track 4 | from .camera_calibration import CameraCalibration 5 | 6 | from .export.colmap_track_export import colmap_track_export 7 | from .export.json_track_export import json_track_export 8 | -------------------------------------------------------------------------------- /pydsopp/transformations/__init__.py: -------------------------------------------------------------------------------- 1 | from .motion import Motion 2 | from .utils import sim3_from_parameters, se3_from_parameters 3 | -------------------------------------------------------------------------------- /pydsopp/transformations/__sophus_import.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | sys.path.append("${PY_SOPHUS}") 4 | import sophus 5 | -------------------------------------------------------------------------------- /pydsopp/transformations/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sympy 3 | 4 | from sympy.algebras.quaternion import Quaternion 5 | from .motion import Motion 6 | 7 | 8 | def sim3_from_parameters(parameters): 9 | """ 10 | create Sim3 matrix from the format: quaternion, translation 11 | Arguments: 12 | parameters parameters in order: quaternion, translation 13 | Returns matrix of transformation 14 | """ 15 | quaternion = Quaternion(parameters[3], *parameters[:3]) 16 | translation = np.array(parameters[4:]) 17 | return Motion(quaternion, translation) 18 | 19 | 20 | def se3_from_parameters(parameters): 21 | """ 22 | create Se3 motion from the format: quaternion, translation 23 | NOTE: quaternion would be normalized 24 | Arguments: 25 | parameters parameters in order: quaternion, translation 26 | Returns se3 transformation 27 | """ 28 | quaternion = Quaternion(parameters[3], *parameters[:3]) 29 | translation = np.array(parameters[4:]) 30 | return Motion(quaternion / quaternion.norm(), translation) 31 | -------------------------------------------------------------------------------- /pydsopp/utils/extract_images.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from tqdm import tqdm 3 | import argparse 4 | from pathlib import Path 5 | 6 | from ..track.track import Track 7 | from ..storage.track_storage import TrackStorage 8 | 9 | if __name__ == "__main__": 10 | 11 | parser = argparse.ArgumentParser(description='control saving data') 12 | parser.add_argument('--images', 13 | metavar='-pc', 14 | dest='images', 15 | required=True, 16 | help='path to the folder for saving images') 17 | 18 | parser.add_argument('--track', 19 | metavar='-t', 20 | dest='track', 21 | required=True, 22 | help='absolute path to track.bin file') 23 | 24 | args = parser.parse_args() 25 | 26 | track_storage = TrackStorage() 27 | track_storage.read(args.track) 28 | track = Track(track_storage) 29 | 30 | if not track.dumped_images: 31 | print(f"{args.track} doesn't contain images") 32 | exit(1) 33 | 34 | path = args.images 35 | Path(path).mkdir(parents=True, exist_ok=True) 36 | 37 | for frame_id, frame in tqdm(enumerate(track.frames), 38 | 'Dumping keyframes', 39 | total=len(track.track.frames)): 40 | cv2.imwrite(f'{path}/{frame_id}.jpg', frame.image()) 41 | -------------------------------------------------------------------------------- /pydsopp/utils/track2colmap.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | from ..track import Track, colmap_track_export 4 | from ..storage.track_storage import TrackStorage 5 | 6 | 7 | def read_track(track_path): 8 | track_storage = TrackStorage() 9 | track_storage.read(track_path) 10 | return Track(track_storage) 11 | 12 | 13 | if __name__ == "__main__": 14 | parser = argparse.ArgumentParser( 15 | description='Track to colmap .txt format exporter') 16 | parser.add_argument( 17 | '--track', 18 | dest='track', 19 | required=True, 20 | help= 21 | 'path to track.bin files (multiple). Multiple tracks option is available only for tracks with same T_world_local pose e.g. after relocalization module', 22 | nargs='+') 23 | parser.add_argument('--output', 24 | dest='output', 25 | required=True, 26 | help='output directory path') 27 | parser.add_argument('--pose_mode', 28 | dest='pose_mode', 29 | default='odometry', 30 | help='Output poses `ecef` or `odometry`') 31 | 32 | args = parser.parse_args() 33 | 34 | tracks = [read_track(track_path) for track_path in args.track] 35 | colmap_track_export(tracks, args.output, args.pose_mode) 36 | -------------------------------------------------------------------------------- /pydsopp/utils/track2json.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | from ..track import Track, json_track_export 4 | from ..storage.track_storage import TrackStorage 5 | 6 | if __name__ == "__main__": 7 | parser = argparse.ArgumentParser(description='Track to json exporter') 8 | parser.add_argument('--track', 9 | dest='track', 10 | required=True, 11 | help='path to track.bin file') 12 | parser.add_argument('--output', 13 | dest='output', 14 | required=True, 15 | help='output json filename') 16 | args = parser.parse_args() 17 | 18 | track_storage = TrackStorage() 19 | track_storage.read(args.track) 20 | track = Track(track_storage) 21 | 22 | json_track_export(track, args.output) 23 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | cmake-format 2 | conan 3 | pydot 4 | pydotplus 5 | networkx 6 | gdown 7 | pyyaml 8 | sympy 9 | scipy 10 | opencv-contrib-python 11 | yapf==0.31.0 12 | protobuf==3.18.0 13 | typing_extensions 14 | dataclasses 15 | laspy 16 | clang-format==10.0.1.1 17 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(common) 2 | add_subdirectory(storage) 3 | 4 | add_subdirectory(features) 5 | add_subdirectory(sensors) 6 | add_subdirectory(measures) 7 | 8 | add_subdirectory(agent) 9 | add_subdirectory(energy) 10 | add_subdirectory(ransac) 11 | add_subdirectory(synchronizer) 12 | add_subdirectory(track) 13 | add_subdirectory(tracker) 14 | add_subdirectory(output) 15 | add_subdirectory(marginalization) 16 | add_subdirectory(feature_based_slam) 17 | add_subdirectory(sanity_checker) 18 | 19 | add_subdirectory(dsopp) 20 | add_subdirectory(application) 21 | 22 | #[[ contact Roadly INC for this functionality ]] 23 | 24 | #[[ 25 | add_subdirectory(relocalization) 26 | add_subdirectory(global_localization) 27 | 28 | ]] 29 | -------------------------------------------------------------------------------- /src/agent/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(agent src/agent.cpp src/agent_settings.cpp) 2 | 3 | target_include_directories( 4 | agent PUBLIC $ 5 | $) 6 | 7 | target_link_libraries(agent PUBLIC sensors camera_calibration proto_storage 8 | settings) 9 | target_link_libraries(agent PRIVATE camera glog) 10 | -------------------------------------------------------------------------------- /src/agent/include/agent/agent.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DSOPP_AGENT_HPP 3 | #define DSOPP_AGENT_HPP 4 | #include 5 | #include 6 | namespace dsopp { 7 | namespace sensors { 8 | class Sensors; 9 | } 10 | /** 11 | * \brief class to represent the main agent of the system. 12 | * 13 | * Agent stores all sensors and constraints on their positions. Agent is the object that we track 14 | */ 15 | class Agent { 16 | public: 17 | /** 18 | * creates an agent from the array of sensors 19 | * 20 | */ 21 | explicit Agent(); 22 | /** 23 | * get the sensors agent have 24 | * 25 | * @return sensors agent have 26 | */ 27 | sensors::Sensors& sensors() const; 28 | 29 | private: 30 | /** Sensors that are processed by the application*/ 31 | std::unique_ptr sensors_; 32 | }; 33 | } // namespace dsopp 34 | 35 | #endif // DSOPP_AGENT_HPP 36 | -------------------------------------------------------------------------------- /src/agent/src/agent.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "agent/agent.hpp" 3 | 4 | #include "sensors/camera/camera.hpp" 5 | #include "sensors/sensors.hpp" 6 | 7 | namespace dsopp { 8 | Agent::Agent() : sensors_(std::make_unique()) {} 9 | 10 | sensors::Sensors& Agent::sensors() const { return *sensors_; } 11 | } // namespace dsopp 12 | -------------------------------------------------------------------------------- /src/application/tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(application_tools src/read_tracks.cpp) 2 | 3 | target_include_directories( 4 | application_tools 5 | PUBLIC $ 6 | $) 7 | 8 | target_link_libraries(application_tools PUBLIC track motion_model settings 9 | stdc++fs storage) 10 | -------------------------------------------------------------------------------- /src/application/tools/include/application/tools/read_tracks.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_READ_TRACKS_HPP 2 | #define DSOPP_READ_TRACKS_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "common/settings.hpp" 8 | #include "track/track.hpp" 9 | namespace dsopp::application::tools { 10 | /** 11 | * loads tracks from proto file 12 | * @param paths vector of proto files 13 | * @return vector of loaded tracks 14 | */ 15 | std::vector>>> readTracks(std::vector paths); 16 | } // namespace dsopp::application::tools 17 | 18 | #endif // DSOPP_READ_TRACKS_HPP 19 | -------------------------------------------------------------------------------- /src/application/tools/src/read_tracks.cpp: -------------------------------------------------------------------------------- 1 | #include "application/tools/read_tracks.hpp" 2 | 3 | #include 4 | 5 | #include "storage/track_storage.hpp" 6 | 7 | namespace dsopp::application::tools { 8 | using Motion = energy::motion::SE3; 9 | 10 | std::vector>> readTracks(std::vector paths) { 11 | std::vector>> tracks; 12 | for (const auto &track_file : paths) { 13 | std::ifstream stream(track_file, std::ios::binary); 14 | if (!stream.is_open()) { 15 | std::cout << "Can't open file " << track_file << std::endl; 16 | tracks.push_back(nullptr); 17 | continue; 18 | } 19 | 20 | track::storage::Track track_storage; 21 | if (!track_storage.read(stream)) { 22 | std::cout << "Can't parse file " << track_file << std::endl; 23 | tracks.push_back(nullptr); 24 | continue; 25 | } 26 | 27 | tracks.push_back(std::make_unique>(track_storage)); 28 | stream.close(); 29 | } 30 | return tracks; 31 | } 32 | } // namespace dsopp::application::tools 33 | -------------------------------------------------------------------------------- /src/application/viewer_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "agent/agent.hpp" 6 | #include "application/tools/read_tracks.hpp" 7 | #include "common/settings.hpp" 8 | 9 | #include "output_interfaces/track_output_interface.hpp" 10 | #include "sensors/sensors.hpp" 11 | #include "track/track.hpp" 12 | #include "visualizer/visualizer.hpp" 13 | 14 | int main(int argc, char *argv[]) { 15 | using Motion = dsopp::energy::motion::SE3; 16 | google::InitGoogleLogging(argv[0]); 17 | if (argc < 2) { 18 | std::cout << "Usage " << argv[0] << " ..." << std::endl; 19 | return 0; 20 | } 21 | 22 | auto visualizer = std::make_unique(1920, 1080); 23 | auto tracks_storage = dsopp::application::tools::readTracks({argv + 1, argv + argc}); 24 | 25 | std::vector> *> tracks; 26 | std::transform(tracks_storage.begin(), tracks_storage.end(), back_inserter(tracks), 27 | [](const auto &track) { return track.get(); }); 28 | 29 | for (const auto &track : tracks) { 30 | if (track != nullptr) { 31 | auto *visualizer_oi = visualizer->createTrackOutputInterface(); 32 | visualizer_oi->finish(*track); 33 | } 34 | } 35 | visualizer->init(); 36 | while (visualizer->running()) { 37 | visualizer->render(); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /src/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(constexpr_tools) 2 | add_subdirectory(time) 3 | add_subdirectory(pattern) 4 | add_subdirectory(patch) 5 | add_subdirectory(image_tools) 6 | add_subdirectory(file_tools) 7 | add_subdirectory(fabric_tools) 8 | add_subdirectory(semantics) 9 | add_subdirectory(timestamp_storage) 10 | 11 | add_library(settings INTERFACE) 12 | 13 | target_include_directories( 14 | settings INTERFACE $ 15 | $) 16 | target_link_libraries(settings INTERFACE eigen) 17 | -------------------------------------------------------------------------------- /src/common/constexpr_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_constexpr_tools INTERFACE) 2 | 3 | target_include_directories( 4 | common_constexpr_tools 5 | INTERFACE $ 6 | $) 7 | -------------------------------------------------------------------------------- /src/common/constexpr_tools/include/common/constexpr_tools/constexpr_tools.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_COMMON_CONSTEXPR_HPP 2 | #define DSOPP_COMMON_CONSTEXPR_HPP 3 | 4 | namespace dsopp::common::constexpr_tools { 5 | template 6 | constexpr void constexpr_for(F&& f) { 7 | if constexpr (Start != End) { 8 | f.template operator()(); 9 | constexpr_for(f); 10 | } 11 | } 12 | } // namespace dsopp::common::constexpr_tools 13 | 14 | #endif // DSOPP_COMMON_CONSTEXPR_HPP 15 | -------------------------------------------------------------------------------- /src/common/fabric_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_fabric_tools src/parameters.cpp) 2 | 3 | target_include_directories( 4 | common_fabric_tools 5 | PUBLIC $ 6 | $) 7 | 8 | target_link_libraries(common_fabric_tools PUBLIC glog) 9 | -------------------------------------------------------------------------------- /src/common/fabric_tools/src/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "common/fabric_tools/parameters.hpp" 2 | 3 | #include 4 | 5 | namespace dsopp::common::fabric_tools { 6 | bool readSwitcherParameter(const std::map ¶meters, const std::string ¶meter_name) { 7 | std::vector positive_switch = {"on", "true"}; 8 | std::vector negative_switch = {"off", "false"}; 9 | 10 | if (parameters.count(parameter_name) != 0) { 11 | std::string parameter_value = std::any_cast(parameters.at(parameter_name)); 12 | for (auto &pattern : positive_switch) { 13 | if (parameter_value == pattern) return true; 14 | } 15 | for (auto &pattern : negative_switch) { 16 | if (parameter_value == pattern) return false; 17 | } 18 | 19 | LOG(ERROR) << "Unsupported option in \"" + parameter_name + "\". Expected on/off or true/false"; 20 | } else { 21 | LOG(WARNING) << "Missing \"" + parameter_name + "\" value. Setting to false"; 22 | } 23 | return false; 24 | } 25 | 26 | } // namespace dsopp::common::fabric_tools 27 | -------------------------------------------------------------------------------- /src/common/file_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_file_tools src/parsing.cpp src/read_tum_poses.cpp 2 | src/camera_frame_times.cpp) 3 | 4 | target_include_directories( 5 | common_file_tools 6 | PUBLIC $ 7 | $) 8 | 9 | target_link_libraries(common_file_tools PUBLIC motion_model settings stdc++fs 10 | time glog) 11 | -------------------------------------------------------------------------------- /src/common/file_tools/include/common/file_tools/camera_frame_times.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_COMMON_FILE_TOOLS_CAMERA_FRAME_TIMES_HPP_ 2 | #define DSOPP_COMMON_FILE_TOOLS_CAMERA_FRAME_TIMES_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "common/settings.hpp" 8 | 9 | namespace dsopp::common::file_tools { 10 | 11 | /** Camera frame times */ 12 | struct CameraFrameTimes { 13 | /** timestamp */ 14 | const uint64_t timestamp; 15 | /** exposure time */ 16 | const Precision exposure_time; 17 | }; 18 | 19 | /** 20 | * function to read all times of frames 21 | * 22 | * @param timestamps_file path to the file with timestamps 23 | * @param[out] times all times of frames 24 | */ 25 | void readTimes(const std::string ×tamps_file, std::map ×); 26 | 27 | } // namespace dsopp::common::file_tools 28 | 29 | #endif // DSOPP_COMMON_FILE_TOOLS_CAMERA_FRAME_TIMES_HPP_ 30 | -------------------------------------------------------------------------------- /src/common/file_tools/include/common/file_tools/read_tum_poses.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_COMMON_FILE_TOOLS_READ_TUM_FORMAT_HPP_ 2 | #define DSOPP_COMMON_FILE_TOOLS_READ_TUM_FORMAT_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "common/settings.hpp" 8 | #include "common/time/time.hpp" 9 | #include "energy/motion/motion.hpp" 10 | 11 | namespace dsopp::common::file_tools { 12 | 13 | /** 14 | * Reads tum timestamps and poses in tum format 15 | * @param stream input stream 16 | * @return map of timestamp - Motion 17 | */ 18 | template 19 | std::map readTumPoses(std::ifstream &stream); 20 | 21 | } // namespace dsopp::common::file_tools 22 | 23 | #endif // DSOPP_COMMON_FILE_TOOLS_READ_TUM_FORMAT_HPP_ 24 | -------------------------------------------------------------------------------- /src/common/file_tools/src/camera_frame_times.cpp: -------------------------------------------------------------------------------- 1 | #include "common/file_tools/camera_frame_times.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "common/file_tools/parsing.hpp" 8 | 9 | namespace dsopp::common::file_tools { 10 | 11 | void readTimes(const std::string ×tamps_file, std::map ×) { 12 | auto stream = std::ifstream(timestamps_file); 13 | if (!stream.is_open()) { 14 | LOG(WARNING) << "File " << timestamps_file << " does not exist!"; 15 | return; 16 | } 17 | std::string line; 18 | while (std::getline(stream, line)) { 19 | const auto tokens = splitLine(line, ' '); 20 | uint64_t frame_id = std::stoull(tokens[0]); 21 | std::string ts_string = tokens[1]; 22 | const uint64_t timestamp = 23 | static_cast(common::file_tools::stringToTime(ts_string).time_since_epoch().count()); 24 | Precision exposure_time = 1; 25 | if (tokens.size() > 2) { 26 | exposure_time = stringToPrecision(tokens[2]); 27 | } 28 | times.insert({frame_id, {timestamp, exposure_time}}); 29 | } 30 | stream.close(); 31 | } 32 | 33 | } // namespace dsopp::common::file_tools 34 | -------------------------------------------------------------------------------- /src/common/file_tools/src/read_tum_poses.cpp: -------------------------------------------------------------------------------- 1 | #include "common/file_tools/read_tum_poses.hpp" 2 | 3 | #include "common/file_tools/parsing.hpp" 4 | #include "energy/motion/se3_motion.hpp" 5 | 6 | namespace dsopp::common::file_tools { 7 | 8 | template 9 | std::map readTumPoses(std::ifstream &stream) { 10 | std::map out; 11 | std::string ts; 12 | Precision x, y, z; 13 | Precision rot_w, rot_x, rot_y, rot_z; 14 | while (stream >> ts >> x >> y >> z >> rot_x >> rot_y >> rot_z >> rot_w) { 15 | Sophus::SE3 T_w_c; 16 | T_w_c.translation() = Eigen::Vector3(x, y, z); 17 | T_w_c.setQuaternion(Eigen::Quaternion(rot_w, rot_x, rot_y, rot_z).normalized()); 18 | time timestamp = stringToTime(ts); 19 | out.insert({timestamp, Motion(T_w_c)}); 20 | } 21 | return out; 22 | } 23 | 24 | template std::map> readTumPoses(std::ifstream &stream); 25 | 26 | } // namespace dsopp::common::file_tools 27 | -------------------------------------------------------------------------------- /src/common/image_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_image_tools src/conversion.cpp) 2 | 3 | target_include_directories( 4 | common_image_tools 5 | PUBLIC $ 6 | $) 7 | 8 | target_link_libraries(common_image_tools PUBLIC features) 9 | target_link_libraries(common_image_tools PRIVATE settings eigen) 10 | -------------------------------------------------------------------------------- /src/common/include/common/settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_COMMON_SETTINGS_HPP 2 | #define DSOPP_COMMON_SETTINGS_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace dsopp { 9 | /** precision of all computation (float is faster but less accurate) */ 10 | #if USE_FLOAT 11 | using Precision = float; 12 | #else 13 | using Precision = double; 14 | #endif 15 | constexpr Precision operator"" _p(long double arg) { return static_cast(arg); } 16 | constexpr Precision operator"" _p(unsigned long long arg) { return static_cast(arg); } 17 | /** Aligned allocator for the current Precision */ 18 | using PrecisionAllocator = Eigen::aligned_allocator; 19 | } // namespace dsopp 20 | 21 | #endif // DSOPP_COMMON_SETTINGS_HPP 22 | -------------------------------------------------------------------------------- /src/common/patch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_patch INTERFACE) 2 | 3 | target_include_directories( 4 | common_patch INTERFACE $ 5 | $) 6 | 7 | target_link_libraries(common_patch INTERFACE eigen) 8 | -------------------------------------------------------------------------------- /src/common/patch/include/common/patch/patch.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_COMMON_PATCH_HPP 2 | #define DSOPP_COMMON_PATCH_HPP 3 | 4 | #include 5 | 6 | namespace dsopp { 7 | namespace patches { 8 | /** Patch */ 9 | template 10 | struct Patch { 11 | /** patch storage order */ 12 | static constexpr Eigen::StorageOptions StorageOrder = C == 1 ? Eigen::ColMajor : Eigen::RowMajor; 13 | }; 14 | } // namespace patches 15 | /** alias for main patch storage order in all slam library */ 16 | template 17 | constexpr Eigen::StorageOptions PatchStorageOrder = patches::Patch::StorageOrder; 18 | } // namespace dsopp 19 | 20 | #endif // DSOPP_COMMON_PATCH_HPP 21 | -------------------------------------------------------------------------------- /src/common/pattern/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_pattern INTERFACE) 2 | 3 | target_include_directories( 4 | common_pattern 5 | INTERFACE $ 6 | $) 7 | 8 | target_link_libraries(common_pattern INTERFACE eigen settings) 9 | -------------------------------------------------------------------------------- /src/common/pattern/include/common/pattern/pattern.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_PATTERN_COMMON_HPP_ 2 | #define DSOPP_PATTERN_COMMON_HPP_ 3 | 4 | #include 5 | 6 | #include "common/settings.hpp" 7 | 8 | namespace dsopp { 9 | namespace patterns { 10 | /** 11 | * \brief Dso 8 point neighbourhood pattern image 12 | * For details, see paper 'Direct Sparse Odometry' by Engel et al. 13 | * https://arxiv.org/pdf/1607.02565.pdf 14 | */ 15 | struct EightPointPattern { 16 | /** number of neighbours points in pattern image */ 17 | static constexpr int kSize = 8; 18 | /** position of the center in the patch */ 19 | static constexpr int kCenter = 4; 20 | /** raw pattern data in x_i, y_i order */ 21 | static constexpr Precision pattern_data[kSize * 2] = { 22 | // clang-format off 23 | 0, 2, 24 | -1, 1, 25 | 1, 1, 26 | -2, 0, 27 | 0, 0, 28 | 2, 0, 29 | -1, -1, 30 | 0, -2 31 | // clang-format on 32 | }; 33 | /** alias for pattern Eigen type */ 34 | using EigenType = const Eigen::Matrix; 35 | }; 36 | } // namespace patterns 37 | /** alias for main pattern in all slam library */ 38 | using Pattern = patterns::EightPointPattern; 39 | } // namespace dsopp 40 | 41 | #endif // DSOPP_PATTERN_COMMON_HPP_ 42 | -------------------------------------------------------------------------------- /src/common/semantics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_semantics src/semantic_legend.cpp src/semantic_filter.cpp) 2 | 3 | target_include_directories( 4 | common_semantics PUBLIC $ 5 | $) 6 | 7 | target_link_libraries(common_semantics PUBLIC eigen) 8 | target_link_libraries(common_semantics PRIVATE glog common_file_tools 9 | proto_storage) 10 | -------------------------------------------------------------------------------- /src/common/semantics/src/semantic_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "semantics/semantic_filter.hpp" 2 | 3 | namespace dsopp::semantics { 4 | 5 | SemanticFilter::SemanticFilter(const SemanticLegend &legend, const std::vector &filter_names, 6 | bool filter_indicated, bool filter_by_semantic) 7 | : filter_by_semantic_(filter_by_semantic) { 8 | for (const auto &tag : legend.tags()) { 9 | auto found = std::find(filter_names.begin(), filter_names.end(), tag.name) != filter_names.end(); 10 | is_filtered_[tag.code] = filter_indicated ? found : !found; 11 | } 12 | } 13 | 14 | bool SemanticFilter::filtered(size_t code) const { return is_filtered_[code]; } 15 | 16 | bool SemanticFilter::filterBySemantic() const { return filter_by_semantic_; } 17 | 18 | } // namespace dsopp::semantics 19 | -------------------------------------------------------------------------------- /src/common/time/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(time src/time.cpp) 2 | 3 | target_include_directories( 4 | time PUBLIC $ 5 | $) 6 | -------------------------------------------------------------------------------- /src/common/time/include/common/time/time.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SRC_TIME_HPP_ 2 | #define DSOPP_SRC_TIME_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | namespace dsopp { 8 | /** shortcut for timestamp */ 9 | using time = std::chrono::time_point; 10 | std::ostream& operator<<(std::ostream& os, const time&); 11 | } // namespace dsopp 12 | 13 | #endif // DSOPP_SRC_TIME_HPP_ 14 | -------------------------------------------------------------------------------- /src/common/time/src/time.cpp: -------------------------------------------------------------------------------- 1 | #include "common/time/time.hpp" 2 | 3 | #include 4 | #include 5 | 6 | namespace { 7 | std::string convertTimePointToString(const dsopp::time& time_point) { 8 | std::time_t time = std::chrono::system_clock::to_time_t(time_point); 9 | auto ms = std::chrono::duration_cast(time_point.time_since_epoch()) - 10 | std::chrono::duration_cast(time_point.time_since_epoch()); 11 | std::stringstream ss; 12 | ss << std::put_time(std::localtime(&time), "%H h %M m %S s "); 13 | return ss.str() + std::to_string(ms.count()) + std::string(" ms"); 14 | } 15 | } // namespace 16 | 17 | std::ostream& dsopp::operator<<(std::ostream& os, const dsopp::time& time_point) { 18 | os << convertTimePointToString(time_point); 19 | return os; 20 | } 21 | -------------------------------------------------------------------------------- /src/common/timestamp_storage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(common_timestamp_storage src/timestamp_storage.cpp) 2 | 3 | target_include_directories( 4 | common_timestamp_storage 5 | PUBLIC $ 6 | $) 7 | 8 | target_link_libraries(common_timestamp_storage PUBLIC motion_model time 9 | settings) 10 | -------------------------------------------------------------------------------- /src/common/timestamp_storage/include/common/timestamp_storage/timestamp_storage.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_COMMON_TIMESTAMP_STORAGE_HPP_ 2 | #define DSOPP_COMMON_TIMESTAMP_STORAGE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "common/time/time.hpp" 9 | 10 | namespace dsopp::common::timestamp_storage { 11 | 12 | /** \brief an interface for storing and accessing data through nearest timestamp 13 | * 14 | * @tparam T type of the data 15 | */ 16 | template 17 | class TimestampStorage { 18 | public: 19 | TimestampStorage() = default; 20 | /** 21 | * @param data data 22 | */ 23 | TimestampStorage(std::map &&data); 24 | 25 | /** 26 | * @param ts time 27 | * @return data with timestamp 28 | */ 29 | std::optional getData(time ts) const; 30 | 31 | /** 32 | * @param ts timestamp 33 | * @param d data 34 | */ 35 | void pushData(time ts, const T &d); 36 | 37 | protected: 38 | /** some data with timestamps */ 39 | std::map data_; 40 | }; 41 | 42 | } // namespace dsopp::common::timestamp_storage 43 | 44 | #endif // DSOPP_COMMON_TIMESTAMP_STORAGE_HPP_ 45 | -------------------------------------------------------------------------------- /src/common/timestamp_storage/src/timestamp_storage.cpp: -------------------------------------------------------------------------------- 1 | #include "common/timestamp_storage/timestamp_storage.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "common/settings.hpp" 9 | #include "energy/motion/se3_motion.hpp" 10 | 11 | namespace dsopp::common::timestamp_storage { 12 | template 13 | TimestampStorage::TimestampStorage(std::map &&data) : data_(std::move(data)) {} 14 | 15 | template 16 | std::optional TimestampStorage::getData(time ts) const { 17 | auto found = data_.find(ts); 18 | if (found == data_.end()) { 19 | return std::nullopt; 20 | } 21 | return found->second; 22 | } 23 | 24 | template 25 | void TimestampStorage::pushData(time ts, const T &d) { 26 | data_.emplace(ts, d); 27 | } 28 | 29 | template class TimestampStorage>; 30 | template class TimestampStorage>; 31 | template class TimestampStorage; 32 | 33 | } // namespace dsopp::common::timestamp_storage 34 | -------------------------------------------------------------------------------- /src/dsopp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(dsopp src/dsopp.cpp src/config_loader.cpp) 2 | 3 | target_include_directories( 4 | dsopp PUBLIC $ 5 | $) 6 | 7 | target_link_libraries(dsopp PUBLIC motion_model output_interfaces) 8 | 9 | target_link_libraries( 10 | dsopp 11 | PRIVATE agent 12 | synchronizer 13 | problems 14 | sanity_checker 15 | marginalization 16 | feature_based_slam_tracker 17 | features 18 | yaml-cpp 19 | sensors 20 | sensors_builder 21 | camera_calibration 22 | tracker 23 | tracker_depth_estimators 24 | measures 25 | glog 26 | stdc++fs 27 | track) 28 | -------------------------------------------------------------------------------- /src/dsopp/include/dsopp/config_loader.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DSOPP_CONFIG_LOADER_HPP 3 | #define DSOPP_CONFIG_LOADER_HPP 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "energy/motion/motion.hpp" 10 | #include "energy/motion/se3_motion.hpp" 11 | 12 | namespace dsopp { 13 | template 14 | class DSOPP; 15 | /** 16 | * \brief ConfigLoader reads application configuration from the provided configuration file. 17 | * 18 | * ConfigLoader parses yaml file and creates SLAM system with a required configuration. 19 | * */ 20 | class ConfigLoader { 21 | public: 22 | /** 23 | * creates loader from the specified yaml file 24 | * @param file path to yaml config file 25 | */ 26 | explicit ConfigLoader(const std::string &file); 27 | 28 | /** 29 | * method to access loaded agent. 30 | * @param config_args argument to replace values from the config file 31 | * @return loaded application or nullptr on failure 32 | */ 33 | template 34 | std::unique_ptr> application(const std::map &config_args = {}); 35 | 36 | private: 37 | /** path to yaml configuration file*/ 38 | const std::string &file_; 39 | }; 40 | } // namespace dsopp 41 | 42 | #endif // DSOPP_CONFIG_LOADER_HPP 43 | -------------------------------------------------------------------------------- /src/energy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(motion) 2 | add_subdirectory(camera_model) 3 | add_subdirectory(projector) 4 | add_subdirectory(epipolar_geometry) 5 | add_subdirectory(problems) 6 | add_subdirectory(n_point_solvers) 7 | -------------------------------------------------------------------------------- /src/energy/camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | camera_model 3 | src/camera_model_base.cpp src/pinhole_camera.cpp src/atan_camera.cpp 4 | src/ios_camera_model.cpp src/simple_radial.cpp) 5 | 6 | target_include_directories( 7 | camera_model PUBLIC $ 8 | $) 9 | 10 | target_link_libraries(camera_model PUBLIC motion_model time settings ceres) 11 | target_link_libraries(camera_model PRIVATE glog) 12 | -------------------------------------------------------------------------------- /src/energy/camera_model/include/energy/camera_model/distroted_camera.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_DISTROTED_CAMERA_HPP 2 | #define DSOPP_DISTROTED_CAMERA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "common/settings.hpp" 8 | 9 | namespace dsopp::energy::model { 10 | template 11 | concept DistortedCamera = requires(const Eigen::Vector3 &ray, Eigen::Vector2 &pt, 12 | const Eigen::Vector2 &pt1, Eigen::Vector3 &ray1, 13 | const CameraModel &cam) { 14 | { cam.project(ray, pt) } 15 | ->std::same_as; 16 | { cam.unproject(pt1, ray1) } 17 | ->std::same_as; 18 | { cam.focalX() } 19 | ->std::same_as; 20 | { cam.focalY() } 21 | ->std::same_as; 22 | }; 23 | 24 | } // namespace dsopp::energy::model 25 | 26 | #endif // DSOPP_DISTROTED_CAMERA_HPP 27 | -------------------------------------------------------------------------------- /src/energy/camera_model/include/energy/camera_model/polynomial_solver.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_ENERGY_CAMERA_MODELS_POLYNOMIAL_SOLVER_HPP_ 2 | #define DSOPP_ENERGY_CAMERA_MODELS_POLYNOMIAL_SOLVER_HPP_ 3 | 4 | #include 5 | 6 | #include "common/settings.hpp" 7 | 8 | namespace dsopp::energy::model::polynomial_solver { 9 | 10 | template 11 | Scalar minimalPositiveRoot(Eigen::VectorX polynomial) { 12 | const Precision kEps = 1e-8_p; 13 | 14 | polynomial /= polynomial.norm(); 15 | 16 | long n = polynomial.rows() - 1; 17 | while (abs(polynomial[n]) < Scalar(kEps)) { 18 | n--; 19 | } 20 | if (n == 0) { 21 | return Scalar(0); 22 | } 23 | Eigen::MatrixX companion(n, n); 24 | companion.setZero(); 25 | companion.block(0, n - 1, n, 1) = -polynomial.head(n) / polynomial[n]; 26 | companion.block(1, 0, n - 1, n - 1).diagonal().setOnes(); 27 | 28 | auto lambda = (companion.eigenvalues().eval()); 29 | 30 | Scalar ans = Scalar(std::numeric_limits::infinity()); 31 | 32 | for (int i = 0; i < lambda.rows(); ++i) { 33 | auto l = lambda[i]; 34 | 35 | if (abs(l.imag()) > Scalar(Eigen::NumTraits::epsilon()) * abs(l.real()) || l.real() < Scalar(0.0)) 36 | continue; 37 | ans = std::min(ans, l.real()); 38 | } 39 | 40 | return ans; 41 | } 42 | 43 | } // namespace dsopp::energy::model::polynomial_solver 44 | 45 | #endif // DSOPP_ENERGY_CAMERA_MODELS_POLYNOMIAL_SOLVER_HPP_ 46 | -------------------------------------------------------------------------------- /src/energy/camera_model/src/atan_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/camera_model/fisheye/atan_camera.hpp" 2 | 3 | namespace dsopp::energy::model { 4 | 5 | AtanCamera::AtanCamera(const Eigen::Vector2 &image_size, const Eigen::Vector2 &focal_lengths, 6 | const Eigen::Vector2 ¢er, const Eigen::VectorX &polynomial, 7 | time::duration shutter_time) 8 | : CameraModelBase(image_size, shutter_time) { 9 | long N = focal_lengths.rows() + center.rows() + polynomial.rows(); 10 | data_.resize(N); 11 | data_.head<2>() = focal_lengths; 12 | data_.segment<2>(2) = center; 13 | data_.tail(polynomial.rows()) = polynomial; 14 | } 15 | 16 | Precision AtanCamera::focalX() const { return data_(0); } 17 | Precision AtanCamera::focalY() const { return data_(1); } 18 | 19 | } // namespace dsopp::energy::model 20 | -------------------------------------------------------------------------------- /src/energy/camera_model/src/camera_model_base.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/camera_model/camera_model_base.hpp" 2 | 3 | namespace dsopp::energy::model { 4 | CameraModelBase::CameraModelBase(const Eigen::Vector2 &image_size, time::duration shutter_time, 5 | const size_t scale) 6 | : image_size_(image_size / scale), shutter_time_(shutter_time / scale) {} 7 | 8 | const Eigen::Vector2 &CameraModelBase::image_size() const { return image_size_; } 9 | 10 | time::duration CameraModelBase::shutterTime() const { return shutter_time_; } 11 | 12 | } // namespace dsopp::energy::model 13 | -------------------------------------------------------------------------------- /src/energy/camera_model/src/ios_camera_model.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/camera_model/pinhole/ios_camera_model.hpp" 2 | 3 | namespace dsopp::energy::model { 4 | 5 | IOSCamera::IOSCamera(const Eigen::Vector2 &image_size, const Eigen::Vector2 &focal_lengths, 6 | const Eigen::Vector2 ¢er, const Eigen::VectorX &lookup_distortion, 7 | time::duration shutter_time) 8 | : CameraModelBase(image_size, shutter_time) { 9 | long N = focal_lengths.rows() + center.rows() + lookup_distortion.rows(); 10 | data_.resize(N); 11 | data_.head<2>() = focal_lengths; 12 | data_.segment<2>(2) = center; 13 | data_.tail(lookup_distortion.rows()) = lookup_distortion; 14 | 15 | /** estimating max radius */ 16 | Precision max_x = std::max(center(0), image_size(0) - center(0)); 17 | Precision max_y = std::max(center(1), image_size(1) - center(1)); 18 | 19 | max_radius_ = std::sqrt(max_x * max_x + max_y * max_y); 20 | } 21 | 22 | Precision IOSCamera::focalX() const { return data_(0); } 23 | Precision IOSCamera::focalY() const { return data_(1); } 24 | 25 | } // namespace dsopp::energy::model 26 | -------------------------------------------------------------------------------- /src/energy/camera_model/src/pinhole_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/camera_model/pinhole/pinhole_camera.hpp" 2 | 3 | #include 4 | 5 | namespace dsopp { 6 | namespace energy { 7 | namespace model {} // namespace model 8 | } // namespace energy 9 | } // namespace dsopp 10 | -------------------------------------------------------------------------------- /src/energy/camera_model/src/simple_radial.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/camera_model/pinhole/simple_radial.hpp" 2 | 3 | namespace dsopp::energy::model {} // namespace dsopp::energy::model 4 | -------------------------------------------------------------------------------- /src/energy/epipolar_geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | epipolar_geometry 3 | src/epipolar_line.cpp src/epipolar_line_builder.cpp 4 | src/se3_epipolar_line_triangulator.cpp src/essential_matrix.cpp) 5 | 6 | target_include_directories( 7 | epipolar_geometry 8 | PUBLIC $ 9 | $) 10 | 11 | target_link_libraries(epipolar_geometry PUBLIC camera_projector settings) 12 | target_link_libraries(epipolar_geometry PRIVATE glog) 13 | -------------------------------------------------------------------------------- /src/energy/epipolar_geometry/include/energy/epipolar_geometry/essential_matrix.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_ESSENTIAL_MATRIX_HPP 2 | #define DSOPP_ESSENTIAL_MATRIX_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace dsopp { 10 | namespace energy { 11 | namespace epipolar_geometry { 12 | /** 13 | * Convert an essential matrix to four reference to target transformation variants. The algorithm from the paper 14 | * 'An Efficient Solution to the Five-Point Relative Pose Problem' by David Nist´er is used (Section 3.1). 15 | * @param essential_matrix 16 | * @return vector with four transformation variants 17 | */ 18 | std::vector essentialMatrixToTransformation(const Eigen::Matrix3d &essential_matrix); 19 | 20 | /** 21 | * Convert reference to target transformation to the essential matrix using equation E = hat(t) * R 22 | * @param t_t_r reference to target transformation 23 | * @return essential matrix 24 | */ 25 | Eigen::Matrix3d transformationToEssentialMatrix(const Sophus::SE3d &t_t_r); 26 | 27 | } // namespace epipolar_geometry 28 | } // namespace energy 29 | } // namespace dsopp 30 | 31 | #endif // DSOPP_ESSENTIAL_MATRIX_HPP 32 | -------------------------------------------------------------------------------- /src/energy/epipolar_geometry/src/epipolar_line_builder.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/epipolar_geometry/epipolar_line_builder.hpp" 2 | 3 | namespace dsopp { 4 | namespace energy { 5 | namespace epipolar_geometry {} 6 | } // namespace energy 7 | } // namespace dsopp 8 | -------------------------------------------------------------------------------- /src/energy/epipolar_geometry/src/essential_matrix.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/epipolar_geometry/essential_matrix.hpp" 2 | 3 | namespace dsopp { 4 | namespace energy { 5 | namespace epipolar_geometry { 6 | 7 | std::vector essentialMatrixToTransformation(const Eigen::Matrix3d &essential_matrix) { 8 | Eigen::JacobiSVD svd_holder(essential_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); 9 | Eigen::MatrixXd U = svd_holder.matrixU(); 10 | Eigen::MatrixXd V = svd_holder.matrixV(); 11 | U *= U.determinant(); 12 | V *= V.determinant(); 13 | Eigen::Matrix3d W; 14 | W << 0, -1, 0, 1, 0, 0, 0, 0, 1; 15 | 16 | std::vector rotations = {U * W * V.transpose(), U * W * V.transpose(), 17 | U * W.transpose() * V.transpose(), U * W.transpose() * V.transpose()}; 18 | std::vector translations = {U.col(2), -U.col(2), U.col(2), -U.col(2)}; 19 | std::vector result; 20 | for (size_t i = 0; i < rotations.size(); i++) { 21 | result.emplace_back(rotations[i], translations[i]); 22 | } 23 | 24 | return result; 25 | } 26 | 27 | Eigen::Matrix3d transformationToEssentialMatrix(const Sophus::SE3d &t_t_r) { 28 | return Sophus::SO3d::hat(t_t_r.translation()) * t_t_r.rotationMatrix(); 29 | } 30 | 31 | } // namespace epipolar_geometry 32 | } // namespace energy 33 | } // namespace dsopp 34 | -------------------------------------------------------------------------------- /src/energy/motion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | motion_model 3 | src/motion.cpp src/se3_motion.cpp 4 | #[[ contact Roadly INC for this functionality 5 | src/uniform_rolling_shutter.cpp 6 | ]] 7 | ) 8 | 9 | target_include_directories( 10 | motion_model PUBLIC $ 11 | $) 12 | 13 | target_link_libraries(motion_model PUBLIC eigen sophus) 14 | target_link_libraries(motion_model PRIVATE glog ceres) 15 | -------------------------------------------------------------------------------- /src/energy/motion/src/motion.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/motion/motion.hpp" 2 | 3 | #include "energy/motion/se3_motion.hpp" 4 | 5 | namespace dsopp::energy::motion { 6 | static_assert(Motion>); 7 | // static_assert(Motion>); 8 | 9 | static_assert(MotionProduct::Product>); 10 | // static_assert(MotionProduct::Product>); 11 | 12 | } // namespace dsopp::energy::motion 13 | -------------------------------------------------------------------------------- /src/energy/motion/src/se3_motion.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/motion/se3_motion.hpp" 2 | 3 | namespace dsopp::energy::motion {} 4 | -------------------------------------------------------------------------------- /src/energy/n_point_solvers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(n_point_solvers src/pure_rotation_estimator.cpp) 2 | 3 | target_include_directories( 4 | n_point_solvers PUBLIC $ 5 | $) 6 | 7 | target_include_directories(n_point_solvers PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) 8 | 9 | target_link_libraries(n_point_solvers PUBLIC eigen epipolar_geometry 10 | motion_model) 11 | 12 | target_link_libraries(n_point_solvers PRIVATE glog) 13 | -------------------------------------------------------------------------------- /src/energy/problems/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | problems 3 | src/ceres_photometric_bundle_adjustment.cpp 4 | src/geometric_bundle_adjustment/ceres_geometric_bundle_adjustment_solver.cpp 5 | src/geometric_bundle_adjustment/local_frame.cpp 6 | src/so3xs2_refinement.cpp 7 | src/bundle_adjustment_photometric_evaluation_callback.cpp 8 | src/photometric_bundle_adjustment.cpp 9 | src/eigen_pose_alignment.cpp 10 | src/eigen_photometric_bundle_adjustment.cpp 11 | src/normal_linear_system.cpp 12 | src/void_photometric_bundle_adjustment.cpp 13 | src/precalculated_pose_alignment.cpp) 14 | 15 | target_include_directories( 16 | problems PUBLIC $ 17 | $) 18 | 19 | target_include_directories( 20 | problems PRIVATE $) 21 | 22 | target_link_libraries( 23 | problems 24 | PUBLIC motion_model 25 | ceres 26 | common_pattern 27 | common_patch 28 | features 29 | measures 30 | camera_calibration 31 | camera_projector 32 | track_connections 33 | track_frames 34 | $<$:common_image_tools> 35 | camera 36 | common_timestamp_storage) 37 | 38 | target_link_libraries(problems PRIVATE glog ceres track_landmarks tbb) 39 | -------------------------------------------------------------------------------- /src/energy/problems/include/energy/problems/cost_functors/motion_priors.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_ENERGY_PROBLEMS_COST_FUNCTORS_MOTION_PRIORS_HPP_ 2 | #define DSOPP_ENERGY_PROBLEMS_COST_FUNCTORS_MOTION_PRIORS_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dsopp::energy::problem::cost_functors {} // namespace dsopp::energy::problem::cost_functors 9 | #endif // DSOPP_ENERGY_PROBLEMS_COST_FUNCTORS_MOTION_PRIORS_HPP_ 10 | -------------------------------------------------------------------------------- /src/energy/problems/include/energy/problems/depth_map.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_DEPTH_MAP_HPP 2 | #define DSOPP_DEPTH_MAP_HPP 3 | 4 | #include 5 | 6 | #include "common/settings.hpp" 7 | 8 | namespace dsopp::energy::problem { 9 | 10 | /** 11 | * Depth map corresponding to the image 12 | */ 13 | struct DepthMap { 14 | /** 15 | * Variation-weighted depth 16 | */ 17 | struct WeightedIdepth { 18 | /** inverse depth */ 19 | Precision idepth = 0; 20 | /** weight */ 21 | Precision weight = 0; 22 | /** 23 | * sum operator overloading 24 | * @param rhs idepth to sum 25 | * @return sum 26 | * */ 27 | WeightedIdepth operator+(const WeightedIdepth &rhs) const { 28 | return WeightedIdepth{idepth + rhs.idepth, weight + rhs.weight}; 29 | } 30 | }; 31 | /** 32 | * Creating depth map with given size 33 | * @param width, height given sizes 34 | */ 35 | DepthMap(long width, long height) : map(width, height) {} 36 | /** depth map */ 37 | Eigen::MatrixX map; 38 | }; 39 | 40 | } // namespace dsopp::energy::problem 41 | 42 | #endif // DSOPP_DEPTH_MAP_HPP 43 | -------------------------------------------------------------------------------- /src/energy/problems/include/energy/problems/photometric_bundle_adjustment/frame_parameterization.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FRAME_PARAMETERIZATION_HPP 2 | #define DSOPP_FRAME_PARAMETERIZATION_HPP 3 | 4 | namespace dsopp { 5 | namespace energy { 6 | namespace problem { 7 | 8 | enum struct FrameParameterization : uint8_t { kFree = 0, kFixed = 1 }; 9 | 10 | } 11 | } // namespace energy 12 | } // namespace dsopp 13 | 14 | #endif // DSOPP_FRAME_PARAMETERIZATION_HPP 15 | -------------------------------------------------------------------------------- /src/energy/problems/include/energy/problems/photometric_bundle_adjustment/void_photometric_bundle_adjustment.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_ENERGY_PROBLEM_PHOTOMETRIC_BUNDLE_ADJUSTMENT_VOID_PHOTOMETRIC_BUNDLE_ADJUSTMENT_HPP_ 2 | #define DSOPP_ENERGY_PROBLEM_PHOTOMETRIC_BUNDLE_ADJUSTMENT_VOID_PHOTOMETRIC_BUNDLE_ADJUSTMENT_HPP_ 3 | 4 | #include "common/pattern/pattern.hpp" 5 | #include "energy/motion/motion.hpp" 6 | #include "energy/problems/photometric_bundle_adjustment/photometric_bundle_adjustment.hpp" 7 | #include "track/frames/active_keyframe.hpp" 8 | 9 | namespace dsopp::energy::problem { 10 | 11 | /** 12 | * \brief Solver class, which does nothing 13 | * 14 | * This solver is sort of switch, that tunrs off global optimization of poses and idepthes 15 | */ 16 | template 17 | class VoidPhotometricBundleAdjustment : public PhotometricBundleAdjustment { 19 | public: 20 | VoidPhotometricBundleAdjustment(); 21 | /** 22 | * @return 0 23 | */ 24 | Precision solve(const size_t); 25 | }; 26 | 27 | } // namespace dsopp::energy::problem 28 | #endif // DSOPP_ENERGY_PROBLEM_PHOTOMETRIC_BUNDLE_ADJUSTMENT_VOID_PHOTOMETRIC_BUNDLE_ADJUSTMENT_HPP_ 29 | -------------------------------------------------------------------------------- /src/energy/problems/include/energy/problems/so3xs2_refinement.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_ENERGY_PROBLEM_SO3XS2_REFINEMENT_HPP_ 2 | #define DSOPP_ENERGY_PROBLEM_SO3XS2_REFINEMENT_HPP_ 3 | 4 | #include "energy/camera_model/pinhole/pinhole_camera.hpp" 5 | 6 | #include 7 | #include 8 | 9 | namespace dsopp::energy::problem { 10 | 11 | /** 12 | * method to optimize pose as essential matrix 13 | * 14 | * @param focal focal lengths of camera 15 | * @param[out] t_t_r transformation from reference to target 16 | * @param points_reference projections from reference frame 17 | * @param points_target projections from target frame 18 | * @param threshold huber-loss threshold in pixels 19 | * @param number_of_threads 20 | */ 21 | template 22 | void refineSO3xS2(typename std::conditional::type focal, Sophus::SE3d &t_t_r, 23 | const std::vector &points_reference, 24 | const std::vector &points_target, double threshold = 1.5, 25 | const int number_of_threads = 1); 26 | 27 | } // namespace dsopp::energy::problem 28 | 29 | #endif // DSOPP_ENERGY_PROBLEM_SO3XS2_REFINEMENT_HPP_ 30 | -------------------------------------------------------------------------------- /src/energy/problems/src/geometric_bundle_adjustment/local_frame.cpp: -------------------------------------------------------------------------------- 1 | #include "energy/problems/geometric_bundle_adjustment/local_frame.hpp" 2 | 3 | namespace dsopp::energy::problem::geometric_bundle_adjustment { 4 | 5 | LocalFrame::LocalFrame(size_t frame_id, const motion::SE3 t_agent_world_) 6 | : t_agent_world(t_agent_world_), frame_id_(frame_id) {} 7 | 8 | void LocalFrame::pushObservation(const Eigen::Vector2d &pixel_observation, Eigen::Vector3d &point_3d) { 9 | observed_points_.push_back(pixel_observation); 10 | associated_point_3d_.push_back(point_3d); 11 | } 12 | 13 | size_t LocalFrame::size() const { return observed_points_.size(); } 14 | 15 | std::pair LocalFrame::point(size_t idx) { 16 | return {observed_points_[idx], associated_point_3d_[idx]}; 17 | } 18 | 19 | size_t LocalFrame::frameId() const { return frame_id_; } 20 | 21 | } // namespace dsopp::energy::problem::geometric_bundle_adjustment 22 | -------------------------------------------------------------------------------- /src/energy/projector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(camera_projector INTERFACE) 2 | 3 | target_include_directories( 4 | camera_projector 5 | INTERFACE $ 6 | $) 7 | 8 | target_link_libraries(camera_projector INTERFACE camera_model motion_model) 9 | -------------------------------------------------------------------------------- /src/feature_based_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(features) 2 | add_subdirectory(initialization_strategy) 3 | add_subdirectory(tracker) 4 | add_subdirectory(track) 5 | -------------------------------------------------------------------------------- /src/feature_based_slam/features/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | feature_based_slam_features 3 | src/distinct_feature.cpp src/distinct_features_frame.cpp 4 | src/distinct_features_extractor_orb.cpp src/optical_flow.cpp 5 | src/correspondences_finder.cpp src/orb.cpp) 6 | 7 | target_include_directories( 8 | feature_based_slam_features 9 | PUBLIC $ 10 | $) 11 | 12 | target_link_libraries(feature_based_slam_features PUBLIC eigen settings 13 | camera_calibration) 14 | -------------------------------------------------------------------------------- /src/feature_based_slam/features/include/feature_based_slam/features/correspondence.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_FEATURES_CORRESPONDENCE_HPP 2 | #define DSOPP_FEATURE_BASED_SLAM_FEATURES_CORRESPONDENCE_HPP 3 | 4 | #include "common/settings.hpp" 5 | 6 | namespace dsopp { 7 | namespace feature_based_slam { 8 | namespace features { 9 | /** 10 | * \brief Correspondence between two features from the two frames 11 | */ 12 | struct Correspondence { 13 | /** true if the correspondence is inlier and false otherwise */ 14 | bool is_inlier; 15 | /** index of the feature from the `from` frame */ 16 | size_t idx_from; 17 | /** index of the feature from the `to` frame */ 18 | size_t idx_to; 19 | }; 20 | } // namespace features 21 | } // namespace feature_based_slam 22 | } // namespace dsopp 23 | 24 | #endif // DSOPP_FEATURE_BASED_SLAM_FEATURES_CORRESPONDENCE_HPP 25 | -------------------------------------------------------------------------------- /src/feature_based_slam/features/include/feature_based_slam/features/distinct_feature.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DSOPP_FEATURE_BASED_SLAM_FEATURES_DISTINCT_FEATURE_HPP 3 | #define DSOPP_FEATURE_BASED_SLAM_FEATURES_DISTINCT_FEATURE_HPP 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include "common/settings.hpp" 10 | 11 | namespace dsopp { 12 | namespace feature_based_slam { 13 | namespace features { 14 | /** 15 | * \brief DistinctFeature represent an identifiable feature. 16 | * 17 | * Object of this class stores coordinates and descriptor of the distinct feature. 18 | */ 19 | class DistinctFeature { 20 | public: 21 | /** 22 | * creates a distinct feature from coordinates. 23 | * @param coordinates coordinates of the distinct feature 24 | */ 25 | DistinctFeature(const Eigen::Vector &coordinates); 26 | /** 27 | * @return coordinates of the point 28 | */ 29 | const Eigen::Vector &coordinates() const; 30 | 31 | ~DistinctFeature(); 32 | 33 | private: 34 | /** feature coordinates*/ 35 | Eigen::Vector coordinates_; 36 | }; 37 | } // namespace features 38 | } // namespace feature_based_slam 39 | } // namespace dsopp 40 | 41 | #endif // DSOPP_FEATURE_BASED_SLAM_FEATURES_DISTINCT_FEATURE_HPP 42 | -------------------------------------------------------------------------------- /src/feature_based_slam/features/src/correspondences_finder.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/features/correspondences_finder.hpp" 2 | 3 | namespace dsopp { 4 | namespace feature_based_slam { 5 | namespace features { 6 | 7 | std::pair, std::vector> findCorrespondences( 8 | const DistinctFeaturesFrame *features_from, cv::Mat image_from, cv::Mat image_to, 9 | const sensors::calibration::CameraMask &mask, const features::DistinctFeaturesExtractor &feature_extractor, 10 | CorrespondencesFinder finder) { 11 | // reference frame doesn't exist 12 | if (!features_from) { 13 | auto reference_features_frame = feature_extractor.extract(image_to, mask, true); 14 | return {std::move(reference_features_frame), std::vector{}}; 15 | } 16 | 17 | return finder(*features_from, image_from, image_to, &feature_extractor, mask); 18 | } 19 | 20 | } // namespace features 21 | } // namespace feature_based_slam 22 | } // namespace dsopp 23 | -------------------------------------------------------------------------------- /src/feature_based_slam/features/src/distinct_feature.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/features//distinct_feature.hpp" 2 | 3 | namespace dsopp { 4 | namespace feature_based_slam { 5 | namespace features { 6 | DistinctFeature::DistinctFeature(const Eigen::Vector &coordinates) : coordinates_(coordinates) {} 7 | 8 | const Eigen::Vector &DistinctFeature::coordinates() const { return coordinates_; } 9 | 10 | DistinctFeature::~DistinctFeature() = default; 11 | } // namespace features 12 | } // namespace feature_based_slam 13 | } // namespace dsopp 14 | -------------------------------------------------------------------------------- /src/feature_based_slam/features/src/distinct_features_frame.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/features/distinct_features_frame.hpp" 2 | #include "feature_based_slam/features/distinct_feature.hpp" 3 | 4 | #include 5 | namespace dsopp { 6 | namespace feature_based_slam { 7 | namespace features { 8 | DistinctFeaturesFrame::DistinctFeaturesFrame(std::vector &&distinct_features, cv::Mat descriptors) 9 | : distinct_features_(std::move(distinct_features)), descriptors_(descriptors) {} 10 | 11 | const std::vector &DistinctFeaturesFrame::features() const { return distinct_features_; } 12 | 13 | bool DistinctFeaturesFrame::hasDescriptors() const { return !descriptors_.empty(); } 14 | 15 | const cv::Mat DistinctFeaturesFrame::descriptors() const { return descriptors_; } 16 | 17 | DistinctFeaturesFrame::~DistinctFeaturesFrame() = default; 18 | } // namespace features 19 | } // namespace feature_based_slam 20 | } // namespace dsopp 21 | -------------------------------------------------------------------------------- /src/feature_based_slam/initialization_strategy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | feature_based_slam_initialization_strategy 3 | src/frequency_initializer_keyframe_strategy.cpp 4 | src/wait_for_movement_keyframe_strategy.cpp) 5 | 6 | target_include_directories( 7 | feature_based_slam_initialization_strategy 8 | PUBLIC $ 9 | $) 10 | 11 | target_link_libraries( 12 | feature_based_slam_initialization_strategy 13 | PUBLIC sophus feature_based_slam_track feature_based_slam_features) 14 | target_link_libraries(feature_based_slam_initialization_strategy 15 | PRIVATE time settings features) 16 | -------------------------------------------------------------------------------- /src/feature_based_slam/initialization_strategy/src/frequency_initializer_keyframe_strategy.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/initialization_strategy/frequency_initializer_keyframe_strategy.hpp" 2 | 3 | namespace dsopp::feature_based_slam::initialization_strategy { 4 | 5 | FrequencyInitializerKeyframeStrategy::FrequencyInitializerKeyframeStrategy(Precision frequency) 6 | : period_ms_(static_cast(1.0 / frequency * 1000)) {} 7 | 8 | FrameStatus FrequencyInitializerKeyframeStrategy::needNewFrame(const dsopp::features::CameraFeatures &new_frame, 9 | const std::deque &previous_frames, 10 | const size_t, const size_t) { 11 | if (previous_frames.empty()) return FrameStatus::kKeepFrame; 12 | 13 | auto first_timestamp = previous_frames.front().timestamp; 14 | long delta_t_ms = 15 | std::chrono::duration_cast(new_frame.timestamp() - first_timestamp).count(); 16 | if (delta_t_ms >= period_ms_) { 17 | return FrameStatus::kFinish; 18 | } 19 | return FrameStatus::kKeepFrame; 20 | } 21 | 22 | bool FrequencyInitializerKeyframeStrategy::managesStandstill() { return false; } 23 | 24 | FrequencyInitializerKeyframeStrategy::~FrequencyInitializerKeyframeStrategy() = default; 25 | 26 | } // namespace dsopp::feature_based_slam::initialization_strategy 27 | -------------------------------------------------------------------------------- /src/feature_based_slam/track/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(feature_based_slam_track src/frame.cpp src/landmark.cpp 2 | src/landmarks.cpp) 3 | 4 | target_include_directories( 5 | feature_based_slam_track 6 | PUBLIC $ 7 | $) 8 | 9 | target_link_libraries(feature_based_slam_track 10 | PUBLIC sophus feature_based_slam_features) 11 | target_link_libraries(feature_based_slam_track PRIVATE time settings features) 12 | -------------------------------------------------------------------------------- /src/feature_based_slam/track/include/feature_based_slam/track/track.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_TRACK_TRACK_HPP 2 | #define DSOPP_FEATURE_BASED_SLAM_TRACK_TRACK_HPP 3 | 4 | #include 5 | 6 | #include "feature_based_slam/track/frame.hpp" 7 | #include "feature_based_slam/track/landmarks.hpp" 8 | 9 | namespace dsopp { 10 | namespace feature_based_slam { 11 | namespace track { 12 | /** 13 | * \brief Track in feature based slam. Consists of frames and landmarks that are projected onto frames. 14 | */ 15 | struct Track { 16 | /** all frames in the track */ 17 | std::deque frames; 18 | /** all 3d landmarks in the track */ 19 | Landmarks landmarks; 20 | }; 21 | } // namespace track 22 | } // namespace feature_based_slam 23 | } // namespace dsopp 24 | 25 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACK_TRACK_HPP 26 | -------------------------------------------------------------------------------- /src/feature_based_slam/track/src/frame.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/track/frame.hpp" 2 | 3 | namespace dsopp { 4 | namespace feature_based_slam { 5 | namespace track { 6 | 7 | Frame::Frame(const time &_timestamp, const size_t _frame_id, cv::Mat _image) 8 | : timestamp(_timestamp), 9 | frame_id(_frame_id), 10 | t_world_agent( 11 | energy::motion::SE3::exp(Eigen::Vector::DoF>::Zero())), 12 | initialized(false), 13 | image(_image) {} 14 | 15 | } // namespace track 16 | } // namespace feature_based_slam 17 | } // namespace dsopp 18 | -------------------------------------------------------------------------------- /src/feature_based_slam/track/src/landmark.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/track/landmark.hpp" 2 | 3 | #include "feature_based_slam/features/distinct_feature.hpp" 4 | 5 | namespace dsopp { 6 | namespace feature_based_slam { 7 | namespace track { 8 | 9 | Landmark::Landmark(const Eigen::Vector3 &position) : initialized_(true), position_(position) {} 10 | 11 | Landmark::Landmark() { initialized_ = false; } 12 | 13 | const Eigen::Vector3 *Landmark::position() const { return initialized_ ? &position_ : nullptr; } 14 | 15 | void Landmark::setPosition(const Eigen::Vector3 &position) { 16 | initialized_ = true; 17 | position_ = position; 18 | } 19 | 20 | void Landmark::addProjection(size_t frame_id, const features::DistinctFeature &feature) { 21 | projections_.insert({frame_id, feature}); 22 | } 23 | 24 | const features::DistinctFeature *Landmark::projection(size_t frame_id) const { 25 | return projections_.contains(frame_id) ? &projections_.at(frame_id) : nullptr; 26 | } 27 | 28 | void Landmark::removeProjection(size_t frame_id) { 29 | projections_.erase(frame_id); 30 | initialized_ = initialized_ && projections_.size() > 1; 31 | } 32 | 33 | } // namespace track 34 | } // namespace feature_based_slam 35 | } // namespace dsopp 36 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/include/feature_based_slam/tracker/fabric.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_TRACKER_FABRIC_HPP_ 2 | #define DSOPP_FEATURE_BASED_SLAM_TRACKER_FABRIC_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "feature_based_slam/tracker/tracker.hpp" 10 | #include "sensors/sensors.hpp" 11 | 12 | namespace dsopp::feature_based_slam::tracker { 13 | 14 | /** 15 | * cretes feature based slam tracker from the given parameters 16 | * 17 | * @tparam Calibration camera calibration type 18 | * 19 | * @param parameters parameters 20 | * @param sensors array of system sensors, required to check for the presence of the required sensor 21 | * @return unique pointer to feature based slam tracker object 22 | */ 23 | std::unique_ptr create(const std::map ¶meters, const sensors::Sensors &sensors); 24 | 25 | } // namespace dsopp::feature_based_slam::tracker 26 | 27 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACKER_FABRIC_HPP_ 28 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/internal/feature_based_slam/tracker/add_new_landmarks.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_TRACKER_ADD_NEW_LAMDMARKS_HPP 2 | #define DSOPP_FEATURE_BASED_SLAM_TRACKER_ADD_NEW_LAMDMARKS_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace dsopp { 8 | namespace feature_based_slam { 9 | namespace track { 10 | class Landmarks; 11 | } 12 | namespace features { 13 | struct Correspondence; 14 | class DistinctFeaturesFrame; 15 | } // namespace features 16 | namespace tracker { 17 | /** 18 | * Add new landmarks to projections that do not match with other landmarks. It will be landmarks without position and 19 | * with one projection; 20 | * 21 | * @param landmarks object with all landmarks from the track 22 | * @param correspondences correspondences between new features and landmarks 23 | * @param feature_frame new features 24 | * @param frame_id id of the new frame 25 | */ 26 | void addNewLandmarks(track::Landmarks &landmarks, const std::vector &correspondences, 27 | const features::DistinctFeaturesFrame &feature_frame, size_t frame_id); 28 | 29 | } // namespace tracker 30 | } // namespace feature_based_slam 31 | } // namespace dsopp 32 | 33 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACKER_ADD_NEW_LAMDMARKS_HPP 34 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/internal/feature_based_slam/tracker/feature_frame_from_landmarks.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_TRACKER_CREATE_LANDMARKS_HPP 2 | #define DSOPP_FEATURE_BASED_SLAM_TRACKER_CREATE_LANDMARKS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dsopp { 9 | namespace feature_based_slam { 10 | namespace track { 11 | struct Frame; 12 | class Landmarks; 13 | } // namespace track 14 | namespace tracker { 15 | struct LandmarkFeatureFrame; 16 | /** 17 | * Create 2d feature frame from the landmarks. This is required for use in matches. Now this function get all 18 | * projections of the landmarks on the frame with the given id. 19 | * 20 | * @param frames all frames from the track 21 | * @param landmarks all landmarks from the track 22 | * @param frame_id frame from which projections will be taken 23 | */ 24 | std::vector featureFrameFromLandmarks(std::deque &frames, 25 | track::Landmarks &landmarks, size_t frame_id); 26 | 27 | } // namespace tracker 28 | } // namespace feature_based_slam 29 | } // namespace dsopp 30 | 31 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACKER_CREATE_LANDMARKS_HPP 32 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/internal/feature_based_slam/tracker/initialize_poses.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_TRACKER_SLAM_INITIALIZE_POSES_HPP 2 | #define DSOPP_FEATURE_BASED_TRACKER_SLAM_INITIALIZE_POSES_HPP 3 | 4 | #include "energy/motion/se3_motion.hpp" 5 | #include "feature_based_slam/tracker/initializer.hpp" 6 | #include "sensors/camera_calibration/camera_calibration.hpp" 7 | 8 | namespace dsopp { 9 | namespace feature_based_slam { 10 | namespace track { 11 | struct Track; 12 | struct Connections; 13 | } // namespace track 14 | namespace tracker { 15 | /** 16 | * Initialize feature based slam tracker 17 | * 18 | * @tparam Calibration camera calibration type 19 | * 20 | * @param track track with the frames and landmarks 21 | * @param connections correspondences between 2d features in the frames 22 | * @param model camera model 23 | * @param options all constants which will be used in the feature based slam algorithms and optimizations 24 | * @return true if tracker was initialized and false otherwise 25 | */ 26 | template 27 | bool initializePoses(track::Track &track, const Model &model, const Options &options, const size_t number_of_threads); 28 | 29 | } // namespace tracker 30 | } // namespace feature_based_slam 31 | } // namespace dsopp 32 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACKER_INITIALIZE_POSES_HPP 33 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/internal/feature_based_slam/tracker/landmark_feature_frame.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_TRACKER_LANDMARK_FEATURE_FRAME_HPP 2 | #define DSOPP_FEATURE_BASED_SLAM_TRACKER_LANDMARK_FEATURE_FRAME_HPP 3 | 4 | #include 5 | 6 | #include "feature_based_slam/features/distinct_feature.hpp" 7 | #include "feature_based_slam/features/distinct_features_frame.hpp" 8 | 9 | namespace dsopp { 10 | namespace feature_based_slam { 11 | namespace tracker { 12 | /** 13 | * Structure for storing conversion of landmarks to feature frame. 14 | */ 15 | struct LandmarkFeatureFrame { 16 | /** image from which features were taken */ 17 | cv::Mat image; 18 | /** features obtained from projections of landmarks. Could be nullptr, if there are no landmarks */ 19 | std::unique_ptr features = nullptr; 20 | /** correspondences between feature and landmark */ 21 | std::vector landmark_idx; 22 | }; 23 | 24 | } // namespace tracker 25 | } // namespace feature_based_slam 26 | } // namespace dsopp 27 | 28 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACKER_LANDMARK_FEATURE_FRAME_HPP 29 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/internal/feature_based_slam/tracker/remove_outliers.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FEATURE_BASED_SLAM_TRACKER_REMOVE_OUTLIERS_HPP 2 | #define DSOPP_FEATURE_BASED_SLAM_TRACKER_REMOVE_OUTLIERS_HPP 3 | 4 | #include 5 | 6 | #include "energy/motion/se3_motion.hpp" 7 | #include "sensors/camera_calibration/camera_calibration.hpp" 8 | 9 | namespace dsopp { 10 | namespace feature_based_slam { 11 | namespace track { 12 | struct Frame; 13 | class Landmarks; 14 | } // namespace track 15 | namespace tracker { 16 | 17 | /** 18 | * remove projections of landmarks that are very different from the corresponding features of the frame 19 | * 20 | * @param landmarks 3d landmarks 21 | * @param model camera model 22 | * @param frames container with frames information 23 | * @param last_frames_to_filter number of the frames from the end of the track which we want to clear 24 | * @param outlier_threshold outlier threshold in pixels 25 | */ 26 | template 27 | void removeOutliers(track::Landmarks &landmarks, const Model &model, std::deque &frames, 28 | const size_t last_frames_to_filter = 10, const Precision outlier_threshold = 0.5); 29 | 30 | } // namespace tracker 31 | } // namespace feature_based_slam 32 | } // namespace dsopp 33 | 34 | #endif // DSOPP_FEATURE_BASED_SLAM_TRACKER_REMOVE_OUTLIERS_HPP 35 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/src/add_new_landmarks.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/tracker/add_new_landmarks.hpp" 2 | 3 | #include "feature_based_slam/features/correspondence.hpp" 4 | #include "feature_based_slam/track/landmarks.hpp" 5 | #include "feature_based_slam/tracker/landmark_feature_frame.hpp" 6 | 7 | namespace dsopp { 8 | namespace feature_based_slam { 9 | namespace tracker { 10 | void addNewLandmarks(track::Landmarks &landmarks, const std::vector &correspondences, 11 | const features::DistinctFeaturesFrame &feature_frame, size_t frame_id) { 12 | const auto &features = feature_frame.features(); 13 | std::vector look_up(features.size(), false); 14 | for (const auto &correspondence : correspondences) { 15 | if (correspondence.is_inlier) { 16 | look_up[correspondence.idx_to] = true; 17 | } 18 | } 19 | 20 | for (size_t i = 0; i < features.size(); ++i) { 21 | if (!look_up[i]) { 22 | auto landmark_idx = landmarks.add(); 23 | landmarks.addProjectionToLandmark(landmark_idx, frame_id, features[i]); 24 | } 25 | } 26 | } 27 | 28 | } // namespace tracker 29 | } // namespace feature_based_slam 30 | } // namespace dsopp 31 | -------------------------------------------------------------------------------- /src/feature_based_slam/tracker/src/add_new_projections_to_landmarks.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_based_slam/tracker/add_new_projections_to_landmarks.hpp" 2 | 3 | #include "feature_based_slam/features/correspondence.hpp" 4 | #include "feature_based_slam/track/landmarks.hpp" 5 | #include "feature_based_slam/tracker/landmark_feature_frame.hpp" 6 | 7 | namespace dsopp { 8 | namespace feature_based_slam { 9 | namespace tracker { 10 | void addNewProjectionsToLandmarks(track::Landmarks &landmarks, const LandmarkFeatureFrame &landmark_features, 11 | const std::vector &correspondences, 12 | const features::DistinctFeaturesFrame &feature_frame, size_t frame_id) { 13 | const auto &features = feature_frame.features(); 14 | for (const auto &correspondence : correspondences) { 15 | if (correspondence.is_inlier) { 16 | auto landmark_idx = landmark_features.landmark_idx[correspondence.idx_from]; 17 | landmarks.addProjectionToLandmark(landmark_idx, frame_id, features[correspondence.idx_to]); 18 | } 19 | } 20 | } 21 | 22 | } // namespace tracker 23 | } // namespace feature_based_slam 24 | } // namespace dsopp 25 | -------------------------------------------------------------------------------- /src/features/include/features/camera/eigen_tracking_features_extractor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SRC_FEATURES_CAMERA_EIGEN_TRACKING_FEATURES_EXTRACTOR_HPP 2 | #define DSOPP_SRC_FEATURES_CAMERA_EIGEN_TRACKING_FEATURES_EXTRACTOR_HPP 3 | 4 | #include "features/camera/tracking_features_extractor.hpp" 5 | 6 | #include 7 | #include 8 | 9 | #include "common/settings.hpp" 10 | 11 | namespace dsopp { 12 | namespace features { 13 | class TrackingFeaturesFrame; 14 | /** 15 | * \brief TrackingFeatureExtractor extract feature points for tracking from frame data. 16 | * 17 | * Object of this class extract feature points from frame data for future 18 | * calculating transformation between images. 19 | */ 20 | class EigenTrackingFeaturesExtractor : public TrackingFeaturesExtractor { 21 | public: 22 | /** 23 | * Creates a tracking feature extractor. 24 | * @param point_density_for_detector desired number of the feature points 25 | */ 26 | explicit EigenTrackingFeaturesExtractor(const Precision point_density_for_detector = 1500); 27 | 28 | std::unique_ptr extract(cv::Mat image, const sensors::calibration::CameraMask &mask) override; 29 | }; 30 | 31 | } // namespace features 32 | } // namespace dsopp 33 | 34 | #endif // DSOPP_SRC_FEATURES_CAMERA_EIGEN_TRACKING_FEATURES_EXTRACTOR_HPP 35 | -------------------------------------------------------------------------------- /src/features/include/features/camera/photometrically_corrected_image.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_PHOTOMETRICALLY_CORRECTED_IMAGE_HPP 2 | #define DSOPP_PHOTOMETRICALLY_CORRECTED_IMAGE_HPP 3 | 4 | #include 5 | 6 | #include "opencv2/core.hpp" 7 | 8 | #include "common/settings.hpp" 9 | #include "features/camera/pixel_map.hpp" 10 | 11 | namespace dsopp::features { 12 | /** 13 | * Photometrically corrects grayscale uchar image 14 | * 15 | * @param image grayscale uchar image 16 | * @param photometric_calibration photometric calibration 17 | * @param vignetting vignetting 18 | * @return photometrically corrected grayscale Precision image 19 | */ 20 | std::vector photometricallyCorrectedImage( 21 | const cv::Mat &image, const std::array &photometric_calibration, const cv::Mat &vignetting); 22 | } // namespace dsopp::features 23 | 24 | #endif // DSOPP_PHOTOMETRICALLY_CORRECTED_IMAGE_HPP 25 | -------------------------------------------------------------------------------- /src/features/include/features/camera/tracking_feature.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SRC_FEATURES_CAMERA_TRACKING_FEATURE_HPP_ 2 | #define DSOPP_SRC_FEATURES_CAMERA_TRACKING_FEATURE_HPP_ 3 | 4 | #include 5 | 6 | #include "common/settings.hpp" 7 | 8 | namespace dsopp { 9 | namespace features { 10 | /** 11 | * \brief TrackingFeature represent an identifiable feature for tracking. 12 | * 13 | * Object of this class stores coordinates the tracking feature. 14 | */ 15 | class TrackingFeature { 16 | public: 17 | /** 18 | * creates a tracking feature from coordinates. 19 | * @param coordinates coordinates of the tracking feature 20 | */ 21 | TrackingFeature(const Eigen::Matrix &coordinates); 22 | /** 23 | * @return coordinates of the point 24 | */ 25 | const Eigen::Matrix &coordinates() const; 26 | 27 | protected: 28 | /** feature coordinates*/ 29 | Eigen::Matrix coordinates_; 30 | }; 31 | } // namespace features 32 | } // namespace dsopp 33 | 34 | #endif // DSOPP_SRC_FEATURES_CAMERA_TRACKING_FEATURE_HPP_ 35 | -------------------------------------------------------------------------------- /src/features/include/features/camera/tracking_features_frame.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DSOPP_SRC_FEATURES_CAMERA_TRACKING_FEATURES_FRAME_HPP_ 3 | #define DSOPP_SRC_FEATURES_CAMERA_TRACKING_FEATURES_FRAME_HPP_ 4 | 5 | #include 6 | #include 7 | 8 | namespace dsopp { 9 | namespace features { 10 | class TrackingFeature; 11 | /** 12 | * \brief TrackingFeaturesFrame is a container for tracking features 13 | * 14 | * TrackingFeaturesFrame is a container for tracking features from one frame data. 15 | */ 16 | class TrackingFeaturesFrame { 17 | public: 18 | /** 19 | * creates a features frame from a vector of tracking features. 20 | * @param tracking_features vector of tracking features 21 | */ 22 | explicit TrackingFeaturesFrame(std::vector &&tracking_features); 23 | 24 | ~TrackingFeaturesFrame(); 25 | /** 26 | * @return tracking features 27 | */ 28 | const std::vector &features() const; 29 | 30 | private: 31 | /** vector of tracking features */ 32 | std::vector features_; 33 | }; 34 | } // namespace features 35 | } // namespace dsopp 36 | 37 | #endif // DSOPP_SRC_FEATURES_CAMERA_TRACKING_FEATURES_FRAME_HPP_ 38 | -------------------------------------------------------------------------------- /src/features/internal/features/camera/calculate_pixelinfo.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_CALCULATE_PIXELINFO_HPP 2 | #define DSOPP_CALCULATE_PIXELINFO_HPP 3 | 4 | namespace dsopp::features { 5 | 6 | template 7 | void calculate_pixelinfo(const T *input, T *output, const int width, const int height); 8 | 9 | } 10 | 11 | #endif // DSOPP_CALCULATE_PIXELINFO_HPP -------------------------------------------------------------------------------- /src/features/internal/features/camera/extraction_tools.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SRC_FEATURES_EXTRACTION_TOOLS_HPP 2 | #define DSOPP_SRC_FEATURES_EXTRACTION_TOOLS_HPP 3 | 4 | #ifdef BUILD_WITH_LIBTORCH 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "features/camera/pixel_map.hpp" 11 | 12 | namespace dsopp::features { 13 | /** 14 | * Run ``model`` on ``img_initial`` and output tuple of elements from the extractor 15 | * @param img_initial image 16 | * @param model model 17 | * @return vector of NN outputs 18 | */ 19 | std::vector runExtractor(const cv::Mat &img_initial, torch::jit::script::Module &model); 20 | 21 | /** 22 | * Flatten tensor and put it into ``Precision`` vector 23 | * @param model_output tensor output of the model 24 | * @param channels_number number of expected channels 25 | * @return ``Precision`` vector 26 | */ 27 | std::vector flatten(torch::Tensor &model_output, int channels_number); 28 | 29 | } // namespace dsopp::features 30 | 31 | #endif 32 | #endif 33 | -------------------------------------------------------------------------------- /src/features/src/pattern_patch.cpp: -------------------------------------------------------------------------------- 1 | #include "features/camera/pattern_patch.hpp" 2 | 3 | namespace dsopp::features {} // namespace dsopp::features 4 | -------------------------------------------------------------------------------- /src/features/src/pixel_data_frame_extractor.cpp: -------------------------------------------------------------------------------- 1 | #include "features/camera/pixel_data_frame_extractor.hpp" 2 | 3 | #include "features/camera/pixel_data_frame.hpp" 4 | 5 | namespace dsopp { 6 | namespace features { 7 | PixelDataFrameExtractor::PixelDataFrameExtractor(const std::array& photometric_calibration, 8 | const cv::Mat& undistorted_vignetting, const size_t& pyramid_levels) 9 | : photometric_calibration_(photometric_calibration), 10 | undistorted_vignetting_(undistorted_vignetting), 11 | pyramid_levels_(pyramid_levels) {} 12 | std::unique_ptr PixelDataFrameExtractor::extract(const cv::Mat& frame) const { 13 | return std::make_unique(frame, photometric_calibration_, undistorted_vignetting_, 14 | pyramid_levels_); 15 | } 16 | } // namespace features 17 | } // namespace dsopp 18 | -------------------------------------------------------------------------------- /src/features/src/tracking_feature.cpp: -------------------------------------------------------------------------------- 1 | #include "features/camera/tracking_feature.hpp" 2 | 3 | namespace dsopp { 4 | namespace features { 5 | TrackingFeature::TrackingFeature(const Eigen::Vector2 &coordinates) : coordinates_(coordinates) {} 6 | const Eigen::Vector2 &TrackingFeature::coordinates() const { return coordinates_; } 7 | } // namespace features 8 | } // namespace dsopp 9 | -------------------------------------------------------------------------------- /src/features/src/tracking_features_frame.cpp: -------------------------------------------------------------------------------- 1 | #include "features/camera/tracking_features_frame.hpp" 2 | #include "features/camera/tracking_feature.hpp" 3 | 4 | #include 5 | namespace dsopp { 6 | namespace features { 7 | TrackingFeaturesFrame::TrackingFeaturesFrame(std::vector &&tracking_features) 8 | : features_(std::move(tracking_features)) {} 9 | const std::vector &TrackingFeaturesFrame::features() const { return features_; } 10 | TrackingFeaturesFrame::~TrackingFeaturesFrame() = default; 11 | } // namespace features 12 | } // namespace dsopp 13 | -------------------------------------------------------------------------------- /src/marginalization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | marginalization 3 | src/frame_marginalization_strategy.cpp 4 | src/maximum_size_frame_marginalization_strategy.cpp 5 | src/sparse_frame_marginalization_strategy.cpp src/fabric.cpp) 6 | 7 | target_include_directories( 8 | marginalization PUBLIC $ 9 | $) 10 | 11 | target_link_libraries(marginalization PUBLIC motion_model) 12 | target_link_libraries(marginalization PRIVATE glog track track_frames 13 | track_landmarks track_connections) 14 | -------------------------------------------------------------------------------- /src/marginalization/include/marginalization/fabric.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_MARGINALIZATION_STRATEGY_FABRIC_HPP 2 | #define DSOPP_MARGINALIZATION_STRATEGY_FABRIC_HPP 3 | 4 | #include "marginalization/frame_marginalization_strategy.hpp" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace dsopp { 13 | namespace marginalization { 14 | 15 | template 16 | std::unique_ptr> create(const std::map ¶meters); 17 | 18 | } // namespace marginalization 19 | } // namespace dsopp 20 | 21 | #endif // DSOPP_MARGINALIZATION_STRATEGY_FABRIC_HPP 22 | -------------------------------------------------------------------------------- /src/marginalization/include/marginalization/frame_marginalization_strategy.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_FRAME_MARGINALIZATION_STRATEGY_HPP 2 | #define DSOPP_FRAME_MARGINALIZATION_STRATEGY_HPP 3 | #include 4 | #include 5 | 6 | #include "common/settings.hpp" 7 | #include "energy/motion/motion.hpp" 8 | #include "energy/motion/se3_motion.hpp" 9 | 10 | namespace dsopp { 11 | namespace track { 12 | template 13 | class ActiveOdometryTrack; 14 | } 15 | namespace marginalization { 16 | /** 17 | * interface for marginalization strategies 18 | * @tparam Motion Motion type 19 | */ 20 | template 21 | class FrameMarginalizationStrategy { 22 | public: 23 | /** 24 | * method decides which frames to marginalize 25 | * @param track track to be marginalized 26 | */ 27 | virtual void marginalize(track::ActiveOdometryTrack& track) = 0; 28 | virtual ~FrameMarginalizationStrategy() = 0; 29 | }; 30 | 31 | } // namespace marginalization 32 | } // namespace dsopp 33 | #endif // DSOPP_FRAME_MARGINALIZATION_STRATEGY_HPP 34 | -------------------------------------------------------------------------------- /src/marginalization/src/frame_marginalization_strategy.cpp: -------------------------------------------------------------------------------- 1 | #include "marginalization/frame_marginalization_strategy.hpp" 2 | 3 | #include "energy/motion/se3_motion.hpp" 4 | 5 | namespace dsopp { 6 | namespace marginalization { 7 | 8 | template 9 | FrameMarginalizationStrategy::~FrameMarginalizationStrategy() = default; 10 | 11 | template class FrameMarginalizationStrategy>; 12 | 13 | } // namespace marginalization 14 | 15 | } // namespace dsopp 16 | -------------------------------------------------------------------------------- /src/marginalization/src/maximum_size_frame_marginalization_strategy.cpp: -------------------------------------------------------------------------------- 1 | #include "marginalization/maximum_size_frame_marginalization_strategy.hpp" 2 | 3 | #include "energy/motion/se3_motion.hpp" 4 | 5 | #include "track/active_odometry_track.hpp" 6 | 7 | #include 8 | namespace dsopp { 9 | namespace marginalization { 10 | 11 | template 12 | MaximumSizeFrameMarginalizationStrategy::MaximumSizeFrameMarginalizationStrategy(size_t maximum_size) 13 | : maximum_size_(maximum_size) {} 14 | 15 | template 16 | void MaximumSizeFrameMarginalizationStrategy::marginalize(track::ActiveOdometryTrack& track) { 17 | while (track.activeFrames().size() > maximum_size_) { 18 | track.marginalizeFrame(0); 19 | } 20 | } 21 | template 22 | MaximumSizeFrameMarginalizationStrategy::~MaximumSizeFrameMarginalizationStrategy() = default; 23 | 24 | template class MaximumSizeFrameMarginalizationStrategy>; 25 | 26 | } // namespace marginalization 27 | } // namespace dsopp 28 | -------------------------------------------------------------------------------- /src/measures/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(measures INTERFACE) 2 | 3 | target_include_directories( 4 | measures INTERFACE $ 5 | $) 6 | 7 | target_link_libraries(measures INTERFACE common_pattern eigen) 8 | -------------------------------------------------------------------------------- /src/measures/include/measures/similarity_measure.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SIMILARITY_MEASURE_HPP 2 | #define DSOPP_SIMILARITY_MEASURE_HPP 3 | 4 | #include "common/pattern/pattern.hpp" 5 | 6 | #include 7 | #include 8 | 9 | namespace dsopp { 10 | namespace measure { 11 | /** \brief SimilarityMeasure interface 12 | * 13 | * SimilarityMeasure is an concept for similarity measure implementation. 14 | * 15 | */ 16 | template 17 | concept SimilarityMeasure = requires(const Eigen::Vector& patch) { 18 | { Measure::calculate(patch, patch) } 19 | ->std::same_as; 20 | }; 21 | /** \brief DifferentiableSimilarityMeasure interface 22 | * 23 | * DifferentiableSimilarityMeasure is a similarity measure can be used in optimization solvers. 24 | */ 25 | template 26 | concept DifferentiableSimilarityMeasure = SimilarityMeasure&& requires(Eigen::Vector& patch) { 27 | { Measure::residuals(patch, patch, patch) } 28 | ->std::same_as; 29 | }; 30 | } // namespace measure 31 | } // namespace dsopp 32 | #endif // DSOPP_SIMILARITY_MEASURE_HPP 33 | -------------------------------------------------------------------------------- /src/output/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(output_interfaces) 2 | add_subdirectory(visualizer) 3 | add_subdirectory(persistent) 4 | -------------------------------------------------------------------------------- /src/output/output_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(output_interfaces INTERFACE) 2 | 3 | target_include_directories( 4 | output_interfaces 5 | INTERFACE $ 6 | $) 7 | 8 | target_link_libraries(output_interfaces INTERFACE motion_model agent) 9 | -------------------------------------------------------------------------------- /src/output/output_interfaces/include/output_interfaces/camera_output_interfaces.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_OUTPUT_CAMERA_OUTPUT_INTERFACES_HPP 2 | #define DSOPP_OUTPUT_CAMERA_OUTPUT_INTERFACES_HPP 3 | 4 | #include "common/settings.hpp" 5 | #include "output_interfaces/image_output_interface.hpp" 6 | 7 | namespace dsopp { 8 | namespace output { 9 | /** 10 | * Store all variants of the output interfaces for the camera. 11 | */ 12 | struct CameraOutputInterfaces { 13 | /** current tracking frame */ 14 | ImageOutputInterface *current_frame = nullptr; 15 | /** current keyframe */ 16 | ImageOutputInterface *current_keyframe = nullptr; 17 | }; 18 | } // namespace output 19 | } // namespace dsopp 20 | 21 | #endif // DSOPP_OUTPUT_CAMERA_OUTPUT_INTERFACES_HPP 22 | -------------------------------------------------------------------------------- /src/output/output_interfaces/include/output_interfaces/image_output_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_IMAGE_OUTPUT_INTERFACE_HPP 2 | #define DSOPP_IMAGE_OUTPUT_INTERFACE_HPP 3 | 4 | #include 5 | 6 | namespace dsopp { 7 | namespace output { 8 | /** 9 | * \brief Output interface for image. 10 | * 11 | * An interface to push debug image. 12 | */ 13 | class ImageOutputInterface { 14 | public: 15 | /** 16 | * push image to be displayed 17 | * @param image to be displayed 18 | */ 19 | virtual void pushImage(const cv::Mat image) = 0; 20 | virtual ~ImageOutputInterface() = default; 21 | }; 22 | } // namespace output 23 | } // namespace dsopp 24 | 25 | #endif // DSOPP_IMAGE_OUTPUT_INTERFACE_HPP 26 | -------------------------------------------------------------------------------- /src/output/output_interfaces/include/output_interfaces/text_output_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_OUTPUT_TEXT_OUTPUT_INTERFACE_HPP_ 2 | #define DSOPP_OUTPUT_TEXT_OUTPUT_INTERFACE_HPP_ 3 | 4 | #include 5 | 6 | namespace dsopp::output { 7 | 8 | /** 9 | * \brief Output interface for text. 10 | * 11 | * An interface to push text info. 12 | */ 13 | class TextOutputInterface { 14 | public: 15 | /** 16 | * push text to be rendered 17 | * @param text input text 18 | */ 19 | virtual void pushText(const std::string &text) = 0; 20 | virtual ~TextOutputInterface() = default; 21 | }; 22 | 23 | } // namespace dsopp::output 24 | 25 | #endif // DSOPP_OUTPUT_TEXT_OUTPUT_INTERFACE_HPP_ 26 | -------------------------------------------------------------------------------- /src/output/persistent/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(output_persistent src/protobuf_exporter.cpp) 2 | 3 | target_include_directories( 4 | output_persistent 5 | PUBLIC $ 6 | $) 7 | 8 | target_link_libraries(output_persistent PUBLIC output_interfaces track_frames 9 | motion_model time storage) 10 | target_link_libraries(output_persistent PRIVATE glog track) 11 | -------------------------------------------------------------------------------- /src/output/visualizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(VISUALIZATION) 2 | add_library( 3 | visualizer 4 | src/visualizer_track_output_interface.cpp 5 | src/visualizer_image_output_interface.cpp 6 | src/visualizer_text_output_interface.cpp 7 | src/local_track.cpp 8 | src/local_frame.cpp 9 | src/local_odometry_track.cpp 10 | src/local_odometry_frame.cpp 11 | src/local_tracking_frame.cpp 12 | src/visualizer.cpp) 13 | 14 | target_include_directories( 15 | visualizer PUBLIC $ 16 | $) 17 | 18 | target_include_directories( 19 | visualizer PRIVATE $) 20 | 21 | target_link_libraries(visualizer PUBLIC output_interfaces track_frames 22 | pangolin motion_model time) 23 | target_link_libraries(visualizer PRIVATE glog track) 24 | endif() 25 | -------------------------------------------------------------------------------- /src/output/visualizer/internal/visualizer/buffer_storage.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_BUFFER_STORAGE_HPP 2 | #define DSOPP_BUFFER_STORAGE_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace dsopp { 10 | namespace output { 11 | /** GL buffers of points with colors from the frame for the visualization */ 12 | struct BufferStorage { 13 | /** vertex buffer for drawing the frame */ 14 | std::map points; 15 | /** color buffer for drawing the frame */ 16 | std::map colors; 17 | /** default color buffer for drawing the frame */ 18 | std::map default_colors; 19 | /** Track colors, unique for each track */ 20 | std::map track_specific_color; 21 | /** seamntic colots for drawing the frame */ 22 | std::map semantic_colors; 23 | }; 24 | } // namespace output 25 | } // namespace dsopp 26 | 27 | #endif // DSOPP_BUFFER_STORAGE_HPP 28 | -------------------------------------------------------------------------------- /src/output/visualizer/internal/visualizer/local_odometry_track.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_LOCAL_ODOMETRY_TRACK_HPP 2 | #define DSOPP_LOCAL_ODOMETRY_TRACK_HPP 3 | 4 | #include 5 | 6 | #include "energy/motion/motion.hpp" 7 | #include "energy/motion/se3_motion.hpp" 8 | #include "visualizer/local_odometry_frame.hpp" 9 | 10 | namespace dsopp { 11 | namespace track { 12 | template 13 | class OdometryTrack; 14 | template 15 | class ActiveOdometryTrack; 16 | } // namespace track 17 | 18 | namespace output { 19 | /** 20 | * Struct to store the local copy of the odometry track. 21 | */ 22 | struct LocalOdometryTrack { 23 | /** 24 | * create a local copy of the odometry track 25 | * @param track odometry track to copy 26 | */ 27 | template 28 | LocalOdometryTrack(const track::OdometryTrack &track); 29 | /** 30 | * create a local copy of the active track 31 | * @param track active odometry track to copy 32 | */ 33 | template 34 | LocalOdometryTrack(const track::ActiveOdometryTrack &track); 35 | LocalOdometryTrack() = default; 36 | /** local copy of the keyframes */ 37 | std::list frames; 38 | }; 39 | } // namespace output 40 | } // namespace dsopp 41 | 42 | #endif // DSOPP_LOCAL_ODOMETRY_TRACK_HPP 43 | -------------------------------------------------------------------------------- /src/output/visualizer/internal/visualizer/local_tracking_frame.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_LOCAL_TRACKING_FRAME_HPP 2 | #define DSOPP_LOCAL_TRACKING_FRAME_HPP 3 | 4 | #include "visualizer/local_frame.hpp" 5 | 6 | #include 7 | 8 | #include "common/time/time.hpp" 9 | 10 | namespace dsopp { 11 | namespace output { 12 | /** 13 | * Struct to store the local copy of the tracking(attached to keyframe) frame 14 | */ 15 | struct LocalTrackingFrame : public LocalFrame { 16 | /** 17 | * create a local copy of the tracking frame 18 | * @param frame tracking frame to copy 19 | * @param _color tracking frame color 20 | */ 21 | template 22 | LocalTrackingFrame(const TrackingFrameType &frame, const Eigen::Vector4d &_color); 23 | }; 24 | } // namespace output 25 | } // namespace dsopp 26 | 27 | #endif // DSOPP_LOCAL_TRACKING_FRAME_HPP 28 | -------------------------------------------------------------------------------- /src/output/visualizer/internal/visualizer/points_storage.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_POINTS_STORAGE_HPP 2 | #define DSOPP_POINTS_STORAGE_HPP 3 | 4 | #include "track/frames/points_storage.hpp" 5 | 6 | namespace dsopp { 7 | namespace output { 8 | using PointVisualizationType = track::storage::PointStorageType; 9 | using PointsStorage = track::storage::PointsStorage; 10 | } // namespace output 11 | } // namespace dsopp 12 | 13 | #endif // DSOPP_POINTS_STORAGE_HPP 14 | -------------------------------------------------------------------------------- /src/output/visualizer/internal/visualizer/renderable.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_RENDERABLE_HPP 2 | #define DSOPP_RENDERABLE_HPP 3 | namespace pangolin { 4 | class View; 5 | } 6 | namespace dsopp { 7 | namespace output { 8 | struct OutputParameters; 9 | /** 10 | * \brief Object of this class can render 3D scene. 11 | * 12 | * Object of this class can draw some objects in the 3D scene. 13 | */ 14 | class Renderable { 15 | public: 16 | /** 17 | * function to call on visualizer initialization 18 | */ 19 | virtual void init(pangolin::View &){}; 20 | /** 21 | * render objects in the 3D scene. 22 | */ 23 | virtual void render() = 0; 24 | /** 25 | * set new parameters of rendering. 26 | * @param output_parameters new parameters 27 | */ 28 | virtual void setParameters(const OutputParameters &output_parameters) { (void)output_parameters; }; 29 | virtual ~Renderable() = default; 30 | }; 31 | 32 | } // namespace output 33 | } // namespace dsopp 34 | 35 | #endif // DSOPP_RENDERABLE_HPP 36 | -------------------------------------------------------------------------------- /src/output/visualizer/internal/visualizer/visualizer_output_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SRC_OUTPUT_VISUALIZER_OUTPUT_INTERFACE_HPP 2 | #define DSOPP_SRC_OUTPUT_VISUALIZER_OUTPUT_INTERFACE_HPP 3 | 4 | namespace dsopp { 5 | namespace output { 6 | /** 7 | * Base class for all Visualizer Interfaces, required to manage ownership 8 | */ 9 | class VisualizerOutputInterface { 10 | public: 11 | virtual ~VisualizerOutputInterface() = default; 12 | }; 13 | } // namespace output 14 | } // namespace dsopp 15 | 16 | #endif // DSOPP_SRC_OUTPUT_VISUALIZER_OUTPUT_INTERFACE_HPP 17 | -------------------------------------------------------------------------------- /src/output/visualizer/src/local_frame.cpp: -------------------------------------------------------------------------------- 1 | #include "visualizer/local_frame.hpp" 2 | 3 | namespace dsopp { 4 | namespace output { 5 | 6 | namespace tools { 7 | const std::optional getDebugCamera(const energy::motion::SE3 &) { return std::nullopt; } 8 | } // namespace tools 9 | } // namespace output 10 | } // namespace dsopp 11 | -------------------------------------------------------------------------------- /src/output/visualizer/src/local_odometry_track.cpp: -------------------------------------------------------------------------------- 1 | #include "visualizer/local_odometry_track.hpp" 2 | 3 | #include "track/active_odometry_track.hpp" 4 | #include "track/odometry_track.hpp" 5 | 6 | namespace dsopp { 7 | namespace output { 8 | template 9 | LocalOdometryTrack::LocalOdometryTrack(const track::OdometryTrack &track) { 10 | for (const auto &frame : track.keyframes()) { 11 | frames.emplace_back(*frame); 12 | } 13 | } 14 | 15 | template 16 | LocalOdometryTrack::LocalOdometryTrack(const track::ActiveOdometryTrack &track) { 17 | for (const auto &marginalized_frame : track.marginalizedFrames()) { 18 | frames.emplace_back(*marginalized_frame); 19 | } 20 | for (const auto &active_frame : track.activeFrames()) { 21 | frames.emplace_back(*active_frame); 22 | } 23 | } 24 | 25 | #ifndef BUILDING_DOCS 26 | template LocalOdometryTrack::LocalOdometryTrack(const track::OdometryTrack> &); 27 | template LocalOdometryTrack::LocalOdometryTrack(const track::ActiveOdometryTrack> &); 28 | #endif /* BUILDING_DOCS */ 29 | } // namespace output 30 | } // namespace dsopp 31 | -------------------------------------------------------------------------------- /src/output/visualizer/src/local_tracking_frame.cpp: -------------------------------------------------------------------------------- 1 | #include "visualizer/local_tracking_frame.hpp" 2 | 3 | #include "common/settings.hpp" 4 | 5 | #include "track/frames/slam_internal_tracking_frame.hpp" 6 | #include "track/frames/tracking_frame.hpp" 7 | 8 | namespace dsopp { 9 | namespace output { 10 | template 11 | LocalTrackingFrame::LocalTrackingFrame(const TrackingFrameType &frame, const Eigen::Vector4d &_color) 12 | : LocalFrame(frame, _color) {} 13 | 14 | #ifndef BUILDING_DOCS 15 | template LocalTrackingFrame::LocalTrackingFrame(const track::TrackingFrame> &, 16 | const Eigen::Vector4d &); 17 | template LocalTrackingFrame::LocalTrackingFrame( 18 | const track::SLAMInternalTrackingFrame> &, const Eigen::Vector4d &); 19 | 20 | #endif /* BUILDING_DOCS */ 21 | } // namespace output 22 | } // namespace dsopp 23 | -------------------------------------------------------------------------------- /src/output/visualizer/src/visualizer_text_output_interface.cpp: -------------------------------------------------------------------------------- 1 | #include "visualizer/visualizer_text_output_interface.hpp" 2 | #include 3 | 4 | namespace dsopp::output { 5 | 6 | VisualizerTextOutputInterface::VisualizerTextOutputInterface(const std::string &field_name) 7 | : text_field_name_(field_name) {} 8 | 9 | void VisualizerTextOutputInterface::pushText(const std::string &text) { 10 | std::lock_guard lock(mutex_); 11 | update_text_ = true; 12 | text_to_show_ = text; 13 | } 14 | 15 | void VisualizerTextOutputInterface::render() { 16 | std::lock_guard lock(mutex_); 17 | if (update_text_) { 18 | *text_variable_ = text_to_show_; 19 | update_text_ = false; 20 | } 21 | } 22 | 23 | void VisualizerTextOutputInterface::init(pangolin::View &) { 24 | // NOTE that `META_FLAG_READONLY` here is needed, cuz otherwise field stops updating with mouse press 25 | text_variable_ = 26 | std::make_unique>("status." + text_field_name_, "", pangolin::META_FLAG_READONLY); 27 | } 28 | 29 | VisualizerTextOutputInterface::~VisualizerTextOutputInterface() = default; 30 | 31 | } // namespace dsopp::output 32 | -------------------------------------------------------------------------------- /src/ransac/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(ransac INTERFACE) 2 | 3 | target_include_directories( 4 | ransac INTERFACE $ 5 | $) 6 | 7 | target_link_libraries(ransac INTERFACE eigen sophus n_point_solvers) 8 | -------------------------------------------------------------------------------- /src/ransac/include/ransac/random_sequence_generator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_RANSAC_RANDOM_SEQUENCE_GENERATOR_HPP_ 2 | #define DSOPP_RANSAC_RANDOM_SEQUENCE_GENERATOR_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | namespace dsopp::ransac { 8 | 9 | /** 10 | * Function to generate sequence of N random number less than max 11 | * 12 | * @tparam N number of numbers 13 | * @param max upper limit 14 | * @retrun sequence distinct of random numbers 15 | */ 16 | template 17 | std::array generateRandomSequence(const size_t max) { 18 | std::array sequence; 19 | sequence[0] = static_cast(rand()) % max; 20 | 21 | for (size_t i = 1; i < N; ++i) { 22 | size_t next_element = static_cast(rand()) % max; 23 | 24 | auto begin = sequence.begin(); 25 | auto end = sequence.begin() + i; 26 | 27 | while (std::find_if(begin, end, [&next_element](size_t v) { return v == next_element; }) != end) { 28 | next_element = static_cast(rand()) % max; 29 | } 30 | 31 | sequence[i] = next_element; 32 | } 33 | return sequence; 34 | } 35 | 36 | } // namespace dsopp::ransac 37 | 38 | #endif // DSOPP_RANSAC_RANDOM_SEQUENCE_GENERATOR_HPP_ 39 | -------------------------------------------------------------------------------- /src/sanity_checker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sanity_check_status INTERFACE) 2 | 3 | target_include_directories( 4 | sanity_check_status 5 | INTERFACE $ 6 | $) 7 | 8 | add_library( 9 | sanity_checker 10 | src/fabric.cpp 11 | #[[ contact Roadly INC for this functionality 12 | src/ackermann_checker.cpp) 13 | ]] 14 | ) 15 | 16 | target_include_directories( 17 | sanity_checker PUBLIC $ 18 | $) 19 | 20 | target_link_libraries(sanity_checker PUBLIC motion_model) 21 | target_link_libraries( 22 | sanity_checker 23 | PRIVATE track 24 | common_fabric_tools 25 | common_file_tools 26 | settings 27 | time 28 | sophus 29 | glog) 30 | -------------------------------------------------------------------------------- /src/sanity_checker/include/sanity_checker/fabric.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DSOPP_SANITY_CHECKER_FABRIC_HPP 2 | #define DSOPP_SANITY_CHECKER_FABRIC_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "energy/motion/motion.hpp" 10 | 11 | namespace dsopp { 12 | namespace sanity_checker { 13 | template