├── ORB_SLAM2_modified ├── CMakeLists.txt ├── Dependencies.md ├── Examples │ ├── Monocular │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ ├── TUM3.yaml │ │ ├── mono_kitti │ │ ├── mono_kitti.cc │ │ ├── mono_tum │ │ └── mono_tum.cc │ ├── RGB-D │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ ├── TUM3.yaml │ │ ├── associations │ │ │ ├── fr1_desk.txt │ │ │ ├── fr1_desk2.txt │ │ │ ├── fr1_room.txt │ │ │ ├── fr1_xyz.txt │ │ │ ├── fr2_desk.txt │ │ │ ├── fr2_xyz.txt │ │ │ ├── fr3_nstr_tex_near.txt │ │ │ ├── fr3_office.txt │ │ │ ├── fr3_str_tex_far.txt │ │ │ └── fr3_str_tex_near.txt │ │ ├── rgbd_cc │ │ ├── rgbd_cc.cc │ │ ├── rgbd_tum │ │ ├── rgbd_tum.cc │ │ └── xtion.yaml │ ├── ROS │ │ └── ORB-SLAM2_DENSE │ │ │ ├── CMakeLists.txt │ │ │ ├── Mono │ │ │ ├── Pose_transform │ │ │ ├── RGBD │ │ │ ├── launch │ │ │ ├── Octomap.launch │ │ │ ├── move_base.launch │ │ │ └── transform.launch │ │ │ ├── manifest.xml │ │ │ ├── orb_octomap │ │ │ ├── params │ │ │ ├── base_local_planner_params.yaml │ │ │ ├── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── local_costmap_params.yaml │ │ │ └── move_base_params.yaml │ │ │ ├── src │ │ │ ├── Pose_transform.cc │ │ │ ├── octomap.cc │ │ │ ├── ros_mono.cc │ │ │ └── ros_rgbd.cc │ │ │ └── visual_octomap │ └── Stereo │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── stereo_kitti │ │ └── stereo_kitti.cc ├── LICENSE.txt ├── License-gpl.txt ├── ORB_SLAM2_modified.kdev4 ├── README.md ├── Thirdparty │ └── DBoW2 │ │ ├── CMakeLists.txt │ │ ├── DBoW2 │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── FClass.h │ │ ├── FORB.cpp │ │ ├── FORB.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ └── TemplatedVocabulary.h │ │ ├── DUtils │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ │ ├── LICENSE.txt │ │ └── README.txt ├── build.sh ├── cmake_modules │ ├── FindCholmod.cmake │ ├── FindEigen3.cmake │ └── FindG2O.cmake ├── include │ ├── Converter.h │ ├── Frame.h │ ├── FrameDrawer.h │ ├── Initializer.h │ ├── KeyFrame.h │ ├── KeyFrameDatabase.h │ ├── LocalMapping.h │ ├── LoopClosing.h │ ├── Map.h │ ├── MapDrawer.h │ ├── MapPoint.h │ ├── ORBVocabulary.h │ ├── ORBextractor.h │ ├── ORBmatcher.h │ ├── Optimizer.h │ ├── PnPsolver.h │ ├── Sim3Solver.h │ ├── System.h │ ├── Tracking.h │ ├── Viewer.h │ └── pointcloudmapping.h ├── orb_slam2_dense │ ├── CMakeLists.txt │ ├── main.cpp │ ├── orb_slam2_dense.cpp │ ├── orb_slam2_dense.h │ └── orb_slam2_dense.kdev4 ├── run_now.sh └── src │ ├── Converter.cc │ ├── Frame.cc │ ├── FrameDrawer.cc │ ├── Initializer.cc │ ├── KeyFrame.cc │ ├── KeyFrameDatabase.cc │ ├── LocalMapping.cc │ ├── LoopClosing.cc │ ├── Map.cc │ ├── MapDrawer.cc │ ├── MapPoint.cc │ ├── ORBextractor.cc │ ├── ORBmatcher.cc │ ├── Optimizer.cc │ ├── PnPsolver.cc │ ├── Sim3Solver.cc │ ├── System.cc │ ├── Tracking.cc │ ├── Viewer.cc │ └── pointcloudmapping.cc ├── README.md └── g2o_with_orbslam2 ├── CMakeLists.txt ├── EXTERNAL ├── CMakeLists.txt ├── ceres │ ├── LICENSE │ ├── autodiff.h │ ├── eigen.h │ ├── fixed_array.h │ ├── fpclassify.h │ ├── jet.h │ ├── macros.h │ ├── manual_constructor.h │ └── variadic_evaluate.h ├── csparse │ ├── CMakeLists.txt │ ├── License.txt │ ├── README.txt │ ├── cs.h │ ├── cs_add.c │ ├── cs_amd.c │ ├── cs_api.h │ ├── cs_chol.c │ ├── cs_cholsol.c │ ├── cs_compress.c │ ├── cs_counts.c │ ├── cs_cumsum.c │ ├── cs_dfs.c │ ├── cs_dmperm.c │ ├── cs_droptol.c │ ├── cs_dropzeros.c │ ├── cs_dupl.c │ ├── cs_entry.c │ ├── cs_ereach.c │ ├── cs_etree.c │ ├── cs_fkeep.c │ ├── cs_gaxpy.c │ ├── cs_happly.c │ ├── cs_house.c │ ├── cs_ipvec.c │ ├── cs_leaf.c │ ├── cs_load.c │ ├── cs_lsolve.c │ ├── cs_ltsolve.c │ ├── cs_lu.c │ ├── cs_lusol.c │ ├── cs_malloc.c │ ├── cs_maxtrans.c │ ├── cs_multiply.c │ ├── cs_norm.c │ ├── cs_permute.c │ ├── cs_pinv.c │ ├── cs_post.c │ ├── cs_print.c │ ├── cs_pvec.c │ ├── cs_qr.c │ ├── cs_qrsol.c │ ├── cs_randperm.c │ ├── cs_reach.c │ ├── cs_scatter.c │ ├── cs_scc.c │ ├── cs_schol.c │ ├── cs_spsolve.c │ ├── cs_sqr.c │ ├── cs_symperm.c │ ├── cs_tdfs.c │ ├── cs_transpose.c │ ├── cs_updown.c │ ├── cs_usolve.c │ ├── cs_util.c │ ├── cs_utsolve.c │ └── lesser.txt └── freeglut │ ├── CMakeLists.txt │ ├── COPYING │ ├── freeglut_font.cpp │ ├── freeglut_minimal.h │ ├── freeglut_stroke_mono_roman.cpp │ └── freeglut_stroke_roman.cpp ├── README.md ├── cmake_modules ├── FindBLAS.cmake ├── FindCSparse.cmake ├── FindCholmod.cmake ├── FindEigen3.cmake ├── FindG2O.cmake ├── FindLAPACK.cmake ├── FindQGLViewer.cmake └── FindSuiteSparse.cmake ├── config.h.in ├── doc └── .gitignore ├── g2o ├── .gitignore ├── CMakeLists.txt ├── apps │ ├── CMakeLists.txt │ ├── g2o_cli │ │ ├── CMakeLists.txt │ │ ├── dl_wrapper.cpp │ │ ├── dl_wrapper.h │ │ ├── g2o.cpp │ │ ├── g2o_cli_api.h │ │ ├── g2o_common.cpp │ │ ├── g2o_common.h │ │ ├── output_helper.cpp │ │ └── output_helper.h │ ├── g2o_hierarchical │ │ ├── CMakeLists.txt │ │ ├── backbone_tree_action.cpp │ │ ├── backbone_tree_action.h │ │ ├── edge_creator.cpp │ │ ├── edge_creator.h │ │ ├── edge_labeler.cpp │ │ ├── edge_labeler.h │ │ ├── edge_types_cost_function.cpp │ │ ├── edge_types_cost_function.h │ │ ├── g2o_hierarchical.cpp │ │ ├── g2o_hierarchical_api.h │ │ ├── g2o_hierarchical_test_functions.cpp │ │ ├── simple_star_ops.cpp │ │ ├── simple_star_ops.h │ │ ├── star.cpp │ │ └── star.h │ ├── g2o_simulator │ │ ├── CMakeLists.txt │ │ ├── convertSegmentLine.cpp │ │ ├── converteSegmentLine.cpp │ │ ├── g2o_anonymize_observations.cpp │ │ ├── g2o_simulator_api.h │ │ ├── pointsensorparameters.cpp │ │ ├── pointsensorparameters.h │ │ ├── sensor_line3d.cpp │ │ ├── sensor_line3d.h │ │ ├── sensor_odometry.h │ │ ├── sensor_odometry2d.cpp │ │ ├── sensor_odometry2d.h │ │ ├── sensor_odometry3d.cpp │ │ ├── sensor_odometry3d.h │ │ ├── sensor_pointxy.cpp │ │ ├── sensor_pointxy.h │ │ ├── sensor_pointxy_bearing.cpp │ │ ├── sensor_pointxy_bearing.h │ │ ├── sensor_pointxy_offset.cpp │ │ ├── sensor_pointxy_offset.h │ │ ├── sensor_pointxyz.cpp │ │ ├── sensor_pointxyz.h │ │ ├── sensor_pointxyz_depth.cpp │ │ ├── sensor_pointxyz_depth.h │ │ ├── sensor_pointxyz_disparity.cpp │ │ ├── sensor_pointxyz_disparity.h │ │ ├── sensor_pose2d.cpp │ │ ├── sensor_pose2d.h │ │ ├── sensor_pose3d.cpp │ │ ├── sensor_pose3d.h │ │ ├── sensor_pose3d_offset.cpp │ │ ├── sensor_pose3d_offset.h │ │ ├── sensor_se3_prior.cpp │ │ ├── sensor_se3_prior.h │ │ ├── sensor_segment2d.cpp │ │ ├── sensor_segment2d.h │ │ ├── sensor_segment2d_line.cpp │ │ ├── sensor_segment2d_line.h │ │ ├── sensor_segment2d_pointline.cpp │ │ ├── sensor_segment2d_pointline.h │ │ ├── simulator.cpp │ │ ├── simulator.h │ │ ├── simulator2d.h │ │ ├── simulator2d_base.h │ │ ├── simulator2d_segment.cpp │ │ ├── simulator3d.h │ │ ├── simulator3d_base.h │ │ ├── simutils.cpp │ │ ├── simutils.h │ │ ├── test_simulator2d.cpp │ │ └── test_simulator3d.cpp │ ├── g2o_viewer │ │ ├── CMakeLists.txt │ │ ├── base_main_window.ui │ │ ├── base_properties_widget.ui │ │ ├── g2o_qglviewer.cpp │ │ ├── g2o_qglviewer.h │ │ ├── g2o_viewer.cpp │ │ ├── g2o_viewer_api.h │ │ ├── gui_hyper_graph_action.cpp │ │ ├── gui_hyper_graph_action.h │ │ ├── main_window.cpp │ │ ├── main_window.h │ │ ├── properties_widget.cpp │ │ ├── properties_widget.h │ │ ├── run_g2o_viewer.cpp │ │ ├── run_g2o_viewer.h │ │ ├── stream_redirect.cpp │ │ ├── stream_redirect.h │ │ ├── viewer_properties_widget.cpp │ │ └── viewer_properties_widget.h │ └── linked_binaries │ │ └── CMakeLists.txt ├── core │ ├── CMakeLists.txt │ ├── base_binary_edge.h │ ├── base_binary_edge.hpp │ ├── base_edge.h │ ├── base_multi_edge.h │ ├── base_multi_edge.hpp │ ├── base_unary_edge.h │ ├── base_unary_edge.hpp │ ├── base_vertex.h │ ├── base_vertex.hpp │ ├── batch_stats.cpp │ ├── batch_stats.h │ ├── block_solver.h │ ├── block_solver.hpp │ ├── cache.cpp │ ├── cache.h │ ├── creators.h │ ├── eigen_types.h │ ├── estimate_propagator.cpp │ ├── estimate_propagator.h │ ├── factory.cpp │ ├── factory.h │ ├── g2o_core_api.h │ ├── hyper_dijkstra.cpp │ ├── hyper_dijkstra.h │ ├── hyper_graph.cpp │ ├── hyper_graph.h │ ├── hyper_graph_action.cpp │ ├── hyper_graph_action.h │ ├── jacobian_workspace.cpp │ ├── jacobian_workspace.h │ ├── linear_solver.h │ ├── marginal_covariance_cholesky.cpp │ ├── marginal_covariance_cholesky.h │ ├── matrix_operations.h │ ├── matrix_structure.cpp │ ├── matrix_structure.h │ ├── openmp_mutex.h │ ├── optimizable_graph.cpp │ ├── optimizable_graph.h │ ├── optimization_algorithm.cpp │ ├── optimization_algorithm.h │ ├── optimization_algorithm_dogleg.cpp │ ├── optimization_algorithm_dogleg.h │ ├── optimization_algorithm_factory.cpp │ ├── optimization_algorithm_factory.h │ ├── optimization_algorithm_gauss_newton.cpp │ ├── optimization_algorithm_gauss_newton.h │ ├── optimization_algorithm_levenberg.cpp │ ├── optimization_algorithm_levenberg.h │ ├── optimization_algorithm_property.h │ ├── optimization_algorithm_with_hessian.cpp │ ├── optimization_algorithm_with_hessian.h │ ├── parameter.cpp │ ├── parameter.h │ ├── parameter_container.cpp │ ├── parameter_container.h │ ├── robust_kernel.cpp │ ├── robust_kernel.h │ ├── robust_kernel_factory.cpp │ ├── robust_kernel_factory.h │ ├── robust_kernel_impl.cpp │ ├── robust_kernel_impl.h │ ├── solver.cpp │ ├── solver.h │ ├── sparse_block_matrix.h │ ├── sparse_block_matrix.hpp │ ├── sparse_block_matrix_ccs.h │ ├── sparse_block_matrix_diagonal.h │ ├── sparse_block_matrix_test.cpp │ ├── sparse_optimizer.cpp │ ├── sparse_optimizer.h │ ├── sparse_optimizer_terminate_action.cpp │ └── sparse_optimizer_terminate_action.h ├── examples │ ├── CMakeLists.txt │ ├── ba │ │ ├── CMakeLists.txt │ │ └── ba_demo.cpp │ ├── ba_anchored_inverse_depth │ │ ├── CMakeLists.txt │ │ └── ba_anchored_inverse_depth_demo.cpp │ ├── bal │ │ ├── CMakeLists.txt │ │ └── bal_example.cpp │ ├── calibration_odom_laser │ │ ├── CMakeLists.txt │ │ ├── closed_form_calibration.cpp │ │ ├── closed_form_calibration.h │ │ ├── edge_se2_pure_calib.cpp │ │ ├── edge_se2_pure_calib.h │ │ ├── g2o_calibration_odom_laser_api.h │ │ ├── gm2dl_io.cpp │ │ ├── gm2dl_io.h │ │ ├── motion_information.h │ │ ├── sclam_helpers.cpp │ │ ├── sclam_helpers.h │ │ ├── sclam_laser_calib.cpp │ │ ├── sclam_odom_laser.cpp │ │ └── sclam_pure_calibration.cpp │ ├── data_convert │ │ ├── CMakeLists.txt │ │ └── convert_sba_slam3d.cpp │ ├── data_fitting │ │ ├── CMakeLists.txt │ │ ├── circle_fit.cpp │ │ └── curve_fit.cpp │ ├── g2o_unfold │ │ ├── g2o-unfold.cpp │ │ ├── tools.cpp │ │ └── tools.h │ ├── icp │ │ ├── CMakeLists.txt │ │ ├── gicp-test1.dat │ │ ├── gicp_demo.cpp │ │ └── gicp_sba_demo.cpp │ ├── interactive_slam │ │ ├── CMakeLists.txt │ │ ├── README.txt │ │ ├── g2o_incremental │ │ │ ├── CMakeLists.txt │ │ │ ├── README.txt │ │ │ ├── g2o_incremental.cpp │ │ │ ├── g2o_incremental_api.h │ │ │ ├── graph_optimizer_sparse_incremental.cpp │ │ │ ├── graph_optimizer_sparse_incremental.h │ │ │ ├── linear_solver_cholmod_online.h │ │ │ └── protocol.txt │ │ ├── g2o_interactive │ │ │ ├── CMakeLists.txt │ │ │ ├── fast_output.h │ │ │ ├── g2o_interactive_api.h │ │ │ ├── g2o_online.cpp │ │ │ ├── g2o_slam_interface.cpp │ │ │ ├── g2o_slam_interface.h │ │ │ ├── generate_commands.cpp │ │ │ ├── graph_optimizer_sparse_online.cpp │ │ │ ├── graph_optimizer_sparse_online.h │ │ │ ├── protocol.txt │ │ │ ├── types_online.cpp │ │ │ ├── types_slam2d_online.h │ │ │ └── types_slam3d_online.h │ │ └── slam_parser │ │ │ ├── CMakeLists.txt │ │ │ ├── interface │ │ │ ├── CMakeLists.txt │ │ │ ├── abstract_slam_interface.h │ │ │ ├── parser_interface.cpp │ │ │ ├── parser_interface.h │ │ │ ├── slam_context_interface.cpp │ │ │ └── slam_context_interface.h │ │ │ └── parser │ │ │ ├── CMakeLists.txt │ │ │ ├── FlexLexer.h │ │ │ ├── bison_parser.cpp │ │ │ ├── bison_parser.h │ │ │ ├── commands.h │ │ │ ├── driver.cpp │ │ │ ├── driver.h │ │ │ ├── flex_scanner.cpp │ │ │ ├── grammar.sh │ │ │ ├── location.hh │ │ │ ├── parser.yy │ │ │ ├── position.hh │ │ │ ├── scanner.h │ │ │ ├── scanner.l │ │ │ ├── slam_context.cpp │ │ │ ├── slam_context.h │ │ │ ├── stack.hh │ │ │ └── test_slam_parser.cpp │ ├── plane_slam │ │ ├── CMakeLists.txt │ │ ├── plane_test.cpp │ │ └── simulator_3d_plane.cpp │ ├── sba │ │ ├── CMakeLists.txt │ │ └── sba_demo.cpp │ ├── simple_optimize │ │ ├── CMakeLists.txt │ │ └── simple_optimize.cpp │ ├── slam2d │ │ ├── CMakeLists.txt │ │ ├── base_main_window.ui │ │ ├── draw_helpers.cpp │ │ ├── draw_helpers.h │ │ ├── main_window.cpp │ │ ├── main_window.h │ │ ├── slam2d_g2o.cpp │ │ ├── slam2d_viewer.cpp │ │ └── slam2d_viewer.h │ ├── sphere │ │ ├── CMakeLists.txt │ │ └── create_sphere.cpp │ ├── target │ │ ├── CMakeLists.txt │ │ ├── constant_velocity_target.cpp │ │ ├── continuous_to_discrete.h │ │ ├── static_target.cpp │ │ ├── targetTypes3D.hpp │ │ └── targetTypes6D.hpp │ └── tutorial_slam2d │ │ ├── CMakeLists.txt │ │ ├── edge_se2.cpp │ │ ├── edge_se2.h │ │ ├── edge_se2_pointxy.cpp │ │ ├── edge_se2_pointxy.h │ │ ├── g2o_tutorial_slam2d_api.h │ │ ├── parameter_se2_offset.cpp │ │ ├── parameter_se2_offset.h │ │ ├── rand.h │ │ ├── se2.h │ │ ├── simulator.cpp │ │ ├── simulator.h │ │ ├── tutorial_slam2d.cpp │ │ ├── types_tutorial_slam2d.cpp │ │ ├── types_tutorial_slam2d.h │ │ ├── vertex_point_xy.cpp │ │ ├── vertex_point_xy.h │ │ ├── vertex_se2.cpp │ │ └── vertex_se2.h ├── solvers │ ├── CMakeLists.txt │ ├── cholmod │ │ ├── CMakeLists.txt │ │ ├── linear_solver_cholmod.h │ │ └── solver_cholmod.cpp │ ├── csparse │ │ ├── CMakeLists.txt │ │ ├── csparse_helper.cpp │ │ ├── csparse_helper.h │ │ ├── g2o_csparse_api.h │ │ ├── g2o_csparse_extension_api.h │ │ ├── linear_solver_csparse.h │ │ └── solver_csparse.cpp │ ├── dense │ │ ├── CMakeLists.txt │ │ ├── linear_solver_dense.h │ │ └── solver_dense.cpp │ ├── eigen │ │ ├── CMakeLists.txt │ │ ├── linear_solver_eigen.h │ │ └── solver_eigen.cpp │ ├── pcg │ │ ├── CMakeLists.txt │ │ ├── linear_solver_pcg.h │ │ ├── linear_solver_pcg.hpp │ │ └── solver_pcg.cpp │ ├── slam2d_linear │ │ ├── CMakeLists.txt │ │ ├── g2o_slam2d_linear_api.h │ │ ├── slam2d_linear.cpp │ │ ├── solver_slam2d_linear.cpp │ │ └── solver_slam2d_linear.h │ └── structure_only │ │ ├── CMakeLists.txt │ │ ├── structure_only.cpp │ │ └── structure_only_solver.h ├── stuff │ ├── CMakeLists.txt │ ├── color_macros.h │ ├── command_args.cpp │ ├── command_args.h │ ├── filesys_tools.cpp │ ├── filesys_tools.h │ ├── g2o_stuff_api.h │ ├── macros.h │ ├── misc.h │ ├── opengl_primitives.cpp │ ├── opengl_primitives.h │ ├── opengl_wrapper.h │ ├── os_specific.c │ ├── os_specific.h │ ├── property.cpp │ ├── property.h │ ├── sampler.cpp │ ├── sampler.h │ ├── scoped_pointer.h │ ├── sparse_helper.cpp │ ├── sparse_helper.h │ ├── string_tools.cpp │ ├── string_tools.h │ ├── tictoc.cpp │ ├── tictoc.h │ ├── timeutil.cpp │ ├── timeutil.h │ └── unscented.h ├── types │ ├── CMakeLists.txt │ ├── data │ │ ├── CMakeLists.txt │ │ ├── data_queue.cpp │ │ ├── data_queue.h │ │ ├── g2o_types_data_api.h │ │ ├── laser_parameters.cpp │ │ ├── laser_parameters.h │ │ ├── raw_laser.cpp │ │ ├── raw_laser.h │ │ ├── robot_data.cpp │ │ ├── robot_data.h │ │ ├── robot_laser.cpp │ │ ├── robot_laser.h │ │ ├── types_data.cpp │ │ ├── types_data.h │ │ ├── vertex_ellipse.cpp │ │ ├── vertex_ellipse.h │ │ ├── vertex_tag.cpp │ │ └── vertex_tag.h │ ├── deprecated │ │ ├── CMakeLists.txt │ │ └── slam3d │ │ │ ├── CMakeLists.txt │ │ │ ├── edge_se3_offset.cpp │ │ │ ├── edge_se3_offset.h │ │ │ ├── edge_se3_pointxyz.cpp │ │ │ ├── edge_se3_pointxyz.h │ │ │ ├── edge_se3_pointxyz_depth.cpp │ │ │ ├── edge_se3_pointxyz_depth.h │ │ │ ├── edge_se3_pointxyz_disparity.cpp │ │ │ ├── edge_se3_pointxyz_disparity.h │ │ │ ├── edge_se3_prior.cpp │ │ │ ├── edge_se3_prior.h │ │ │ ├── edge_se3_quat.cpp │ │ │ ├── edge_se3_quat.h │ │ │ ├── g2o_deprecated_types_slam3d_api.h │ │ │ ├── parameter_camera.cpp │ │ │ ├── parameter_camera.h │ │ │ ├── parameter_se3_offset.cpp │ │ │ ├── parameter_se3_offset.h │ │ │ ├── se3quat_gradients.cpp │ │ │ ├── se3quat_gradients.h │ │ │ ├── types_slam3d.cpp │ │ │ ├── types_slam3d.h │ │ │ ├── vertex_pointxyz.cpp │ │ │ ├── vertex_pointxyz.h │ │ │ ├── vertex_se3_quat.cpp │ │ │ └── vertex_se3_quat.h │ ├── icp │ │ ├── CMakeLists.txt │ │ ├── g2o_types_icp_api.h │ │ ├── types_icp.cpp │ │ └── types_icp.h │ ├── sba │ │ ├── CMakeLists.txt │ │ ├── g2o_types_sba_api.h │ │ ├── sbacam.h │ │ ├── types_sba.cpp │ │ ├── types_sba.h │ │ ├── types_six_dof_expmap.cpp │ │ └── types_six_dof_expmap.h │ ├── sclam2d │ │ ├── CMakeLists.txt │ │ ├── edge_se2_odom_differential_calib.cpp │ │ ├── edge_se2_odom_differential_calib.h │ │ ├── edge_se2_sensor_calib.cpp │ │ ├── edge_se2_sensor_calib.h │ │ ├── g2o_types_sclam2d_api.h │ │ ├── odometry_measurement.cpp │ │ ├── odometry_measurement.h │ │ ├── types_sclam2d.cpp │ │ ├── types_sclam2d.h │ │ ├── vertex_odom_differential_params.cpp │ │ └── vertex_odom_differential_params.h │ ├── sim3 │ │ ├── CMakeLists.txt │ │ ├── sim3.h │ │ ├── types_seven_dof_expmap.cpp │ │ └── types_seven_dof_expmap.h │ ├── slam2d │ │ ├── CMakeLists.txt │ │ ├── edge_pointxy.cpp │ │ ├── edge_pointxy.h │ │ ├── edge_se2.cpp │ │ ├── edge_se2.h │ │ ├── edge_se2_lotsofxy.cpp │ │ ├── edge_se2_lotsofxy.h │ │ ├── edge_se2_offset.cpp │ │ ├── edge_se2_offset.h │ │ ├── edge_se2_pointxy.cpp │ │ ├── edge_se2_pointxy.h │ │ ├── edge_se2_pointxy_bearing.cpp │ │ ├── edge_se2_pointxy_bearing.h │ │ ├── edge_se2_pointxy_calib.cpp │ │ ├── edge_se2_pointxy_calib.h │ │ ├── edge_se2_pointxy_offset.cpp │ │ ├── edge_se2_pointxy_offset.h │ │ ├── edge_se2_prior.cpp │ │ ├── edge_se2_prior.h │ │ ├── edge_se2_twopointsxy.cpp │ │ ├── edge_se2_twopointsxy.h │ │ ├── edge_se2_xyprior.cpp │ │ ├── edge_se2_xyprior.h │ │ ├── g2o_types_slam2d_api.h │ │ ├── parameter_se2_offset.cpp │ │ ├── parameter_se2_offset.h │ │ ├── se2.h │ │ ├── types_slam2d.cpp │ │ ├── types_slam2d.h │ │ ├── vertex_point_xy.cpp │ │ ├── vertex_point_xy.h │ │ ├── vertex_se2.cpp │ │ └── vertex_se2.h │ ├── slam2d_addons │ │ ├── CMakeLists.txt │ │ ├── edge_line2d.cpp │ │ ├── edge_line2d.h │ │ ├── edge_line2d_pointxy.cpp │ │ ├── edge_line2d_pointxy.h │ │ ├── edge_se2_line2d.cpp │ │ ├── edge_se2_line2d.h │ │ ├── edge_se2_segment2d.cpp │ │ ├── edge_se2_segment2d.h │ │ ├── edge_se2_segment2d_line.cpp │ │ ├── edge_se2_segment2d_line.h │ │ ├── edge_se2_segment2d_pointLine.cpp │ │ ├── edge_se2_segment2d_pointLine.h │ │ ├── g2o_types_slam2d_addons_api.h │ │ ├── line_2d.h │ │ ├── types_slam2d_addons.cpp │ │ ├── types_slam2d_addons.h │ │ ├── vertex_line2d.cpp │ │ ├── vertex_line2d.h │ │ ├── vertex_segment2d.cpp │ │ └── vertex_segment2d.h │ ├── slam3d │ │ ├── CMakeLists.txt │ │ ├── dquat2mat.cpp │ │ ├── dquat2mat.h │ │ ├── dquat2mat.wxm │ │ ├── dquat2mat_maxima_generated.cpp │ │ ├── edge_pointxyz.cpp │ │ ├── edge_pointxyz.h │ │ ├── edge_se3.cpp │ │ ├── edge_se3.h │ │ ├── edge_se3_lotsofxyz.cpp │ │ ├── edge_se3_lotsofxyz.h │ │ ├── edge_se3_offset.cpp │ │ ├── edge_se3_offset.h │ │ ├── edge_se3_pointxyz.cpp │ │ ├── edge_se3_pointxyz.h │ │ ├── edge_se3_pointxyz_depth.cpp │ │ ├── edge_se3_pointxyz_depth.h │ │ ├── edge_se3_pointxyz_disparity.cpp │ │ ├── edge_se3_pointxyz_disparity.h │ │ ├── edge_se3_prior.cpp │ │ ├── edge_se3_prior.h │ │ ├── g2o_types_slam3d_api.h │ │ ├── isometry3d_gradients.cpp │ │ ├── isometry3d_gradients.h │ │ ├── isometry3d_mappings.cpp │ │ ├── isometry3d_mappings.h │ │ ├── parameter_camera.cpp │ │ ├── parameter_camera.h │ │ ├── parameter_se3_offset.cpp │ │ ├── parameter_se3_offset.h │ │ ├── parameter_stereo_camera.cpp │ │ ├── parameter_stereo_camera.h │ │ ├── se3_ops.h │ │ ├── se3_ops.hpp │ │ ├── se3quat.h │ │ ├── test_isometry3d_mappings.cpp │ │ ├── test_mat2quat_jacobian.cpp │ │ ├── test_slam3d_jacobian.cpp │ │ ├── types_slam3d.cpp │ │ ├── types_slam3d.h │ │ ├── vertex_pointxyz.cpp │ │ ├── vertex_pointxyz.h │ │ ├── vertex_se3.cpp │ │ └── vertex_se3.h │ └── slam3d_addons │ │ ├── CMakeLists.txt │ │ ├── edge_line3d.cpp │ │ ├── edge_line3d.h │ │ ├── edge_plane.cpp │ │ ├── edge_plane.h │ │ ├── edge_se3_calib.cpp │ │ ├── edge_se3_calib.h │ │ ├── edge_se3_euler.cpp │ │ ├── edge_se3_euler.h │ │ ├── edge_se3_line.cpp │ │ ├── edge_se3_line.h │ │ ├── edge_se3_plane_calib.cpp │ │ ├── edge_se3_plane_calib.h │ │ ├── g2o_types_slam3d_addons_api.h │ │ ├── line3d.cpp │ │ ├── line3d.h │ │ ├── line3d_test.cpp │ │ ├── plane3d.h │ │ ├── types_slam3d_addons.cpp │ │ ├── types_slam3d_addons.h │ │ ├── vertex_line3d.cpp │ │ ├── vertex_line3d.h │ │ ├── vertex_plane.cpp │ │ ├── vertex_plane.h │ │ ├── vertex_se3_euler.cpp │ │ └── vertex_se3_euler.h └── what_is_in_these_directories.txt └── script ├── android.toolchain.cmake ├── install-deps-linux.sh └── install-deps-osx.sh /ORB_SLAM2_modified/Examples/Monocular/mono_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/Monocular/mono_kitti -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/Monocular/mono_tum: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/Monocular/mono_tum -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/RGB-D/rgbd_cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/RGB-D/rgbd_cc -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/RGB-D/rgbd_tum: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/RGB-D/rgbd_tum -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/Mono: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/Mono -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/Pose_transform: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/Pose_transform -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/RGBD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/RGBD -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/launch/Octomap.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/launch/transform.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | ​ 6 | 7 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ORB_SLAM2 5 | 6 | 7 | Raul Mur-Artal 8 | GPLv3 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/orb_octomap: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/orb_octomap -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/params/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0.45 3 | min_vel_x: 0.1 4 | max_vel_theta: 1.0 5 | min_in_place_vel_theta: 0.4 6 | 7 | acc_lim_theta: 3.2 8 | acc_lim_x: 2.5 9 | acc_lim_y: 2.5 10 | 11 | holonomic_robot: true -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | origin_z: 0.0 3 | z_resolution: 0.2 4 | z_voxels: 2 5 | 6 | obstacle_range: 2.5 7 | raytrace_range: 3.0 8 | 9 | publish_voxel_map: false 10 | transform_tolerance: 0.5 11 | meter_scoring: true 12 | 13 | footprint: [[-0.21, -0.165], [-0.21, 0.165], [0.21, 0.165], [0.21, -0.165]] 14 | footprint_padding: 0.1 15 | 16 | #plugins: 17 | #- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"} 18 | #- {name: inflater_layer, type: "costmap_2d::InflationLayer"} 19 | 20 | obstacles_layer: 21 | observation_sources: point_cloud_sensor 22 | point_cloud_sensor: {sensor_frame: /map, data_type: PointCloud2, topic: /pclPoint_out, marking: true, clearing: true, min_obstacle_height: 0.15, max_obstacle_height: 1.0} 23 | 24 | inflater_layer: 25 | inflation_radius: 0.05 26 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | map_topic: /projected_map 3 | global_frame: /map 4 | robot_base_frame: /base_link 5 | update_frequency: 1.0 6 | publish_frequency: 0.5 7 | static_map: true 8 | transform_tolerance: 0.5 9 | resolution: 0.05 10 | inflation_radius: 0.1 11 | cost_scaling_factor: 2.0 -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/params/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 20.0 5 | publish_frequency: 5.0 6 | width: 10.0 7 | height: 10.0 8 | resolution: 0.05 9 | static_map: false 10 | rolling_window: true 11 | resolution: 0.05 -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/params/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | 3 | controller_frequency: 20.0 4 | controller_patience: 15.0 5 | 6 | planner_frequency: 20.0 7 | planner_patience: 5.0 8 | 9 | oscillation_timeout: 0.0 10 | oscillation_distance: 0.5 11 | 12 | recovery_behavior_enabled: true 13 | clearing_rotation_allowed: true -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/visual_octomap: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/ROS/ORB-SLAM2_DENSE/visual_octomap -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Examples/Stereo/stereo_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaochq/Navigation_using_Octomap/f6b066af063d5daa54fa0477a23b96edede7a3e4/ORB_SLAM2_modified/Examples/Stereo/stereo_kitti -------------------------------------------------------------------------------- /ORB_SLAM2_modified/LICENSE.txt: -------------------------------------------------------------------------------- 1 | ORB-SLAM2 is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ORB-SLAM2. 3 | 4 | For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors. 5 | 6 | If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/raulmur/ORB_SLAM2 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/ORB_SLAM2_modified.kdev4: -------------------------------------------------------------------------------- 1 | [Project] 2 | Manager=KDevCMakeManager 3 | Name=ORB_SLAM2_modified 4 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV REQUIRED) 28 | 29 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 30 | 31 | include_directories(${OpenCV_INCLUDE_DIRS}) 32 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 33 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 34 | 35 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/Thirdparty/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/DBoW2 ..." 2 | 3 | cd Thirdparty/DBoW2 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../g2o 10 | 11 | echo "Configuring and building Thirdparty/g2o ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../../ 19 | 20 | echo "Uncompress vocabulary ..." 21 | 22 | cd Vocabulary 23 | tar -xf ORBvoc.txt.tar.gz 24 | cd .. 25 | 26 | echo "Configuring and building ORB_SLAM2 ..." 27 | 28 | mkdir build 29 | cd build 30 | cmake .. -DCMAKE_BUILD_TYPE=Release 31 | make -j 32 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | 42 | class KeyFrameDatabase 43 | { 44 | public: 45 | 46 | KeyFrameDatabase(const ORBVocabulary &voc); 47 | 48 | void add(KeyFrame* pKF); 49 | 50 | void erase(KeyFrame* pKF); 51 | 52 | void clear(); 53 | 54 | // Loop Detection 55 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 56 | 57 | // Relocalization 58 | std::vector DetectRelocalizationCandidates(Frame* F); 59 | 60 | protected: 61 | 62 | // Associated vocabulary 63 | const ORBVocabulary* mpVoc; 64 | 65 | // Inverted file 66 | std::vector > mvInvertedFile; 67 | 68 | // Mutex 69 | std::mutex mMutex; 70 | }; 71 | 72 | } //namespace ORB_SLAM 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPDRAWER_H 22 | #define MAPDRAWER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include 28 | 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class MapDrawer 35 | { 36 | public: 37 | MapDrawer(Map* pMap, const string &strSettingPath); 38 | 39 | Map* mpMap; 40 | 41 | void DrawMapPoints(); 42 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 43 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 44 | void SetCurrentCameraPose(const cv::Mat &Tcw); 45 | void SetReferenceKeyFrame(KeyFrame *pKF); 46 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 47 | void returnCampose(cv::Mat &Tcw); 48 | private: 49 | 50 | float mKeyFrameSize; 51 | float mKeyFrameLineWidth; 52 | float mGraphLineWidth; 53 | float mPointSize; 54 | float mCameraSize; 55 | float mCameraLineWidth; 56 | 57 | cv::Mat mCameraPose; 58 | 59 | std::mutex mMutexCamera; 60 | }; 61 | 62 | } //namespace ORB_SLAM 63 | 64 | #endif // MAPDRAWER_H 65 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include "Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include "Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/orb_slam2_dense/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(orb_slam2_dense) 2 | cmake_minimum_required(VERSION 2.6) 3 | find_package(Qt4 REQUIRED) 4 | 5 | include_directories(${QT_INCLUDES} ${CMAKE_CURRENT_BINARY_DIR}) 6 | 7 | set(orb_slam2_dense_SRCS orb_slam2_dense.cpp main.cpp) 8 | qt4_automoc(${orb_slam2_dense_SRCS}) 9 | add_executable(orb_slam2_dense ${orb_slam2_dense_SRCS}) 10 | target_link_libraries(orb_slam2_dense ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) 11 | install(TARGETS orb_slam2_dense RUNTIME DESTINATION bin) 12 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/orb_slam2_dense/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "orb_slam2_dense.h" 3 | 4 | 5 | int main(int argc, char** argv) 6 | { 7 | QApplication app(argc, argv); 8 | orb_slam2_dense foo; 9 | foo.show(); 10 | return app.exec(); 11 | } 12 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/orb_slam2_dense/orb_slam2_dense.cpp: -------------------------------------------------------------------------------- 1 | #include "orb_slam2_dense.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | orb_slam2_dense::orb_slam2_dense() 9 | { 10 | QLabel* l = new QLabel( this ); 11 | l->setText( "Hello World!" ); 12 | setCentralWidget( l ); 13 | QAction* a = new QAction(this); 14 | a->setText( "Quit" ); 15 | connect(a, SIGNAL(triggered()), SLOT(close()) ); 16 | menuBar()->addMenu( "File" )->addAction( a ); 17 | } 18 | 19 | orb_slam2_dense::~orb_slam2_dense() 20 | {} 21 | 22 | #include "orb_slam2_dense.moc" 23 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/orb_slam2_dense/orb_slam2_dense.h: -------------------------------------------------------------------------------- 1 | #ifndef orb_slam2_dense_H 2 | #define orb_slam2_dense_H 3 | 4 | #include 5 | 6 | class orb_slam2_dense : public QMainWindow 7 | { 8 | Q_OBJECT 9 | public: 10 | orb_slam2_dense(); 11 | virtual ~orb_slam2_dense(); 12 | }; 13 | 14 | #endif // orb_slam2_dense_H 15 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/orb_slam2_dense/orb_slam2_dense.kdev4: -------------------------------------------------------------------------------- 1 | [Project] 2 | Name=orb_slam2_dense 3 | Manager=KDevCMakeManager 4 | VersionControl= 5 | -------------------------------------------------------------------------------- /ORB_SLAM2_modified/run_now.sh: -------------------------------------------------------------------------------- 1 | rosrun ORB-SLAM2_DENSE RGBD ./Vocabulary/ORBvoc.txt ./Examples/RGB-D/TUM1.yaml 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Navigation_using_Octomap 2 | The 3D navigation for a turtlebot using octomap 3 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Only build CSPARSE if we need to 2 | IF(BUILD_CSPARSE) 3 | ADD_SUBDIRECTORY(csparse) 4 | ENDIF(BUILD_CSPARSE) 5 | 6 | IF (G2O_HAVE_OPENGL) 7 | ADD_SUBDIRECTORY(freeglut) 8 | ENDIF() 9 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/ceres/LICENSE: -------------------------------------------------------------------------------- 1 | Ceres Solver - A fast non-linear least squares minimizer 2 | Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 | http://code.google.com/p/ceres-solver/ 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | * Neither the name of Google Inc. nor the names of its contributors may be 14 | used to endorse or promote products derived from this software without 15 | specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/License.txt: -------------------------------------------------------------------------------- 1 | CSparse: a Concise Sparse matrix package. 2 | Copyright (c) 2006, Timothy A. Davis. 3 | http://www.cise.ufl.edu/research/sparse/CSparse 4 | 5 | -------------------------------------------------------------------------------- 6 | 7 | CSparse is free software; you can redistribute it and/or 8 | modify it under the terms of the GNU Lesser General Public 9 | License as published by the Free Software Foundation; either 10 | version 2.1 of the License, or (at your option) any later version. 11 | 12 | CSparse is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General Public 18 | License along with this Module; if not, write to the Free Software 19 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/README.txt: -------------------------------------------------------------------------------- 1 | CSparse/Source directory: primary ANSI C source code files for CSparse. 2 | All of these files are printed verbatim in the book. To compile the 3 | libcsparse.a C-callable library, just type "make" in this directory. 4 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_add.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = alpha*A + beta*B */ 3 | cs *cs_add (const cs *A, const cs *B, double alpha, double beta) 4 | { 5 | csi p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values ; 6 | double *x, *Bx, *Cx ; 7 | cs *C ; 8 | if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ; /* check inputs */ 9 | if (A->m != B->m || A->n != B->n) return (NULL) ; 10 | m = A->m ; anz = A->p [A->n] ; 11 | n = B->n ; Bp = B->p ; Bx = B->x ; bnz = Bp [n] ; 12 | w = cs_calloc (m, sizeof (csi)) ; /* get workspace */ 13 | values = (A->x != NULL) && (Bx != NULL) ; 14 | x = values ? cs_malloc (m, sizeof (double)) : NULL ; /* get workspace */ 15 | C = cs_spalloc (m, n, anz + bnz, values, 0) ; /* allocate result*/ 16 | if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ; 17 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 18 | for (j = 0 ; j < n ; j++) 19 | { 20 | Cp [j] = nz ; /* column j of C starts here */ 21 | nz = cs_scatter (A, j, alpha, w, x, j+1, C, nz) ; /* alpha*A(:,j)*/ 22 | nz = cs_scatter (B, j, beta, w, x, j+1, C, nz) ; /* beta*B(:,j) */ 23 | if (values) for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ; 24 | } 25 | Cp [n] = nz ; /* finalize the last column of C */ 26 | cs_sprealloc (C, 0) ; /* remove extra space from C */ 27 | return (cs_done (C, w, x, 1)) ; /* success; free workspace, return C */ 28 | } 29 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_api.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2012 Mark Pupilli 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | /*************************************************************************** 18 | * Description: import/export macros for creating DLLS with Microsoft 19 | * compiler. Any exported function needs to be declared with the 20 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 21 | * for each DLL (arrrrrgh!!!) 22 | * 23 | * 17 Jan 2012 24 | * Email: pupilli@cs.bris.ac.uk 25 | ****************************************************************************/ 26 | #ifndef G2O_CSPARSE_API_H 27 | #define G2O_CSPARSE_API_H 28 | 29 | #include "g2o/config.h" 30 | 31 | #ifdef _MSC_VER 32 | 33 | // We are using a Microsoft compiler: 34 | #ifdef G2O_LGPL_SHARED_LIBS 35 | # ifdef csparse_EXPORTS 36 | # define G2O_CSPARSE_API __declspec(dllexport) 37 | # else 38 | # define G2O_CSPARSE_API __declspec(dllimport) 39 | # endif 40 | #else 41 | #define G2O_CSPARSE_API 42 | #endif 43 | 44 | #else 45 | // Not Microsoft compiler so set empty definition: 46 | # define G2O_CSPARSE_API 47 | #endif 48 | 49 | #endif // G2O_CSPARSE_API_H 50 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_cholsol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x=A\b where A is symmetric positive definite; b overwritten with solution */ 3 | csi cs_cholsol (csi order, const cs *A, double *b) 4 | { 5 | double *x ; 6 | css *S ; 7 | csn *N ; 8 | csi n, ok ; 9 | if (!CS_CSC (A) || !b) return (0) ; /* check inputs */ 10 | n = A->n ; 11 | S = cs_schol (order, A) ; /* ordering and symbolic analysis */ 12 | N = cs_chol (A, S) ; /* numeric Cholesky factorization */ 13 | x = cs_malloc (n, sizeof (double)) ; /* get workspace */ 14 | ok = (S && N && x) ; 15 | if (ok) 16 | { 17 | cs_ipvec (S->pinv, b, x, n) ; /* x = P*b */ 18 | cs_lsolve (N->L, x) ; /* x = L\x */ 19 | cs_ltsolve (N->L, x) ; /* x = L'\x */ 20 | cs_pvec (S->pinv, x, b, n) ; /* b = P'*x */ 21 | } 22 | cs_free (x) ; 23 | cs_sfree (S) ; 24 | cs_nfree (N) ; 25 | return (ok) ; 26 | } 27 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_compress.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = compressed-column form of a triplet matrix T */ 3 | cs *cs_compress (const cs *T) 4 | { 5 | csi m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj ; 6 | double *Cx, *Tx ; 7 | cs *C ; 8 | if (!CS_TRIPLET (T)) return (NULL) ; /* check inputs */ 9 | m = T->m ; n = T->n ; Ti = T->i ; Tj = T->p ; Tx = T->x ; nz = T->nz ; 10 | C = cs_spalloc (m, n, nz, Tx != NULL, 0) ; /* allocate result */ 11 | w = cs_calloc (n, sizeof (csi)) ; /* get workspace */ 12 | if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */ 13 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 14 | for (k = 0 ; k < nz ; k++) w [Tj [k]]++ ; /* column counts */ 15 | cs_cumsum (Cp, w, n) ; /* column pointers */ 16 | for (k = 0 ; k < nz ; k++) 17 | { 18 | Ci [p = w [Tj [k]]++] = Ti [k] ; /* A(i,j) is the pth entry in C */ 19 | if (Cx) Cx [p] = Tx [k] ; 20 | } 21 | return (cs_done (C, w, NULL, 1)) ; /* success; free w and return C */ 22 | } 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_cumsum.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* p [0..n] = cumulative sum of c [0..n-1], and then copy p [0..n-1] into c */ 3 | double cs_cumsum (csi *p, csi *c, csi n) 4 | { 5 | csi i, nz = 0 ; 6 | double nz2 = 0 ; 7 | if (!p || !c) return (-1) ; /* check inputs */ 8 | for (i = 0 ; i < n ; i++) 9 | { 10 | p [i] = nz ; 11 | nz += c [i] ; 12 | nz2 += c [i] ; /* also in double to avoid csi overflow */ 13 | c [i] = p [i] ; /* also copy p[0..n-1] back into c[0..n-1]*/ 14 | } 15 | p [n] = nz ; 16 | return (nz2) ; /* return sum (c [0..n-1]) */ 17 | } 18 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_dfs.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* depth-first-search of the graph of a matrix, starting at node j */ 3 | csi cs_dfs (csi j, cs *G, csi top, csi *xi, csi *pstack, const csi *pinv) 4 | { 5 | csi i, p, p2, done, jnew, head = 0, *Gp, *Gi ; 6 | if (!CS_CSC (G) || !xi || !pstack) return (-1) ; /* check inputs */ 7 | Gp = G->p ; Gi = G->i ; 8 | xi [0] = j ; /* initialize the recursion stack */ 9 | while (head >= 0) 10 | { 11 | j = xi [head] ; /* get j from the top of the recursion stack */ 12 | jnew = pinv ? (pinv [j]) : j ; 13 | if (!CS_MARKED (Gp, j)) 14 | { 15 | CS_MARK (Gp, j) ; /* mark node j as visited */ 16 | pstack [head] = (jnew < 0) ? 0 : CS_UNFLIP (Gp [jnew]) ; 17 | } 18 | done = 1 ; /* node j done if no unvisited neighbors */ 19 | p2 = (jnew < 0) ? 0 : CS_UNFLIP (Gp [jnew+1]) ; 20 | for (p = pstack [head] ; p < p2 ; p++) /* examine all neighbors of j */ 21 | { 22 | i = Gi [p] ; /* consider neighbor node i */ 23 | if (CS_MARKED (Gp, i)) continue ; /* skip visited node i */ 24 | pstack [head] = p ; /* pause depth-first search of node j */ 25 | xi [++head] = i ; /* start dfs at node i */ 26 | done = 0 ; /* node j is not done */ 27 | break ; /* break, to start dfs (i) */ 28 | } 29 | if (done) /* depth-first search at node j is done */ 30 | { 31 | head-- ; /* remove j from the recursion stack */ 32 | xi [--top] = j ; /* and place in the output stack */ 33 | } 34 | } 35 | return (top) ; 36 | } 37 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_droptol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | static csi cs_tol (csi i, csi j, double aij, void *tol) 3 | { 4 | return (fabs (aij) > *((double *) tol)) ; 5 | } 6 | csi cs_droptol (cs *A, double tol) 7 | { 8 | return (cs_fkeep (A, &cs_tol, &tol)) ; /* keep all large entries */ 9 | } 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_dropzeros.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | static csi cs_nonzero (csi i, csi j, double aij, void *other) 3 | { 4 | return (aij != 0) ; 5 | } 6 | csi cs_dropzeros (cs *A) 7 | { 8 | return (cs_fkeep (A, &cs_nonzero, NULL)) ; /* keep all nonzero entries */ 9 | } 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_dupl.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* remove duplicate entries from A */ 3 | csi cs_dupl (cs *A) 4 | { 5 | csi i, j, p, q, nz = 0, n, m, *Ap, *Ai, *w ; 6 | double *Ax ; 7 | if (!CS_CSC (A)) return (0) ; /* check inputs */ 8 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | w = cs_malloc (m, sizeof (csi)) ; /* get workspace */ 10 | if (!w) return (0) ; /* out of memory */ 11 | for (i = 0 ; i < m ; i++) w [i] = -1 ; /* row i not yet seen */ 12 | for (j = 0 ; j < n ; j++) 13 | { 14 | q = nz ; /* column j will start at q */ 15 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 16 | { 17 | i = Ai [p] ; /* A(i,j) is nonzero */ 18 | if (w [i] >= q) 19 | { 20 | Ax [w [i]] += Ax [p] ; /* A(i,j) is a duplicate */ 21 | } 22 | else 23 | { 24 | w [i] = nz ; /* record where row i occurs */ 25 | Ai [nz] = i ; /* keep A(i,j) */ 26 | Ax [nz++] = Ax [p] ; 27 | } 28 | } 29 | Ap [j] = q ; /* record start of column j */ 30 | } 31 | Ap [n] = nz ; /* finalize A */ 32 | cs_free (w) ; /* free workspace */ 33 | return (cs_sprealloc (A, 0)) ; /* remove extra space from A */ 34 | } 35 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_entry.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* add an entry to a triplet matrix; return 1 if ok, 0 otherwise */ 3 | csi cs_entry (cs *T, csi i, csi j, double x) 4 | { 5 | if (!CS_TRIPLET (T) || i < 0 || j < 0) return (0) ; /* check inputs */ 6 | if (T->nz >= T->nzmax && !cs_sprealloc (T,2*(T->nzmax))) return (0) ; 7 | if (T->x) T->x [T->nz] = x ; 8 | T->i [T->nz] = i ; 9 | T->p [T->nz++] = j ; 10 | T->m = CS_MAX (T->m, i+1) ; 11 | T->n = CS_MAX (T->n, j+1) ; 12 | return (1) ; 13 | } 14 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_ereach.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* find nonzero pattern of Cholesky L(k,1:k-1) using etree and triu(A(:,k)) */ 3 | csi cs_ereach (const cs *A, csi k, const csi *parent, csi *s, csi *w) 4 | { 5 | csi i, p, n, len, top, *Ap, *Ai ; 6 | if (!CS_CSC (A) || !parent || !s || !w) return (-1) ; /* check inputs */ 7 | top = n = A->n ; Ap = A->p ; Ai = A->i ; 8 | CS_MARK (w, k) ; /* mark node k as visited */ 9 | for (p = Ap [k] ; p < Ap [k+1] ; p++) 10 | { 11 | i = Ai [p] ; /* A(i,k) is nonzero */ 12 | if (i > k) continue ; /* only use upper triangular part of A */ 13 | for (len = 0 ; !CS_MARKED (w,i) ; i = parent [i]) /* traverse up etree*/ 14 | { 15 | s [len++] = i ; /* L(k,i) is nonzero */ 16 | CS_MARK (w, i) ; /* mark i as visited */ 17 | } 18 | while (len > 0) s [--top] = s [--len] ; /* push path onto stack */ 19 | } 20 | for (p = top ; p < n ; p++) CS_MARK (w, s [p]) ; /* unmark all nodes */ 21 | CS_MARK (w, k) ; /* unmark node k */ 22 | return (top) ; /* s [top..n-1] contains pattern of L(k,:)*/ 23 | } 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_etree.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* compute the etree of A (using triu(A), or A'A without forming A'A */ 3 | csi *cs_etree (const cs *A, csi ata) 4 | { 5 | csi i, k, p, m, n, inext, *Ap, *Ai, *w, *parent, *ancestor, *prev ; 6 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 7 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; 8 | parent = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 9 | w = cs_malloc (n + (ata ? m : 0), sizeof (csi)) ; /* get workspace */ 10 | if (!w || !parent) return (cs_idone (parent, NULL, w, 0)) ; 11 | ancestor = w ; prev = w + n ; 12 | if (ata) for (i = 0 ; i < m ; i++) prev [i] = -1 ; 13 | for (k = 0 ; k < n ; k++) 14 | { 15 | parent [k] = -1 ; /* node k has no parent yet */ 16 | ancestor [k] = -1 ; /* nor does k have an ancestor */ 17 | for (p = Ap [k] ; p < Ap [k+1] ; p++) 18 | { 19 | i = ata ? (prev [Ai [p]]) : (Ai [p]) ; 20 | for ( ; i != -1 && i < k ; i = inext) /* traverse from i to k */ 21 | { 22 | inext = ancestor [i] ; /* inext = ancestor of i */ 23 | ancestor [i] = k ; /* path compression */ 24 | if (inext == -1) parent [i] = k ; /* no anc., parent is k */ 25 | } 26 | if (ata) prev [Ai [p]] = k ; 27 | } 28 | } 29 | return (cs_idone (parent, NULL, w, 1)) ; 30 | } 31 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_fkeep.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* drop entries for which fkeep(A(i,j)) is false; return nz if OK, else -1 */ 3 | csi cs_fkeep (cs *A, csi (*fkeep) (csi, csi, double, void *), void *other) 4 | { 5 | csi j, p, nz = 0, n, *Ap, *Ai ; 6 | double *Ax ; 7 | if (!CS_CSC (A) || !fkeep) return (-1) ; /* check inputs */ 8 | n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | p = Ap [j] ; /* get current location of col j */ 12 | Ap [j] = nz ; /* record new location of col j */ 13 | for ( ; p < Ap [j+1] ; p++) 14 | { 15 | if (fkeep (Ai [p], j, Ax ? Ax [p] : 1, other)) 16 | { 17 | if (Ax) Ax [nz] = Ax [p] ; /* keep A(i,j) */ 18 | Ai [nz++] = Ai [p] ; 19 | } 20 | } 21 | } 22 | Ap [n] = nz ; /* finalize A */ 23 | cs_sprealloc (A, 0) ; /* remove extra space from A */ 24 | return (nz) ; 25 | } 26 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_gaxpy.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* y = A*x+y */ 3 | csi cs_gaxpy (const cs *A, const double *x, double *y) 4 | { 5 | csi p, j, n, *Ap, *Ai ; 6 | double *Ax ; 7 | if (!CS_CSC (A) || !x || !y) return (0) ; /* check inputs */ 8 | n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 12 | { 13 | y [Ai [p]] += Ax [p] * x [j] ; 14 | } 15 | } 16 | return (1) ; 17 | } 18 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_happly.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* apply the ith Householder vector to x */ 3 | csi cs_happly (const cs *V, csi i, double beta, double *x) 4 | { 5 | csi p, *Vp, *Vi ; 6 | double *Vx, tau = 0 ; 7 | if (!CS_CSC (V) || !x) return (0) ; /* check inputs */ 8 | Vp = V->p ; Vi = V->i ; Vx = V->x ; 9 | for (p = Vp [i] ; p < Vp [i+1] ; p++) /* tau = v'*x */ 10 | { 11 | tau += Vx [p] * x [Vi [p]] ; 12 | } 13 | tau *= beta ; /* tau = beta*(v'*x) */ 14 | for (p = Vp [i] ; p < Vp [i+1] ; p++) /* x = x - v*tau */ 15 | { 16 | x [Vi [p]] -= Vx [p] * tau ; 17 | } 18 | return (1) ; 19 | } 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_house.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* create a Householder reflection [v,beta,s]=house(x), overwrite x with v, 3 | * where (I-beta*v*v')*x = s*e1. See Algo 5.1.1, Golub & Van Loan, 3rd ed. */ 4 | double cs_house (double *x, double *beta, csi n) 5 | { 6 | double s, sigma = 0 ; 7 | csi i ; 8 | if (!x || !beta) return (-1) ; /* check inputs */ 9 | for (i = 1 ; i < n ; i++) sigma += x [i] * x [i] ; 10 | if (sigma == 0) 11 | { 12 | s = fabs (x [0]) ; /* s = |x(0)| */ 13 | (*beta) = (x [0] <= 0) ? 2 : 0 ; 14 | x [0] = 1 ; 15 | } 16 | else 17 | { 18 | s = sqrt (x [0] * x [0] + sigma) ; /* s = norm (x) */ 19 | x [0] = (x [0] <= 0) ? (x [0] - s) : (-sigma / (x [0] + s)) ; 20 | (*beta) = -1. / (s * x [0]) ; 21 | } 22 | return (s) ; 23 | } 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_ipvec.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x(p) = b, for dense vectors x and b; p=NULL denotes identity */ 3 | csi cs_ipvec (const csi *p, const double *b, double *x, csi n) 4 | { 5 | csi k ; 6 | if (!x || !b) return (0) ; /* check inputs */ 7 | for (k = 0 ; k < n ; k++) x [p ? p [k] : k] = b [k] ; 8 | return (1) ; 9 | } 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_leaf.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* consider A(i,j), node j in ith row subtree and return lca(jprev,j) */ 3 | csi cs_leaf (csi i, csi j, const csi *first, csi *maxfirst, csi *prevleaf, 4 | csi *ancestor, csi *jleaf) 5 | { 6 | csi q, s, sparent, jprev ; 7 | if (!first || !maxfirst || !prevleaf || !ancestor || !jleaf) return (-1) ; 8 | *jleaf = 0 ; 9 | if (i <= j || first [j] <= maxfirst [i]) return (-1) ; /* j not a leaf */ 10 | maxfirst [i] = first [j] ; /* update max first[j] seen so far */ 11 | jprev = prevleaf [i] ; /* jprev = previous leaf of ith subtree */ 12 | prevleaf [i] = j ; 13 | *jleaf = (jprev == -1) ? 1: 2 ; /* j is first or subsequent leaf */ 14 | if (*jleaf == 1) return (i) ; /* if 1st leaf, q = root of ith subtree */ 15 | for (q = jprev ; q != ancestor [q] ; q = ancestor [q]) ; 16 | for (s = jprev ; s != q ; s = sparent) 17 | { 18 | sparent = ancestor [s] ; /* path compression */ 19 | ancestor [s] = q ; 20 | } 21 | return (q) ; /* q = least common ancester (jprev,j) */ 22 | } 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_load.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* load a triplet matrix from a file */ 3 | cs *cs_load (FILE *f) 4 | { 5 | double i, j ; /* use double for integers to avoid csi conflicts */ 6 | double x ; 7 | cs *T ; 8 | if (!f) return (NULL) ; /* check inputs */ 9 | T = cs_spalloc (0, 0, 1, 1, 1) ; /* allocate result */ 10 | while (fscanf (f, "%lg %lg %lg\n", &i, &j, &x) == 3) 11 | { 12 | if (!cs_entry (T, (csi) i, (csi) j, x)) return (cs_spfree (T)) ; 13 | } 14 | return (T) ; 15 | } 16 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_lsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve Lx=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_lsolve (const cs *L, double *x) 4 | { 5 | csi p, j, n, *Lp, *Li ; 6 | double *Lx ; 7 | if (!CS_CSC (L) || !x) return (0) ; /* check inputs */ 8 | n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | x [j] /= Lx [Lp [j]] ; 12 | for (p = Lp [j]+1 ; p < Lp [j+1] ; p++) 13 | { 14 | x [Li [p]] -= Lx [p] * x [j] ; 15 | } 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_ltsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve L'x=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_ltsolve (const cs *L, double *x) 4 | { 5 | csi p, j, n, *Lp, *Li ; 6 | double *Lx ; 7 | if (!CS_CSC (L) || !x) return (0) ; /* check inputs */ 8 | n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ; 9 | for (j = n-1 ; j >= 0 ; j--) 10 | { 11 | for (p = Lp [j]+1 ; p < Lp [j+1] ; p++) 12 | { 13 | x [j] -= Lx [p] * x [Li [p]] ; 14 | } 15 | x [j] /= Lx [Lp [j]] ; 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_lusol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x=A\b where A is unsymmetric; b overwritten with solution */ 3 | csi cs_lusol (csi order, const cs *A, double *b, double tol) 4 | { 5 | double *x ; 6 | css *S ; 7 | csn *N ; 8 | csi n, ok ; 9 | if (!CS_CSC (A) || !b) return (0) ; /* check inputs */ 10 | n = A->n ; 11 | S = cs_sqr (order, A, 0) ; /* ordering and symbolic analysis */ 12 | N = cs_lu (A, S, tol) ; /* numeric LU factorization */ 13 | x = cs_malloc (n, sizeof (double)) ; /* get workspace */ 14 | ok = (S && N && x) ; 15 | if (ok) 16 | { 17 | cs_ipvec (N->pinv, b, x, n) ; /* x = b(p) */ 18 | cs_lsolve (N->L, x) ; /* x = L\x */ 19 | cs_usolve (N->U, x) ; /* x = U\x */ 20 | cs_ipvec (S->q, x, b, n) ; /* b(q) = x */ 21 | } 22 | cs_free (x) ; 23 | cs_sfree (S) ; 24 | cs_nfree (N) ; 25 | return (ok) ; 26 | } 27 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_malloc.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | #ifdef MATLAB_MEX_FILE 3 | #define malloc mxMalloc 4 | #define free mxFree 5 | #define realloc mxRealloc 6 | #define calloc mxCalloc 7 | #endif 8 | 9 | /* wrapper for malloc */ 10 | void *cs_malloc (csi n, size_t size) 11 | { 12 | return (malloc (CS_MAX (n,1) * size)) ; 13 | } 14 | 15 | /* wrapper for calloc */ 16 | void *cs_calloc (csi n, size_t size) 17 | { 18 | return (calloc (CS_MAX (n,1), size)) ; 19 | } 20 | 21 | /* wrapper for free */ 22 | void *cs_free (void *p) 23 | { 24 | if (p) free (p) ; /* free p if it is not already NULL */ 25 | return (NULL) ; /* return NULL to simplify the use of cs_free */ 26 | } 27 | 28 | /* wrapper for realloc */ 29 | void *cs_realloc (void *p, csi n, size_t size, csi *ok) 30 | { 31 | void *pnew ; 32 | pnew = realloc (p, CS_MAX (n,1) * size) ; /* realloc the block */ 33 | *ok = (pnew != NULL) ; /* realloc fails if pnew is NULL */ 34 | return ((*ok) ? pnew : p) ; /* return original p if failure */ 35 | } 36 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_multiply.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A*B */ 3 | cs *cs_multiply (const cs *A, const cs *B) 4 | { 5 | csi p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values, *Bi ; 6 | double *x, *Bx, *Cx ; 7 | cs *C ; 8 | if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ; /* check inputs */ 9 | if (A->n != B->m) return (NULL) ; 10 | m = A->m ; anz = A->p [A->n] ; 11 | n = B->n ; Bp = B->p ; Bi = B->i ; Bx = B->x ; bnz = Bp [n] ; 12 | w = cs_calloc (m, sizeof (csi)) ; /* get workspace */ 13 | values = (A->x != NULL) && (Bx != NULL) ; 14 | x = values ? cs_malloc (m, sizeof (double)) : NULL ; /* get workspace */ 15 | C = cs_spalloc (m, n, anz + bnz, values, 0) ; /* allocate result */ 16 | if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ; 17 | Cp = C->p ; 18 | for (j = 0 ; j < n ; j++) 19 | { 20 | if (nz + m > C->nzmax && !cs_sprealloc (C, 2*(C->nzmax)+m)) 21 | { 22 | return (cs_done (C, w, x, 0)) ; /* out of memory */ 23 | } 24 | Ci = C->i ; Cx = C->x ; /* C->i and C->x may be reallocated */ 25 | Cp [j] = nz ; /* column j of C starts here */ 26 | for (p = Bp [j] ; p < Bp [j+1] ; p++) 27 | { 28 | nz = cs_scatter (A, Bi [p], Bx ? Bx [p] : 1, w, x, j+1, C, nz) ; 29 | } 30 | if (values) for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ; 31 | } 32 | Cp [n] = nz ; /* finalize the last column of C */ 33 | cs_sprealloc (C, 0) ; /* remove extra space from C */ 34 | return (cs_done (C, w, x, 1)) ; /* success; free workspace, return C */ 35 | } 36 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_norm.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* 1-norm of a sparse matrix = max (sum (abs (A))), largest column sum */ 3 | double cs_norm (const cs *A) 4 | { 5 | csi p, j, n, *Ap ; 6 | double *Ax, norm = 0, s ; 7 | if (!CS_CSC (A) || !A->x) return (-1) ; /* check inputs */ 8 | n = A->n ; Ap = A->p ; Ax = A->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | for (s = 0, p = Ap [j] ; p < Ap [j+1] ; p++) s += fabs (Ax [p]) ; 12 | norm = CS_MAX (norm, s) ; 13 | } 14 | return (norm) ; 15 | } 16 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_permute.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A(p,q) where p and q are permutations of 0..m-1 and 0..n-1. */ 3 | cs *cs_permute (const cs *A, const csi *pinv, const csi *q, csi values) 4 | { 5 | csi t, j, k, nz = 0, m, n, *Ap, *Ai, *Cp, *Ci ; 6 | double *Cx, *Ax ; 7 | cs *C ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 10 | C = cs_spalloc (m, n, Ap [n], values && Ax != NULL, 0) ; /* alloc result */ 11 | if (!C) return (cs_done (C, NULL, NULL, 0)) ; /* out of memory */ 12 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 13 | for (k = 0 ; k < n ; k++) 14 | { 15 | Cp [k] = nz ; /* column k of C is column q[k] of A */ 16 | j = q ? (q [k]) : k ; 17 | for (t = Ap [j] ; t < Ap [j+1] ; t++) 18 | { 19 | if (Cx) Cx [nz] = Ax [t] ; /* row i of A is row pinv[i] of C */ 20 | Ci [nz++] = pinv ? (pinv [Ai [t]]) : Ai [t] ; 21 | } 22 | } 23 | Cp [n] = nz ; /* finalize the last column of C */ 24 | return (cs_done (C, NULL, NULL, 1)) ; 25 | } 26 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_pinv.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* pinv = p', or p = pinv' */ 3 | csi *cs_pinv (csi const *p, csi n) 4 | { 5 | csi k, *pinv ; 6 | if (!p) return (NULL) ; /* p = NULL denotes identity */ 7 | pinv = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 8 | if (!pinv) return (NULL) ; /* out of memory */ 9 | for (k = 0 ; k < n ; k++) pinv [p [k]] = k ;/* invert the permutation */ 10 | return (pinv) ; /* return result */ 11 | } 12 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_post.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* post order a forest */ 3 | csi *cs_post (const csi *parent, csi n) 4 | { 5 | csi j, k = 0, *post, *w, *head, *next, *stack ; 6 | if (!parent) return (NULL) ; /* check inputs */ 7 | post = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 8 | w = cs_malloc (3*n, sizeof (csi)) ; /* get workspace */ 9 | if (!w || !post) return (cs_idone (post, NULL, w, 0)) ; 10 | head = w ; next = w + n ; stack = w + 2*n ; 11 | for (j = 0 ; j < n ; j++) head [j] = -1 ; /* empty linked lists */ 12 | for (j = n-1 ; j >= 0 ; j--) /* traverse nodes in reverse order*/ 13 | { 14 | if (parent [j] == -1) continue ; /* j is a root */ 15 | next [j] = head [parent [j]] ; /* add j to list of its parent */ 16 | head [parent [j]] = j ; 17 | } 18 | for (j = 0 ; j < n ; j++) 19 | { 20 | if (parent [j] != -1) continue ; /* skip j if it is not a root */ 21 | k = cs_tdfs (j, k, head, next, post, stack) ; 22 | } 23 | return (cs_idone (post, NULL, w, 1)) ; /* success; free w, return post */ 24 | } 25 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_print.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* print a sparse matrix; use %g for integers to avoid differences with csi */ 3 | csi cs_print (const cs *A, csi brief) 4 | { 5 | csi p, j, m, n, nzmax, nz, *Ap, *Ai ; 6 | double *Ax ; 7 | if (!A) { printf ("(null)\n") ; return (0) ; } 8 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | nzmax = A->nzmax ; nz = A->nz ; 10 | printf ("CSparse Version %d.%d.%d, %s. %s\n", CS_VER, CS_SUBVER, 11 | CS_SUBSUB, CS_DATE, CS_COPYRIGHT) ; 12 | if (nz < 0) 13 | { 14 | printf ("%g-by-%g, nzmax: %g nnz: %g, 1-norm: %g\n", (double) m, 15 | (double) n, (double) nzmax, (double) (Ap [n]), cs_norm (A)) ; 16 | for (j = 0 ; j < n ; j++) 17 | { 18 | printf (" col %g : locations %g to %g\n", (double) j, 19 | (double) (Ap [j]), (double) (Ap [j+1]-1)) ; 20 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 21 | { 22 | printf (" %g : %g\n", (double) (Ai [p]), Ax ? Ax [p] : 1) ; 23 | if (brief && p > 20) { printf (" ...\n") ; return (1) ; } 24 | } 25 | } 26 | } 27 | else 28 | { 29 | printf ("triplet: %g-by-%g, nzmax: %g nnz: %g\n", (double) m, 30 | (double) n, (double) nzmax, (double) nz) ; 31 | for (p = 0 ; p < nz ; p++) 32 | { 33 | printf (" %g %g : %g\n", (double) (Ai [p]), (double) (Ap [p]), 34 | Ax ? Ax [p] : 1) ; 35 | if (brief && p > 20) { printf (" ...\n") ; return (1) ; } 36 | } 37 | } 38 | return (1) ; 39 | } 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_pvec.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x = b(p), for dense vectors x and b; p=NULL denotes identity */ 3 | csi cs_pvec (const csi *p, const double *b, double *x, csi n) 4 | { 5 | csi k ; 6 | if (!x || !b) return (0) ; /* check inputs */ 7 | for (k = 0 ; k < n ; k++) x [k] = b [p ? p [k] : k] ; 8 | return (1) ; 9 | } 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_randperm.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* return a random permutation vector, the identity perm, or p = n-1:-1:0. 3 | * seed = -1 means p = n-1:-1:0. seed = 0 means p = identity. otherwise 4 | * p = random permutation. */ 5 | csi *cs_randperm (csi n, csi seed) 6 | { 7 | csi *p, k, j, t ; 8 | if (seed == 0) return (NULL) ; /* return p = NULL (identity) */ 9 | p = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 10 | if (!p) return (NULL) ; /* out of memory */ 11 | for (k = 0 ; k < n ; k++) p [k] = n-k-1 ; 12 | if (seed == -1) return (p) ; /* return reverse permutation */ 13 | srand (seed) ; /* get new random number seed */ 14 | for (k = 0 ; k < n ; k++) 15 | { 16 | j = k + (rand ( ) % (n-k)) ; /* j = rand integer in range k to n-1 */ 17 | t = p [j] ; /* swap p[k] and p[j] */ 18 | p [j] = p [k] ; 19 | p [k] = t ; 20 | } 21 | return (p) ; 22 | } 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_reach.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* xi [top...n-1] = nodes reachable from graph of G*P' via nodes in B(:,k). 3 | * xi [n...2n-1] used as workspace */ 4 | csi cs_reach (cs *G, const cs *B, csi k, csi *xi, const csi *pinv) 5 | { 6 | csi p, n, top, *Bp, *Bi, *Gp ; 7 | if (!CS_CSC (G) || !CS_CSC (B) || !xi) return (-1) ; /* check inputs */ 8 | n = G->n ; Bp = B->p ; Bi = B->i ; Gp = G->p ; 9 | top = n ; 10 | for (p = Bp [k] ; p < Bp [k+1] ; p++) 11 | { 12 | if (!CS_MARKED (Gp, Bi [p])) /* start a dfs at unmarked node i */ 13 | { 14 | top = cs_dfs (Bi [p], G, top, xi, xi+n, pinv) ; 15 | } 16 | } 17 | for (p = top ; p < n ; p++) CS_MARK (Gp, xi [p]) ; /* restore G */ 18 | return (top) ; 19 | } 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_scatter.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x = x + beta * A(:,j), where x is a dense vector and A(:,j) is sparse */ 3 | csi cs_scatter (const cs *A, csi j, double beta, csi *w, double *x, csi mark, 4 | cs *C, csi nz) 5 | { 6 | csi i, p, *Ap, *Ai, *Ci ; 7 | double *Ax ; 8 | if (!CS_CSC (A) || !w || !CS_CSC (C)) return (-1) ; /* check inputs */ 9 | Ap = A->p ; Ai = A->i ; Ax = A->x ; Ci = C->i ; 10 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 11 | { 12 | i = Ai [p] ; /* A(i,j) is nonzero */ 13 | if (w [i] < mark) 14 | { 15 | w [i] = mark ; /* i is new entry in column j */ 16 | Ci [nz++] = i ; /* add i to pattern of C(:,j) */ 17 | if (x) x [i] = beta * Ax [p] ; /* x(i) = beta*A(i,j) */ 18 | } 19 | else if (x) x [i] += beta * Ax [p] ; /* i exists in C(:,j) already */ 20 | } 21 | return (nz) ; 22 | } 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_scc.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* find the strongly connected components of a square matrix */ 3 | csd *cs_scc (cs *A) /* matrix A temporarily modified, then restored */ 4 | { 5 | csi n, i, k, b, nb = 0, top, *xi, *pstack, *p, *r, *Ap, *ATp, *rcopy, *Blk ; 6 | cs *AT ; 7 | csd *D ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | n = A->n ; Ap = A->p ; 10 | D = cs_dalloc (n, 0) ; /* allocate result */ 11 | AT = cs_transpose (A, 0) ; /* AT = A' */ 12 | xi = cs_malloc (2*n+1, sizeof (csi)) ; /* get workspace */ 13 | if (!D || !AT || !xi) return (cs_ddone (D, AT, xi, 0)) ; 14 | Blk = xi ; rcopy = pstack = xi + n ; 15 | p = D->p ; r = D->r ; ATp = AT->p ; 16 | top = n ; 17 | for (i = 0 ; i < n ; i++) /* first dfs(A) to find finish times (xi) */ 18 | { 19 | if (!CS_MARKED (Ap, i)) top = cs_dfs (i, A, top, xi, pstack, NULL) ; 20 | } 21 | for (i = 0 ; i < n ; i++) CS_MARK (Ap, i) ; /* restore A; unmark all nodes*/ 22 | top = n ; 23 | nb = n ; 24 | for (k = 0 ; k < n ; k++) /* dfs(A') to find strongly connnected comp */ 25 | { 26 | i = xi [k] ; /* get i in reverse order of finish times */ 27 | if (CS_MARKED (ATp, i)) continue ; /* skip node i if already ordered */ 28 | r [nb--] = top ; /* node i is the start of a component in p */ 29 | top = cs_dfs (i, AT, top, p, pstack, NULL) ; 30 | } 31 | r [nb] = 0 ; /* first block starts at zero; shift r up */ 32 | for (k = nb ; k <= n ; k++) r [k-nb] = r [k] ; 33 | D->nb = nb = n-nb ; /* nb = # of strongly connected components */ 34 | for (b = 0 ; b < nb ; b++) /* sort each block in natural order */ 35 | { 36 | for (k = r [b] ; k < r [b+1] ; k++) Blk [p [k]] = b ; 37 | } 38 | for (b = 0 ; b <= nb ; b++) rcopy [b] = r [b] ; 39 | for (i = 0 ; i < n ; i++) p [rcopy [Blk [i]]++] = i ; 40 | return (cs_ddone (D, AT, xi, 1)) ; 41 | } 42 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_schol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* ordering and symbolic analysis for a Cholesky factorization */ 3 | css *cs_schol (csi order, const cs *A) 4 | { 5 | csi n, *c, *post, *P ; 6 | cs *C ; 7 | css *S ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | n = A->n ; 10 | S = cs_calloc (1, sizeof (css)) ; /* allocate result S */ 11 | if (!S) return (NULL) ; /* out of memory */ 12 | P = cs_amd (order, A) ; /* P = amd(A+A'), or natural */ 13 | S->pinv = cs_pinv (P, n) ; /* find inverse permutation */ 14 | cs_free (P) ; 15 | if (order && !S->pinv) return (cs_sfree (S)) ; 16 | C = cs_symperm (A, S->pinv, 0) ; /* C = spones(triu(A(P,P))) */ 17 | S->parent = cs_etree (C, 0) ; /* find etree of C */ 18 | post = cs_post (S->parent, n) ; /* postorder the etree */ 19 | c = cs_counts (C, S->parent, post, 0) ; /* find column counts of chol(C) */ 20 | cs_free (post) ; 21 | cs_spfree (C) ; 22 | S->cp = cs_malloc (n+1, sizeof (csi)) ; /* allocate result S->cp */ 23 | S->unz = S->lnz = cs_cumsum (S->cp, c, n) ; /* find column pointers for L */ 24 | cs_free (c) ; 25 | return ((S->lnz >= 0) ? S : cs_sfree (S)) ; 26 | } 27 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_spsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve Gx=b(:,k), where G is either upper (lo=0) or lower (lo=1) triangular */ 3 | csi cs_spsolve (cs *G, const cs *B, csi k, csi *xi, double *x, const csi *pinv, 4 | csi lo) 5 | { 6 | csi j, J, p, q, px, top, n, *Gp, *Gi, *Bp, *Bi ; 7 | double *Gx, *Bx ; 8 | if (!CS_CSC (G) || !CS_CSC (B) || !xi || !x) return (-1) ; 9 | Gp = G->p ; Gi = G->i ; Gx = G->x ; n = G->n ; 10 | Bp = B->p ; Bi = B->i ; Bx = B->x ; 11 | top = cs_reach (G, B, k, xi, pinv) ; /* xi[top..n-1]=Reach(B(:,k)) */ 12 | for (p = top ; p < n ; p++) x [xi [p]] = 0 ; /* clear x */ 13 | for (p = Bp [k] ; p < Bp [k+1] ; p++) x [Bi [p]] = Bx [p] ; /* scatter B */ 14 | for (px = top ; px < n ; px++) 15 | { 16 | j = xi [px] ; /* x(j) is nonzero */ 17 | J = pinv ? (pinv [j]) : j ; /* j maps to col J of G */ 18 | if (J < 0) continue ; /* column J is empty */ 19 | x [j] /= Gx [lo ? (Gp [J]) : (Gp [J+1]-1)] ;/* x(j) /= G(j,j) */ 20 | p = lo ? (Gp [J]+1) : (Gp [J]) ; /* lo: L(j,j) 1st entry */ 21 | q = lo ? (Gp [J+1]) : (Gp [J+1]-1) ; /* up: U(j,j) last entry */ 22 | for ( ; p < q ; p++) 23 | { 24 | x [Gi [p]] -= Gx [p] * x [j] ; /* x(i) -= G(i,j) * x(j) */ 25 | } 26 | } 27 | return (top) ; /* return top of stack */ 28 | } 29 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_symperm.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A(p,p) where A and C are symmetric the upper part stored; pinv not p */ 3 | cs *cs_symperm (const cs *A, const csi *pinv, csi values) 4 | { 5 | csi i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w ; 6 | double *Cx, *Ax ; 7 | cs *C ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 10 | C = cs_spalloc (n, n, Ap [n], values && (Ax != NULL), 0) ; /* alloc result*/ 11 | w = cs_calloc (n, sizeof (csi)) ; /* get workspace */ 12 | if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */ 13 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 14 | for (j = 0 ; j < n ; j++) /* count entries in each column of C */ 15 | { 16 | j2 = pinv ? pinv [j] : j ; /* column j of A is column j2 of C */ 17 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 18 | { 19 | i = Ai [p] ; 20 | if (i > j) continue ; /* skip lower triangular part of A */ 21 | i2 = pinv ? pinv [i] : i ; /* row i of A is row i2 of C */ 22 | w [CS_MAX (i2, j2)]++ ; /* column count of C */ 23 | } 24 | } 25 | cs_cumsum (Cp, w, n) ; /* compute column pointers of C */ 26 | for (j = 0 ; j < n ; j++) 27 | { 28 | j2 = pinv ? pinv [j] : j ; /* column j of A is column j2 of C */ 29 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 30 | { 31 | i = Ai [p] ; 32 | if (i > j) continue ; /* skip lower triangular part of A*/ 33 | i2 = pinv ? pinv [i] : i ; /* row i of A is row i2 of C */ 34 | Ci [q = w [CS_MAX (i2, j2)]++] = CS_MIN (i2, j2) ; 35 | if (Cx) Cx [q] = Ax [p] ; 36 | } 37 | } 38 | return (cs_done (C, w, NULL, 1)) ; /* success; free workspace, return C */ 39 | } 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_tdfs.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* depth-first search and postorder of a tree rooted at node j */ 3 | csi cs_tdfs (csi j, csi k, csi *head, const csi *next, csi *post, csi *stack) 4 | { 5 | csi i, p, top = 0 ; 6 | if (!head || !next || !post || !stack) return (-1) ; /* check inputs */ 7 | stack [0] = j ; /* place j on the stack */ 8 | while (top >= 0) /* while (stack is not empty) */ 9 | { 10 | p = stack [top] ; /* p = top of stack */ 11 | i = head [p] ; /* i = youngest child of p */ 12 | if (i == -1) 13 | { 14 | top-- ; /* p has no unordered children left */ 15 | post [k++] = p ; /* node p is the kth postordered node */ 16 | } 17 | else 18 | { 19 | head [p] = next [i] ; /* remove i from children of p */ 20 | stack [++top] = i ; /* start dfs on child node i */ 21 | } 22 | } 23 | return (k) ; 24 | } 25 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_transpose.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A' */ 3 | cs *cs_transpose (const cs *A, csi values) 4 | { 5 | csi p, q, j, *Cp, *Ci, n, m, *Ap, *Ai, *w ; 6 | double *Cx, *Ax ; 7 | cs *C ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 10 | C = cs_spalloc (n, m, Ap [n], values && Ax, 0) ; /* allocate result */ 11 | w = cs_calloc (m, sizeof (csi)) ; /* get workspace */ 12 | if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */ 13 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 14 | for (p = 0 ; p < Ap [n] ; p++) w [Ai [p]]++ ; /* row counts */ 15 | cs_cumsum (Cp, w, m) ; /* row pointers */ 16 | for (j = 0 ; j < n ; j++) 17 | { 18 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 19 | { 20 | Ci [q = w [Ai [p]]++] = j ; /* place A(i,j) as entry C(j,i) */ 21 | if (Cx) Cx [q] = Ax [p] ; 22 | } 23 | } 24 | return (cs_done (C, w, NULL, 1)) ; /* success; free w and return C */ 25 | } 26 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_updown.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* sparse Cholesky update/downdate, L*L' + sigma*w*w' (sigma = +1 or -1) */ 3 | csi cs_updown (cs *L, csi sigma, const cs *C, const csi *parent) 4 | { 5 | csi n, p, f, j, *Lp, *Li, *Cp, *Ci ; 6 | double *Lx, *Cx, alpha, beta = 1, delta, gamma, w1, w2, *w, beta2 = 1 ; 7 | if (!CS_CSC (L) || !CS_CSC (C) || !parent) return (0) ; /* check inputs */ 8 | Lp = L->p ; Li = L->i ; Lx = L->x ; n = L->n ; 9 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 10 | if ((p = Cp [0]) >= Cp [1]) return (1) ; /* return if C empty */ 11 | w = cs_malloc (n, sizeof (double)) ; /* get workspace */ 12 | if (!w) return (0) ; /* out of memory */ 13 | f = Ci [p] ; 14 | for ( ; p < Cp [1] ; p++) f = CS_MIN (f, Ci [p]) ; /* f = min (find (C)) */ 15 | for (j = f ; j != -1 ; j = parent [j]) w [j] = 0 ; /* clear workspace w */ 16 | for (p = Cp [0] ; p < Cp [1] ; p++) w [Ci [p]] = Cx [p] ; /* w = C */ 17 | for (j = f ; j != -1 ; j = parent [j]) /* walk path f up to root */ 18 | { 19 | p = Lp [j] ; 20 | alpha = w [j] / Lx [p] ; /* alpha = w(j) / L(j,j) */ 21 | beta2 = beta*beta + sigma*alpha*alpha ; 22 | if (beta2 <= 0) break ; /* not positive definite */ 23 | beta2 = sqrt (beta2) ; 24 | delta = (sigma > 0) ? (beta / beta2) : (beta2 / beta) ; 25 | gamma = sigma * alpha / (beta2 * beta) ; 26 | Lx [p] = delta * Lx [p] + ((sigma > 0) ? (gamma * w [j]) : 0) ; 27 | beta = beta2 ; 28 | for (p++ ; p < Lp [j+1] ; p++) 29 | { 30 | w1 = w [Li [p]] ; 31 | w [Li [p]] = w2 = w1 - alpha * Lx [p] ; 32 | Lx [p] = delta * Lx [p] + gamma * ((sigma > 0) ? w1 : w2) ; 33 | } 34 | } 35 | cs_free (w) ; 36 | return (beta2 > 0) ; 37 | } 38 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_usolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve Ux=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_usolve (const cs *U, double *x) 4 | { 5 | csi p, j, n, *Up, *Ui ; 6 | double *Ux ; 7 | if (!CS_CSC (U) || !x) return (0) ; /* check inputs */ 8 | n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ; 9 | for (j = n-1 ; j >= 0 ; j--) 10 | { 11 | x [j] /= Ux [Up [j+1]-1] ; 12 | for (p = Up [j] ; p < Up [j+1]-1 ; p++) 13 | { 14 | x [Ui [p]] -= Ux [p] * x [j] ; 15 | } 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/csparse/cs_utsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve U'x=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_utsolve (const cs *U, double *x) 4 | { 5 | csi p, j, n, *Up, *Ui ; 6 | double *Ux ; 7 | if (!CS_CSC (U) || !x) return (0) ; /* check inputs */ 8 | n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | for (p = Up [j] ; p < Up [j+1]-1 ; p++) 12 | { 13 | x [j] -= Ux [p] * x [Ui [p]] ; 14 | } 15 | x [j] /= Ux [Up [j+1]-1] ; 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/freeglut/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(freeglut_minimal ${G2O_LIB_TYPE} 2 | freeglut_font.cpp 3 | freeglut_stroke_mono_roman.cpp 4 | freeglut_stroke_roman.cpp 5 | ) 6 | 7 | TARGET_LINK_LIBRARIES(freeglut_minimal ${OPENGL_gl_LIBRARY}) 8 | 9 | SET_TARGET_PROPERTIES(freeglut_minimal PROPERTIES OUTPUT_NAME ${LIB_PREFIX}ext_freeglut_minimal) 10 | 11 | INSTALL(TARGETS freeglut_minimal 12 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 13 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 14 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 15 | ) 16 | 17 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 18 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/EXTERNAL/freeglut) 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/EXTERNAL/freeglut/COPYING: -------------------------------------------------------------------------------- 1 | 2 | Freeglut Copyright 3 | ------------------ 4 | 5 | Freeglut code without an explicit copyright is covered by the following 6 | copyright: 7 | 8 | Copyright (c) 1999-2000 Pawel W. Olszta. All Rights Reserved. 9 | Permission is hereby granted, free of charge, to any person obtaining a copy 10 | of this software and associated documentation files (the "Software"), to deal 11 | in the Software without restriction, including without limitation the rights 12 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | copies or substantial portions of the Software. 14 | 15 | The above copyright notice and this permission notice shall be included in 16 | all copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 21 | PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 22 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 23 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24 | 25 | Except as contained in this notice, the name of Pawel W. Olszta shall not be 26 | used in advertising or otherwise to promote the sale, use or other dealings 27 | in this Software without prior written authorization from Pawel W. Olszta. 28 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/cmake_modules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | ) 14 | 15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 16 | PATHS 17 | /usr/lib 18 | /usr/local/lib 19 | /opt/local/lib 20 | /sw/lib 21 | ) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 25 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 26 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/cmake_modules/FindQGLViewer.cmake: -------------------------------------------------------------------------------- 1 | # Need to find both Qt{4,5} and QGLViewer if the QQL support is to be built 2 | FIND_PACKAGE(Qt4 COMPONENTS QtCore QtXml QtOpenGL QtGui) 3 | IF(NOT Qt4_FOUND) 4 | FIND_PACKAGE(Qt5 QUIET COMPONENTS Core Xml OpenGL Gui Widgets) 5 | IF(NOT Qt4_FOUND AND NOT Qt5_FOUND) 6 | MESSAGE("Qt{4,5} not found. Install it and set Qt{4,5}_DIR accordingly") 7 | IF (WIN32) 8 | MESSAGE(" In Windows, Qt5_DIR should be something like C:/Qt/5.4/msvc2013_64_opengl/lib/cmake/Qt5") 9 | ENDIF() 10 | ENDIF() 11 | ENDIF() 12 | 13 | FIND_PATH(QGLVIEWER_INCLUDE_DIR qglviewer.h 14 | /usr/include/QGLViewer 15 | /opt/local/include/QGLViewer 16 | /usr/local/include/QGLViewer 17 | /sw/include/QGLViewer 18 | ENV QGLVIEWERROOT 19 | ) 20 | 21 | find_library(QGLVIEWER_LIBRARY_RELEASE 22 | NAMES qglviewer-qt4 qglviewer QGLViewer QGLViewer2 23 | PATHS /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ENV QGLVIEWERROOT 28 | ENV LD_LIBRARY_PATH 29 | ENV LIBRARY_PATH 30 | PATH_SUFFIXES QGLViewer QGLViewer/release 31 | ) 32 | find_library(QGLVIEWER_LIBRARY_DEBUG 33 | NAMES dqglviewer dQGLViewer dQGLViewer2 QGLViewerd2 34 | PATHS /usr/lib 35 | /usr/local/lib 36 | /opt/local/lib 37 | /sw/lib 38 | ENV QGLVIEWERROOT 39 | ENV LD_LIBRARY_PATH 40 | ENV LIBRARY_PATH 41 | PATH_SUFFIXES QGLViewer QGLViewer/release 42 | ) 43 | 44 | if(QGLVIEWER_LIBRARY_RELEASE) 45 | if(QGLVIEWER_LIBRARY_DEBUG) 46 | set(QGLVIEWER_LIBRARY optimized ${QGLVIEWER_LIBRARY_RELEASE} debug ${QGLVIEWER_LIBRARY_DEBUG}) 47 | else() 48 | set(QGLVIEWER_LIBRARY ${QGLVIEWER_LIBRARY_RELEASE}) 49 | endif() 50 | endif() 51 | 52 | include(FindPackageHandleStandardArgs) 53 | find_package_handle_standard_args(QGLVIEWER DEFAULT_MSG 54 | QGLVIEWER_INCLUDE_DIR QGLVIEWER_LIBRARY) 55 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | 5 | #cmakedefine G2O_HAVE_OPENGL 1 6 | #cmakedefine G2O_OPENGL_FOUND 1 7 | #cmakedefine G2O_OPENMP 1 8 | #cmakedefine G2O_SHARED_LIBS 1 9 | #cmakedefine G2O_LGPL_SHARED_LIBS 1 10 | 11 | // available sparse matrix libraries 12 | #cmakedefine G2O_HAVE_CHOLMOD 1 13 | #cmakedefine G2O_HAVE_CSPARSE 1 14 | 15 | #cmakedefine G2O_CXX_COMPILER "@G2O_CXX_COMPILER@" 16 | 17 | #ifdef __cplusplus 18 | #include 19 | #endif 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/doc/.gitignore: -------------------------------------------------------------------------------- 1 | *.aux 2 | *.bbl 3 | *.blg 4 | *.dvi 5 | *.log 6 | doxygen/html 7 | doxygen/YES 8 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/.gitignore: -------------------------------------------------------------------------------- 1 | playground 2 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 2 | 3 | ADD_SUBDIRECTORY(stuff) 4 | ADD_SUBDIRECTORY(core) 5 | 6 | IF(G2O_BUILD_APPS) 7 | ADD_SUBDIRECTORY(apps) 8 | ENDIF(G2O_BUILD_APPS) 9 | 10 | # Pre-canned types 11 | ADD_SUBDIRECTORY(types) 12 | 13 | # Solvers 14 | ADD_SUBDIRECTORY(solvers) 15 | 16 | # Examples 17 | IF(G2O_BUILD_EXAMPLES) 18 | ADD_SUBDIRECTORY(examples) 19 | ENDIF(G2O_BUILD_EXAMPLES) 20 | 21 | IF(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/playground") 22 | # Playground 23 | OPTION (G2O_BUILD_PLAYGROUND "Build g2o playground" ON) 24 | IF (G2O_BUILD_PLAYGROUND) 25 | MESSAGE(STATUS "building playground") 26 | ADD_SUBDIRECTORY(playground) 27 | ENDIF() 28 | ENDIF() 29 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(g2o_cli) 2 | ADD_SUBDIRECTORY(g2o_hierarchical) 3 | 4 | IF (G2O_HAVE_OPENGL) 5 | ADD_SUBDIRECTORY(g2o_simulator) 6 | ENDIF() 7 | 8 | IF(QGLVIEWER_FOUND AND (Qt4_FOUND OR Qt5_FOUND) ) 9 | ADD_SUBDIRECTORY(g2o_viewer) 10 | ENDIF() 11 | 12 | IF(G2O_BUILD_LINKED_APPS) 13 | ADD_SUBDIRECTORY(linked_binaries) 14 | ENDIF() 15 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_cli/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(g2o_cli_library ${G2O_LIB_TYPE} 2 | dl_wrapper.cpp dl_wrapper.h 3 | output_helper.cpp output_helper.h 4 | g2o_common.cpp g2o_common.h 5 | g2o_cli_api.h 6 | ) 7 | 8 | SET_TARGET_PROPERTIES(g2o_cli_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}cli) 9 | 10 | SET_PROPERTY(TARGET g2o_cli_library APPEND PROPERTY COMPILE_DEFINITIONS G2O_DEFAULT_TYPES_DIR_="${CMAKE_LIBRARY_OUTPUT_DIRECTORY}" G2O_DEFAULT_SOLVERS_DIR_="${CMAKE_LIBRARY_OUTPUT_DIRECTORY}") 11 | 12 | TARGET_LINK_LIBRARIES(g2o_cli_library core) 13 | IF(UNIX) 14 | TARGET_LINK_LIBRARIES(g2o_cli_library dl) 15 | ENDIF(UNIX) 16 | 17 | ADD_EXECUTABLE(g2o_cli_application 18 | g2o.cpp) 19 | 20 | TARGET_LINK_LIBRARIES(g2o_cli_application g2o_cli_library) 21 | 22 | if(POLICY CMP0043) 23 | cmake_policy(SET CMP0043 OLD) 24 | endif() 25 | SET_PROPERTY(TARGET g2o_cli_library APPEND PROPERTY COMPILE_DEFINITIONS_DEBUG G2O_LIBRARY_POSTFIX="${CMAKE_DEBUG_POSTFIX}") 26 | SET_PROPERTY(TARGET g2o_cli_library APPEND PROPERTY COMPILE_DEFINITIONS_RELWITHDEBINFO G2O_LIBRARY_POSTFIX="${CMAKE_RELWITHDEBINFO_POSTFIX}") 27 | SET_PROPERTY(TARGET g2o_cli_library APPEND PROPERTY COMPILE_DEFINITIONS_MINSIZEREL G2O_LIBRARY_POSTFIX="${CMAKE_MINSIZEREL_POSTFIX}") 28 | 29 | SET_TARGET_PROPERTIES(g2o_cli_application PROPERTIES OUTPUT_NAME g2o${EXE_POSTFIX}) 30 | 31 | 32 | INSTALL(TARGETS g2o_cli_library g2o_cli_application 33 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 34 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 35 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 36 | ) 37 | 38 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 39 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/apps/g2o_cli) 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_cli/g2o_cli_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_CLI_API_H 11 | #define G2O_CLI_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef g2o_cli_library_EXPORTS 19 | #define G2O_CLI_API __declspec(dllexport) 20 | #else 21 | #define G2O_CLI_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_CLI_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_CLI_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SLAM2D_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_hierarchical/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(g2o_hierarchical_library ${G2O_LIB_TYPE} 2 | edge_labeler.cpp 3 | edge_creator.cpp 4 | star.cpp 5 | edge_types_cost_function.cpp 6 | backbone_tree_action.cpp 7 | simple_star_ops.cpp 8 | g2o_hierarchical_test_functions.cpp 9 | edge_labeler.h 10 | edge_creator.h 11 | star.h 12 | edge_types_cost_function.h 13 | backbone_tree_action.h 14 | simple_star_ops.h 15 | g2o_hierarchical_api.h 16 | ) 17 | 18 | 19 | SET_TARGET_PROPERTIES(g2o_hierarchical_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}hierarchical) 20 | 21 | TARGET_LINK_LIBRARIES(g2o_hierarchical_library core) 22 | 23 | ADD_EXECUTABLE(g2o_hierarchical_application 24 | g2o_hierarchical.cpp) 25 | 26 | TARGET_LINK_LIBRARIES(g2o_hierarchical_application g2o_hierarchical_library g2o_cli_library types_slam3d ) #types_slam3d_new 27 | 28 | SET_TARGET_PROPERTIES(g2o_hierarchical_application PROPERTIES OUTPUT_NAME g2o_hierarchical${EXE_POSTFIX}) 29 | 30 | # INSTALL(TARGETS g2o_hierarchical_library g2o_cli_application 31 | # RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 32 | # LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 33 | # ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 34 | # ) 35 | 36 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 37 | INSTALL(FILES 38 | ${headers} 39 | DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/apps/g2o_hierarchical) 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_hierarchical/edge_types_cost_function.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "g2o/core/factory.h" 3 | #include "edge_types_cost_function.h" 4 | 5 | namespace g2o { 6 | 7 | EdgeTypesCostFunction::EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level): 8 | _edgeTag(edgeTag), 9 | _vertexTag(vertexTag), 10 | _factory(Factory::instance()), 11 | _level(level) 12 | {} 13 | 14 | double EdgeTypesCostFunction::operator() (HyperGraph::Edge* e_, HyperGraph::Vertex* from, HyperGraph::Vertex* to){ 15 | OptimizableGraph::Edge*e =(OptimizableGraph::Edge*)(e_); 16 | if (e->level()==_level && _factory->tag(e)==_edgeTag && _factory->tag(from)==_vertexTag && _factory->tag(to)==_vertexTag) { 17 | return 1.; 18 | } 19 | return std::numeric_limits::max(); 20 | } 21 | 22 | } 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_hierarchical/edge_types_cost_function.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_EDGE_TYPES_COST_FUNCTION_ 2 | #define G2O_EDGE_TYPES_COST_FUNCTION_ 3 | 4 | #include 5 | #include "g2o/core/sparse_optimizer.h" 6 | #include "g2o/core/hyper_dijkstra.h" 7 | 8 | namespace g2o { 9 | 10 | /** 11 | * Cost function for Hyper-Dijkstra that returns 1 when for edges that belong 12 | * to a given type and maxdouble otherwise. It can be used to construct a 13 | * backbone of a hierarchical graph by running Dijkstra. 14 | */ 15 | struct EdgeTypesCostFunction: public HyperDijkstra::CostFunction { 16 | //! creates a cost function that matches edges at a given level, whose tag is the one given and that are leaving/leading to vertices 17 | //! of a selected type. 18 | //! @param edgeTag: the tag of the edge type to consider 19 | //! @param vertexTag: the tag of the vertex to consider 20 | //! @param level: the level of the edge 21 | EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level); 22 | 23 | //!cost operator 24 | virtual double operator() (HyperGraph::Edge* e_, HyperGraph::Vertex* from, HyperGraph::Vertex* to); 25 | 26 | std::string _edgeTag; 27 | std::string _vertexTag; 28 | Factory* _factory; 29 | int _level; 30 | }; 31 | 32 | } 33 | #endif 34 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_hierarchical/g2o_hierarchical_api.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_HIERARCHICAL_API_H 2 | #define G2O_HIERARCHICAL_API_H 3 | 4 | #include "g2o/config.h" 5 | 6 | #ifdef _MSC_VER 7 | // We are using a Microsoft compiler: 8 | #ifdef G2O_SHARED_LIBS 9 | #ifdef g2o_hierarchical_library_EXPORTS 10 | #define G2O_HIERARCHICAL_API __declspec(dllexport) 11 | #else 12 | #define G2O_HIERARCHICAL_API __declspec(dllimport) 13 | #endif 14 | #else 15 | #define G2O_HIERARCHICAL_API 16 | #endif 17 | 18 | #else 19 | // Not Microsoft compiler so set empty definition: 20 | #define G2O_HIERARCHICAL_API 21 | #endif 22 | 23 | #endif // G2O_CSPARSE_API_H 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_hierarchical/simple_star_ops.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_SIMPLE_STAR_OPS_ 2 | #define G2O_SIMPLE_STAR_OPS_ 3 | 4 | #include 5 | #include 6 | #include "g2o/core/sparse_optimizer.h" 7 | #include "g2o/core/hyper_dijkstra.h" 8 | #include "star.h" 9 | #include "edge_creator.h" 10 | #include "edge_labeler.h" 11 | 12 | #include "g2o_hierarchical_api.h" 13 | 14 | namespace g2o { 15 | 16 | 17 | G2O_HIERARCHICAL_API void constructEdgeStarMap(EdgeStarMap& esmap, StarSet& stars, bool low=true); 18 | 19 | size_t vertexEdgesInStar(HyperGraph::EdgeSet& eset, HyperGraph::Vertex* v, Star* s, EdgeStarMap& esmap); 20 | 21 | void starsInVertex(StarSet& stars, HyperGraph::Vertex* v, EdgeStarMap& esmap); 22 | 23 | void assignHierarchicalEdges(StarSet& stars, EdgeStarMap& esmap, EdgeLabeler* labeler, EdgeCreator* creator, SparseOptimizer* optimizer, int minNumEdges, int maxIterations); 24 | 25 | G2O_HIERARCHICAL_API void computeBorder(StarSet& stars, EdgeStarMap& hesmap); 26 | 27 | G2O_HIERARCHICAL_API void computeSimpleStars(StarSet& stars, 28 | SparseOptimizer* optimizer, 29 | EdgeLabeler* labeler, 30 | EdgeCreator* creator, 31 | OptimizableGraph::Vertex* gauge_, 32 | std::string edgeTag, std::string vertexTag, 33 | int level, 34 | int step, 35 | int backboneIterations=1, 36 | int starIterations=30, 37 | double rejectionThreshold=1e-5, bool debug=false); 38 | 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_simulator/g2o_simulator_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_SIMULATOR_API_H 11 | #define G2O_SIMULATOR_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef g2o_simulator_library_EXPORTS 19 | #define G2O_SIMULATOR_API __declspec(dllexport) 20 | #else 21 | #define G2O_SIMULATOR_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_SIMULATOR_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_SIMULATOR_API 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_simulator/pointsensorparameters.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "pointsensorparameters.h" 28 | 29 | #include "g2o/stuff/misc.h" // for M_PI 30 | 31 | // Robot2D 32 | namespace g2o { 33 | PointSensorParameters::PointSensorParameters(){ 34 | _maxRange2 = 25; 35 | _minRange2 = 0.01; 36 | _fov = M_PI/2; 37 | _maxAngularDifference = M_PI/2; 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_simulator/sensor_odometry3d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SENSOR_ODOMETRY3D_H_ 28 | #define G2O_SENSOR_ODOMETRY3D_H_ 29 | 30 | #include "simulator3d_base.h" 31 | #include "g2o_simulator_api.h" 32 | 33 | namespace g2o { 34 | 35 | class G2O_SIMULATOR_API SensorOdometry3D: public BinarySensor { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | SensorOdometry3D(const std::string& name_); 39 | virtual void sense(); 40 | void addNoise(EdgeType* e); 41 | }; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_simulator/simulator2d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SIMULATOR2D_ 28 | #define G2O_SIMULATOR2D_ 29 | 30 | #include "simulator2d_base.h" 31 | #include "sensor_odometry2d.h" 32 | #include "sensor_pose2d.h" 33 | #include "sensor_pointxy.h" 34 | #include "sensor_pointxy_bearing.h" 35 | #include "sensor_pointxy_offset.h" 36 | #include "sensor_segment2d.h" 37 | #include "sensor_segment2d_line.h" 38 | #include "sensor_segment2d_pointline.h" 39 | 40 | 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_simulator/simulator2d_base.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SIMULATOR2D_BASE_H_ 28 | #define G2O_SIMULATOR2D_BASE_H_ 29 | 30 | #include "g2o/types/slam2d/types_slam2d.h" 31 | #include "g2o/types/slam2d_addons/types_slam2d_addons.h" 32 | #include "simulator.h" 33 | 34 | namespace g2o { 35 | 36 | typedef WorldObject WorldObjectSE2; 37 | 38 | typedef WorldObject WorldObjectPointXY; 39 | 40 | typedef WorldObject WorldObjectSegment2D; 41 | 42 | typedef Robot Robot2D; 43 | 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_simulator/simulator3d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SIMULATOR3D_ 28 | #define G2O_SIMULATOR3D_ 29 | 30 | #include "sensor_odometry3d.h" 31 | #include "sensor_pose3d.h" 32 | #include "sensor_pointxyz.h" 33 | #include "sensor_pointxyz_disparity.h" 34 | #include "sensor_pointxyz_depth.h" 35 | #include "sensor_pose3d_offset.h" 36 | #include "sensor_se3_prior.h" 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/g2o_viewer.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #include 20 | 21 | #include "run_g2o_viewer.h" 22 | 23 | #include "g2o/core/optimizable_graph.h" 24 | #include "g2o/apps/g2o_cli/g2o_common.h" 25 | #include "g2o/apps/g2o_cli/dl_wrapper.h" 26 | #include "g2o/stuff/command_args.h" 27 | 28 | #include 29 | using namespace std; 30 | using namespace g2o; 31 | 32 | int main(int argc, char** argv) 33 | { 34 | OptimizableGraph::initMultiThreading(); 35 | QApplication qapp(argc, argv); 36 | 37 | CommandArgs arg; 38 | #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES 39 | string dummy; 40 | arg.param("solverlib", dummy, "", "specify a solver library which will be loaded"); 41 | arg.param("typeslib", dummy, "", "specify a types library which will be loaded"); 42 | // loading the standard solver / types 43 | DlWrapper dlTypesWrapper; 44 | loadStandardTypes(dlTypesWrapper, argc, argv); 45 | // register all the solvers 46 | DlWrapper dlSolverWrapper; 47 | loadStandardSolver(dlSolverWrapper, argc, argv); 48 | #endif 49 | 50 | // run the viewer 51 | return RunG2OViewer::run(argc, argv, arg); 52 | } 53 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/g2o_viewer_api.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_VIEWER_API_H 2 | #define G2O_VIEWER_API_H 3 | 4 | #include "g2o/config.h" 5 | 6 | #ifdef _MSC_VER 7 | // We are using a Microsoft compiler: 8 | 9 | #ifdef G2O_SHARED_LIBS 10 | #ifdef viewer_library_EXPORTS 11 | #define G2O_VIEWER_API __declspec(dllexport) 12 | #else 13 | #define G2O_VIEWER_API __declspec(dllimport) 14 | #endif 15 | #else 16 | #define G2O_VIEWER_API 17 | #endif 18 | 19 | #else 20 | // Not Microsoft compiler so set empty definition: 21 | #define G2O_VIEWER_API 22 | #endif 23 | 24 | #endif // G2O_STUFF_API_H 25 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/gui_hyper_graph_action.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #include "gui_hyper_graph_action.h" 20 | 21 | #include "g2o_qglviewer.h" 22 | 23 | #include 24 | 25 | namespace g2o { 26 | 27 | GuiHyperGraphAction::GuiHyperGraphAction() : 28 | HyperGraphAction(), 29 | viewer(0), dumpScreenshots(false) 30 | { 31 | } 32 | 33 | GuiHyperGraphAction::~GuiHyperGraphAction() 34 | { 35 | } 36 | 37 | HyperGraphAction* GuiHyperGraphAction::operator()(const HyperGraph* graph, Parameters* parameters) 38 | { 39 | (void) graph; 40 | if (viewer) { 41 | viewer->setUpdateDisplay(true); 42 | viewer->updateGL(); 43 | 44 | if (dumpScreenshots) { 45 | ParametersIteration* p = dynamic_cast(parameters); 46 | if (p) { 47 | viewer->setSnapshotFormat(QString("PNG")); 48 | viewer->setSnapshotQuality(-1); 49 | viewer->saveSnapshot(QString().sprintf("g2o%.6d.png", p->iteration), true); 50 | } 51 | } 52 | 53 | qApp->processEvents(); 54 | return this; 55 | } 56 | return 0; 57 | } 58 | 59 | } // end namespace 60 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/gui_hyper_graph_action.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #ifndef G2O_GUI_HYPER_GRAPH_ACTION_H 20 | #define G2O_GUI_HYPER_GRAPH_ACTION_H 21 | 22 | #include "g2o_viewer_api.h" 23 | #include "g2o/core/hyper_graph_action.h" 24 | 25 | namespace g2o { 26 | 27 | class G2oQGLViewer; 28 | 29 | /** 30 | * \brief action which calls an GUI update after each iteration 31 | */ 32 | class G2O_VIEWER_API GuiHyperGraphAction : public HyperGraphAction 33 | { 34 | public: 35 | GuiHyperGraphAction(); 36 | ~GuiHyperGraphAction(); 37 | 38 | /** 39 | * calling updateGL, processEvents to visualize the current state after each iteration 40 | */ 41 | HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0); 42 | 43 | G2oQGLViewer* viewer; ///< the viewer which visualizes the graph 44 | bool dumpScreenshots; 45 | }; 46 | 47 | } // end namespace 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/properties_widget.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #ifndef G2O_PROPERTIES_WINDOW_H 20 | #define G2O_PROPERTIES_WINDOW_H 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "g2o_viewer_api.h" 28 | #include "ui_base_properties_widget.h" 29 | 30 | namespace g2o 31 | { 32 | class G2oQGLViewer; 33 | class PropertyMap; 34 | } 35 | 36 | class G2O_VIEWER_API PropertiesWidget : public QDialog, public Ui::BasePropertiesWidget 37 | { 38 | Q_OBJECT 39 | public: 40 | PropertiesWidget(QWidget * parent = 0, Qt::WindowFlags f = 0); 41 | virtual ~PropertiesWidget(); 42 | 43 | void setProperties(g2o::PropertyMap* properties); 44 | 45 | public slots: 46 | void on_btnApply_clicked(); 47 | void on_btnOK_clicked(); 48 | 49 | protected: 50 | std::vector _propNames; 51 | g2o::PropertyMap* _properties; 52 | 53 | virtual void updateDisplayedProperties(); 54 | virtual void applyProperties(); 55 | virtual std::string humanReadablePropName(const std::string& propertyName) const; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/run_g2o_viewer.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "g2o_viewer_api.h" 28 | 29 | namespace g2o { 30 | 31 | class CommandArgs; 32 | 33 | /** 34 | * \brief wrapper for running the g2o viewer 35 | */ 36 | class G2O_VIEWER_API RunG2OViewer 37 | { 38 | public: 39 | static int run(int argc, char** argv, CommandArgs& arg); 40 | }; 41 | 42 | } // end namespace 43 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/stream_redirect.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef G2O_STREAM_REDIRECT_H 18 | #define G2O_STREAM_REDIRECT_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "g2o_viewer_api.h" 26 | 27 | class QPlainTextEdit; 28 | 29 | /** 30 | * \brief redirect a stream to a QPlainTextEdit 31 | */ 32 | class G2O_VIEWER_API StreamRedirect : public std::basic_streambuf 33 | { 34 | public: 35 | typedef std::char_traits::int_type int_type; 36 | 37 | public: 38 | StreamRedirect(std::ostream &stream, QPlainTextEdit* te); 39 | ~StreamRedirect(); 40 | 41 | protected: 42 | virtual std::char_traits::int_type overflow(int_type v); 43 | virtual std::streamsize xsputn(const char *p, std::streamsize n); 44 | 45 | private: 46 | std::ostream& _stream; 47 | std::streambuf* _old_buf; 48 | std::string _buffer; 49 | QPlainTextEdit* _te; 50 | QMutex _mutex; 51 | }; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/apps/g2o_viewer/viewer_properties_widget.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #ifndef G2O_VIEWER_PROPERTIES_WIDGET_H 20 | #define G2O_VIEWER_PROPERTIES_WIDGET_H 21 | 22 | #include "g2o_viewer_api.h" 23 | #include "properties_widget.h" 24 | 25 | namespace g2o 26 | { 27 | class G2oQGLViewer; 28 | } 29 | 30 | class G2O_VIEWER_API ViewerPropertiesWidget : public PropertiesWidget 31 | { 32 | public: 33 | ViewerPropertiesWidget(QWidget * parent = 0, Qt::WindowFlags f = 0); 34 | virtual ~ViewerPropertiesWidget(); 35 | 36 | void setViewer(g2o::G2oQGLViewer* viewer); 37 | 38 | protected: 39 | g2o::G2oQGLViewer* _viewer; 40 | 41 | virtual void applyProperties(); 42 | virtual std::string humanReadablePropName(const std::string& probName) const; 43 | }; 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/core/g2o_core_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_CORE_API_H 11 | #define G2O_CORE_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef core_EXPORTS 19 | #define G2O_CORE_API __declspec(dllexport) 20 | #else 21 | #define G2O_CORE_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_CORE_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_CORE_API 30 | #endif 31 | 32 | #endif // G2O_CORE_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/core/sparse_optimizer_terminate_action.h: -------------------------------------------------------------------------------- 1 | #ifndef SPARSE_OPTIMIZER_TERMINATE_ACTION_H 2 | #define SPARSE_OPTIMIZER_TERMINATE_ACTION_H 3 | 4 | #include "g2o_core_api.h" 5 | #include "hyper_graph_action.h" 6 | 7 | namespace g2o { 8 | 9 | class SparseOptimizer; 10 | 11 | /** 12 | * \brief stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 13 | * 14 | * stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 15 | * If the gain is larger than zero and below the threshold, then the optimizer is stopped. 16 | * Typically usage of this action includes adding it as a postIteration action, by calling 17 | * addPostIterationAction on a sparse optimizer. 18 | */ 19 | class G2O_CORE_API SparseOptimizerTerminateAction : public HyperGraphAction 20 | { 21 | public: 22 | SparseOptimizerTerminateAction(); 23 | virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0); 24 | 25 | double gainThreshold() const { return _gainThreshold;} 26 | void setGainThreshold(double gainThreshold); 27 | 28 | int maxIterations() const { return _maxIterations;} 29 | void setMaxIterations(int maxit); 30 | 31 | protected: 32 | double _gainThreshold; 33 | double _lastChi; 34 | bool _auxTerminateFlag; 35 | int _maxIterations; 36 | 37 | void setOptimizerStopFlag(const SparseOptimizer* optimizer, bool stop); 38 | }; 39 | 40 | } // end namespace 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(data_fitting) 2 | 3 | ADD_SUBDIRECTORY(sphere) 4 | 5 | # The condition on cholmod is required here because the cholmod solver 6 | # is explicitly used in these examples. 7 | IF(CHOLMOD_FOUND) 8 | ADD_SUBDIRECTORY(target) 9 | ADD_SUBDIRECTORY(ba) 10 | ADD_SUBDIRECTORY(ba_anchored_inverse_depth) 11 | ENDIF(CHOLMOD_FOUND) 12 | 13 | # Examples which explicitly use CSparse 14 | IF(CSPARSE_FOUND) 15 | ADD_SUBDIRECTORY(tutorial_slam2d) 16 | ADD_SUBDIRECTORY(icp) 17 | ADD_SUBDIRECTORY(calibration_odom_laser) 18 | ADD_SUBDIRECTORY(simple_optimize) 19 | ADD_SUBDIRECTORY(plane_slam) 20 | ENDIF() 21 | 22 | IF(CSPARSE_FOUND OR CHOLMOD_FOUND) 23 | ADD_SUBDIRECTORY(sba) 24 | ADD_SUBDIRECTORY(bal) 25 | ENDIF() 26 | 27 | IF(QT4_FOUND AND QGLVIEWER_FOUND AND CSPARSE_FOUND) 28 | ADD_SUBDIRECTORY(slam2d) 29 | ENDIF() 30 | 31 | ADD_SUBDIRECTORY(data_convert) 32 | ADD_SUBDIRECTORY(interactive_slam) 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/ba/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(ba_demo 2 | ba_demo.cpp 3 | ) 4 | 5 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 6 | 7 | TARGET_LINK_LIBRARIES(ba_demo core solver_cholmod types_sba) 8 | 9 | 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/ba_anchored_inverse_depth/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(ba_anchored_inverse_depth_demo 2 | ba_anchored_inverse_depth_demo.cpp 3 | ) 4 | 5 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 6 | 7 | TARGET_LINK_LIBRARIES(ba_anchored_inverse_depth_demo core solver_cholmod types_sba) 8 | 9 | 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/bal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | IF(CHOLMOD_INCLUDE_DIR OR CSPARSE_INCLUDE_DIR) 2 | ADD_EXECUTABLE(bal_example bal_example.cpp) 3 | ENDIF() 4 | 5 | # Link CHOLMOD if available, CSparse otherwise 6 | IF(CHOLMOD_FOUND) 7 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 8 | TARGET_LINK_LIBRARIES(bal_example solver_cholmod) 9 | ELSEIF(CSPARSE_FOUND) 10 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 11 | TARGET_LINK_LIBRARIES(bal_example solver_csparse) 12 | ENDIF() 13 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/calibration_odom_laser/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 2 | 3 | ADD_LIBRARY(calibration_odom_laser_library ${G2O_LIB_TYPE} 4 | gm2dl_io.cpp gm2dl_io.h 5 | sclam_helpers.cpp sclam_helpers.h 6 | motion_information.h 7 | edge_se2_pure_calib.cpp edge_se2_pure_calib.h 8 | closed_form_calibration.cpp closed_form_calibration.h 9 | g2o_calibration_odom_laser_api.h 10 | ) 11 | SET_TARGET_PROPERTIES(calibration_odom_laser_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}calibration_odom_laser) 12 | TARGET_LINK_LIBRARIES(calibration_odom_laser_library core solver_csparse types_sclam2d types_data) 13 | 14 | ADD_EXECUTABLE(sclam_odom_laser 15 | sclam_odom_laser.cpp 16 | ) 17 | SET_TARGET_PROPERTIES(sclam_odom_laser PROPERTIES OUTPUT_NAME sclam_odom_laser${EXE_POSTFIX}) 18 | TARGET_LINK_LIBRARIES(sclam_odom_laser calibration_odom_laser_library) 19 | 20 | ADD_EXECUTABLE(sclam_pure_calibration 21 | sclam_pure_calibration.cpp 22 | ) 23 | SET_TARGET_PROPERTIES(sclam_pure_calibration PROPERTIES OUTPUT_NAME sclam_pure_calibration${EXE_POSTFIX}) 24 | TARGET_LINK_LIBRARIES(sclam_pure_calibration calibration_odom_laser_library) 25 | 26 | ADD_EXECUTABLE(sclam_laser_calib 27 | sclam_laser_calib.cpp 28 | ) 29 | SET_TARGET_PROPERTIES(sclam_laser_calib PROPERTIES OUTPUT_NAME sclam_laser_calib${EXE_POSTFIX}) 30 | TARGET_LINK_LIBRARIES(sclam_laser_calib calibration_odom_laser_library) 31 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/calibration_odom_laser/g2o_calibration_odom_laser_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_CALIBRATION_ODOM_LASER_API_H 11 | #define G2O_CALIBRATION_ODOM_LASER_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef calibration_odom_laser_library_EXPORTS 19 | #define G2O_CALIBRATION_ODOM_LASER_API __declspec(dllexport) 20 | #else 21 | #define G2O_CALIBRATION_ODOM_LASER_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_CALIBRATION_ODOM_LASER_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_CALIBRATION_ODOM_LASER_API 30 | #endif 31 | 32 | #endif // G2O_CALIBRATION_ODOM_LASER_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/data_convert/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(convert_sba_slam3d 2 | convert_sba_slam3d.cpp 3 | ) 4 | SET_TARGET_PROPERTIES(convert_sba_slam3d PROPERTIES OUTPUT_NAME convert_sba_slam3d${EXE_POSTFIX}) 5 | TARGET_LINK_LIBRARIES(convert_sba_slam3d core types_slam3d types_sba) 6 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/data_fitting/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 2 | 3 | IF(CSPARSE_INCLUDE_DIR) 4 | ADD_EXECUTABLE(circle_fit circle_fit.cpp) 5 | SET_TARGET_PROPERTIES(circle_fit PROPERTIES OUTPUT_NAME circle_fit${EXE_POSTFIX}) 6 | TARGET_LINK_LIBRARIES(circle_fit core solver_csparse) 7 | ENDIF() 8 | 9 | ADD_EXECUTABLE(curve_fit curve_fit.cpp) 10 | SET_TARGET_PROPERTIES(curve_fit PROPERTIES OUTPUT_NAME curve_fit${EXE_POSTFIX}) 11 | TARGET_LINK_LIBRARIES(curve_fit core) 12 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/g2o_unfold/tools.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_AIS_GRAPH_TOOLS_HH 2 | #define G2O_AIS_GRAPH_TOOLS_HH 3 | 4 | #include "g2o/core/hyper_graph.h" 5 | #include "g2o/core/hyper_dijkstra.h" 6 | 7 | namespace g2o{ 8 | 9 | void findConnectedEdgesWithCostLimit(HyperGraph::EdgeSet& selected, 10 | HyperGraph::EdgeSet& border, 11 | HyperGraph::Edge* start, 12 | HyperDijkstra::CostFunction* cost, 13 | double maxEdgeCost, 14 | double comparisonConditioner=1e-6); 15 | }; 16 | 17 | #endif 18 | 19 | 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(gicp_demo 2 | gicp_demo.cpp 3 | ) 4 | 5 | ADD_EXECUTABLE(gicp_sba_demo 6 | gicp_sba_demo.cpp 7 | ) 8 | 9 | SET_TARGET_PROPERTIES(gicp_sba_demo PROPERTIES OUTPUT_NAME gicp_sba_demo${EXE_POSTFIX}) 10 | 11 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 12 | 13 | TARGET_LINK_LIBRARIES(gicp_demo core types_sba types_slam3d types_icp ${OPENGL_LIBRARIES} solver_csparse) 14 | TARGET_LINK_LIBRARIES(gicp_sba_demo core types_sba types_slam3d types_icp ${OPENGL_LIBRARIES} solver_csparse) 15 | 16 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/icp/gicp-test1.dat: -------------------------------------------------------------------------------- 1 | # Simple GICP example 2 | # 3 | # Vertices at origin, second one is rotated 4 | VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 5 | VERTEX_SE3:QUAT 1 0 0 0 0.5 0 0 0.5 6 | # 7 | # Edges displaced by 1m on Z axis 8 | EDGE_V_V_GICP 1 0 0 0 1 0 0 1 0 0 2 0 0 1 9 | EDGE_V_V_GICP 1 0 0 1 1 0 0 1 0 1 2 0 0 1 10 | EDGE_V_V_GICP 1 0 1 0 1 0 0 1 1 0 2 0 0 1 11 | EDGE_V_V_GICP 1 0 1 1 1 0 0 1 1 1 2 0 0 1 -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(slam_parser) 2 | 3 | IF(CHOLMOD_FOUND) 4 | ADD_SUBDIRECTORY(g2o_interactive) 5 | ADD_SUBDIRECTORY(g2o_incremental) 6 | ENDIF(CHOLMOD_FOUND) 7 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/README.txt: -------------------------------------------------------------------------------- 1 | This directory implements an interface to g2o which reads the input data 2 | via stdin and outputs upon request the current estimate to stdout. 3 | 4 | In particular, the interface follows the grammar proposed for the RSS'11 5 | workshop "Automated SLAM Evaluation", see 6 | http://slameval.willowgarage.com/workshop/submissions/ 7 | 8 | Brief description: 9 | - slam_parser (LGPL v3) 10 | The parser for the SLAM protocol 11 | - g2o_interactive (LGPL v3) 12 | Run batch optimization every N nodes 13 | - g2o_incremental (GPL v3) 14 | Incrementally solve the optimzation, batch every 100 nodes 15 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/g2o_incremental/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(g2o_incremental_library ${G2O_LIB_TYPE} 2 | graph_optimizer_sparse_incremental.cpp graph_optimizer_sparse_incremental.h 3 | ) 4 | 5 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 6 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/../) 7 | 8 | SET_TARGET_PROPERTIES(g2o_incremental_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}incremental) 9 | TARGET_LINK_LIBRARIES(g2o_incremental_library g2o_interactive_library solver_cholmod ${CAMD_LIBRARY}) 10 | 11 | ADD_EXECUTABLE(g2o_incremental_application g2o_incremental.cpp) 12 | TARGET_LINK_LIBRARIES(g2o_incremental_application g2o_incremental_library) 13 | SET_TARGET_PROPERTIES(g2o_incremental_application PROPERTIES OUTPUT_NAME g2o_incremental${EXE_POSTFIX}) 14 | 15 | INSTALL(TARGETS g2o_incremental_library g2o_incremental_application 16 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 17 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 18 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 19 | ) 20 | 21 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 22 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/examples/interactive_slam/g2o_incremental) 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/g2o_incremental/README.txt: -------------------------------------------------------------------------------- 1 | g2o_incremental implements a variant of the iSAM algorithm. 2 | In contrast to iSAM the update of the Cholesky factor is done 3 | using rank updates as provided via the CHOLMOD library. 4 | 5 | “iSAM: Incremental Smoothing and Mapping” 6 | by M. Kaess, A. Ranganathan, and F. Dellaert. 7 | IEEE Trans. on Robotics, vol. 24, no. 6, Dec. 2008, pp. 1365-1378 8 | 9 | Furthermore, it implements a simple protocol to control the operation of the 10 | algorithm. See protocol.txt for more details or 11 | http://slameval.willowgarage.com/workshop/submissions/ for an evaluation system 12 | as well as data sets. 13 | 14 | In the standard configuration g2o_incremental solves after inserting 10 nodes. 15 | This behavior can be modified via a command line option. Furthermore, by 16 | specifying -g on the command line a gnuplot instance is created to visualize 17 | the graph. 18 | 19 | Please note that both the visualization via Gnuplot and the verbose output 20 | affect the timing results. 21 | 22 | g2o_incremental is licensed under GPLv3. 23 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/g2o_incremental/g2o_incremental_api.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_INCREMENTAL_API_H 2 | #define G2O_INCREMENTAL_API_H 3 | 4 | #include "g2o/config.h" 5 | 6 | #ifdef _MSC_VER 7 | // We are using a Microsoft compiler: 8 | #ifdef G2O_SHARED_LIBS 9 | #ifdef g2o_incremental_library_EXPORTS 10 | #define G2O_INCREMENTAL_API __declspec(dllexport) 11 | #else 12 | #define G2O_INCREMENTAL_API __declspec(dllimport) 13 | #endif 14 | #else 15 | #define G2O_INCREMENTAL_API 16 | #endif 17 | 18 | #else 19 | // Not Microsoft compiler so set empty definition: 20 | #define G2O_INCREMENTAL_API 21 | #endif 22 | 23 | #endif // G2O_CORE_API_H 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/g2o_interactive/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(g2o_interactive_library ${G2O_LIB_TYPE} 2 | g2o_slam_interface.cpp g2o_slam_interface.h 3 | graph_optimizer_sparse_online.cpp graph_optimizer_sparse_online.h 4 | types_online.cpp 5 | ) 6 | 7 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/../) 8 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 9 | 10 | SET_TARGET_PROPERTIES(g2o_interactive_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}interactive) 11 | TARGET_LINK_LIBRARIES(g2o_interactive_library core types_slam2d types_slam3d solver_cholmod parser_library interface_library) 12 | 13 | ADD_EXECUTABLE(g2o_online_application g2o_online.cpp) 14 | TARGET_LINK_LIBRARIES(g2o_online_application g2o_interactive_library) 15 | SET_TARGET_PROPERTIES(g2o_online_application PROPERTIES OUTPUT_NAME g2o_online${EXE_POSTFIX}) 16 | 17 | #ADD_EXECUTABLE(generate_commands_application generate_commands.cpp) 18 | #TARGET_LINK_LIBRARIES(generate_commands_application g2o_interactive_library) 19 | #SET_TARGET_PROPERTIES(generate_commands_application PROPERTIES OUTPUT_NAME generate_commands) 20 | 21 | INSTALL(TARGETS g2o_interactive_library g2o_online_application 22 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 23 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 24 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 25 | ) 26 | 27 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 28 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/examples/interactive_slam/g2o_interactive) 29 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/g2o_interactive/g2o_interactive_api.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_INTERACTIVE_API_H 2 | #define G2O_INTERACTIVE_API_H 3 | 4 | #include "g2o/config.h" 5 | 6 | #ifdef _MSC_VER 7 | // We are using a Microsoft compiler: 8 | #ifdef G2O_SHARED_LIBS 9 | #ifdef g2o_interactive_library_EXPORTS 10 | #define G2O_INTERACTIVE_API __declspec(dllexport) 11 | #else 12 | #define G2O_INTERACTIVE_API __declspec(dllimport) 13 | #endif 14 | #else 15 | #define G2O_INTERACTIVE_API 16 | #endif 17 | 18 | #else 19 | // Not Microsoft compiler so set empty definition: 20 | #define G2O_INTERACTIVE_API 21 | #endif 22 | 23 | #endif // G2O_CORE_API_H 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(parser) 2 | ADD_SUBDIRECTORY(interface) 3 | #ADD_SUBDIRECTORY(example) 4 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(interface_library_src 2 | abstract_slam_interface.h 3 | parser_interface.cpp 4 | parser_interface.h 5 | slam_context_interface.cpp 6 | slam_context_interface.h 7 | ) 8 | 9 | IF (MSVC) 10 | ADD_LIBRARY(interface_library STATIC 11 | ${interface_library_src} 12 | ) 13 | ELSE() 14 | ADD_LIBRARY(interface_library ${G2O_LIB_TYPE} 15 | ${interface_library_src} 16 | ) 17 | ENDIF() 18 | 19 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/../../) 20 | 21 | SET_TARGET_PROPERTIES(interface_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}interface) 22 | TARGET_LINK_LIBRARIES(interface_library parser_library) 23 | 24 | INSTALL(TARGETS interface_library 25 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 26 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 27 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 28 | ) 29 | 30 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h") 31 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/slam_parser/interface) 32 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/parser/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(G2O_PARSER_SRC 2 | slam_context.h slam_context.cpp 3 | bison_parser.cpp bison_parser.h 4 | commands.h 5 | driver.h driver.cpp 6 | location.hh 7 | position.hh 8 | scanner.h flex_scanner.cpp 9 | stack.hh 10 | ) 11 | 12 | IF (MSVC) 13 | ADD_LIBRARY(parser_library STATIC 14 | ${G2O_PARSER_SRC} 15 | ) 16 | ELSE() 17 | ADD_LIBRARY(parser_library ${G2O_LIB_TYPE} 18 | ${G2O_PARSER_SRC} 19 | ) 20 | ENDIF() 21 | 22 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) 23 | 24 | SET_TARGET_PROPERTIES(parser_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}parser) 25 | 26 | #ADD_EXECUTABLE(test_slam_parser test_slam_parser.cpp) 27 | #TARGET_LINK_LIBRARIES(test_slam_parser parser_library) 28 | #SET_TARGET_PROPERTIES(test_slam_parser PROPERTIES OUTPUT_NAME test_slam_parser${EXE_POSTFIX}) 29 | 30 | INSTALL(TARGETS parser_library 31 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 32 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 33 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 34 | ) 35 | 36 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hh") 37 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/slam_parser/parser) 38 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/parser/grammar.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # wrapper script to generate the parser by flex / bison 4 | 5 | flex -o flex_scanner.cpp -i scanner.l 6 | 7 | cp /usr/include/FlexLexer.h . 8 | 9 | bison -o bison_parser.cpp --defines=bison_parser.h parser.yy 10 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/parser/scanner.h: -------------------------------------------------------------------------------- 1 | #ifndef SCANNER_H 2 | #define SCANNER_H 3 | 4 | #ifndef YY_DECL 5 | 6 | #define YY_DECL \ 7 | SlamParser::Parser::token_type \ 8 | SlamParser::Scanner::lex( \ 9 | SlamParser::Parser::semantic_type* yylval, \ 10 | SlamParser::Parser::location_type* yylloc \ 11 | ) 12 | #endif 13 | 14 | #ifndef __FLEX_LEXER_H 15 | #define yyFlexLexer SlamFlexLexer 16 | #include "FlexLexer.h" 17 | #undef yyFlexLexer 18 | #endif 19 | 20 | #include "bison_parser.h" 21 | 22 | namespace SlamParser { 23 | 24 | class Scanner : public SlamFlexLexer 25 | { 26 | public: 27 | /** Create a new scanner object. The streams arg_yyin and arg_yyout default 28 | * to cin and cout, but that assignment is only made when initializing in 29 | * yylex(). */ 30 | Scanner(std::istream* arg_yyin = 0, 31 | std::ostream* arg_yyout = 0); 32 | 33 | /** Required for virtual functions */ 34 | virtual ~Scanner(); 35 | 36 | /** This is the main lexing function. It is generated by flex according to 37 | * the macro declaration YY_DECL above. The generated bison parser then 38 | * calls this virtual function to fetch new tokens. */ 39 | virtual Parser::token_type lex( 40 | Parser::semantic_type* yylval, 41 | Parser::location_type* yylloc 42 | ); 43 | 44 | /** Enable debug output (via arg_yyout) if compiled into the scanner. */ 45 | void set_debug(bool b); 46 | }; 47 | 48 | } // end namespace 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/parser/slam_context.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef SLAM_CONTEXT_H 28 | #define SLAM_CONTEXT_H 29 | 30 | #include 31 | 32 | namespace SlamParser { 33 | 34 | class CommandNode; 35 | 36 | class SlamContext 37 | { 38 | public: 39 | SlamContext(); 40 | virtual ~SlamContext(); 41 | 42 | virtual bool process(CommandNode* commandNode); 43 | }; 44 | 45 | } // end namespace 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/interactive_slam/slam_parser/parser/test_slam_parser.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include 28 | 29 | #include "slam_context.h" 30 | #include "driver.h" 31 | #include "commands.h" 32 | using namespace std; 33 | 34 | int main () 35 | { 36 | SlamParser::SlamContext slamContext; 37 | SlamParser::Driver driver(slamContext); 38 | driver.trace_parsing = true; 39 | driver.trace_scanning = true; 40 | 41 | bool parseStatus = driver.parse_stream(cin); 42 | if (! parseStatus) 43 | return 1; 44 | } 45 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/plane_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 2 | 3 | ADD_EXECUTABLE(simulator_3d_plane 4 | simulator_3d_plane.cpp 5 | ) 6 | TARGET_LINK_LIBRARIES(simulator_3d_plane solver_csparse types_slam3d_addons) 7 | SET_TARGET_PROPERTIES(simulator_3d_plane PROPERTIES OUTPUT_NAME simulator_3d_plane${EXE_POSTFIX}) 8 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/sba/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | IF(CHOLMOD_INCLUDE_DIR OR CSPARSE_INCLUDE_DIR) 2 | ADD_EXECUTABLE(sba_demo sba_demo.cpp) 3 | TARGET_LINK_LIBRARIES(sba_demo core types_icp types_sba) 4 | SET_TARGET_PROPERTIES(sba_demo PROPERTIES OUTPUT_NAME sba_demo${EXE_POSTFIX}) 5 | ENDIF() 6 | 7 | # Link CHOLMOD if available, CSparse otherwise 8 | IF(CHOLMOD_FOUND) 9 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 10 | TARGET_LINK_LIBRARIES(sba_demo solver_cholmod) 11 | ELSEIF(CSPARSE_FOUND) 12 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 13 | TARGET_LINK_LIBRARIES(sba_demo solver_csparse) 14 | ENDIF() 15 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/simple_optimize/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 2 | 3 | ADD_EXECUTABLE(simple_optimize 4 | simple_optimize.cpp 5 | ) 6 | 7 | SET_TARGET_PROPERTIES(simple_optimize PROPERTIES OUTPUT_NAME simple_optimize${EXE_POSTFIX}) 8 | 9 | TARGET_LINK_LIBRARIES(simple_optimize core solver_csparse) 10 | TARGET_LINK_LIBRARIES(simple_optimize types_slam2d) 11 | TARGET_LINK_LIBRARIES(simple_optimize types_slam3d) 12 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/slam2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE(${QT_USE_FILE}) 2 | 3 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 4 | 5 | QT4_WRAP_UI(UI_HEADERS base_main_window.ui) 6 | QT4_WRAP_CPP(UI_SOURCES main_window.h) 7 | 8 | ADD_EXECUTABLE(slam2d_g2o 9 | main_window.cpp 10 | slam2d_viewer.cpp 11 | slam2d_viewer.h 12 | slam2d_g2o.cpp 13 | draw_helpers.cpp 14 | ${UI_HEADERS} 15 | ${UI_SOURCES} 16 | ) 17 | 18 | SET_TARGET_PROPERTIES(slam2d_g2o PROPERTIES OUTPUT_NAME slam2d_g2o${EXE_POSTFIX}) 19 | 20 | INCLUDE_DIRECTORIES(${QGLVIEWER_INCLUDE_DIR} ${CMAKE_BINARY_DIR} ${QT_QTCORE_INCLUDE_DIR} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR} ${QT_QTCORE_INCLUDE_DIR}/..) 21 | 22 | TARGET_LINK_LIBRARIES(slam2d_g2o core solver_csparse types_slam2d ${QGLVIEWER_LIBRARY} ${QT_QTXML_LIBRARY} ${QT_QTOPENGL_LIBRARY} ${QT_QTGUI_LIBRARY} ${QT_QTCORE_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) 23 | 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/slam2d/draw_helpers.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_DRAW_HELPERS_H 28 | #define G2O_DRAW_HELPERS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * draw a disk 36 | */ 37 | void drawDisk(GLfloat radius); 38 | 39 | /** 40 | * draw a circle using GL_LINES 41 | */ 42 | void drawCircle(GLfloat radius, int segments = 32); 43 | 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/slam2d/main_window.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #ifndef G2O_MAIN_WINDOW_H 20 | #define G2O_MAIN_WINDOW_H 21 | 22 | #include "ui_base_main_window.h" 23 | 24 | namespace g2o { 25 | class OptimizationAlgorithm; 26 | } 27 | 28 | class MainWindow : public QMainWindow, public Ui::BaseMainWindow 29 | { 30 | Q_OBJECT 31 | public: 32 | MainWindow(QWidget * parent = 0, Qt::WindowFlags flags = 0); 33 | ~MainWindow(); 34 | 35 | g2o::OptimizationAlgorithm* solverGaussNewton; 36 | g2o::OptimizationAlgorithm* solverLevenberg; 37 | 38 | public slots: 39 | void on_actionLoad_triggered(bool); 40 | void on_actionSave_triggered(bool); 41 | void on_actionQuit_triggered(bool); 42 | void on_btnOptimize_clicked(); 43 | void on_btnInitialGuess_clicked(); 44 | 45 | protected: 46 | void fixGraph(); 47 | 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/slam2d/slam2d_viewer.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // This file is part of g2o. 5 | // 6 | // g2o is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // g2o is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with g2o. If not, see . 18 | 19 | #ifndef G2O_QGL_GRAPH_VIEWER_H 20 | #define G2O_QGL_GRAPH_VIEWER_H 21 | 22 | #include "qglviewer.h" 23 | 24 | namespace g2o { 25 | 26 | class SparseOptimizer; 27 | 28 | class Slam2DViewer : public QGLViewer 29 | { 30 | public: 31 | Slam2DViewer(QWidget* parent=NULL, const QGLWidget* shareWidget=0, Qt::WFlags flags=0); 32 | ~Slam2DViewer(); 33 | virtual void draw(); 34 | void init(); 35 | 36 | public: 37 | SparseOptimizer* graph; 38 | bool drawCovariance; 39 | }; 40 | 41 | } // end namespace 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/sphere/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(create_sphere 2 | create_sphere.cpp 3 | ) 4 | 5 | SET_TARGET_PROPERTIES(create_sphere PROPERTIES OUTPUT_NAME create_sphere${EXE_POSTFIX}) 6 | TARGET_LINK_LIBRARIES(create_sphere core types_slam3d) 7 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/target/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 2 | 3 | ADD_EXECUTABLE(static_target 4 | static_target.cpp 5 | ) 6 | TARGET_LINK_LIBRARIES(static_target core solver_cholmod) 7 | SET_TARGET_PROPERTIES(static_target PROPERTIES OUTPUT_NAME static_target${EXE_POSTFIX}) 8 | 9 | ADD_EXECUTABLE(constant_velocity_target 10 | constant_velocity_target.cpp 11 | ) 12 | 13 | SET_TARGET_PROPERTIES(constant_velocity_target PROPERTIES OUTPUT_NAME constant_velocity_target${EXE_POSTFIX}) 14 | 15 | TARGET_LINK_LIBRARIES(constant_velocity_target core solver_cholmod solver_pcg types_slam2d) 16 | 17 | 18 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/target/continuous_to_discrete.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONTINUOUS_TO_DISCRETE_H_ 2 | #define G2O_CONTINUOUS_TO_DISCRETE_H_ 3 | 4 | #include 5 | #include 6 | 7 | // Form for fixed-size matrices 8 | template 9 | void continuousToDiscrete(MatrixType& Fd, MatrixType& Qd, 10 | const MatrixType& Fc, const MatrixType& Qc, double dt) 11 | { 12 | enum 13 | { 14 | NX = MatrixType::ColsAtCompileTime, 15 | NY = MatrixType::RowsAtCompileTime, 16 | NX2 = 2 * MatrixType::RowsAtCompileTime 17 | }; 18 | 19 | typedef Eigen::Matrix DoubleSizedMatrixType; 20 | DoubleSizedMatrixType bigA(NX2,NX2), bigB(NX2,NX2); 21 | 22 | // Construct the "big A matrix" 23 | bigA.template topLeftCorner()=-Fc*dt; 24 | bigA.template topRightCorner()= Qc * dt; 25 | bigA.template bottomLeftCorner().setZero(); 26 | bigA.template bottomRightCorner()=Fc.transpose() * dt; 27 | 28 | // bigB = expm(bigA) 29 | Eigen::MatrixExponential me(bigA); 30 | me.compute(bigB); 31 | 32 | // Extract the discrete time components 33 | Fd = bigB.template bottomRightCorner().transpose(); 34 | Qd = Fd * bigB.template topRightCorner(); 35 | } 36 | 37 | #endif // __CONTINUOUS_TO_DISCRETE_H__ 38 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/tutorial_slam2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 2 | 3 | ADD_LIBRARY(tutorial_slam2d_library ${G2O_LIB_TYPE} 4 | edge_se2_pointxy.cpp se2.h vertex_point_xy.h 5 | edge_se2.cpp edge_se2_pointxy.h vertex_se2.cpp 6 | edge_se2.h rand.h vertex_point_xy.cpp vertex_se2.h 7 | parameter_se2_offset.h parameter_se2_offset.cpp 8 | types_tutorial_slam2d.h types_tutorial_slam2d.cpp 9 | simulator.h simulator.cpp 10 | g2o_tutorial_slam2d_api.h 11 | ) 12 | 13 | SET_TARGET_PROPERTIES(tutorial_slam2d_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}tutorial_slam2d) 14 | 15 | TARGET_LINK_LIBRARIES(tutorial_slam2d_library core solver_csparse) 16 | 17 | ADD_EXECUTABLE(tutorial_slam2d 18 | tutorial_slam2d.cpp 19 | ) 20 | 21 | SET_TARGET_PROPERTIES(tutorial_slam2d PROPERTIES OUTPUT_NAME tutorial_slam2d${EXE_POSTFIX}) 22 | 23 | TARGET_LINK_LIBRARIES(tutorial_slam2d tutorial_slam2d_library) 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/tutorial_slam2d/g2o_tutorial_slam2d_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TUTORIAL_SLAM2D_API_H 11 | #define G2O_TUTORIAL_SLAM2D_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef tutorial_slam2d_library_EXPORTS 19 | #define G2O_TUTORIAL_SLAM2D_API __declspec(dllexport) 20 | #else 21 | #define G2O_TUTORIAL_SLAM2D_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TUTORIAL_SLAM2D_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TUTORIAL_SLAM2D_API 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TYPES_TUTORIAL_SLAM2D_ 28 | #define G2O_TYPES_TUTORIAL_SLAM2D_ 29 | 30 | #include "vertex_se2.h" 31 | 32 | #include "parameter_se2_offset.h" 33 | 34 | #include "edge_se2.h" 35 | #include "edge_se2_pointxy.h" 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(pcg) 2 | ADD_SUBDIRECTORY(dense) 3 | ADD_SUBDIRECTORY(structure_only) 4 | 5 | IF(CSPARSE_FOUND) 6 | ADD_SUBDIRECTORY(csparse) 7 | ADD_SUBDIRECTORY(slam2d_linear) 8 | ENDIF() 9 | 10 | IF(CHOLMOD_FOUND) 11 | ADD_SUBDIRECTORY(cholmod) 12 | ENDIF(CHOLMOD_FOUND) 13 | 14 | # Sparse Module of Eigen is stable starting from 3.1 15 | IF (DEFINED EIGEN3_VERSION) 16 | IF(${EIGEN3_VERSION} VERSION_GREATER "3.1.0" OR ${EIGEN3_VERSION} VERSION_EQUAL "3.1.0") 17 | ADD_SUBDIRECTORY(eigen) 18 | ENDIF() 19 | ENDIF() 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/cholmod/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(solver_cholmod ${G2O_LIB_TYPE} 2 | solver_cholmod.cpp 3 | linear_solver_cholmod.h 4 | ) 5 | 6 | INCLUDE_DIRECTORIES(${CHOLMOD_INCLUDE_DIR}) 7 | 8 | SET_TARGET_PROPERTIES(solver_cholmod PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_cholmod) 9 | 10 | ADD_DEFINITIONS(${BLAS_DEFINITIONS} ${LAPACK_DEFINITIONS}) 11 | TARGET_LINK_LIBRARIES(solver_cholmod ${CHOLMOD_LIBRARIES} ${BLAS_LIBRARIES} ${LAPACK_LIBRARIES} core) 12 | 13 | INSTALL(TARGETS solver_cholmod 14 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 15 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 16 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 17 | ) 18 | 19 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 20 | 21 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/cholmod) 22 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/csparse/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 2 | 3 | ADD_LIBRARY(csparse_extension ${G2O_LGPL_LIB_TYPE} 4 | csparse_helper.cpp csparse_helper.h 5 | g2o_csparse_extension_api.h 6 | ) 7 | SET_TARGET_PROPERTIES(csparse_extension PROPERTIES OUTPUT_NAME ${LIB_PREFIX}csparse_extension) 8 | 9 | IF(BUILD_CSPARSE) 10 | TARGET_LINK_LIBRARIES(csparse_extension csparse) 11 | ELSE() 12 | TARGET_LINK_LIBRARIES(csparse_extension ${CSPARSE_LIBRARY}) 13 | ENDIF() 14 | 15 | 16 | ADD_LIBRARY(solver_csparse ${G2O_LIB_TYPE} 17 | solver_csparse.cpp 18 | linear_solver_csparse.h 19 | g2o_csparse_api.h 20 | ) 21 | SET_TARGET_PROPERTIES(solver_csparse PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_csparse) 22 | 23 | TARGET_LINK_LIBRARIES(solver_csparse csparse_extension core) 24 | 25 | 26 | INSTALL(TARGETS solver_csparse csparse_extension 27 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 28 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 29 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 30 | ) 31 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 32 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/csparse) 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/csparse/csparse_helper.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef G2O_CSPARSE_HELPER_H 18 | #define G2O_CSPARSE_HELPER_H 19 | 20 | #ifndef NCOMPLEX 21 | #define NCOMPLEX 22 | #endif 23 | #include 24 | 25 | #include "g2o_csparse_extension_api.h" 26 | 27 | namespace g2o { 28 | 29 | namespace csparse_extension { 30 | 31 | /** 32 | * write the sparse matrix to a file loadable with ocatve 33 | */ 34 | G2O_CSPARSE_EXTENSION_API bool writeCs2Octave(const char* filename, const cs* A, bool upperTriangular = true); 35 | 36 | // our extensions to csparse 37 | G2O_CSPARSE_EXTENSION_API csn* cs_chol_workspace (const cs *A, const css *S, int* cin, double* xin); 38 | G2O_CSPARSE_EXTENSION_API int cs_cholsolsymb(const cs *A, double *b, const css* S, double* workspace, int* work); 39 | 40 | } // end namespace 41 | } // end namespace 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/csparse/g2o_csparse_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_SOLVER_CSPARSE_API_H 11 | #define G2O_SOLVER_CSPARSE_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef solver_csparse_EXPORTS 19 | #define G2O_SOLVER_CSPARSE_API __declspec(dllexport) 20 | #else 21 | #define G2O_SOLVER_CSPARSE_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_SOLVER_CSPARSE_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_SOLVER_CSPARSE_API 30 | #endif 31 | 32 | #endif // G2O_CSPARSE_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/csparse/g2o_csparse_extension_api.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2012 Rainer Kuemmerle 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef G2O_CSPARSE_EXTENSION_API_H 18 | #define G2O_CSPARSE_EXTENSION_API_H 19 | 20 | #include "g2o/config.h" 21 | 22 | #ifdef _MSC_VER 23 | // We are using a Microsoft compiler: 24 | #ifdef G2O_LGPL_SHARED_LIBS 25 | #ifdef csparse_extension_EXPORTS 26 | #define G2O_CSPARSE_EXTENSION_API __declspec(dllexport) 27 | #else 28 | #define G2O_CSPARSE_EXTENSION_API __declspec(dllimport) 29 | #endif 30 | #else 31 | #define G2O_CSPARSE_EXTENSION_API 32 | #endif 33 | 34 | #else 35 | // Not Microsoft compiler so set empty definition: 36 | #define G2O_CSPARSE_EXTENSION_API 37 | #endif 38 | 39 | #endif // G2O_CSPARSE_API_H 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/dense/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(solver_dense ${G2O_LIB_TYPE} 2 | solver_dense.cpp linear_solver_dense.h 3 | ) 4 | 5 | SET_TARGET_PROPERTIES(solver_dense PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_dense) 6 | 7 | TARGET_LINK_LIBRARIES(solver_dense core) 8 | 9 | INSTALL(TARGETS solver_dense 10 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 11 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 12 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 13 | ) 14 | 15 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 16 | 17 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/dense) 18 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${G2O_EIGEN3_INCLUDE}) 2 | 3 | ADD_LIBRARY(solver_eigen ${G2O_LIB_TYPE} 4 | solver_eigen.cpp 5 | linear_solver_eigen.h 6 | ) 7 | SET_TARGET_PROPERTIES(solver_eigen PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_eigen) 8 | TARGET_LINK_LIBRARIES(solver_eigen core) 9 | 10 | INSTALL(TARGETS solver_eigen 11 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 12 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 13 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 14 | ) 15 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 16 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/eigen) 17 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/pcg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(solver_pcg ${G2O_LIB_TYPE} 2 | solver_pcg.cpp 3 | linear_solver_pcg.h 4 | linear_solver_pcg.hpp 5 | ) 6 | 7 | SET_TARGET_PROPERTIES(solver_pcg PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_pcg) 8 | 9 | TARGET_LINK_LIBRARIES(solver_pcg core) 10 | 11 | INSTALL(TARGETS solver_pcg 12 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 13 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 14 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 15 | ) 16 | 17 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 18 | 19 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/pcg) 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/slam2d_linear/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(solver_slam2d_linear ${G2O_LIB_TYPE} 2 | slam2d_linear.cpp 3 | solver_slam2d_linear.h solver_slam2d_linear.cpp 4 | g2o_slam2d_linear_api.h 5 | ) 6 | 7 | INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR}) 8 | 9 | SET_TARGET_PROPERTIES(solver_slam2d_linear PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_slam2d_linear) 10 | TARGET_LINK_LIBRARIES(solver_slam2d_linear solver_csparse types_slam2d) 11 | 12 | INSTALL(TARGETS solver_slam2d_linear 13 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 14 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 15 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 16 | ) 17 | 18 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 19 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/slam2d_linear) 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/slam2d_linear/g2o_slam2d_linear_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_SLAM2D_LINEAR_API_H 11 | #define G2O_SLAM2D_LINEAR_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef solver_slam2d_linear_EXPORTS 19 | #define G2O_SLAM2D_LINEAR_API __declspec(dllexport) 20 | #else 21 | #define G2O_SLAM2D_LINEAR_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_SLAM2D_LINEAR_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_SLAM2D_LINEAR_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SLAM2D_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/solvers/structure_only/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(solver_structure_only ${G2O_LIB_TYPE} 2 | structure_only.cpp 3 | structure_only_solver.h 4 | ) 5 | 6 | SET_TARGET_PROPERTIES(solver_structure_only PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_structure_only) 7 | TARGET_LINK_LIBRARIES(solver_structure_only core) 8 | 9 | INSTALL(TARGETS solver_structure_only 10 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 11 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 12 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 13 | ) 14 | 15 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 16 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/solvers/structure_only) 17 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/stuff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(stuff ${G2O_LIB_TYPE} 2 | filesys_tools.h string_tools.h 3 | color_macros.h macros.h timeutil.cpp 4 | command_args.cpp misc.h sparse_helper.cpp timeutil.h 5 | command_args.h os_specific.c sparse_helper.h 6 | filesys_tools.cpp os_specific.h string_tools.cpp 7 | property.cpp property.h 8 | sampler.cpp sampler.h unscented.h 9 | tictoc.cpp tictoc.h 10 | g2o_stuff_api.h 11 | ) 12 | 13 | SET_TARGET_PROPERTIES(stuff PROPERTIES OUTPUT_NAME ${LIB_PREFIX}stuff) 14 | 15 | INSTALL(TARGETS stuff 16 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 17 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 18 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 19 | ) 20 | 21 | # build our OpenGL helper library 22 | IF(OPENGL_FOUND AND G2O_HAVE_OPENGL) 23 | ADD_LIBRARY(opengl_helper ${G2O_LIB_TYPE} 24 | opengl_primitives.cpp opengl_primitives.h 25 | ) 26 | INSTALL(TARGETS opengl_helper 27 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 28 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 29 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 30 | ) 31 | TARGET_LINK_LIBRARIES(opengl_helper ${OPENGL_LIBRARIES}) 32 | SET_TARGET_PROPERTIES(opengl_helper PROPERTIES OUTPUT_NAME ${LIB_PREFIX}opengl_helper) 33 | ENDIF() 34 | 35 | IF(${CMAKE_SYSTEM_NAME} MATCHES "Linux" AND NOT ANDROID) 36 | TARGET_LINK_LIBRARIES(stuff rt) 37 | ENDIF() 38 | 39 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 40 | 41 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/stuff) 42 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/stuff/g2o_stuff_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_STUFF_API_H 11 | #define G2O_STUFF_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | 18 | #ifdef G2O_SHARED_LIBS 19 | #ifdef stuff_EXPORTS 20 | #define G2O_STUFF_API __declspec(dllexport) 21 | #else 22 | #define G2O_STUFF_API __declspec(dllimport) 23 | #endif 24 | #else 25 | #define G2O_STUFF_API 26 | #endif 27 | 28 | #else 29 | // Not Microsoft compiler so set empty definition: 30 | #define G2O_STUFF_API 31 | #endif 32 | 33 | #endif // G2O_STUFF_API_H 34 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/stuff/opengl_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_OPENGL_WRAPPER_H 2 | #define G2O_OPENGL_WRAPPER_H 3 | 4 | #include "g2o/config.h" 5 | 6 | #ifdef WINDOWS 7 | #include 8 | #endif 9 | 10 | #ifdef G2O_OPENGL_FOUND 11 | # ifdef __APPLE__ 12 | # include 13 | # else 14 | # include 15 | # endif 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Pre-canned types 2 | ADD_SUBDIRECTORY(data) 3 | ADD_SUBDIRECTORY(slam2d) 4 | ADD_SUBDIRECTORY(slam3d) 5 | ADD_SUBDIRECTORY(sba) 6 | ADD_SUBDIRECTORY(sim3) 7 | ADD_SUBDIRECTORY(icp) 8 | ADD_SUBDIRECTORY(sclam2d) 9 | 10 | ADD_SUBDIRECTORY(slam2d_addons) 11 | ADD_SUBDIRECTORY(slam3d_addons) 12 | 13 | OPTION(G2O_BUILD_DEPRECATED_TYPES "Compile the deprecated types" OFF) 14 | IF(G2O_BUILD_DEPRECATED_TYPES) 15 | ADD_SUBDIRECTORY(deprecated) 16 | ENDIF() 17 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/data/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_data ${G2O_LIB_TYPE} 2 | types_data.cpp 3 | robot_data.cpp robot_data.h 4 | vertex_tag.cpp vertex_tag.h 5 | vertex_ellipse.cpp vertex_ellipse.h 6 | laser_parameters.cpp laser_parameters.h 7 | raw_laser.cpp raw_laser.h 8 | robot_laser.cpp robot_laser.h 9 | data_queue.cpp data_queue.h 10 | g2o_types_data_api.h 11 | ) 12 | 13 | SET_TARGET_PROPERTIES(types_data PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_data) 14 | 15 | TARGET_LINK_LIBRARIES(types_data core types_slam2d) 16 | IF(G2O_HAVE_OPENGL) 17 | TARGET_LINK_LIBRARIES(types_data freeglut_minimal opengl_helper) 18 | ENDIF() 19 | 20 | INSTALL(TARGETS types_data 21 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 22 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 23 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 24 | ) 25 | 26 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 27 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/data) 28 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/data/g2o_types_data_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_DATA_API_H 11 | #define G2O_TYPES_DATA_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_data_EXPORTS 19 | #define G2O_TYPES_DATA_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_DATA_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_DATA_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_DATA_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_DATA_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/data/types_data.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TYPES_DATA_H 28 | #define G2O_TYPES_DATA_H 29 | 30 | #include "robot_laser.h" 31 | #include "vertex_tag.h" 32 | #include "vertex_ellipse.h" 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/deprecated/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(slam3d) 2 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/deprecated/slam3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(deprecated_types_slam3d ${G2O_LIB_TYPE} 2 | se3quat_gradients.cpp 3 | se3quat_gradients.h 4 | vertex_se3_quat.cpp 5 | vertex_se3_quat.h 6 | edge_se3_quat.cpp 7 | edge_se3_quat.h 8 | vertex_pointxyz.cpp 9 | vertex_pointxyz.h 10 | parameter_se3_offset.cpp 11 | parameter_se3_offset.h 12 | edge_se3_offset.cpp 13 | edge_se3_offset.h 14 | edge_se3_prior.cpp 15 | edge_se3_prior.h 16 | edge_se3_pointxyz.cpp 17 | edge_se3_pointxyz.h 18 | parameter_camera.cpp 19 | parameter_camera.h 20 | edge_se3_pointxyz_disparity.cpp 21 | edge_se3_pointxyz_disparity.h 22 | edge_se3_pointxyz_depth.cpp 23 | edge_se3_pointxyz_depth.h 24 | types_slam3d.cpp 25 | types_slam3d.h 26 | g2o_deprecated_types_slam3d_api.h 27 | ) 28 | 29 | SET_TARGET_PROPERTIES(deprecated_types_slam3d PROPERTIES OUTPUT_NAME ${LIB_PREFIX}deprecated_types_slam3d) 30 | TARGET_LINK_LIBRARIES(deprecated_types_slam3d ${OPENGL_gl_LIBRARY} core types_slam3d) 31 | 32 | INSTALL(TARGETS deprecated_types_slam3d 33 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 34 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 35 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 36 | ) 37 | 38 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 39 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/deprecated/slam3d) 40 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/deprecated/slam3d/g2o_deprecated_types_slam3d_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_DEPRECATED_TYPES_SLAM3D_API_H 11 | #define G2O_DEPRECATED_TYPES_SLAM3D_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef deprecated_types_slam3d_EXPORTS 19 | #define G2O_DEPRECATED_TYPES_SLAM3D_API __declspec(dllexport) 20 | #else 21 | #define G2O_DEPRECATED_TYPES_SLAM3D_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_DEPRECATED_TYPES_SLAM3D_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_DEPRECATED_TYPES_SLAM3D_API 30 | #endif 31 | 32 | #endif // G2O_DEPRECATED_TYPES_SLAM3D_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_icp ${G2O_LIB_TYPE} 2 | types_icp.cpp types_icp.h 3 | g2o_types_icp_api.h 4 | ) 5 | 6 | SET_TARGET_PROPERTIES(types_icp PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_icp) 7 | 8 | TARGET_LINK_LIBRARIES(types_icp types_sba types_slam3d) 9 | 10 | INSTALL(TARGETS types_icp 11 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 12 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 13 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 14 | ) 15 | 16 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 17 | 18 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/icp) 19 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/icp/g2o_types_icp_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_ICP_API_H 11 | #define G2O_TYPES_ICP_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_icp_EXPORTS 19 | #define G2O_TYPES_ICP_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_ICP_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_ICP_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_ICP_API 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/sba/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_sba ${G2O_LIB_TYPE} 2 | types_sba.h types_six_dof_expmap.h 3 | types_sba.cpp types_six_dof_expmap.cpp 4 | g2o_types_sba_api.h 5 | ) 6 | 7 | SET_TARGET_PROPERTIES(types_sba PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_sba) 8 | 9 | TARGET_LINK_LIBRARIES(types_sba core types_slam3d) 10 | 11 | INSTALL(TARGETS types_sba 12 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 13 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 14 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 15 | ) 16 | 17 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 18 | 19 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/sba) 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/sba/g2o_types_sba_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_SBA_API_H 11 | #define G2O_TYPES_SBA_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_sba_EXPORTS 19 | #define G2O_TYPES_SBA_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_SBA_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_SBA_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_SBA_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SBA_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/sclam2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_sclam2d ${G2O_LIB_TYPE} 2 | edge_se2_sensor_calib.cpp edge_se2_sensor_calib.h 3 | vertex_odom_differential_params.cpp vertex_odom_differential_params.h 4 | edge_se2_odom_differential_calib.cpp edge_se2_odom_differential_calib.h 5 | odometry_measurement.cpp odometry_measurement.h 6 | types_sclam2d.cpp types_sclam2d.h 7 | g2o_types_sclam2d_api.h 8 | ) 9 | 10 | SET_TARGET_PROPERTIES(types_sclam2d PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_sclam2d) 11 | 12 | TARGET_LINK_LIBRARIES(types_sclam2d types_slam2d core) 13 | IF(G2O_HAVE_OPENGL) 14 | TARGET_LINK_LIBRARIES(types_sclam2d ${OPENGL_gl_LIBRARY}) 15 | ENDIF() 16 | 17 | INSTALL(TARGETS types_sclam2d 18 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 19 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 20 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 21 | ) 22 | 23 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 24 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/sclam2d) 25 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/sclam2d/g2o_types_sclam2d_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_SCLAM2D_API_H 11 | #define G2O_TYPES_SCLAM2D_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_sclam2d_EXPORTS 19 | #define G2O_TYPES_SCLAM2D_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_SCLAM2D_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_SCLAM2D_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_SCLAM2D_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SCLAM2D_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/sclam2d/types_sclam2d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TYPES_SCLAM_H 28 | #define G2O_TYPES_SCLAM_H 29 | 30 | #include "edge_se2_sensor_calib.h" 31 | #include "edge_se2_odom_differential_calib.h" 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/sim3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_sim3 ${G2O_LIB_TYPE} 2 | types_seven_dof_expmap.cpp 3 | types_seven_dof_expmap.h 4 | ) 5 | 6 | SET_TARGET_PROPERTIES(types_sim3 PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_sim3) 7 | 8 | TARGET_LINK_LIBRARIES(types_sim3 types_sba) 9 | 10 | INSTALL(TARGETS types_sim3 11 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 12 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 13 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 14 | ) 15 | 16 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 17 | 18 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/sim3) 19 | 20 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_slam2d ${G2O_LIB_TYPE} 2 | se2.h 3 | edge_se2_pointxy_bearing.h edge_se2_prior.h 4 | edge_se2.cpp edge_se2_pointxy_calib.cpp types_slam2d.cpp 5 | edge_se2.h edge_se2_pointxy_calib.h vertex_point_xy.cpp 6 | edge_se2_pointxy.cpp vertex_point_xy.h 7 | edge_se2_pointxy.h vertex_se2.cpp 8 | edge_se2_pointxy_bearing.cpp edge_se2_prior.cpp vertex_se2.h 9 | parameter_se2_offset.cpp parameter_se2_offset.h 10 | edge_se2_offset.cpp edge_se2_offset.h 11 | edge_se2_pointxy_offset.cpp edge_se2_pointxy_offset.h 12 | edge_se2_xyprior.cpp edge_se2_xyprior.h 13 | edge_pointxy.cpp edge_pointxy.h 14 | edge_se2_twopointsxy.cpp edge_se2_twopointsxy.h 15 | edge_se2_lotsofxy.cpp edge_se2_lotsofxy.h 16 | g2o_types_slam2d_api.h 17 | ) 18 | 19 | SET_TARGET_PROPERTIES(types_slam2d PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_slam2d) 20 | 21 | TARGET_LINK_LIBRARIES(types_slam2d core) 22 | IF(G2O_HAVE_OPENGL) 23 | TARGET_LINK_LIBRARIES(types_slam2d opengl_helper ${OPENGL_gl_LIBRARY} ) 24 | ENDIF() 25 | 26 | INSTALL(TARGETS types_slam2d 27 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 28 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 29 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 30 | ) 31 | 32 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 33 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/slam2d) 34 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d/edge_se2_lotsofxy.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_EDGE_SE2_LOTSOF_XY 2 | #define G2O_EDGE_SE2_LOTSOF_XY 3 | 4 | #include "g2o/config.h" 5 | #include "g2o_types_slam2d_api.h" 6 | #include "g2o/core/base_multi_edge.h" 7 | #include "vertex_se2.h" 8 | #include "vertex_point_xy.h" 9 | 10 | namespace g2o { 11 | 12 | class G2O_TYPES_SLAM2D_API EdgeSE2LotsOfXY : public BaseMultiEdge<-1,VectorXD> 13 | { 14 | protected: 15 | unsigned int _observedPoints; 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 19 | EdgeSE2LotsOfXY(); 20 | 21 | void setDimension(int dimension_) 22 | { 23 | _dimension = dimension_; 24 | _information.resize(dimension_, dimension_); 25 | _error.resize(dimension_, 1); 26 | _measurement.resize(dimension_, 1); 27 | } 28 | 29 | void setSize(int vertices) 30 | { 31 | resize(vertices); 32 | _observedPoints = vertices-1; 33 | setDimension(_observedPoints*2); 34 | } 35 | 36 | virtual void computeError(); 37 | 38 | virtual bool read(std::istream& is); 39 | virtual bool write(std::ostream& os) const; 40 | 41 | virtual bool setMeasurementFromState(); 42 | 43 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 44 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 45 | 46 | virtual void linearizeOplus(); 47 | }; 48 | 49 | } // end namespace g2o 50 | 51 | #endif // G2O_EDGE_SE2_LOTSOF_XY 52 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d/edge_se2_twopointsxy.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_EDGE_SE2_TWOPOINTS_XY_H 2 | #define G2O_EDGE_SE2_TWOPOINTS_XY_H 3 | 4 | #include "g2o/config.h" 5 | #include "g2o_types_slam2d_api.h" 6 | #include "g2o/core/base_multi_edge.h" 7 | #include "vertex_se2.h" 8 | #include "vertex_point_xy.h" 9 | 10 | namespace g2o{ 11 | 12 | class G2O_TYPES_SLAM2D_API EdgeSE2TwoPointsXY : public BaseMultiEdge<4, Vector4D> 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 16 | EdgeSE2TwoPointsXY(); 17 | 18 | virtual void computeError(); 19 | 20 | virtual bool read(std::istream& is); 21 | virtual bool write(std::ostream& os) const; 22 | 23 | virtual bool setMeasurementFromState(); 24 | 25 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 26 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 27 | }; 28 | } 29 | #endif // G2O_EDGE_SE2_TWOPOINTS_XY_H 30 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d/g2o_types_slam2d_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_SLAM2D_API_H 11 | #define G2O_TYPES_SLAM2D_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_slam2d_EXPORTS 19 | #define G2O_TYPES_SLAM2D_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_SLAM2D_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_SLAM2D_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_SLAM2D_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SLAM2D_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d_addons/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_slam2d_addons ${G2O_LIB_TYPE} 2 | line_2d.h 3 | types_slam2d_addons.cpp types_slam2d_addons.h 4 | vertex_segment2d.cpp vertex_segment2d.h 5 | edge_se2_segment2d.cpp edge_se2_segment2d.h 6 | edge_se2_segment2d_line.cpp edge_se2_segment2d_line.h 7 | edge_se2_segment2d_pointLine.cpp edge_se2_segment2d_pointLine.h 8 | vertex_line2d.cpp vertex_line2d.h 9 | edge_se2_line2d.cpp edge_se2_line2d.h 10 | edge_line2d_pointxy.cpp edge_line2d_pointxy.h 11 | edge_line2d.cpp edge_line2d.h 12 | g2o_types_slam2d_addons_api.h 13 | ) 14 | 15 | SET_TARGET_PROPERTIES(types_slam2d_addons PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_slam2d_addons) 16 | 17 | TARGET_LINK_LIBRARIES(types_slam2d_addons types_slam2d core) 18 | IF(G2O_HAVE_OPENGL) 19 | TARGET_LINK_LIBRARIES(types_slam2d_addons ${OPENGL_gl_LIBRARY}) 20 | ENDIF() 21 | 22 | INSTALL(TARGETS types_slam2d_addons 23 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 24 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 25 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 26 | ) 27 | 28 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 29 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/slam2d_addons) 30 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp: -------------------------------------------------------------------------------- 1 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 2 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 3 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 4 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 5 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 6 | 7 | #include "edge_se2_segment2d_pointLine.h" 8 | 9 | #ifdef WINDOWS 10 | #include 11 | #endif 12 | 13 | #ifdef G2O_HAVE_OPENGL 14 | #ifdef __APPLE__ 15 | #include 16 | #else 17 | #include 18 | #endif 19 | #endif 20 | 21 | namespace g2o { 22 | 23 | EdgeSE2Segment2DPointLine::EdgeSE2Segment2DPointLine() : 24 | BaseBinaryEdge<3, Vector3D, VertexSE2, VertexSegment2D>() 25 | { 26 | _pointNum = 0; 27 | } 28 | 29 | bool EdgeSE2Segment2DPointLine::read(std::istream& is) 30 | { 31 | is >> _pointNum; 32 | for (size_t i = 0; i < 3 ; i++) 33 | is >> _measurement[i]; 34 | for (size_t i = 0; i < 3 ; i++) 35 | for (size_t j = i; j < 3 ; j++) { 36 | is >> _information (i,j); 37 | _information (j,i) = _information (i,j); 38 | } 39 | return true; 40 | } 41 | 42 | bool EdgeSE2Segment2DPointLine::write(std::ostream& os) const 43 | { 44 | os << _pointNum << " "; 45 | for (size_t i = 0; i < 3 ; i++) 46 | os << _measurement[i] << " "; 47 | for (size_t i = 0; i < 3 ; i++) 48 | for (size_t j = i; j < 3 ; j++) { 49 | os << _information (i,j) << " "; 50 | } 51 | return os.good(); 52 | } 53 | 54 | } // end namespace 55 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d_addons/g2o_types_slam2d_addons_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_SLAM2D_ADDONS_API_H 11 | #define G2O_TYPES_SLAM2D_ADDONS_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_slam2d_addons_EXPORTS 19 | #define G2O_TYPES_SLAM2D_ADDONS_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_SLAM2D_ADDONS_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_SLAM2D_ADDONS_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_SLAM2D_ADDONS_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SLAM2D_ADDONS_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam2d_addons/types_slam2d_addons.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TYPES_SLAM2D_SEGMENT_ 28 | #define G2O_TYPES_SLAM2D_SEGMENT_ 29 | 30 | #include "g2o/config.h" 31 | #include "g2o/types/slam2d/types_slam2d.h" 32 | 33 | // line slam ver 1 34 | #include "edge_se2_segment2d.h" 35 | #include "edge_se2_segment2d_line.h" 36 | #include "edge_se2_segment2d_pointLine.h" 37 | 38 | // line slam ver 2 39 | #include "edge_se2_line2d.h" 40 | #include "edge_line2d_pointxy.h" 41 | #include "edge_line2d.h" 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d/dquat2mat.h: -------------------------------------------------------------------------------- 1 | #ifndef _DQUAT2MAT_H_ 2 | #define _DQUAT2MAT_H_ 3 | #include 4 | #include "g2o_types_slam3d_api.h" 5 | 6 | namespace g2o { 7 | namespace internal { 8 | 9 | void G2O_TYPES_SLAM3D_API compute_dq_dR ( Eigen::Matrix& dq_dR , const double& r11 , const double& r21 , const double& r31 , const double& r12 , const double& r22 , const double& r32 , const double& r13 , const double& r23 , const double& r33 ); 10 | 11 | void G2O_TYPES_SLAM3D_API compute_dR_dq ( Eigen::Matrix& dR_dq , const double& qx , const double& qy , const double& qz , const double& qw ) ; 12 | } 13 | } 14 | #endif 15 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d/edge_se3_lotsofxyz.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_SE3_LOTSOF_XYZ 2 | #define G2O_SE3_LOTSOF_XYZ 3 | 4 | #include "g2o/config.h" 5 | #include "g2o_types_slam3d_api.h" 6 | #include "g2o/core/base_multi_edge.h" 7 | #include "vertex_se3.h" 8 | #include "vertex_pointxyz.h" 9 | 10 | namespace g2o{ 11 | 12 | class G2O_TYPES_SLAM3D_API EdgeSE3LotsOfXYZ : public BaseMultiEdge<-1, VectorXD>{ 13 | 14 | protected: 15 | unsigned int _observedPoints; 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 19 | EdgeSE3LotsOfXYZ(); 20 | 21 | void setDimension(int dimension_){ 22 | _dimension = dimension_; 23 | _information.resize(dimension_, dimension_); 24 | _error.resize(dimension_, 1); 25 | _measurement.resize(dimension_, 1); 26 | } 27 | 28 | void setSize(int vertices){ 29 | resize(vertices); 30 | _observedPoints = vertices-1; 31 | setDimension(_observedPoints*3); 32 | } 33 | 34 | 35 | virtual void computeError(); 36 | 37 | virtual bool read(std::istream& is); 38 | virtual bool write(std::ostream& os) const; 39 | 40 | virtual bool setMeasurementFromState(); 41 | 42 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 43 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 44 | 45 | virtual void linearizeOplus(); 46 | 47 | }; 48 | 49 | } 50 | 51 | 52 | 53 | #endif // G2O_SE3_LOTSOF_XYZ 54 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d/g2o_types_slam3d_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_SLAM3D_API_H 11 | #define G2O_TYPES_SLAM3D_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_slam3d_EXPORTS 19 | #define G2O_TYPES_SLAM3D_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_SLAM3D_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_SLAM3D_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_SLAM3D_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SLAM3D_API_H 33 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d/isometry3d_gradients.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "isometry3d_gradients.h" 28 | #include 29 | namespace g2o { 30 | using namespace std; 31 | using namespace Eigen; 32 | 33 | namespace internal { 34 | #include "dquat2mat.cpp" 35 | } // end namespace internal 36 | 37 | } // end namespace 38 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d/types_slam3d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef G2O_TYPES_SLAM3D_ 18 | #define G2O_TYPES_SLAM3D_ 19 | 20 | #include "g2o/config.h" 21 | #include "g2o/core/base_vertex.h" 22 | #include "g2o/core/base_binary_edge.h" 23 | #include "g2o/core/hyper_graph_action.h" 24 | 25 | #define THREE_D_TYPES_ANALYTIC_JACOBIAN 26 | 27 | #include "vertex_se3.h" 28 | #include "edge_se3.h" 29 | #include "vertex_pointxyz.h" 30 | 31 | #include "parameter_se3_offset.h" 32 | #include "edge_se3_pointxyz.h" 33 | #include "edge_se3_offset.h" 34 | 35 | #include "parameter_camera.h" 36 | #include "parameter_stereo_camera.h" 37 | #include "edge_se3_pointxyz_disparity.h" 38 | #include "edge_se3_pointxyz_depth.h" 39 | #include "edge_se3_prior.h" 40 | 41 | #include "edge_pointxyz.h" 42 | 43 | #include "edge_se3_lotsofxyz.h" 44 | #endif 45 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d_addons/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_slam3d_addons ${G2O_LIB_TYPE} 2 | vertex_se3_euler.cpp 3 | vertex_se3_euler.h 4 | edge_se3_euler.cpp 5 | edge_se3_euler.h 6 | vertex_plane.cpp 7 | vertex_plane.h 8 | edge_se3_plane_calib.cpp 9 | edge_se3_plane_calib.h 10 | line3d.cpp line3d.h 11 | vertex_line3d.cpp vertex_line3d.h 12 | edge_se3_line.cpp edge_se3_line.h 13 | edge_line3d.cpp edge_line3d.h 14 | edge_plane.cpp edge_plane.h 15 | edge_se3_calib.cpp edge_se3_calib.h 16 | types_slam3d_addons.cpp 17 | types_slam3d_addons.h 18 | ) 19 | 20 | 21 | SET_TARGET_PROPERTIES(types_slam3d_addons PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_slam3d_addons) 22 | 23 | TARGET_LINK_LIBRARIES(types_slam3d_addons types_slam3d core) 24 | IF(G2O_HAVE_OPENGL) 25 | TARGET_LINK_LIBRARIES(types_slam3d_addons ${OPENGL_gl_LIBRARY} ) 26 | ENDIF() 27 | 28 | ADD_EXECUTABLE(test_line3d line3d_test.cpp) 29 | TARGET_LINK_LIBRARIES(test_line3d types_slam3d_addons types_slam3d) 30 | 31 | INSTALL(TARGETS types_slam3d_addons 32 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 33 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 34 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 35 | ) 36 | 37 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 38 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/slam3d_addons) 39 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d_addons/g2o_types_slam3d_addons_api.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_TYPES_SLAM3D_ADDONS_API_H 2 | #define G2O_TYPES_SLAM3D_ADDONS_API_H 3 | 4 | #include "g2o/config.h" 5 | 6 | #ifdef _MSC_VER 7 | // We are using a Microsoft compiler: 8 | #ifdef G2O_SHARED_LIBS 9 | #ifdef types_slam3d_addons_EXPORTS 10 | #define G2O_TYPES_SLAM3D_ADDONS_API __declspec(dllexport) 11 | #else 12 | #define G2O_TYPES_SLAM3D_ADDONS_API __declspec(dllimport) 13 | #endif 14 | #else 15 | #define G2O_TYPES_SLAM3D_ADDONS_API 16 | #endif 17 | 18 | #else 19 | // Not Microsoft compiler so set empty definition: 20 | #define G2O_TYPES_SLAM3D_ADDONS_API 21 | #endif 22 | 23 | #endif // G2O_TYPES_SLAM3D_ADDONS_API_H 24 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/g2o/types/slam3d_addons/line3d_test.cpp: -------------------------------------------------------------------------------- 1 | #include "line3d.h" 2 | #include "g2o/types/slam3d/isometry3d_mappings.h" 3 | #include "g2o/types/slam3d/isometry3d_gradients.h" 4 | #include 5 | 6 | using namespace std; 7 | using namespace Eigen; 8 | using namespace g2o; 9 | using namespace g2o::internal; 10 | 11 | template 12 | ostream& printVector(ostream& os, const T& t) 13 | { 14 | for (int i=0; i> lv[i]; 35 | setEstimate(Line3D(lv)); 36 | return true; 37 | } 38 | 39 | bool VertexLine3D::write(std::ostream& os) const { 40 | Vector6d lv=_estimate; 41 | for (int i=0; i<6; i++){ 42 | os << lv[i] << " "; 43 | } 44 | return os.good(); 45 | } 46 | 47 | } 48 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/script/install-deps-linux.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -ev 4 | 5 | sudo apt-get update -qq 6 | sudo apt-get install -qq libqt4-dev libqt4-opengl-dev libqglviewer-dev libeigen3-dev libsuitesparse-dev 7 | -------------------------------------------------------------------------------- /g2o_with_orbslam2/script/install-deps-osx.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -ev 4 | 5 | brew update 6 | 7 | if brew outdated | grep -qx cmake; then 8 | brew upgrade cmake; 9 | fi 10 | 11 | brew install eigen 12 | --------------------------------------------------------------------------------