├── .gitignore
├── 3rdParty
└── g2o
│ ├── 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
│ ├── Makefile
│ ├── 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
│ ├── Makefile
│ ├── README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt
│ ├── doxygen
│ │ ├── doxy.config
│ │ └── readme.txt
│ ├── g2o.pdf
│ ├── g2o.tex
│ ├── license-bsd.txt
│ ├── license-gpl.txt
│ ├── license-lgpl.txt
│ ├── pics
│ │ ├── classes.eps
│ │ ├── classes.fig
│ │ ├── classes.svg
│ │ ├── hgraph.eps
│ │ ├── hgraph.fig
│ │ ├── slam.eps
│ │ ├── slam.fig
│ │ ├── viewer.eps
│ │ └── viewer.png
│ └── robots.bib
│ ├── 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
│ │ │ │ ├── example
│ │ │ │ ├── CMakeLists.txt
│ │ │ │ ├── example_slam_interface.cpp
│ │ │ │ ├── example_slam_interface.h
│ │ │ │ └── test_slam_interface.cpp
│ │ │ │ ├── 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_plane.cpp
│ │ │ ├── edge_se3_plane.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_pointxyz_reprojectionError.cpp
│ │ │ ├── edge_se3_pointxyz_reprojectionError.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_plane_quat.cpp
│ │ │ ├── vertex_plane_quat.h
│ │ │ ├── vertex_pointxyz.cpp
│ │ │ ├── vertex_pointxyz.h
│ │ │ ├── vertex_se3.cpp
│ │ │ ├── vertex_se3.h
│ │ │ ├── vertex_se3_quat.cpp
│ │ │ └── vertex_se3_quat.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
├── CMakeLists.txt
├── LICENSE
├── README.md
├── cmake_modules
├── FindCSparse.cmake
├── FindEigen3.cmake
└── FindG2O.cmake
├── res
├── groundtruth.txt
├── planes.txt
└── visualize.m
└── src
└── main.cpp
/.gitignore:
--------------------------------------------------------------------------------
1 | # Compiled Object files
2 | *.slo
3 | *.lo
4 | *.o
5 | *.obj
6 |
7 | # Precompiled Headers
8 | *.gch
9 | *.pch
10 |
11 | # Compiled Dynamic libraries
12 | *.so
13 | *.dylib
14 | *.dll
15 |
16 | # Fortran module files
17 | *.mod
18 | *.smod
19 |
20 | # Compiled Static libraries
21 | *.lai
22 | *.la
23 | *.a
24 | *.lib
25 |
26 | # Executables
27 | *.exe
28 | *.out
29 | *.app
30 |
31 | res/
32 | build/
33 | cmake-build-debug/
34 | cmake-build-release/
35 |
36 | 3rdParty/g2o/bin/
37 | 3rdParty/g2o/build/
38 | 3rdParty/g2o/lib/
39 |
40 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/EXTERNAL/csparse/cs_qrsol.c:
--------------------------------------------------------------------------------
1 | #include "cs.h"
2 | /* x=A\b where A can be rectangular; b overwritten with solution */
3 | csi cs_qrsol (csi order, const cs *A, double *b)
4 | {
5 | double *x ;
6 | css *S ;
7 | csn *N ;
8 | cs *AT = NULL ;
9 | csi k, m, n, ok ;
10 | if (!CS_CSC (A) || !b) return (0) ; /* check inputs */
11 | n = A->n ;
12 | m = A->m ;
13 | if (m >= n)
14 | {
15 | S = cs_sqr (order, A, 1) ; /* ordering and symbolic analysis */
16 | N = cs_qr (A, S) ; /* numeric QR factorization */
17 | x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ; /* get workspace */
18 | ok = (S && N && x) ;
19 | if (ok)
20 | {
21 | cs_ipvec (S->pinv, b, x, m) ; /* x(0:m-1) = b(p(0:m-1) */
22 | for (k = 0 ; k < n ; k++) /* apply Householder refl. to x */
23 | {
24 | cs_happly (N->L, k, N->B [k], x) ;
25 | }
26 | cs_usolve (N->U, x) ; /* x = R\x */
27 | cs_ipvec (S->q, x, b, n) ; /* b(q(0:n-1)) = x(0:n-1) */
28 | }
29 | }
30 | else
31 | {
32 | AT = cs_transpose (A, 1) ; /* Ax=b is underdetermined */
33 | S = cs_sqr (order, AT, 1) ; /* ordering and symbolic analysis */
34 | N = cs_qr (AT, S) ; /* numeric QR factorization of A' */
35 | x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ; /* get workspace */
36 | ok = (AT && S && N && x) ;
37 | if (ok)
38 | {
39 | cs_pvec (S->q, b, x, m) ; /* x(q(0:m-1)) = b(0:m-1) */
40 | cs_utsolve (N->U, x) ; /* x = R'\x */
41 | for (k = m-1 ; k >= 0 ; k--) /* apply Householder refl. to x */
42 | {
43 | cs_happly (N->L, k, N->B [k], x) ;
44 | }
45 | cs_pvec (S->pinv, x, b, n) ; /* b(0:n-1) = x(p(0:n-1)) */
46 | }
47 | }
48 | cs_free (x) ;
49 | cs_sfree (S) ;
50 | cs_nfree (N) ;
51 | cs_spfree (AT) ;
52 | return (ok) ;
53 | }
54 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/Makefile:
--------------------------------------------------------------------------------
1 | SHELL = /bin/bash
2 |
3 | ifeq ($(VERBOSE), 1)
4 | QUIET=
5 | else
6 | QUIET=-s --no-print-directory
7 | endif
8 |
9 | all: build/Makefile
10 | @ $(MAKE) $(QUIET) -C build
11 |
12 | debug: build/Makefile
13 | @ $(MAKE) $(QUIET) -C build
14 |
15 | clean: build/Makefile
16 | @ $(MAKE) $(QUIET) -C build clean
17 |
18 | build/Makefile:
19 | @ echo "Running cmake to generate Makefile"; \
20 | cd build; \
21 | cmake ../; \
22 | cd -
23 |
24 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/.gitignore:
--------------------------------------------------------------------------------
1 | *.aux
2 | *.bbl
3 | *.blg
4 | *.dvi
5 | *.log
6 | doxygen/html
7 | doxygen/YES
8 |
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/Makefile:
--------------------------------------------------------------------------------
1 | all: g2o.pdf
2 |
3 | %.pdf: %.dvi
4 | dvipdf $<
5 |
6 | g2o.dvi: g2o.tex
7 | latex g2o.tex
8 | bibtex g2o
9 | latex g2o.tex
10 | latex g2o.tex
11 |
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/doxygen/readme.txt:
--------------------------------------------------------------------------------
1 | run the following in the current folder
2 |
3 | doxygen doxy.config
4 |
5 | assumes that dot tool is installed and available from path
6 |
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/g2o.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LRMPUT/PlaneSLAM/d215d11de2808e14a00d15e7af661ac204a2917b/3rdParty/g2o/doc/g2o.pdf
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/license-bsd.txt:
--------------------------------------------------------------------------------
1 | g2o - General Graph Optimization
2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,
3 | Kurt Konolige, and Wolfram Burgard
4 | All rights reserved.
5 |
6 | Redistribution and use in source and binary forms, with or without
7 | modification, are permitted provided that the following conditions are
8 | met:
9 |
10 | * Redistributions of source code must retain the above copyright notice,
11 | this list of conditions and the following disclaimer.
12 | * Redistributions in binary form must reproduce the above copyright
13 | notice, this list of conditions and the following disclaimer in the
14 | documentation and/or other materials provided with the distribution.
15 |
16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 |
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/pics/viewer.eps:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LRMPUT/PlaneSLAM/d215d11de2808e14a00d15e7af661ac204a2917b/3rdParty/g2o/doc/pics/viewer.eps
--------------------------------------------------------------------------------
/3rdParty/g2o/doc/pics/viewer.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LRMPUT/PlaneSLAM/d215d11de2808e14a00d15e7af661ac204a2917b/3rdParty/g2o/doc/pics/viewer.png
--------------------------------------------------------------------------------
/3rdParty/g2o/g2o/.gitignore:
--------------------------------------------------------------------------------
1 | playground
2 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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 |
--------------------------------------------------------------------------------
/3rdParty/g2o/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