├── data ├── sim_structured │ ├── map.bt │ ├── sim_structured.bag │ ├── sim_structured_1.pcd │ ├── sim_structured_10.pcd │ ├── sim_structured_11.pcd │ ├── sim_structured_12.pcd │ ├── sim_structured_2.pcd │ ├── sim_structured_3.pcd │ ├── sim_structured_4.pcd │ ├── sim_structured_5.pcd │ ├── sim_structured_6.pcd │ ├── sim_structured_7.pcd │ ├── sim_structured_8.pcd │ └── sim_structured_9.pcd ├── sim_unstructured │ ├── map.bt │ ├── sim_unstructured.bag │ ├── sim_unstructured_1.pcd │ ├── sim_unstructured_2.pcd │ ├── sim_unstructured_3.pcd │ ├── sim_unstructured_4.pcd │ ├── sim_unstructured_5.pcd │ ├── sim_unstructured_6.pcd │ ├── sim_unstructured_7.pcd │ ├── sim_unstructured_8.pcd │ ├── sim_unstructured_9.pcd │ ├── sim_unstructured_h.bag │ ├── sim_unstructured_10.pcd │ ├── sim_unstructured_11.pcd │ └── sim_unstructured_12.pcd └── sim_structured_long_term │ ├── sim_structured_long_term_1.pcd │ ├── sim_structured_long_term_10.pcd │ ├── sim_structured_long_term_11.pcd │ ├── sim_structured_long_term_12.pcd │ ├── sim_structured_long_term_13.pcd │ ├── sim_structured_long_term_14.pcd │ ├── sim_structured_long_term_15.pcd │ ├── sim_structured_long_term_16.pcd │ ├── sim_structured_long_term_17.pcd │ ├── sim_structured_long_term_18.pcd │ ├── sim_structured_long_term_19.pcd │ ├── sim_structured_long_term_2.pcd │ ├── sim_structured_long_term_20.pcd │ ├── sim_structured_long_term_21.pcd │ ├── sim_structured_long_term_22.pcd │ ├── sim_structured_long_term_23.pcd │ ├── sim_structured_long_term_24.pcd │ ├── sim_structured_long_term_25.pcd │ ├── sim_structured_long_term_26.pcd │ ├── sim_structured_long_term_27.pcd │ ├── sim_structured_long_term_28.pcd │ ├── sim_structured_long_term_29.pcd │ ├── sim_structured_long_term_3.pcd │ ├── sim_structured_long_term_30.pcd │ ├── sim_structured_long_term_31.pcd │ ├── sim_structured_long_term_32.pcd │ ├── sim_structured_long_term_33.pcd │ ├── sim_structured_long_term_34.pcd │ ├── sim_structured_long_term_35.pcd │ ├── sim_structured_long_term_36.pcd │ ├── sim_structured_long_term_37.pcd │ ├── sim_structured_long_term_38.pcd │ ├── sim_structured_long_term_39.pcd │ ├── sim_structured_long_term_4.pcd │ ├── sim_structured_long_term_40.pcd │ ├── sim_structured_long_term_41.pcd │ ├── sim_structured_long_term_42.pcd │ ├── sim_structured_long_term_43.pcd │ ├── sim_structured_long_term_44.pcd │ ├── sim_structured_long_term_45.pcd │ ├── sim_structured_long_term_46.pcd │ ├── sim_structured_long_term_47.pcd │ ├── sim_structured_long_term_48.pcd │ ├── sim_structured_long_term_49.pcd │ ├── sim_structured_long_term_5.pcd │ ├── sim_structured_long_term_50.pcd │ ├── sim_structured_long_term_51.pcd │ ├── sim_structured_long_term_52.pcd │ ├── sim_structured_long_term_53.pcd │ ├── sim_structured_long_term_54.pcd │ ├── sim_structured_long_term_55.pcd │ ├── sim_structured_long_term_56.pcd │ ├── sim_structured_long_term_57.pcd │ ├── sim_structured_long_term_58.pcd │ ├── sim_structured_long_term_59.pcd │ ├── sim_structured_long_term_6.pcd │ ├── sim_structured_long_term_60.pcd │ ├── sim_structured_long_term_7.pcd │ ├── sim_structured_long_term_8.pcd │ └── sim_structured_long_term_9.pcd ├── config ├── datasets │ ├── sim_structured.yaml │ ├── sim_unstructured.yaml │ └── sim_structured_long_term.yaml └── methods │ ├── gpoctomap.yaml │ ├── gpoctomap_large_map.yaml │ ├── bgkloctomap.yaml │ ├── bgkoctomap.yaml │ ├── bgklvoctomap.yaml │ ├── bgklvoctomap_large_map.yaml │ ├── bgkloctomap_large_map.yaml │ └── bgkoctomap_large_map.yaml ├── launch ├── la3dm_server.launch └── la3dm_static.launch ├── LICENSE ├── .travis.yml ├── include ├── bgklvoctomap │ ├── .vscode │ │ └── settings.json │ ├── bgklvoctree_node.h │ ├── bgklvblock.h │ ├── bgklvoctree.h │ └── bgklvinference.h ├── bgkoctomap │ ├── bgkoctree_node.h │ ├── bgkblock.h │ ├── bgkinference.h │ └── bgkoctree.h ├── bgkloctomap │ ├── bgkloctree_node.h │ ├── bgklblock.h │ └── bgkloctree.h ├── gpoctomap │ ├── gpoctree_node.h │ ├── gpblock.h │ ├── gpoctree.h │ └── gpregressor.h └── common │ └── markerarray_pub.h ├── src ├── common │ ├── point3f.cpp │ └── point6f.cpp ├── bgkoctomap │ ├── bgkoctree_node.cpp │ ├── bgkoctree.cpp │ ├── bgkoctomap_static_node.cpp │ └── bgkblock.cpp ├── bgkloctomap │ ├── bgkloctree_node.cpp │ └── bgkloctree.cpp ├── gpoctomap │ ├── gpoctree_node.cpp │ ├── gpoctree.cpp │ ├── gpoctomap_static_node.cpp │ └── gpblock.cpp └── bgklvoctomap │ ├── bgklvoctree_node.cpp │ ├── bgklvoctree.cpp │ └── bgklvoctomap_static_node.cpp ├── package.xml ├── README.md └── rviz ├── sim_structured_long_term.rviz ├── sim_structured.rviz └── sim_unstructured.rviz /data/sim_structured/map.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/map.bt -------------------------------------------------------------------------------- /data/sim_unstructured/map.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/map.bt -------------------------------------------------------------------------------- /data/sim_structured/sim_structured.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured.bag -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_1.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_1.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_10.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_10.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_11.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_11.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_12.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_12.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_2.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_2.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_3.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_3.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_4.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_4.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_5.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_5.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_6.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_6.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_7.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_7.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_8.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_8.pcd -------------------------------------------------------------------------------- /data/sim_structured/sim_structured_9.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured/sim_structured_9.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured.bag -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_1.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_1.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_2.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_2.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_3.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_3.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_4.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_4.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_5.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_5.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_6.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_6.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_7.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_7.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_8.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_8.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_9.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_9.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_h.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_h.bag -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_10.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_10.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_11.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_11.pcd -------------------------------------------------------------------------------- /data/sim_unstructured/sim_unstructured_12.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_unstructured/sim_unstructured_12.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_1.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_1.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_10.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_10.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_11.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_11.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_12.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_12.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_13.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_13.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_14.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_14.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_15.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_15.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_16.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_16.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_17.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_17.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_18.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_18.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_19.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_19.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_2.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_2.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_20.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_20.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_21.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_21.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_22.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_22.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_23.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_23.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_24.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_24.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_25.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_25.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_26.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_26.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_27.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_27.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_28.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_28.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_29.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_29.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_3.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_3.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_30.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_30.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_31.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_31.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_32.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_32.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_33.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_33.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_34.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_34.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_35.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_35.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_36.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_36.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_37.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_37.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_38.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_38.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_39.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_39.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_4.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_4.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_40.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_40.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_41.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_41.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_42.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_42.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_43.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_43.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_44.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_44.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_45.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_45.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_46.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_46.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_47.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_47.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_48.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_48.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_49.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_49.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_5.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_5.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_50.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_50.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_51.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_51.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_52.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_52.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_53.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_53.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_54.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_54.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_55.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_55.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_56.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_56.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_57.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_57.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_58.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_58.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_59.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_59.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_6.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_6.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_60.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_60.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_7.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_7.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_8.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_8.pcd -------------------------------------------------------------------------------- /data/sim_structured_long_term/sim_structured_long_term_9.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/la3dm/HEAD/data/sim_structured_long_term/sim_structured_long_term_9.pcd -------------------------------------------------------------------------------- /config/datasets/sim_structured.yaml: -------------------------------------------------------------------------------- 1 | # sim_structured config 2 | 3 | # Information about range-finder data 4 | scan_num: 12 # How many scans to use 5 | max_range: 8 # Sensor max range (m) 6 | 7 | # Bounds on map height 8 | min_z: 0 9 | max_z: 4.3 10 | 11 | original_size: false 12 | predict: false 13 | -------------------------------------------------------------------------------- /config/datasets/sim_unstructured.yaml: -------------------------------------------------------------------------------- 1 | # sim_unstructured config 2 | 3 | # Information about range-finder data 4 | scan_num: 12 # How many scans to use 5 | max_range: 8 # Sensor max range (m) 6 | 7 | # Bounds on map height 8 | min_z: 0 9 | max_z: 4.3 10 | 11 | original_size: false 12 | predict: false 13 | -------------------------------------------------------------------------------- /config/datasets/sim_structured_long_term.yaml: -------------------------------------------------------------------------------- 1 | # sim_structured_long_term config 2 | 3 | # Information about range-finder data 4 | scan_num: 15 # How many scans to use 5 | max_range: 8 # Sensor max range (m) 6 | 7 | # Bounds on map height 8 | min_z: 0 9 | max_z: 4.3 10 | 11 | original_size: true 12 | predict: false 13 | -------------------------------------------------------------------------------- /config/methods/gpoctomap.yaml: -------------------------------------------------------------------------------- 1 | # GPOctoMap config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.1 6 | block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | 8 | # Kernel parameters 9 | sf2: 1.0 10 | ell: 1.0 11 | 12 | # Sampling resolutions 13 | free_resolution: 0.1 # Free space sampling resolution 14 | ds_resolution: 0.1 # Downsampling factor 15 | 16 | # Free/Occupied Thresholds 17 | free_thresh: 0.3 18 | occupied_thresh: 0.7 19 | 20 | # GP parameters 21 | noise: 0.01 22 | l: 100 23 | max_var: 1000 24 | min_var: 0.001 25 | max_known_var: 00.02 26 | -------------------------------------------------------------------------------- /config/methods/gpoctomap_large_map.yaml: -------------------------------------------------------------------------------- 1 | # GP/BGKL+ Large Map config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.2 6 | block_depth: 4 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | max_range: 30 # Sensor max range (m) 8 | original_size: true 9 | 10 | # Bounds on map height 11 | min_z: -3.0 12 | max_z: 3.0 13 | 14 | # Kernel parameters 15 | sf2: 1.0 16 | ell: 1.0 17 | 18 | # Sampling resolutions 19 | free_resolution: 0.1 20 | ds_resolution: 0.5 # Downsampling factor 21 | 22 | # Free/Occupied Thresholds 23 | free_thresh: 0.3 24 | occupied_thresh: 0.7 25 | 26 | # GP parameters 27 | noise: 0.01 28 | l: 100 29 | max_var: 1000 30 | min_var: 0.001 31 | max_known_var: 0.02 32 | 33 | 34 | -------------------------------------------------------------------------------- /config/methods/bgkloctomap.yaml: -------------------------------------------------------------------------------- 1 | # BGKLOctoMap config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.1 6 | block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | 8 | # Kernel parameters 9 | sf2: 0.1 # Actually sigma_0 in sparse kernel 10 | ell: 0.2 # Length scale of the sparse kernel 11 | 12 | # Sampling resolutions 13 | free_resolution: 0.3 # Free space sampling res 14 | ds_resolution: 0.1 # Downsampling factor 15 | 16 | # Free/Occupied Thresholds 17 | free_thresh: 0.3 18 | occupied_thresh: 0.7 19 | 20 | # BGK Inference positive and negative class prior pseudocounts 21 | prior_A: 0.001 # Positive class (occupied) 22 | prior_B: 0.001 # Negative class (unoccupied) 23 | var_thresh: 0.15 # Threshold on variance to distinguish known/unknown 24 | -------------------------------------------------------------------------------- /config/methods/bgkoctomap.yaml: -------------------------------------------------------------------------------- 1 | # BGKOctoMap config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.1 6 | block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | 8 | # Kernel parameters 9 | sf2: 1.0 # Actually sigma_0 in sparse kernel 10 | ell: 0.2 # Length scale of the sparse kernel 11 | 12 | # Sampling resolutions 13 | free_resolution: 0.5 # Free space sampling resolution 14 | ds_resolution: 0.1 # Downsampling factor 15 | 16 | # Free/Occupied Thresholds 17 | free_thresh: 0.3 18 | occupied_thresh: 0.7 19 | var_thresh: 100.0 # Threshold on variance to distinguish known/unknown 20 | 21 | # BGK Inference positive and negative class prior pseudocounts 22 | prior_A: 0.001 # Positive class (occupied) 23 | prior_B: 0.001 # Negative class (unoccupied) 24 | -------------------------------------------------------------------------------- /config/methods/bgklvoctomap.yaml: -------------------------------------------------------------------------------- 1 | # BGKLVOctoMap config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.1 6 | block_depth: 5 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | 8 | # Kernel parameters 9 | sf2: 0.1 # Actually sigma_0 in sparse kernel 10 | ell: 0.2 # Length scale of the sparse kernel 11 | 12 | # Sampling resolutions 13 | free_resolution: 0.1 # Free space sampling res 14 | ds_resolution: 0.1 # Downsampling factor 15 | 16 | # Free/Occupied Thresholds 17 | free_thresh: 0.3 18 | occupied_thresh: 0.7 19 | 20 | # BGK Inference positive and negative class prior pseudocounts 21 | prior_A: 0.001 # Positive class (occupied) 22 | prior_B: 0.001 # Negative class (unoccupied) 23 | var_thresh: 0.2 # Threshold on variance to distinguish known/unknown 24 | min_W: 0.001 #minimum unknown threshold 25 | -------------------------------------------------------------------------------- /config/methods/bgklvoctomap_large_map.yaml: -------------------------------------------------------------------------------- 1 | # BGKLV Large Map config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.2 6 | block_depth: 6 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | max_range: 30 # Sensor max range (m) 8 | original_size: true 9 | 10 | # Bounds on map height 11 | min_z: -3.0 12 | max_z: 3.0 13 | 14 | # Kernel parameters 15 | sf2: 0.1 16 | ell: 0.6 17 | 18 | # Sampling resolutions 19 | free_resolution: 0.1 20 | ds_resolution: 0.5 # Downsampling factor 21 | 22 | # Free/Occupied Thresholds 23 | free_thresh: 0.3 24 | occupied_thresh: 0.7 25 | 26 | # BGKL+ parameters 27 | prior_A: 0.001 # Positive class (occupied) 28 | prior_B: 0.001 # Negative class (unoccupied) 29 | min_W: 0.01 #minimum unknown threshold 30 | var_thresh: 0.001 # Threshold on variance to distinguish known/unknown 31 | 32 | -------------------------------------------------------------------------------- /config/methods/bgkloctomap_large_map.yaml: -------------------------------------------------------------------------------- 1 | # BGKL Large Map config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.2 6 | block_depth: 5 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | max_range: 30 # Sensor max range (m) 8 | original_size: true 9 | 10 | # Bounds on map height 11 | min_z: -3.0 12 | max_z: 3.0 13 | 14 | # Kernel parameters 15 | sf2: 0.1 16 | ell: 0.6 17 | 18 | # Sampling resolutions 19 | free_resolution: 6.5 #6.5 for block depth 5, 3.25 for block depth 4, 13 for depth 6 20 | ds_resolution: 0.5 # Downsampling factor 21 | 22 | # Free/Occupied Thresholds 23 | free_thresh: 0.3 24 | occupied_thresh: 0.7 25 | 26 | # BGKL parameters 27 | prior_A: 0.001 # Positive class (occupied) 28 | prior_B: 0.001 # Negative class (unoccupied) 29 | var_thresh: 100.0 # Threshold on variance to distinguish known/unknown 30 | 31 | 32 | -------------------------------------------------------------------------------- /config/methods/bgkoctomap_large_map.yaml: -------------------------------------------------------------------------------- 1 | # BGKOctoMap config 2 | 3 | # Map topic, grid cell mininum resolution 4 | topic: /occupied_cells_vis_array 5 | resolution: 0.2 6 | block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016) 7 | max_range: 30 # Sensor max range (m) 8 | original_size: true 9 | 10 | # Bounds on map height 11 | min_z: -3.0 12 | max_z: 3.0 13 | 14 | # Kernel parameters 15 | sf2: 1.0 # Actually sigma_0 in sparse kernel 16 | ell: 0.2 # Length scale of the sparse kernel 17 | 18 | # Sampling resolutions 19 | free_resolution: 0.2 # Free space sampling resolution 20 | ds_resolution: 0.1 # Downsampling factor 21 | 22 | # Free/Occupied Thresholds 23 | free_thresh: 0.3 24 | occupied_thresh: 0.7 25 | var_thresh: 100.0 # Threshold on variance to distinguish known/unknown 26 | 27 | # BGK Inference positive and negative class prior pseudocounts 28 | prior_A: 0.001 # Positive class (occupied) 29 | prior_B: 0.001 # Negative class (unoccupied) 30 | -------------------------------------------------------------------------------- /launch/la3dm_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Robust Field Autonomy Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /launch/la3dm_static.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: trusty 2 | sudo: required 3 | language: 4 | - generic 5 | cache: 6 | - apt 7 | services: 8 | - docker 9 | 10 | # Global environment variables 11 | env: 12 | global: 13 | - ROS_DISTRO=kinetic 14 | - ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 15 | # - ROS_CI_DESKTOP="`lsb_release -cs`" 16 | # - CI_SOURCE_PATH=$(pwd) 17 | # - ROS_PARALLEL_JOBS='-j8 -16' 18 | 19 | # ROS Setup 20 | # before_install: 21 | # - docker pull ubuntu:xenial 22 | # - docker run -it ubuntu:xenial 23 | # - apt-get update 24 | # - apt-get install -y wget 25 | # - sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 26 | # - wget http://packages.ros.org/ros.key -O - | apt-key add - 27 | # - apt-get update 28 | # - apt-cache search kinetic-catkin 29 | # - sudo apt-get install -y ros-$ROS_DISTRO-catkin 30 | # - source /opt/ros/$ROS_DISTRO/setup.bash 31 | # - sudo rosdep init 32 | # - rosdep update 33 | 34 | install: 35 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 36 | script: 37 | - .ci_config/travis.sh 38 | 39 | # Create catkin workspace and 40 | # install: 41 | # - mkdir -p ~/catkin_ws/src 42 | # - cd ~/catkin_ws/src 43 | # - catkin_init_workspace 44 | # - cd ~/catkin_ws 45 | # - catkin_make 46 | # - source devel/setup.bash 47 | # Add package 48 | # - cd ~/catkin_ws/src 49 | # - ln -s $CI_SOURCE_PATH . 50 | 51 | # Install dependencies 52 | # before_script: 53 | # - sudo apt-get install ros-kinetic-octomap* 54 | 55 | # Build package 56 | # script: 57 | # - cd ~/catkin_ws 58 | # - catkin_make -------------------------------------------------------------------------------- /include/bgklvoctomap/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "cctype": "cpp", 4 | "clocale": "cpp", 5 | "cmath": "cpp", 6 | "csignal": "cpp", 7 | "cstdarg": "cpp", 8 | "cstddef": "cpp", 9 | "cstdio": "cpp", 10 | "cstdlib": "cpp", 11 | "cstring": "cpp", 12 | "ctime": "cpp", 13 | "cwchar": "cpp", 14 | "cwctype": "cpp", 15 | "array": "cpp", 16 | "atomic": "cpp", 17 | "strstream": "cpp", 18 | "*.tcc": "cpp", 19 | "bitset": "cpp", 20 | "chrono": "cpp", 21 | "complex": "cpp", 22 | "cstdint": "cpp", 23 | "deque": "cpp", 24 | "forward_list": "cpp", 25 | "list": "cpp", 26 | "unordered_map": "cpp", 27 | "unordered_set": "cpp", 28 | "vector": "cpp", 29 | "exception": "cpp", 30 | "algorithm": "cpp", 31 | "functional": "cpp", 32 | "ratio": "cpp", 33 | "string_view": "cpp", 34 | "system_error": "cpp", 35 | "tuple": "cpp", 36 | "type_traits": "cpp", 37 | "fstream": "cpp", 38 | "initializer_list": "cpp", 39 | "iomanip": "cpp", 40 | "iosfwd": "cpp", 41 | "iostream": "cpp", 42 | "istream": "cpp", 43 | "limits": "cpp", 44 | "memory": "cpp", 45 | "new": "cpp", 46 | "ostream": "cpp", 47 | "numeric": "cpp", 48 | "sstream": "cpp", 49 | "stdexcept": "cpp", 50 | "streambuf": "cpp", 51 | "thread": "cpp", 52 | "cfenv": "cpp", 53 | "cinttypes": "cpp", 54 | "utility": "cpp", 55 | "typeindex": "cpp", 56 | "typeinfo": "cpp", 57 | "string": "cpp", 58 | "mutex": "cpp", 59 | "*.ipp": "cpp", 60 | "core": "cpp" 61 | } 62 | } -------------------------------------------------------------------------------- /src/common/point3f.cpp: -------------------------------------------------------------------------------- 1 | #include "point3f.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace la3dm { 7 | 8 | Vector3 &Vector3::rotate_IP(double roll, double pitch, double yaw) { 9 | double x, y, z; 10 | // pitch (around y) 11 | x = (*this)(0); 12 | z = (*this)(2); 13 | (*this)(0) = (float) (z * sin(pitch) + x * cos(pitch)); 14 | (*this)(2) = (float) (z * cos(pitch) - x * sin(pitch)); 15 | 16 | 17 | // yaw (around z) 18 | x = (*this)(0); 19 | y = (*this)(1); 20 | (*this)(0) = (float) (x * cos(yaw) - y * sin(yaw)); 21 | (*this)(1) = (float) (x * sin(yaw) + y * cos(yaw)); 22 | 23 | // roll (around x) 24 | y = (*this)(1); 25 | z = (*this)(2); 26 | (*this)(1) = (float) (y * cos(roll) - z * sin(roll)); 27 | (*this)(2) = (float) (y * sin(roll) + z * cos(roll)); 28 | 29 | return *this; 30 | } 31 | 32 | std::istream &Vector3::read(std::istream &s) { 33 | int temp; 34 | s >> temp; // should be 3 35 | for (unsigned int i = 0; i < 3; i++) 36 | s >> operator()(i); 37 | return s; 38 | } 39 | 40 | 41 | std::ostream &Vector3::write(std::ostream &s) const { 42 | s << 3; 43 | for (unsigned int i = 0; i < 3; i++) 44 | s << " " << operator()(i); 45 | return s; 46 | } 47 | 48 | 49 | std::istream &Vector3::readBinary(std::istream &s) { 50 | int temp; 51 | s.read((char *) &temp, sizeof(temp)); 52 | double val = 0; 53 | for (unsigned int i = 0; i < 3; i++) { 54 | s.read((char *) &val, sizeof(val)); 55 | operator()(i) = (float) val; 56 | } 57 | return s; 58 | } 59 | 60 | 61 | std::ostream &Vector3::writeBinary(std::ostream &s) const { 62 | int temp = 3; 63 | s.write((char *) &temp, sizeof(temp)); 64 | double val = 0; 65 | for (unsigned int i = 0; i < 3; i++) { 66 | val = operator()(i); 67 | s.write((char *) &val, sizeof(val)); 68 | } 69 | return s; 70 | } 71 | 72 | 73 | std::ostream &operator<<(std::ostream &out, la3dm::Vector3 const &v) { 74 | return out << '(' << v.x() << ' ' << v.y() << ' ' << v.z() << ')'; 75 | } 76 | } -------------------------------------------------------------------------------- /src/bgkoctomap/bgkoctree_node.cpp: -------------------------------------------------------------------------------- 1 | #include "bgkoctree_node.h" 2 | #include 3 | 4 | namespace la3dm { 5 | 6 | /// Default static values 7 | float Occupancy::sf2 = 1.0f; 8 | float Occupancy::ell = 1.0f; 9 | float Occupancy::free_thresh = 0.3f; 10 | float Occupancy::occupied_thresh = 0.7f; 11 | float Occupancy::var_thresh = 1000.0f; 12 | float Occupancy::prior_A = 0.5f; 13 | float Occupancy::prior_B = 0.5f; 14 | 15 | Occupancy::Occupancy(float A, float B) : m_A(Occupancy::prior_A + A), m_B(Occupancy::prior_B + B) { 16 | classified = false; 17 | float var = get_var(); 18 | if (var > Occupancy::var_thresh) 19 | state = State::UNKNOWN; 20 | else { 21 | float p = get_prob(); 22 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 23 | : State::UNKNOWN); 24 | } 25 | } 26 | 27 | float Occupancy::get_prob() const { 28 | return m_A / (m_A + m_B); 29 | } 30 | 31 | void Occupancy::update(float ybar, float kbar) { 32 | classified = true; 33 | m_A += ybar; 34 | m_B += kbar - ybar; 35 | 36 | float var = get_var(); 37 | if (var > Occupancy::var_thresh) 38 | state = State::UNKNOWN; 39 | else { 40 | float p = get_prob(); 41 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 42 | : State::UNKNOWN); 43 | } 44 | } 45 | 46 | std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc) { 47 | os.write((char *) &oc.m_A, sizeof(oc.m_A)); 48 | os.write((char *) &oc.m_B, sizeof(oc.m_B)); 49 | return os; 50 | } 51 | 52 | std::ifstream &operator>>(std::ifstream &is, Occupancy &oc) { 53 | float m_A, m_B; 54 | is.read((char *) &m_A, sizeof(m_A)); 55 | is.read((char *) &m_B, sizeof(m_B)); 56 | oc = OcTreeNode(m_A, m_B); 57 | return is; 58 | } 59 | 60 | std::ostream &operator<<(std::ostream &os, const Occupancy &oc) { 61 | return os << '(' << oc.m_A << ' ' << oc.m_B << ' ' << oc.get_prob() << ')'; 62 | } 63 | } -------------------------------------------------------------------------------- /src/bgkloctomap/bgkloctree_node.cpp: -------------------------------------------------------------------------------- 1 | #include "bgkloctree_node.h" 2 | #include 3 | 4 | namespace la3dm { 5 | 6 | /// Default static values 7 | float Occupancy::sf2 = 1.0f; 8 | float Occupancy::ell = 1.0f; 9 | float Occupancy::free_thresh = 0.3f; 10 | float Occupancy::occupied_thresh = 0.7f; 11 | float Occupancy::var_thresh = 1000.0f; 12 | float Occupancy::prior_A = 0.5f; 13 | float Occupancy::prior_B = 0.5f; 14 | 15 | Occupancy::Occupancy(float A, float B) : m_A(Occupancy::prior_A + A), m_B(Occupancy::prior_B + B) { 16 | classified = false; 17 | float var = get_var(); 18 | if (var > Occupancy::var_thresh) 19 | state = State::UNKNOWN; 20 | else { 21 | float p = get_prob(); 22 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 23 | : State::UNKNOWN); 24 | } 25 | } 26 | 27 | float Occupancy::get_prob() const { 28 | return m_A / (m_A + m_B); 29 | } 30 | 31 | void Occupancy::update(float ybar, float kbar) { 32 | classified = true; 33 | m_A += ybar; 34 | m_B += kbar - ybar; 35 | 36 | float var = get_var(); 37 | if (var > Occupancy::var_thresh) 38 | state = State::UNKNOWN; 39 | else { 40 | float p = get_prob(); 41 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 42 | : State::UNKNOWN); 43 | } 44 | } 45 | 46 | std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc) { 47 | os.write((char *) &oc.m_A, sizeof(oc.m_A)); 48 | os.write((char *) &oc.m_B, sizeof(oc.m_B)); 49 | return os; 50 | } 51 | 52 | std::ifstream &operator>>(std::ifstream &is, Occupancy &oc) { 53 | float m_A, m_B; 54 | is.read((char *) &m_A, sizeof(m_A)); 55 | is.read((char *) &m_B, sizeof(m_B)); 56 | oc = OcTreeNode(m_A, m_B); 57 | return is; 58 | } 59 | 60 | std::ostream &operator<<(std::ostream &os, const Occupancy &oc) { 61 | return os << '(' << oc.m_A << ' ' << oc.m_B << ' ' << oc.get_prob() << ')'; 62 | } 63 | } -------------------------------------------------------------------------------- /src/common/point6f.cpp: -------------------------------------------------------------------------------- 1 | #include "point6f.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace la3dm { 7 | 8 | // Vector3 &Vector3::rotate_IP(double roll, double pitch, double yaw) { 9 | // double x, y, z; 10 | // // pitch (around y) 11 | // x = (*this)(0); 12 | // z = (*this)(2); 13 | // (*this)(0) = (float) (z * sin(pitch) + x * cos(pitch)); 14 | // (*this)(2) = (float) (z * cos(pitch) - x * sin(pitch)); 15 | 16 | 17 | // // yaw (around z) 18 | // x = (*this)(0); 19 | // y = (*this)(1); 20 | // (*this)(0) = (float) (x * cos(yaw) - y * sin(yaw)); 21 | // (*this)(1) = (float) (x * sin(yaw) + y * cos(yaw)); 22 | 23 | // // roll (around x) 24 | // y = (*this)(1); 25 | // z = (*this)(2); 26 | // (*this)(1) = (float) (y * cos(roll) - z * sin(roll)); 27 | // (*this)(2) = (float) (y * sin(roll) + z * cos(roll)); 28 | 29 | // return *this; 30 | // } 31 | 32 | std::istream &Vector6::read(std::istream &s) { 33 | int temp; 34 | s >> temp; // should be 6 35 | for (unsigned int i = 0; i < 6; i++) 36 | s >> operator()(i); 37 | return s; 38 | } 39 | 40 | 41 | std::ostream &Vector6::write(std::ostream &s) const { 42 | s << 6; 43 | for (unsigned int i = 0; i < 6; i++) 44 | s << " " << operator()(i); 45 | return s; 46 | } 47 | 48 | 49 | std::istream &Vector6::readBinary(std::istream &s) { 50 | int temp; 51 | s.read((char *) &temp, sizeof(temp)); 52 | double val = 0; 53 | for (unsigned int i = 0; i < 6; i++) { 54 | s.read((char *) &val, sizeof(val)); 55 | operator()(i) = (float) val; 56 | } 57 | return s; 58 | } 59 | 60 | 61 | std::ostream &Vector6::writeBinary(std::ostream &s) const { 62 | int temp = 6; 63 | s.write((char *) &temp, sizeof(temp)); 64 | double val = 0; 65 | for (unsigned int i = 0; i < 6; i++) { 66 | val = operator()(i); 67 | s.write((char *) &val, sizeof(val)); 68 | } 69 | return s; 70 | } 71 | 72 | 73 | std::ostream &operator<<(std::ostream &out, la3dm::Vector6 const &v) { 74 | return out << '(' << v.x0() << ' ' << v.y0() << ' ' << v.z0() << ' ' << v.x1() << ' ' << v.y1() << ' ' << v.z1() << ')'; 75 | } 76 | } -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | la3dm 4 | 0.0.1 5 | The la3dm package 6 | 7 | 8 | 9 | 10 | kevin 11 | Jinkun Wang 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | kevin 30 | Jinkun Wang 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | geometry_msgs 45 | pcl_ros 46 | roscpp 47 | octomap_ros 48 | visualization_msgs 49 | nav_msgs 50 | 51 | geometry_msgs 52 | pcl_ros 53 | roscpp 54 | octomap_ros 55 | visualization_msgs 56 | nav_msgs 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/gpoctomap/gpoctree_node.cpp: -------------------------------------------------------------------------------- 1 | #include "gpoctree_node.h" 2 | #include "gpregressor.h" 3 | #include 4 | 5 | namespace la3dm { 6 | /// Default static values 7 | float Occupancy::sf2 = 1.0f; 8 | float Occupancy::ell = 1.0f; 9 | float Occupancy::noise = 0.01f; 10 | float Occupancy::l = 100.f; 11 | 12 | float Occupancy::max_ivar = 1000.0f; 13 | float Occupancy::min_ivar = 0.001f; 14 | 15 | float Occupancy::min_known_ivar = 10.0f; 16 | float Occupancy::free_thresh = 0.3f; 17 | float Occupancy::occupied_thresh = 0.7f; 18 | 19 | Occupancy::Occupancy(float m, float var) : m_ivar(m / var), ivar(1.0f / var) { 20 | classified = false; 21 | if (ivar < Occupancy::min_known_ivar) 22 | state = State::UNKNOWN; 23 | else { 24 | ivar = ivar > Occupancy::max_ivar ? Occupancy::max_ivar : ivar; 25 | float p = get_prob(); 26 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 27 | : State::UNKNOWN); 28 | } 29 | } 30 | 31 | float Occupancy::get_prob() const { 32 | // logistic regression function 33 | return 1.0f / (1.0f + (float) exp(-l * m_ivar / Occupancy::max_ivar)); 34 | } 35 | 36 | void Occupancy::update(float new_m, float new_var) { 37 | classified = true; 38 | ivar += 1.0 / new_var - Occupancy::sf2; 39 | m_ivar += new_m / new_var; 40 | if (ivar < Occupancy::min_known_ivar) 41 | state = State::UNKNOWN; 42 | else { 43 | // chop variance 44 | ivar = ivar > Occupancy::max_ivar ? Occupancy::max_ivar : ivar; 45 | float p = get_prob(); 46 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 47 | : State::UNKNOWN); 48 | } 49 | } 50 | 51 | std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc) { 52 | os.write((char *) &oc.m_ivar, sizeof(oc.m_ivar)); 53 | os.write((char *) &oc.ivar, sizeof(oc.ivar)); 54 | return os; 55 | } 56 | 57 | std::ifstream &operator>>(std::ifstream &is, Occupancy &oc) { 58 | float m_ivar, ivar; 59 | is.read((char *) &m_ivar, sizeof(m_ivar)); 60 | is.read((char *) &ivar, sizeof(ivar)); 61 | oc = OcTreeNode(m_ivar / ivar, 1.0f / ivar); 62 | return is; 63 | } 64 | 65 | std::ostream &operator<<(std::ostream &os, const Occupancy &oc) { 66 | return os << '(' << (oc.m_ivar / oc.ivar) << ' ' << (1.0 / oc.ivar) << ' ' << oc.get_prob() << ')'; 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /src/bgklvoctomap/bgklvoctree_node.cpp: -------------------------------------------------------------------------------- 1 | #include "bgklvoctree_node.h" 2 | #include 3 | 4 | namespace la3dm { 5 | 6 | /// Default static values 7 | float Occupancy::sf2 = 1.0f; 8 | float Occupancy::ell = 1.0f; 9 | float Occupancy::free_thresh = 0.3f; 10 | float Occupancy::occupied_thresh = 0.7f; 11 | float Occupancy::var_thresh = 1000.0f; 12 | float Occupancy::prior_A = 0.5f; 13 | float Occupancy::prior_B = 0.5f; 14 | bool Occupancy::original_size = true; 15 | float Occupancy::min_W = 0.1f; 16 | 17 | Occupancy::Occupancy(float A, float B) : m_A(Occupancy::prior_A + A), m_B(Occupancy::prior_B + B) { 18 | classified = false; 19 | float var = get_var(); 20 | if (var > Occupancy::var_thresh) 21 | state = State::UNCERTAIN; 22 | else { 23 | float p = get_prob(); 24 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 25 | : State::UNKNOWN); 26 | } 27 | } 28 | 29 | float Occupancy::get_prob() const { 30 | float prob, W; 31 | if (m_A + m_B < Occupancy::min_W){ 32 | W = Occupancy::min_W; 33 | } 34 | else{ 35 | W = m_A + m_B; 36 | } 37 | 38 | if(m_A > m_B){ 39 | prob = m_A / (W-m_B) + (W-m_A-m_B)*0.5 / (W-m_B); 40 | } 41 | else{ 42 | prob = 0.5*(W-m_B-m_A) / (W-m_A); 43 | } 44 | 45 | return prob; 46 | } 47 | 48 | float Occupancy::get_var() const { 49 | float var, W; 50 | float prob = get_prob(); 51 | 52 | if (m_A + m_B < Occupancy::min_W){ 53 | W = Occupancy::min_W; 54 | } 55 | else{ 56 | W = m_A + m_B; 57 | } 58 | 59 | var = m_A / W * pow(1-prob,2) + (W-m_A-m_B) / W * pow(0.5-prob,2) + m_B / W * pow(prob,2); 60 | return var; 61 | 62 | } 63 | 64 | void Occupancy::update(float ybar, float kbar) { 65 | classified = true; 66 | m_A += ybar; 67 | m_B += kbar - ybar; 68 | 69 | float var = get_var(); 70 | if (var > Occupancy::var_thresh) 71 | state = State::UNCERTAIN; 72 | else { 73 | float p = get_prob(); 74 | state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE 75 | : State::UNKNOWN); 76 | } 77 | } 78 | 79 | std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc) { 80 | os.write((char *) &oc.m_A, sizeof(oc.m_A)); 81 | os.write((char *) &oc.m_B, sizeof(oc.m_B)); 82 | return os; 83 | } 84 | 85 | std::ifstream &operator>>(std::ifstream &is, Occupancy &oc) { 86 | float m_A, m_B; 87 | is.read((char *) &m_A, sizeof(m_A)); 88 | is.read((char *) &m_B, sizeof(m_B)); 89 | oc = OcTreeNode(m_A, m_B); 90 | return is; 91 | } 92 | 93 | std::ostream &operator<<(std::ostream &os, const Occupancy &oc) { 94 | return os << '(' << oc.m_A << ' ' << oc.m_B << ' ' << oc.get_prob() << ')'; 95 | } 96 | } -------------------------------------------------------------------------------- /include/bgkoctomap/bgkoctree_node.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGK_OCCUPANCY_H 2 | #define LA3DM_BGK_OCCUPANCY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace la3dm { 8 | 9 | /// Occupancy state: before pruning: FREE, OCCUPIED, UNKNOWN; after pruning: PRUNED 10 | enum class State : char { 11 | FREE, OCCUPIED, UNKNOWN, PRUNED 12 | }; 13 | 14 | /* 15 | * @brief Inference ouputs and occupancy state. 16 | * 17 | * Occupancy has member variables: m_A and m_B (kernel densities of positive 18 | * and negative class, respectively) and State. 19 | * Before using this class, set the static member variables first. 20 | */ 21 | class Occupancy { 22 | friend std::ostream &operator<<(std::ostream &os, const Occupancy &oc); 23 | 24 | friend std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc); 25 | 26 | friend std::ifstream &operator>>(std::ifstream &is, Occupancy &oc); 27 | 28 | friend class BGKOctoMap; 29 | 30 | public: 31 | /* 32 | * @brief Constructors and destructor. 33 | */ 34 | Occupancy() : m_A(Occupancy::prior_A), m_B(Occupancy::prior_B), state(State::UNKNOWN) { classified = false; } 35 | 36 | Occupancy(float A, float B); 37 | 38 | Occupancy(const Occupancy &other) : m_A(other.m_A), m_B(other.m_B), state(other.state) { } 39 | 40 | Occupancy &operator=(const Occupancy &other) { 41 | m_A = other.m_A; 42 | m_B = other.m_B; 43 | state = other.state; 44 | return *this; 45 | } 46 | 47 | ~Occupancy() { } 48 | 49 | /* 50 | * @brief Exact updates for nonparametric Bayesian kernel inference 51 | * @param ybar kernel density estimate of positive class (occupied) 52 | * @param kbar kernel density of negative class (unoccupied) 53 | */ 54 | void update(float ybar, float kbar); 55 | 56 | /// Get probability of occupancy. 57 | float get_prob() const; 58 | 59 | /// Get variance of occupancy (uncertainty) 60 | inline float get_var() const { return (m_A * m_B) / ( (m_A + m_B) * (m_A + m_B) * (m_A + m_B + 1.0f)); } 61 | 62 | /* 63 | * @brief Get occupancy state of the node. 64 | * @return occupancy state (see State). 65 | */ 66 | inline State get_state() const { return state; } 67 | 68 | /// Prune current node; set state to PRUNED. 69 | inline void prune() { state = State::PRUNED; } 70 | 71 | /// Only FREE and OCCUPIED nodes can be equal. 72 | inline bool operator==(const Occupancy &rhs) const { 73 | return this->state != State::UNKNOWN && this->state == rhs.state; 74 | } 75 | 76 | bool classified; 77 | 78 | private: 79 | float m_A; 80 | float m_B; 81 | State state; 82 | 83 | static float sf2; 84 | static float ell; // length-scale 85 | 86 | static float prior_A; // prior on alpha 87 | static float prior_B; // prior on beta 88 | 89 | static float free_thresh; // FREE occupancy threshold 90 | static float occupied_thresh; // OCCUPIED occupancy threshold 91 | static float var_thresh; 92 | }; 93 | 94 | typedef Occupancy OcTreeNode; 95 | } 96 | 97 | #endif // LA3DM_BGK_OCCUPANCY_H 98 | -------------------------------------------------------------------------------- /include/bgkloctomap/bgkloctree_node.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKL_OCCUPANCY_H 2 | #define LA3DM_BGKL_OCCUPANCY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace la3dm { 8 | 9 | /// Occupancy state: before pruning: FREE, OCCUPIED, UNKNOWN; after pruning: PRUNED 10 | enum class State : char { 11 | FREE, OCCUPIED, UNKNOWN, PRUNED 12 | }; 13 | 14 | /* 15 | * @brief Inference ouputs and occupancy state. 16 | * 17 | * Occupancy has member variables: m_A and m_B (kernel densities of positive 18 | * and negative class, respectively) and State. 19 | * Before using this class, set the static member variables first. 20 | */ 21 | class Occupancy { 22 | friend std::ostream &operator<<(std::ostream &os, const Occupancy &oc); 23 | 24 | friend std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc); 25 | 26 | friend std::ifstream &operator>>(std::ifstream &is, Occupancy &oc); 27 | 28 | friend class BGKLOctoMap; 29 | 30 | public: 31 | /* 32 | * @brief Constructors and destructor. 33 | */ 34 | Occupancy() : m_A(Occupancy::prior_A), m_B(Occupancy::prior_B), state(State::UNKNOWN) { classified = false; } 35 | 36 | Occupancy(float A, float B); 37 | 38 | Occupancy(const Occupancy &other) : m_A(other.m_A), m_B(other.m_B), state(other.state) { } 39 | 40 | Occupancy &operator=(const Occupancy &other) { 41 | m_A = other.m_A; 42 | m_B = other.m_B; 43 | state = other.state; 44 | return *this; 45 | } 46 | 47 | ~Occupancy() { } 48 | 49 | /* 50 | * @brief Exact updates for nonparametric Bayesian kernel inference 51 | * @param ybar kernel density estimate of positive class (occupied) 52 | * @param kbar kernel density of negative class (unoccupied) 53 | */ 54 | void update(float ybar, float kbar); 55 | 56 | /// Get probability of occupancy. 57 | float get_prob() const; 58 | 59 | /// Get variance of occupancy (uncertainty) 60 | inline float get_var() const { return (m_A * m_B) / ( (m_A + m_B) * (m_A + m_B) * (m_A + m_B + 1.0f)); } 61 | 62 | /* 63 | * @brief Get occupancy state of the node. 64 | * @return occupancy state (see State). 65 | */ 66 | inline State get_state() const { return state; } 67 | 68 | /// Prune current node; set state to PRUNED. 69 | inline void prune() { state = State::PRUNED; } 70 | 71 | /// Only FREE and OCCUPIED nodes can be equal. 72 | inline bool operator==(const Occupancy &rhs) const { 73 | return this->state != State::UNKNOWN && this->state == rhs.state; 74 | } 75 | 76 | bool classified; 77 | 78 | private: 79 | float m_A; 80 | float m_B; 81 | State state; 82 | 83 | static float sf2; 84 | static float ell; // length-scale 85 | 86 | static float prior_A; // prior on alpha 87 | static float prior_B; // prior on beta 88 | 89 | static float free_thresh; // FREE occupancy threshold 90 | static float occupied_thresh; // OCCUPIED occupancy threshold 91 | static float var_thresh; 92 | }; 93 | 94 | typedef Occupancy OcTreeNode; 95 | } 96 | 97 | #endif // LA3DM_BGKL_OCCUPANCY_H 98 | -------------------------------------------------------------------------------- /include/bgklvoctomap/bgklvoctree_node.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKLV_OCCUPANCY_H 2 | #define LA3DM_BGKLV_OCCUPANCY_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace la3dm { 9 | 10 | /// Occupancy state: before pruning: FREE, OCCUPIED, UNKNOWN, UNCERTAIN; after pruning: PRUNED 11 | enum class State : char { 12 | FREE, OCCUPIED, UNKNOWN, UNCERTAIN, PRUNED 13 | }; 14 | 15 | /* 16 | * @brief Inference ouputs and occupancy state. 17 | * 18 | * Occupancy has member variables: m_A and m_B (kernel densities of positive 19 | * and negative class, respectively) and State. 20 | * Before using this class, set the static member variables first. 21 | */ 22 | class Occupancy { 23 | friend std::ostream &operator<<(std::ostream &os, const Occupancy &oc); 24 | 25 | friend std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc); 26 | 27 | friend std::ifstream &operator>>(std::ifstream &is, Occupancy &oc); 28 | 29 | friend class BGKLVOctoMap; 30 | 31 | public: 32 | /* 33 | * @brief Constructors and destructor. 34 | */ 35 | Occupancy() : m_A(Occupancy::prior_A), m_B(Occupancy::prior_B), state(State::UNKNOWN) { classified = false; } 36 | 37 | Occupancy(float A, float B); 38 | 39 | Occupancy(const Occupancy &other) : m_A(other.m_A), m_B(other.m_B), state(other.state) { } 40 | 41 | Occupancy &operator=(const Occupancy &other) { 42 | m_A = other.m_A; 43 | m_B = other.m_B; 44 | state = other.state; 45 | return *this; 46 | } 47 | 48 | ~Occupancy() { } 49 | 50 | /* 51 | * @brief Exact updates for nonparametric Bayesian kernel inference 52 | * @param ybar kernel density estimate of positive class (occupied) 53 | * @param kbar kernel density of negative class (unoccupied) 54 | */ 55 | void update(float ybar, float kbar); 56 | 57 | /// Get probability of occupancy. 58 | float get_prob() const; 59 | 60 | /// Get variance of occupancy (uncertainty) 61 | float get_var() const; 62 | 63 | /* 64 | * @brief Get occupancy state of the node. 65 | * @return occupancy state (see State). 66 | */ 67 | inline State get_state() const { return state; } 68 | 69 | /// Prune current node; set state to PRUNED. 70 | inline void prune() { state = State::PRUNED; } 71 | 72 | /// Only FREE and OCCUPIED nodes can be equal. 73 | inline bool operator==(const Occupancy &rhs) const { 74 | return this->state != State::UNKNOWN && this->state != State::UNCERTAIN && this->state == rhs.state; 75 | } 76 | 77 | bool classified; 78 | 79 | private: 80 | float m_A; 81 | float m_B; 82 | State state; 83 | 84 | static float sf2; 85 | static float ell; // length-scale 86 | static float min_W; 87 | 88 | static float prior_A; // prior on alpha 89 | static float prior_B; // prior on beta 90 | static bool original_size; 91 | 92 | static float free_thresh; // FREE occupancy threshold 93 | static float occupied_thresh; // OCCUPIED occupancy threshold 94 | static float var_thresh; 95 | }; 96 | 97 | typedef Occupancy OcTreeNode; 98 | } 99 | 100 | #endif // LA3DM_BGKLV_OCCUPANCY_H 101 | -------------------------------------------------------------------------------- /include/gpoctomap/gpoctree_node.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_GP_OCCUPANCY_H 2 | #define LA3DM_GP_OCCUPANCY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace la3dm { 8 | 9 | /// Occupancy state: before pruning: FREE, OCCUPIED, UNKNOWN; after pruning: PRUNED 10 | enum class State : char { 11 | FREE, OCCUPIED, UNKNOWN, PRUNED 12 | }; 13 | 14 | /* 15 | * @brief GP regression ouputs and occupancy state. 16 | * 17 | * Occupancy has member variables: m_ivar (m*ivar), ivar (1/var) and State. 18 | * This representation speeds up the updates via BCM. 19 | * Before using this class, set the static member variables first. 20 | */ 21 | class Occupancy { 22 | friend std::ostream &operator<<(std::ostream &os, const Occupancy &oc); 23 | 24 | friend std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc); 25 | 26 | friend std::ifstream &operator>>(std::ifstream &is, Occupancy &oc); 27 | 28 | friend class GPOctoMap; 29 | 30 | public: 31 | /* 32 | * @brief Constructors and destructor. 33 | */ 34 | Occupancy() : m_ivar(0.0), ivar(Occupancy::min_ivar), state(State::UNKNOWN) { classified = false; } 35 | 36 | Occupancy(float m, float var); 37 | 38 | Occupancy(const Occupancy &other) : m_ivar(other.m_ivar), ivar(other.ivar), state(other.state) { } 39 | 40 | Occupancy &operator=(const Occupancy &other) { 41 | m_ivar = other.m_ivar; 42 | ivar = other.ivar; 43 | state = other.state; 44 | return *this; 45 | } 46 | 47 | ~Occupancy() { } 48 | 49 | /* 50 | * @brief Bayesian Committee Machine (BCM) update for Gaussian Process regression. 51 | * @param new_m mean resulted from GP regression 52 | * @param new_var variance resulted from GP regression 53 | */ 54 | void update(float new_m, float new_var); 55 | 56 | /// Get probability of occupancy. 57 | float get_prob() const; 58 | 59 | /// Get variance of occupancy (uncertainty) 60 | inline float get_var() const { return 1.0f / ivar; } 61 | 62 | /* 63 | * @brief Get occupancy state of the node. 64 | * @return occupancy state (see State). 65 | */ 66 | inline State get_state() const { return state; } 67 | 68 | /// Prune current node; set state to PRUNED. 69 | inline void prune() { state = State::PRUNED; } 70 | 71 | /// Only FREE and OCCUPIED nodes can be equal. 72 | inline bool operator==(const Occupancy &rhs) const { 73 | return this->state != State::UNKNOWN && this->state == rhs.state; 74 | } 75 | 76 | bool classified; 77 | 78 | private: 79 | float m_ivar; // m / var or m * ivar 80 | float ivar; // 1.0 / var 81 | State state; 82 | 83 | static float sf2; // signal variance 84 | static float ell; // length-scale 85 | static float noise; // noise variance 86 | static float l; // gamma in logistic functions 87 | 88 | static float max_ivar; // minimum variance 89 | static float min_ivar; // maximum variance 90 | static float min_known_ivar; // maximum variance for nodes to be considered as FREE or OCCUPIED 91 | 92 | static float free_thresh; // FREE occupancy threshold 93 | static float occupied_thresh; // OCCUPIED occupancy threshold 94 | }; 95 | 96 | typedef Occupancy OcTreeNode; 97 | } 98 | 99 | #endif // LA3DM_GP_OCCUPANCY_H 100 | -------------------------------------------------------------------------------- /include/gpoctomap/gpblock.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_GP_BLOCK_H 2 | #define LA3DM_GP_BLOCK_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "gpoctree_node.h" 8 | #include "gpoctree.h" 9 | 10 | namespace la3dm { 11 | 12 | /// Hask key to index Block given block's center. 13 | typedef int64_t BlockHashKey; 14 | 15 | /// Initialize Look-Up Table 16 | std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth); 17 | 18 | std::unordered_map init_index_map(const std::unordered_map &key_loc_map, 19 | unsigned short max_depth); 20 | 21 | /// Extended Block 22 | #ifdef PREDICT 23 | typedef std::array ExtendedBlock; 24 | #else 25 | typedef std::array ExtendedBlock; 26 | #endif 27 | 28 | /// Convert from block to hash key. 29 | BlockHashKey block_to_hash_key(point3f center); 30 | 31 | /// Convert from block to hash key. 32 | BlockHashKey block_to_hash_key(float x, float y, float z); 33 | 34 | /// Convert from hash key to block. 35 | point3f hash_key_to_block(BlockHashKey key); 36 | 37 | /// Get current block's extended block. 38 | ExtendedBlock get_extended_block(BlockHashKey key); 39 | 40 | /* 41 | * @brief Block is built on top of OcTree, providing the functions to locate nodes. 42 | * 43 | * Block stores the information needed to locate each OcTreeNode's position: 44 | * fixed resolution, fixed block_size, both of which must be initialized. 45 | * The localization is implemented using Loop-Up Table. 46 | */ 47 | class Block : public OcTree { 48 | friend BlockHashKey block_to_hash_key(point3f center); 49 | 50 | friend BlockHashKey block_to_hash_key(float x, float y, float z); 51 | 52 | friend point3f hash_key_to_block(BlockHashKey key); 53 | 54 | friend ExtendedBlock get_extended_block(BlockHashKey key); 55 | 56 | friend class GPOctoMap; 57 | 58 | public: 59 | Block(); 60 | 61 | Block(point3f center); 62 | 63 | /// @return location of the OcTreeNode given OcTree's LeafIterator. 64 | inline point3f get_loc(const LeafIterator &it) const { 65 | return Block::key_loc_map[it.get_hash_key()] + center; 66 | } 67 | 68 | /// @return size of the OcTreeNode given OcTree's LeafIterator. 69 | inline float get_size(const LeafIterator &it) const { 70 | unsigned short depth, index; 71 | hash_key_to_node(it.get_hash_key(), depth, index); 72 | return float(size / pow(2, depth)); 73 | } 74 | 75 | /// @return center of current Block. 76 | inline point3f get_center() const { return center; } 77 | 78 | /// @return min lim of current Block. 79 | inline point3f get_lim_min() const { return center - point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 80 | 81 | /// @return max lim of current Block. 82 | inline point3f get_lim_max() const { return center + point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 83 | 84 | /// @return ExtendedBlock of current Block. 85 | ExtendedBlock get_extended_block() const; 86 | 87 | OcTreeHashKey get_node(unsigned short x, unsigned short y, unsigned short z) const; 88 | 89 | point3f get_point(unsigned short x, unsigned short y, unsigned short z) const; 90 | 91 | void get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const; 92 | 93 | OcTreeNode &search(float x, float y, float z) const; 94 | 95 | OcTreeNode &search(point3f p) const; 96 | 97 | private: 98 | // Loop-Up Table 99 | static std::unordered_map key_loc_map; 100 | static std::unordered_map index_map; 101 | static float resolution; 102 | static float size; 103 | static unsigned short cell_num; 104 | 105 | point3f center; 106 | }; 107 | 108 | } 109 | 110 | #endif // LA3DM_GP_BLOCK_H 111 | -------------------------------------------------------------------------------- /include/bgkoctomap/bgkblock.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGK_BLOCK_H 2 | #define LA3DM_BGK_BLOCK_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "bgkoctree_node.h" 8 | #include "bgkoctree.h" 9 | 10 | namespace la3dm { 11 | 12 | /// Hask key to index Block given block's center. 13 | typedef int64_t BlockHashKey; 14 | 15 | /// Initialize Look-Up Table 16 | std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth); 17 | 18 | std::unordered_map init_index_map(const std::unordered_map &key_loc_map, 19 | unsigned short max_depth); 20 | 21 | /// Extended Block 22 | #ifdef PREDICT 23 | typedef std::array ExtendedBlock; 24 | #else 25 | typedef std::array ExtendedBlock; 26 | #endif 27 | 28 | /// Convert from block to hash key. 29 | BlockHashKey block_to_hash_key(point3f center); 30 | 31 | /// Convert from block to hash key. 32 | BlockHashKey block_to_hash_key(float x, float y, float z); 33 | 34 | /// Convert from hash key to block. 35 | point3f hash_key_to_block(BlockHashKey key); 36 | 37 | /// Get current block's extended block. 38 | ExtendedBlock get_extended_block(BlockHashKey key); 39 | 40 | /* 41 | * @brief Block is built on top of OcTree, providing the functions to locate nodes. 42 | * 43 | * Block stores the information needed to locate each OcTreeNode's position: 44 | * fixed resolution, fixed block_size, both of which must be initialized. 45 | * The localization is implemented using Loop-Up Table. 46 | */ 47 | class Block : public OcTree { 48 | friend BlockHashKey block_to_hash_key(point3f center); 49 | 50 | friend BlockHashKey block_to_hash_key(float x, float y, float z); 51 | 52 | friend point3f hash_key_to_block(BlockHashKey key); 53 | 54 | friend ExtendedBlock get_extended_block(BlockHashKey key); 55 | 56 | friend class BGKOctoMap; 57 | 58 | public: 59 | Block(); 60 | 61 | Block(point3f center); 62 | 63 | /// @return location of the OcTreeNode given OcTree's LeafIterator. 64 | inline point3f get_loc(const LeafIterator &it) const { 65 | return Block::key_loc_map[it.get_hash_key()] + center; 66 | } 67 | 68 | /// @return size of the OcTreeNode given OcTree's LeafIterator. 69 | inline float get_size(const LeafIterator &it) const { 70 | unsigned short depth, index; 71 | hash_key_to_node(it.get_hash_key(), depth, index); 72 | return float(size / pow(2, depth)); 73 | } 74 | 75 | /// @return center of current Block. 76 | inline point3f get_center() const { return center; } 77 | 78 | /// @return min lim of current Block. 79 | inline point3f get_lim_min() const { return center - point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 80 | 81 | /// @return max lim of current Block. 82 | inline point3f get_lim_max() const { return center + point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 83 | 84 | /// @return ExtendedBlock of current Block. 85 | ExtendedBlock get_extended_block() const; 86 | 87 | OcTreeHashKey get_node(unsigned short x, unsigned short y, unsigned short z) const; 88 | 89 | point3f get_point(unsigned short x, unsigned short y, unsigned short z) const; 90 | 91 | void get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const; 92 | 93 | OcTreeNode &search(float x, float y, float z) const; 94 | 95 | OcTreeNode &search(point3f p) const; 96 | 97 | private: 98 | // Loop-Up Table 99 | static std::unordered_map key_loc_map; 100 | static std::unordered_map index_map; 101 | static float resolution; 102 | static float size; 103 | static unsigned short cell_num; 104 | 105 | point3f center; 106 | }; 107 | } 108 | 109 | #endif // LA3DM_BGK_BLOCK_H 110 | -------------------------------------------------------------------------------- /include/bgkloctomap/bgklblock.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKL_BLOCK_H 2 | #define LA3DM_BGKL_BLOCK_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "bgkloctree_node.h" 8 | #include "bgkloctree.h" 9 | 10 | namespace la3dm { 11 | 12 | /// Hask key to index Block given block's center. 13 | typedef int64_t BlockHashKey; 14 | 15 | /// Initialize Look-Up Table 16 | std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth); 17 | 18 | std::unordered_map init_index_map(const std::unordered_map &key_loc_map, 19 | unsigned short max_depth); 20 | 21 | /// Extended Block 22 | #ifdef PREDICT 23 | typedef std::array ExtendedBlock; 24 | #else 25 | typedef std::array ExtendedBlock; 26 | #endif 27 | 28 | /// Convert from block to hash key. 29 | BlockHashKey block_to_hash_key(point3f center); 30 | 31 | /// Convert from block to hash key. 32 | BlockHashKey block_to_hash_key(float x, float y, float z); 33 | 34 | /// Convert from hash key to block. 35 | point3f hash_key_to_block(BlockHashKey key); 36 | 37 | /// Get current block's extended block. 38 | ExtendedBlock get_extended_block(BlockHashKey key); 39 | 40 | /* 41 | * @brief Block is built on top of OcTree, providing the functions to locate nodes. 42 | * 43 | * Block stores the information needed to locate each OcTreeNode's position: 44 | * fixed resolution, fixed block_size, both of which must be initialized. 45 | * The localization is implemented using Loop-Up Table. 46 | */ 47 | class Block : public OcTree { 48 | friend BlockHashKey block_to_hash_key(point3f center); 49 | 50 | friend BlockHashKey block_to_hash_key(float x, float y, float z); 51 | 52 | friend point3f hash_key_to_block(BlockHashKey key); 53 | 54 | friend ExtendedBlock get_extended_block(BlockHashKey key); 55 | 56 | friend class BGKLOctoMap; 57 | 58 | public: 59 | Block(); 60 | 61 | Block(point3f center); 62 | 63 | /// @return location of the OcTreeNode given OcTree's LeafIterator. 64 | inline point3f get_loc(const LeafIterator &it) const { 65 | return Block::key_loc_map[it.get_hash_key()] + center; 66 | } 67 | 68 | /// @return size of the OcTreeNode given OcTree's LeafIterator. 69 | inline float get_size(const LeafIterator &it) const { 70 | unsigned short depth, index; 71 | hash_key_to_node(it.get_hash_key(), depth, index); 72 | return float(size / pow(2, depth)); 73 | } 74 | 75 | /// @return center of current Block. 76 | inline point3f get_center() const { return center; } 77 | 78 | /// @return min lim of current Block. 79 | inline point3f get_lim_min() const { return center - point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 80 | 81 | /// @return max lim of current Block. 82 | inline point3f get_lim_max() const { return center + point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 83 | 84 | /// @return ExtendedBlock of current Block. 85 | ExtendedBlock get_extended_block() const; 86 | 87 | OcTreeHashKey get_node(unsigned short x, unsigned short y, unsigned short z) const; 88 | 89 | point3f get_point(unsigned short x, unsigned short y, unsigned short z) const; 90 | 91 | void get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const; 92 | 93 | OcTreeNode &search(float x, float y, float z) const; 94 | 95 | OcTreeNode &search(point3f p) const; 96 | 97 | private: 98 | // Loop-Up Table 99 | static std::unordered_map key_loc_map; 100 | static std::unordered_map index_map; 101 | static float resolution; 102 | static float size; 103 | static unsigned short cell_num; 104 | 105 | point3f center; 106 | }; 107 | } 108 | 109 | #endif // LA3DM_BGKL_BLOCK_H 110 | -------------------------------------------------------------------------------- /include/bgklvoctomap/bgklvblock.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKLV_BLOCK_H 2 | #define LA3DM_BGKLV_BLOCK_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "bgklvoctree_node.h" 8 | #include "bgklvoctree.h" 9 | 10 | namespace la3dm { 11 | 12 | /// Hask key to index Block given block's center. 13 | typedef int64_t BlockHashKey; 14 | 15 | /// Initialize Look-Up Table 16 | std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth); 17 | 18 | std::unordered_map init_index_map(const std::unordered_map &key_loc_map, 19 | unsigned short max_depth); 20 | 21 | /// Extended Block 22 | #ifdef PREDICT 23 | typedef std::array ExtendedBlock; 24 | #else 25 | typedef std::array ExtendedBlock; 26 | #endif 27 | 28 | /// Convert from block to hash key. 29 | BlockHashKey block_to_hash_key(point3f center); 30 | 31 | /// Convert from block to hash key. 32 | BlockHashKey block_to_hash_key(float x, float y, float z); 33 | 34 | /// Convert from hash key to block. 35 | point3f hash_key_to_block(BlockHashKey key); 36 | 37 | /// Get current block's extended block. 38 | ExtendedBlock get_extended_block(BlockHashKey key); 39 | 40 | /* 41 | * @brief Block is built on top of OcTree, providing the functions to locate nodes. 42 | * 43 | * Block stores the information needed to locate each OcTreeNode's position: 44 | * fixed resolution, fixed block_size, both of which must be initialized. 45 | * The localization is implemented using Loop-Up Table. 46 | */ 47 | class Block : public OcTree { 48 | friend BlockHashKey block_to_hash_key(point3f center); 49 | 50 | friend BlockHashKey block_to_hash_key(float x, float y, float z); 51 | 52 | friend point3f hash_key_to_block(BlockHashKey key); 53 | 54 | friend ExtendedBlock get_extended_block(BlockHashKey key); 55 | 56 | friend class BGKLVOctoMap; 57 | 58 | public: 59 | Block(); 60 | 61 | Block(point3f center); 62 | 63 | /// @return location of the OcTreeNode given OcTree's LeafIterator. 64 | inline point3f get_loc(const LeafIterator &it) const { 65 | return Block::key_loc_map[it.get_hash_key()] + center; 66 | } 67 | 68 | /// @return size of the OcTreeNode given OcTree's LeafIterator. 69 | inline float get_size(const LeafIterator &it) const { 70 | unsigned short depth; 71 | unsigned long index; 72 | hash_key_to_node(it.get_hash_key(), depth, index); 73 | return float(size / pow(2, depth)); 74 | } 75 | 76 | /// @return center of current Block. 77 | inline point3f get_center() const { return center; } 78 | 79 | /// @return min lim of current Block. 80 | inline point3f get_lim_min() const { return center - point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 81 | 82 | /// @return max lim of current Block. 83 | inline point3f get_lim_max() const { return center + point3f(size / 2.0f, size / 2.0f, size / 2.0f); } 84 | 85 | /// @return ExtendedBlock of current Block. 86 | ExtendedBlock get_extended_block() const; 87 | 88 | OcTreeHashKey get_node(unsigned short x, unsigned short y, unsigned short z) const; 89 | 90 | point3f get_point(unsigned short x, unsigned short y, unsigned short z) const; 91 | 92 | void get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const; 93 | 94 | OcTreeNode &search(float x, float y, float z) const; 95 | 96 | OcTreeNode &search(point3f p) const; 97 | 98 | private: 99 | // Loop-Up Table 100 | static std::unordered_map key_loc_map; 101 | static std::unordered_map index_map; 102 | static float resolution; 103 | static float size; 104 | static unsigned short cell_num; 105 | 106 | point3f center; 107 | }; 108 | } 109 | 110 | #endif // LA3DM_BGKLV_BLOCK_H 111 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Learning-Aided 3D Mapping 2 | 3 | A suite of algorithms for learning-aided mapping. Includes implementations of Gaussian process regression and Bayesian generalized kernel inference for occupancy prediction using test-data octrees. A demonstration of the system can be found here: https://youtu.be/SRXLMALpU20 4 | 5 | ## Overview 6 | 7 | This implementation as it stands now is primarily intended to enable replication of these methods over a few datasets. In addition to the implementation of relevant learning algorithms and data structures, we provide two sets of range data (sim_structured and sim_unstructured) collected in Gazebo for demonstration. Parameters of the sensors and environments are set in the relevant `yaml` files contained in the `config/datasets` directory, while configuration of parameters for the mapping methods can be found in `config/methods`. 8 | 9 | ## Getting Started 10 | 11 | ### Dependencies 12 | 13 | The current package runs with ROS Noetic, but for testing in ROS Kinetic and ROS Indigo, you can set the CMAKE flag in the CMAKELists file to c++11. 14 | 15 | Octomap is a dependancy, which can be installed using the command below. Change distribution as necessary. 16 | 17 | ```bash 18 | $ sudo apt-get install ros-noetic-octomap* 19 | ``` 20 | 21 | ### Building with catkin 22 | 23 | The repository is set up to work with catkin, so to get started you can clone the repository into your catkin workspace `src` folder and compile with `catkin_make`: 24 | 25 | ```bash 26 | my_catkin_workspace/src$ git clone https://github.com/RobustFieldAutonomyLab/la3dm.git 27 | my_catkin_workspace/src$ cd .. 28 | my_catkin_workspace$ catkin_make 29 | my_catkin_workspace$ source ~/my_catkin_workspace/devel/setup.bash 30 | ``` 31 | 32 | ## Running the Demo 33 | 34 | To run the demo on the `sim_structured` environment, simply run: 35 | 36 | ```bash 37 | $ roslaunch la3dm la3dm_static.launch 38 | ``` 39 | 40 | which by default will run using the BGKOctoMap-LV method. If you want to try a different method or dataset, simply pass the 41 | name of the method or dataset as a parameter. For example, if you want to run GPOctoMap on the `sim_unstructured` map, 42 | you would run: 43 | 44 | ```bash 45 | $ roslaunch la3dm la3dm_static.launch method:=gpoctomap dataset:=sim_unstructured 46 | ``` 47 | 48 | ## Relevant Publications 49 | 50 | If you found this code useful, please cite the following: 51 | 52 | Improving Obstacle Boundary Representations in Predictive Occupancy Mapping ([PDF](https://www.sciencedirect.com/science/article/abs/pii/S0921889022000380)) - describes the latest BGKOctoMap-LV addition to the LA3DM library: 53 | 54 | ``` 55 | @article{pearson2022improving, 56 | title={Improving Obstacle Boundary Representations in Predictive Occupancy Mapping}, 57 | author={Pearson, Erik and Doherty, Kevin and Englot, Brendan}, 58 | journal={Robotics and Autonomous Systems}, 59 | volume={153}, 60 | pages={104077}, 61 | year={2022}, 62 | publisher={Elsevier} 63 | } 64 | ``` 65 | 66 | Learning-Aided 3-D Occupancy Mapping with Bayesian Generalized Kernel Inference ([PDF](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8713569)) - describes the BGKOctoMap and BGKOctoMap-L approaches originally included in the LA3DM library. 67 | ``` 68 | @article{Doherty2019, 69 | doi = {10.1109/tro.2019.2912487}, 70 | url = {https://doi.org/10.1109/tro.2019.2912487}, 71 | year = {2019}, 72 | publisher = {Institute of Electrical and Electronics Engineers ({IEEE})}, 73 | pages = {1--14}, 74 | author = {Kevin Doherty and Tixiao Shan and Jinkun Wang and Brendan Englot}, 75 | title = {Learning-Aided 3-D Occupancy Mapping With Bayesian Generalized Kernel Inference}, 76 | journal = {{IEEE} Transactions on Robotics} 77 | } 78 | ``` 79 | 80 | Fast, accurate gaussian process occupancy maps via test-data octrees and nested Bayesian fusion ([PDF](http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=7487232)) - describes the GPOctoMap approach included in the LA3DM library. 81 | ``` 82 | @INPROCEEDINGS{JWang-ICRA-16, 83 | author={J. Wang and B. Englot}, 84 | booktitle={2016 IEEE International Conference on Robotics and Automation (ICRA)}, 85 | title={Fast, accurate gaussian process occupancy maps via test-data octrees and nested Bayesian fusion}, 86 | year={2016}, 87 | pages={1003-1010}, 88 | month={May}, 89 | } 90 | ``` 91 | 92 | Bayesian Generalized Kernel Inference for Occupancy Map Prediction ([PDF](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=7989356)) 93 | ``` 94 | @INPROCEEDINGS{KDoherty-ICRA-17, 95 | author={K. Doherty and J. Wang, and B. Englot}, 96 | booktitle={2017 IEEE International Conference on Robotics and Automation (ICRA)}, 97 | title={Bayesian Generalized Kernel Inference for Occupancy Map Prediction}, 98 | year={2017}, 99 | month={May}, 100 | } 101 | ``` 102 | 103 | ## Contributors 104 | 105 | Jinkun Wang, Kevin Doherty, and Erik Pearson, [Robust Field Autonomy Lab (RFAL)](https://robustfieldautonomylab.github.io/), Stevens Institute of Technology. 106 | -------------------------------------------------------------------------------- /rviz/sim_structured_long_term.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /PointCloud21/Autocompute Value Bounds1 11 | - /MarkerArray1 12 | - /MarkerArray1/Status1 13 | Splitter Ratio: 0.5 14 | Tree Height: 539 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: false 42 | Line Style: 43 | Line Width: 0.03 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: false 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 0 59 | Min Value: 4.3 60 | Value: false 61 | Axis: Z 62 | Channel Name: intensity 63 | Class: rviz/PointCloud2 64 | Color: 255; 255; 255 65 | Color Transformer: AxisColor 66 | Decay Time: 0 67 | Enabled: false 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 4096 71 | Min Color: 0; 0; 0 72 | Min Intensity: 0 73 | Name: PointCloud2 74 | Position Transformer: XYZ 75 | Queue Size: 10 76 | Selectable: true 77 | Size (Pixels): 3 78 | Size (m): 0.1 79 | Style: Boxes 80 | Topic: /bgkoctomap 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: false 85 | - Class: rviz/MarkerArray 86 | Enabled: true 87 | Marker Topic: /occupied_cells_vis_array 88 | Name: MarkerArray 89 | Namespaces: 90 | map: true 91 | Queue Size: 100 92 | Value: true 93 | Enabled: true 94 | Global Options: 95 | Background Color: 255; 255; 255 96 | Fixed Frame: map 97 | Frame Rate: 30 98 | Name: root 99 | Tools: 100 | - Class: rviz/Interact 101 | Hide Inactive Objects: true 102 | - Class: rviz/MoveCamera 103 | - Class: rviz/Select 104 | - Class: rviz/FocusCamera 105 | - Class: rviz/Measure 106 | - Class: rviz/SetInitialPose 107 | Topic: /initialpose 108 | - Class: rviz/SetGoal 109 | Topic: /move_base_simple/goal 110 | - Class: rviz/PublishPoint 111 | Single click: true 112 | Topic: /clicked_point 113 | Value: true 114 | Views: 115 | Current: 116 | Class: rviz/Orbit 117 | Distance: 8.55293 118 | Enable Stereo Rendering: 119 | Stereo Eye Separation: 0.06 120 | Stereo Focal Distance: 1 121 | Swap Stereo Eyes: false 122 | Value: false 123 | Focal Point: 124 | X: 2.91324 125 | Y: 1.62221 126 | Z: 0.81138 127 | Name: Current View 128 | Near Clip Distance: 0.01 129 | Pitch: 0.454797 130 | Target Frame: 131 | Value: Orbit (rviz) 132 | Yaw: 3.59866 133 | Saved: ~ 134 | Window Geometry: 135 | Displays: 136 | collapsed: false 137 | Height: 845 138 | Hide Left Dock: false 139 | Hide Right Dock: true 140 | QMainWindow State: 000000ff00000000fd0000000400000000000002ae000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000bb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000002ac000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650100000000000006400000021a00fffffffb0000000800540069006d006501000000000000045000000000000000000000038c000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 141 | Selection: 142 | collapsed: false 143 | Time: 144 | collapsed: false 145 | Tool Properties: 146 | collapsed: false 147 | Views: 148 | collapsed: true 149 | Width: 1600 150 | X: -10 151 | Y: 22 152 | -------------------------------------------------------------------------------- /src/gpoctomap/gpoctree.cpp: -------------------------------------------------------------------------------- 1 | #include "gpoctree.h" 2 | 3 | #include 4 | 5 | namespace la3dm { 6 | unsigned short OcTree::max_depth = 0; 7 | 8 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index) { 9 | return (depth << 16) + index; 10 | } 11 | 12 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index) { 13 | depth = (unsigned short) (key >> 16); 14 | index = (unsigned short) (key & 0xFFFF); 15 | } 16 | 17 | OcTree::OcTree() { 18 | if (max_depth <= 0) 19 | node_arr = nullptr; 20 | else { 21 | node_arr = new OcTreeNode *[max_depth](); 22 | for (unsigned short i = 0; i < max_depth; ++i) { 23 | node_arr[i] = new OcTreeNode[(int) pow(8, i)](); 24 | } 25 | } 26 | } 27 | 28 | OcTree::~OcTree() { 29 | if (node_arr != nullptr) { 30 | for (unsigned short i = 0; i < max_depth; ++i) { 31 | if (node_arr[i] != nullptr) { 32 | delete[] node_arr[i]; 33 | } 34 | } 35 | delete[] node_arr; 36 | } 37 | } 38 | 39 | OcTree::OcTree(const OcTree &other) { 40 | if (other.node_arr == nullptr) { 41 | node_arr = nullptr; 42 | return; 43 | } 44 | 45 | node_arr = new OcTreeNode *[max_depth](); 46 | for (unsigned short i = 0; i < max_depth; ++i) { 47 | if (other.node_arr[i] != nullptr) { 48 | int n = (int) pow(8, i); 49 | node_arr[i] = new OcTreeNode[n](); 50 | std::copy(node_arr[i], node_arr[i] + n, other.node_arr[i]); 51 | } else 52 | node_arr[i] = nullptr; 53 | } 54 | } 55 | 56 | OcTree &OcTree::operator=(const OcTree &other) { 57 | OcTreeNode **local_node_arr = new OcTreeNode *[max_depth](); 58 | for (unsigned short i = 0; i < max_depth; ++i) { 59 | if (local_node_arr[i] != nullptr) { 60 | int n = (int) pow(8, i); 61 | local_node_arr[i] = new OcTreeNode[n](); 62 | std::copy(local_node_arr[i], local_node_arr[i] + n, other.node_arr[i]); 63 | } else 64 | local_node_arr[i] = nullptr; 65 | } 66 | 67 | node_arr = local_node_arr; 68 | return *this; 69 | } 70 | 71 | bool OcTree::is_leaf(unsigned short depth, unsigned short index) const { 72 | if (node_arr != nullptr && node_arr[depth] != nullptr && node_arr[depth][index].get_state() != State::PRUNED) { 73 | if (depth + 1 < max_depth) { 74 | if (node_arr[depth + 1] == nullptr || node_arr[depth + 1][index * 8].get_state() == State::PRUNED) 75 | return true; 76 | } else { 77 | return true; 78 | } 79 | } 80 | return false; 81 | } 82 | 83 | bool OcTree::is_leaf(OcTreeHashKey key) const { 84 | unsigned short depth = 0; 85 | unsigned short index = 0; 86 | hash_key_to_node(key, depth, index); 87 | return is_leaf(depth, index); 88 | } 89 | 90 | bool OcTree::search(OcTreeHashKey key) const { 91 | unsigned short depth; 92 | unsigned short index; 93 | hash_key_to_node(key, depth, index); 94 | 95 | return node_arr != nullptr && 96 | node_arr[depth] != nullptr && 97 | node_arr[depth][index].get_state() != State::PRUNED; 98 | } 99 | 100 | bool OcTree::prune() { 101 | if (node_arr == nullptr) 102 | return false; 103 | 104 | bool pruned = false; 105 | for (unsigned short depth = max_depth - 1; depth > 0; --depth) { 106 | OcTreeNode *layer = node_arr[depth]; 107 | OcTreeNode *parent_layer = node_arr[depth - 1]; 108 | if (layer == nullptr) 109 | continue; 110 | 111 | bool empty_layer = true; 112 | unsigned int n = (unsigned int) pow(8, depth); 113 | for (unsigned short index = 0; index < n; index += 8) { 114 | State state = layer[index].get_state(); 115 | if (state == State::UNKNOWN) { 116 | empty_layer = false; 117 | continue; 118 | } 119 | if (state == State::PRUNED) 120 | continue; 121 | 122 | bool collapsible = true; 123 | for (unsigned short i = 1; i < 8; ++i) { 124 | if (layer[index + i].get_state() != state) { 125 | collapsible = false; 126 | continue; 127 | } 128 | } 129 | 130 | if (collapsible) { 131 | parent_layer[(int) floor(index / 8)] = layer[index]; 132 | for (unsigned short i = 0; i < 8; ++i) { 133 | layer[index + i].prune(); 134 | } 135 | pruned = true; 136 | } else { 137 | empty_layer = false; 138 | } 139 | } 140 | 141 | if (empty_layer) { 142 | delete[] layer; 143 | node_arr[depth] = nullptr; 144 | } 145 | } 146 | return pruned; 147 | } 148 | 149 | OcTreeNode &OcTree::operator[](OcTreeHashKey key) const { 150 | unsigned short depth; 151 | unsigned short index; 152 | hash_key_to_node(key, depth, index); 153 | return node_arr[depth][index]; 154 | } 155 | } -------------------------------------------------------------------------------- /src/bgkloctomap/bgkloctree.cpp: -------------------------------------------------------------------------------- 1 | #include "bgkloctree.h" 2 | 3 | #include 4 | 5 | namespace la3dm { 6 | 7 | unsigned short OcTree::max_depth = 0; 8 | 9 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index) { 10 | return (depth << 16) + index; 11 | } 12 | 13 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index) { 14 | depth = (unsigned short) (key >> 16); 15 | index = (unsigned short) (key & 0xFFFF); 16 | } 17 | 18 | OcTree::OcTree() { 19 | if (max_depth <= 0) 20 | node_arr = nullptr; 21 | else { 22 | node_arr = new OcTreeNode *[max_depth](); 23 | for (unsigned short i = 0; i < max_depth; ++i) { 24 | node_arr[i] = new OcTreeNode[(int) pow(8, i)](); 25 | } 26 | } 27 | } 28 | 29 | OcTree::~OcTree() { 30 | if (node_arr != nullptr) { 31 | for (unsigned short i = 0; i < max_depth; ++i) { 32 | if (node_arr[i] != nullptr) { 33 | delete[] node_arr[i]; 34 | } 35 | } 36 | delete[] node_arr; 37 | } 38 | } 39 | 40 | OcTree::OcTree(const OcTree &other) { 41 | if (other.node_arr == nullptr) { 42 | node_arr = nullptr; 43 | return; 44 | } 45 | 46 | node_arr = new OcTreeNode *[max_depth](); 47 | for (unsigned short i = 0; i < max_depth; ++i) { 48 | if (other.node_arr[i] != nullptr) { 49 | int n = (int) pow(8, i); 50 | node_arr[i] = new OcTreeNode[n](); 51 | std::copy(node_arr[i], node_arr[i] + n, other.node_arr[i]); 52 | } else 53 | node_arr[i] = nullptr; 54 | } 55 | } 56 | 57 | OcTree &OcTree::operator=(const OcTree &other) { 58 | OcTreeNode **local_node_arr = new OcTreeNode *[max_depth](); 59 | for (unsigned short i = 0; i < max_depth; ++i) { 60 | if (local_node_arr[i] != nullptr) { 61 | int n = (int) pow(8, i); 62 | local_node_arr[i] = new OcTreeNode[n](); 63 | std::copy(local_node_arr[i], local_node_arr[i] + n, other.node_arr[i]); 64 | } else 65 | local_node_arr[i] = nullptr; 66 | } 67 | 68 | node_arr = local_node_arr; 69 | return *this; 70 | } 71 | 72 | bool OcTree::is_leaf(unsigned short depth, unsigned short index) const { 73 | if (node_arr != nullptr && node_arr[depth] != nullptr && node_arr[depth][index].get_state() != State::PRUNED) { 74 | if (depth + 1 < max_depth) { 75 | if (node_arr[depth + 1] == nullptr || node_arr[depth + 1][index * 8].get_state() == State::PRUNED) 76 | return true; 77 | } else { 78 | return true; 79 | } 80 | } 81 | return false; 82 | } 83 | 84 | bool OcTree::is_leaf(OcTreeHashKey key) const { 85 | unsigned short depth = 0; 86 | unsigned short index = 0; 87 | hash_key_to_node(key, depth, index); 88 | return is_leaf(depth, index); 89 | } 90 | 91 | bool OcTree::search(OcTreeHashKey key) const { 92 | unsigned short depth; 93 | unsigned short index; 94 | hash_key_to_node(key, depth, index); 95 | 96 | return node_arr != nullptr && 97 | node_arr[depth] != nullptr && 98 | node_arr[depth][index].get_state() != State::PRUNED; 99 | } 100 | 101 | bool OcTree::prune() { 102 | if (node_arr == nullptr) 103 | return false; 104 | 105 | bool pruned = false; 106 | for (unsigned short depth = max_depth - 1; depth > 0; --depth) { 107 | OcTreeNode *layer = node_arr[depth]; 108 | OcTreeNode *parent_layer = node_arr[depth - 1]; 109 | if (layer == nullptr) 110 | continue; 111 | 112 | bool empty_layer = true; 113 | unsigned int n = (unsigned int) pow(8, depth); 114 | for (unsigned short index = 0; index < n; index += 8) { 115 | State state = layer[index].get_state(); 116 | if (state == State::UNKNOWN) { 117 | empty_layer = false; 118 | continue; 119 | } 120 | if (state == State::PRUNED) 121 | continue; 122 | 123 | bool collapsible = true; 124 | for (unsigned short i = 1; i < 8; ++i) { 125 | if (layer[index + i].get_state() != state) { 126 | collapsible = false; 127 | continue; 128 | } 129 | } 130 | 131 | if (collapsible) { 132 | parent_layer[(int) floor(index / 8)] = layer[index]; 133 | for (unsigned short i = 0; i < 8; ++i) { 134 | layer[index + i].prune(); 135 | } 136 | pruned = true; 137 | } else { 138 | empty_layer = false; 139 | } 140 | } 141 | 142 | if (empty_layer) { 143 | delete[] layer; 144 | node_arr[depth] = nullptr; 145 | } 146 | } 147 | return pruned; 148 | } 149 | 150 | OcTreeNode &OcTree::operator[](OcTreeHashKey key) const { 151 | unsigned short depth; 152 | unsigned short index; 153 | hash_key_to_node(key, depth, index); 154 | return node_arr[depth][index]; 155 | } 156 | } -------------------------------------------------------------------------------- /src/bgklvoctomap/bgklvoctree.cpp: -------------------------------------------------------------------------------- 1 | #include "bgklvoctree.h" 2 | 3 | #include 4 | 5 | namespace la3dm { 6 | 7 | unsigned short OcTree::max_depth = 0; 8 | 9 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned long index) { 10 | return (depth << 28) + index; 11 | } 12 | 13 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned long &index) { 14 | depth = (unsigned short) (key >> 28); 15 | index = (unsigned long) (key & 0xFFFFFFF); 16 | } 17 | 18 | OcTree::OcTree() { 19 | if (max_depth <= 0) 20 | node_arr = nullptr; 21 | else { 22 | node_arr = new OcTreeNode *[max_depth](); 23 | for (unsigned short i = 0; i < max_depth; ++i) { 24 | node_arr[i] = new OcTreeNode[(int) pow(8, i)](); 25 | } 26 | } 27 | } 28 | 29 | OcTree::~OcTree() { 30 | if (node_arr != nullptr) { 31 | for (unsigned short i = 0; i < max_depth; ++i) { 32 | if (node_arr[i] != nullptr) { 33 | delete[] node_arr[i]; 34 | } 35 | } 36 | delete[] node_arr; 37 | } 38 | } 39 | 40 | OcTree::OcTree(const OcTree &other) { 41 | if (other.node_arr == nullptr) { 42 | node_arr = nullptr; 43 | return; 44 | } 45 | 46 | node_arr = new OcTreeNode *[max_depth](); 47 | for (unsigned short i = 0; i < max_depth; ++i) { 48 | if (other.node_arr[i] != nullptr) { 49 | int n = (int) pow(8, i); 50 | node_arr[i] = new OcTreeNode[n](); 51 | std::copy(node_arr[i], node_arr[i] + n, other.node_arr[i]); 52 | } else 53 | node_arr[i] = nullptr; 54 | } 55 | } 56 | 57 | OcTree &OcTree::operator=(const OcTree &other) { 58 | OcTreeNode **local_node_arr = new OcTreeNode *[max_depth](); 59 | for (unsigned short i = 0; i < max_depth; ++i) { 60 | if (local_node_arr[i] != nullptr) { 61 | int n = (int) pow(8, i); 62 | local_node_arr[i] = new OcTreeNode[n](); 63 | std::copy(local_node_arr[i], local_node_arr[i] + n, other.node_arr[i]); 64 | } else 65 | local_node_arr[i] = nullptr; 66 | } 67 | 68 | node_arr = local_node_arr; 69 | return *this; 70 | } 71 | 72 | bool OcTree::is_leaf(unsigned short depth, unsigned long index) const { 73 | if (node_arr != nullptr && node_arr[depth] != nullptr && node_arr[depth][index].get_state() != State::PRUNED) { 74 | if (depth + 1 < max_depth) { 75 | if (node_arr[depth + 1] == nullptr || node_arr[depth + 1][index * 8].get_state() == State::PRUNED) 76 | return true; 77 | } else { 78 | return true; 79 | } 80 | } 81 | return false; 82 | } 83 | 84 | bool OcTree::is_leaf(OcTreeHashKey key) const { 85 | unsigned short depth = 0; 86 | unsigned long index = 0; 87 | hash_key_to_node(key, depth, index); 88 | return is_leaf(depth, index); 89 | } 90 | 91 | bool OcTree::search(OcTreeHashKey key) const { 92 | unsigned short depth; 93 | unsigned long index; 94 | hash_key_to_node(key, depth, index); 95 | 96 | return node_arr != nullptr && 97 | node_arr[depth] != nullptr && 98 | node_arr[depth][index].get_state() != State::PRUNED; 99 | } 100 | 101 | bool OcTree::prune() { 102 | if (node_arr == nullptr) 103 | return false; 104 | 105 | bool pruned = false; 106 | for (unsigned short depth = max_depth - 1; depth > 0; --depth) { 107 | OcTreeNode *layer = node_arr[depth]; 108 | OcTreeNode *parent_layer = node_arr[depth - 1]; 109 | if (layer == nullptr) 110 | continue; 111 | 112 | bool empty_layer = true; 113 | unsigned int n = (unsigned int) pow(8, depth); 114 | for (unsigned long index = 0; index < n; index += 8) { 115 | State state = layer[index].get_state(); 116 | if (state == State::UNKNOWN) { 117 | empty_layer = false; 118 | continue; 119 | } 120 | if (state == State::PRUNED) 121 | continue; 122 | 123 | bool collapsible = true; 124 | for (unsigned short i = 1; i < 8; ++i) { 125 | if (layer[index + i].get_state() != state) { 126 | collapsible = false; 127 | continue; 128 | } 129 | } 130 | 131 | if (collapsible) { 132 | parent_layer[(int) floor(index / 8)] = layer[index]; 133 | for (unsigned short i = 0; i < 8; ++i) { 134 | layer[index + i].prune(); 135 | } 136 | pruned = true; 137 | } else { 138 | empty_layer = false; 139 | } 140 | } 141 | 142 | if (empty_layer) { 143 | delete[] layer; 144 | node_arr[depth] = nullptr; 145 | } 146 | } 147 | return pruned; 148 | } 149 | 150 | OcTreeNode &OcTree::operator[](OcTreeHashKey key) const { 151 | unsigned short depth; 152 | unsigned long index; 153 | hash_key_to_node(key, depth, index); 154 | return node_arr[depth][index]; 155 | } 156 | } -------------------------------------------------------------------------------- /src/bgkoctomap/bgkoctree.cpp: -------------------------------------------------------------------------------- 1 | #include "bgkoctree.h" 2 | 3 | #include 4 | 5 | namespace la3dm { 6 | 7 | unsigned short OcTree::max_depth = 0; 8 | 9 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index) { 10 | return (depth << 16) + index; 11 | } 12 | 13 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index) { 14 | depth = (unsigned short) (key >> 16); 15 | index = (unsigned short) (key & 0xFFFF); 16 | } 17 | 18 | OcTree::OcTree() { 19 | if (max_depth <= 0) 20 | node_arr = nullptr; 21 | else { 22 | node_arr = new OcTreeNode *[max_depth](); 23 | for (unsigned short i = 0; i < max_depth; ++i) { 24 | node_arr[i] = new OcTreeNode[(int) pow(8, i)](); 25 | } 26 | } 27 | } 28 | 29 | OcTree::~OcTree() { 30 | if (node_arr != nullptr) { 31 | for (unsigned short i = 0; i < max_depth; ++i) { 32 | if (node_arr[i] != nullptr) { 33 | delete[] node_arr[i]; 34 | } 35 | } 36 | delete[] node_arr; 37 | } 38 | } 39 | 40 | OcTree::OcTree(const OcTree &other) { 41 | if (other.node_arr == nullptr) { 42 | node_arr = nullptr; 43 | return; 44 | } 45 | 46 | node_arr = new OcTreeNode *[max_depth](); 47 | for (unsigned short i = 0; i < max_depth; ++i) { 48 | if (other.node_arr[i] != nullptr) { 49 | int n = (int) pow(8, i); 50 | node_arr[i] = new OcTreeNode[n](); 51 | std::copy(node_arr[i], node_arr[i] + n, other.node_arr[i]); 52 | } else 53 | node_arr[i] = nullptr; 54 | } 55 | } 56 | 57 | OcTree &OcTree::operator=(const OcTree &other) { 58 | OcTreeNode **local_node_arr = new OcTreeNode *[max_depth](); 59 | for (unsigned short i = 0; i < max_depth; ++i) { 60 | if (local_node_arr[i] != nullptr) { 61 | int n = (int) pow(8, i); 62 | local_node_arr[i] = new OcTreeNode[n](); 63 | std::copy(local_node_arr[i], local_node_arr[i] + n, other.node_arr[i]); 64 | } else 65 | local_node_arr[i] = nullptr; 66 | } 67 | 68 | node_arr = local_node_arr; 69 | return *this; 70 | } 71 | 72 | bool OcTree::is_leaf(unsigned short depth, unsigned short index) const { 73 | if (node_arr != nullptr && node_arr[depth] != nullptr && node_arr[depth][index].get_state() != State::PRUNED) { 74 | if (depth + 1 < max_depth) { 75 | if (node_arr[depth + 1] == nullptr || node_arr[depth + 1][index * 8].get_state() == State::PRUNED) 76 | return true; 77 | } else { 78 | return true; 79 | } 80 | } 81 | return false; 82 | } 83 | 84 | bool OcTree::is_leaf(OcTreeHashKey key) const { 85 | unsigned short depth = 0; 86 | unsigned short index = 0; 87 | hash_key_to_node(key, depth, index); 88 | return is_leaf(depth, index); 89 | } 90 | 91 | bool OcTree::search(OcTreeHashKey key) const { 92 | unsigned short depth; 93 | unsigned short index; 94 | hash_key_to_node(key, depth, index); 95 | 96 | return node_arr != nullptr && 97 | node_arr[depth] != nullptr && 98 | node_arr[depth][index].get_state() != State::PRUNED; 99 | } 100 | 101 | bool OcTree::prune() { 102 | if (node_arr == nullptr) 103 | return false; 104 | 105 | bool pruned = false; 106 | for (unsigned short depth = max_depth - 1; depth > 0; --depth) { 107 | OcTreeNode *layer = node_arr[depth]; 108 | OcTreeNode *parent_layer = node_arr[depth - 1]; 109 | if (layer == nullptr) 110 | continue; 111 | 112 | bool empty_layer = true; 113 | unsigned int n = (unsigned int) pow(8, depth); 114 | for (unsigned short index = 0; index < n; index += 8) { 115 | State state = layer[index].get_state(); 116 | if (state == State::UNKNOWN) { 117 | empty_layer = false; 118 | continue; 119 | } 120 | if (state == State::PRUNED) 121 | continue; 122 | 123 | bool collapsible = true; 124 | for (unsigned short i = 1; i < 8; ++i) { 125 | if (layer[index + i].get_state() != state) { 126 | collapsible = false; 127 | continue; 128 | } 129 | } 130 | 131 | if (collapsible) { 132 | parent_layer[(int) floor(index / 8)] = layer[index]; 133 | for (unsigned short i = 0; i < 8; ++i) { 134 | layer[index + i].prune(); 135 | } 136 | pruned = true; 137 | } else { 138 | empty_layer = false; 139 | } 140 | } 141 | 142 | if (empty_layer) { 143 | delete[] layer; 144 | node_arr[depth] = nullptr; 145 | } 146 | } 147 | return pruned; 148 | } 149 | 150 | OcTreeNode &OcTree::operator[](OcTreeHashKey key) const { 151 | unsigned short depth; 152 | unsigned short index; 153 | hash_key_to_node(key, depth, index); 154 | return node_arr[depth][index]; 155 | } 156 | } -------------------------------------------------------------------------------- /include/bgkoctomap/bgkinference.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGK_H 2 | #define LA3DM_BGK_H 3 | 4 | namespace la3dm { 5 | 6 | /* 7 | * @brief Bayesian Generalized Kernel Inference on Bernoulli distribution 8 | * @param dim dimension of data (2, 3, etc.) 9 | * @param T data type (float, double, etc.) 10 | * @ref Nonparametric Bayesian inference on multivariate exponential families 11 | */ 12 | template 13 | class BGKInference { 14 | public: 15 | /// Eigen matrix type for training and test data and kernel 16 | using MatrixXType = Eigen::Matrix; 17 | using MatrixKType = Eigen::Matrix; 18 | using MatrixDKType = Eigen::Matrix; 19 | using MatrixYType = Eigen::Matrix; 20 | 21 | BGKInference(T sf2, T ell) : sf2(sf2), ell(ell), trained(false) { } 22 | 23 | /* 24 | * @brief Fit BGK Model 25 | * @param x input vector (3N, row major) 26 | * @param y target vector (N) 27 | */ 28 | void train(const std::vector &x, const std::vector &y) { 29 | assert(x.size() % dim == 0 && (int) (x.size() / dim) == y.size()); 30 | MatrixXType _x = Eigen::Map(x.data(), x.size() / dim, dim); 31 | MatrixYType _y = Eigen::Map(y.data(), y.size(), 1); 32 | train(_x, _y); 33 | } 34 | 35 | /* 36 | * @brief Fit BGK Model 37 | * @param x input matrix (NX3) 38 | * @param y target matrix (NX1) 39 | */ 40 | void train(const MatrixXType &x, const MatrixYType &y) { 41 | this->x = MatrixXType(x); 42 | this->y = MatrixYType(y); 43 | trained = true; 44 | } 45 | 46 | /* 47 | * @brief Predict with BGK Model 48 | * @param xs input vector (3M, row major) 49 | * @param ybar positive class kernel density estimate (\bar{y}) 50 | * @param kbar kernel density estimate (\bar{k}) 51 | */ 52 | void predict(const std::vector &xs, std::vector &ybar, std::vector &kbar) const { 53 | assert(xs.size() % dim == 0); 54 | MatrixXType _xs = Eigen::Map(xs.data(), xs.size() / dim, dim); 55 | 56 | MatrixYType _ybar, _kbar; 57 | predict(_xs, _ybar, _kbar); 58 | 59 | ybar.resize(_ybar.rows()); 60 | kbar.resize(_kbar.rows()); 61 | for (int r = 0; r < _ybar.rows(); ++r) { 62 | ybar[r] = _ybar(r, 0); 63 | kbar[r] = _kbar(r, 0); 64 | } 65 | } 66 | 67 | /* 68 | * @brief Predict with nonparametric Bayesian generalized kernel inference 69 | * @param xs input vector (M x 3) 70 | * @param ybar positive class kernel density estimate (M x 1) 71 | * @param kbar kernel density estimate (M x 1) 72 | */ 73 | void predict(const MatrixXType &xs, MatrixYType &ybar, MatrixYType &kbar) const { 74 | assert(trained == true); 75 | MatrixKType Ks; 76 | covSparse(xs, x, Ks); 77 | ybar = (Ks * y).array(); 78 | kbar = Ks.rowwise().sum().array(); 79 | } 80 | 81 | private: 82 | /* 83 | * @brief Compute Euclid distances between two vectors. 84 | * @param x input vector 85 | * @param z input vecotr 86 | * @return d distance matrix 87 | */ 88 | void dist(const MatrixXType &x, const MatrixXType &z, MatrixKType &d) const { 89 | d = MatrixKType::Zero(x.rows(), z.rows()); 90 | for (int i = 0; i < x.rows(); ++i) { 91 | d.row(i) = (z.rowwise() - x.row(i)).rowwise().norm(); 92 | } 93 | } 94 | 95 | /* 96 | * @brief Matern3 kernel. 97 | * @param x input vector 98 | * @param z input vector 99 | * @return Kxz covariance matrix 100 | */ 101 | void covMaterniso3(const MatrixXType &x, const MatrixXType &z, MatrixKType &Kxz) const { 102 | dist(1.73205 / ell * x, 1.73205 / ell * z, Kxz); 103 | Kxz = ((1 + Kxz.array()) * exp(-Kxz.array())).matrix() * sf2; 104 | } 105 | 106 | /* 107 | * @brief Sparse kernel. 108 | * @param x input vector 109 | * @param z input vector 110 | * @return Kxz covariance matrix 111 | * @ref A sparse covariance function for exact gaussian process inference in large datasets. 112 | */ 113 | void covSparse(const MatrixXType &x, const MatrixXType &z, MatrixKType &Kxz) const { 114 | dist(x / ell, z / ell, Kxz); 115 | Kxz = (((2.0f + (Kxz * 2.0f * 3.1415926f).array().cos()) * (1.0f - Kxz.array()) / 3.0f) + 116 | (Kxz * 2.0f * 3.1415926f).array().sin() / (2.0f * 3.1415926f)).matrix() * sf2; 117 | 118 | // Clean up for values with distance outside length scale 119 | // Possible because Kxz <= 0 when dist >= ell 120 | for (int i = 0; i < Kxz.rows(); ++i) 121 | { 122 | for (int j = 0; j < Kxz.cols(); ++j) 123 | if (Kxz(i,j) < 0.0) 124 | Kxz(i,j) = 0.0f; 125 | } 126 | } 127 | 128 | T sf2; // signal variance 129 | T ell; // length-scale 130 | 131 | MatrixXType x; // temporary storage of training data 132 | MatrixYType y; // temporary storage of training labels 133 | 134 | bool trained; // true if bgkinference stored training data 135 | }; 136 | 137 | typedef BGKInference<3, float> BGK3f; 138 | 139 | } 140 | #endif // LA3DM_BGK_H 141 | -------------------------------------------------------------------------------- /include/gpoctomap/gpoctree.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_GP_OCTREE_H 2 | #define LA3DM_GP_OCTREE_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "gpoctree_node.h" 8 | 9 | namespace la3dm { 10 | 11 | /// Hash key to index OcTree nodes given depth and the index in that layer. 12 | typedef int OcTreeHashKey; 13 | 14 | /// Convert from node to hask key. 15 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index); 16 | 17 | /// Convert from hash key to node. 18 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index); 19 | 20 | /* 21 | * @brief A simple OcTree to organize GP occupancy data in one block. 22 | * 23 | * OcTree doesn't store positions of nodes in order to reduce memory usage. 24 | * The nodes in OcTrees are indexed by OcTreeHashKey which can be used to 25 | * retrieve positions later (See Block). 26 | * For the purpose of mapping, this OcTree has fixed depth which should be 27 | * set before using OcTrees. 28 | */ 29 | class OcTree { 30 | friend class GPOctoMap; 31 | 32 | public: 33 | OcTree(); 34 | 35 | ~OcTree(); 36 | 37 | OcTree(const OcTree &other); 38 | 39 | OcTree &operator=(const OcTree &other); 40 | 41 | /* 42 | * @brief Rursively pruning OcTreeNodes with the same state. 43 | * 44 | * Prune nodes by setting nodes to PRUNED. 45 | * Delete the layer if all nodes are pruned. 46 | */ 47 | bool prune(); 48 | 49 | /// @return true if this node is a leaf node. 50 | bool is_leaf(OcTreeHashKey key) const; 51 | 52 | /// @return true if this node is a leaf node. 53 | bool is_leaf(unsigned short depth, unsigned short index) const; 54 | 55 | /// @return true if this node exists and is not pruned. 56 | bool search(OcTreeHashKey key) const; 57 | 58 | /// @return Occupancy of the node (without checking if it exists!) 59 | OcTreeNode &operator[](OcTreeHashKey key) const; 60 | 61 | /// Leaf iterator for OcTrees: iterate all leaf nodes not pruned. 62 | class LeafIterator : public std::iterator { 63 | public: 64 | LeafIterator() : tree(nullptr) { } 65 | 66 | LeafIterator(const OcTree *tree) 67 | : tree(tree != nullptr && tree->node_arr != nullptr ? tree : nullptr) { 68 | if (tree != nullptr) { 69 | stack.emplace(0, 0); 70 | stack.emplace(0, 0); 71 | ++(*this); 72 | } 73 | } 74 | 75 | LeafIterator(const LeafIterator &other) : tree(other.tree), stack(other.stack) { } 76 | 77 | LeafIterator &operator=(const LeafIterator &other) { 78 | tree = other.tree; 79 | stack = other.stack; 80 | return *this; 81 | } 82 | 83 | bool operator==(const LeafIterator &other) const { 84 | return (tree == other.tree) && 85 | (stack.size() == other.stack.size()) && 86 | (stack.size() == 0 || (stack.size() > 0 && 87 | (stack.top().depth == other.stack.top().depth) && 88 | (stack.top().index == other.stack.top().index))); 89 | } 90 | 91 | bool operator!=(const LeafIterator &other) const { 92 | return !(this->operator==(other)); 93 | } 94 | 95 | LeafIterator operator++(int) { 96 | LeafIterator result(*this); 97 | ++(*this); 98 | return result; 99 | } 100 | 101 | LeafIterator &operator++() { 102 | if (stack.empty()) { 103 | tree = nullptr; 104 | } else { 105 | stack.pop(); 106 | while (!stack.empty() && !tree->is_leaf(stack.top().depth, stack.top().index)) 107 | single_inc(); 108 | if (stack.empty()) 109 | tree = nullptr; 110 | } 111 | return *this; 112 | } 113 | 114 | inline OcTreeNode &operator*() const { 115 | return (*tree)[get_hash_key()]; 116 | } 117 | 118 | inline OcTreeNode &get_node() const { 119 | return operator*(); 120 | } 121 | 122 | inline OcTreeHashKey get_hash_key() const { 123 | OcTreeHashKey key = node_to_hash_key(stack.top().depth, stack.top().index); 124 | return key; 125 | } 126 | 127 | protected: 128 | void single_inc() { 129 | StackElement top(stack.top()); 130 | stack.pop(); 131 | 132 | for (int i = 0; i < 8; ++i) { 133 | stack.emplace(top.depth + 1, top.index * 8 + i); 134 | } 135 | } 136 | 137 | struct StackElement { 138 | unsigned short depth; 139 | unsigned short index; 140 | 141 | StackElement(unsigned short depth, unsigned short index) 142 | : depth(depth), index(index) { } 143 | }; 144 | 145 | const OcTree *tree; 146 | std::stack > stack; 147 | }; 148 | 149 | /// @return the beginning of leaf iterator 150 | inline LeafIterator begin_leaf() const { return LeafIterator(this); }; 151 | 152 | /// @return the end of leaf iterator 153 | inline LeafIterator end_leaf() const { return LeafIterator(nullptr); }; 154 | 155 | private: 156 | OcTreeNode **node_arr; 157 | static unsigned short max_depth; 158 | }; 159 | } 160 | 161 | #endif // LA3DM_GP_OCTREE_H 162 | -------------------------------------------------------------------------------- /include/bgkoctomap/bgkoctree.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGK_OCTREE_H 2 | #define LA3DM_BGK_OCTREE_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "bgkoctree_node.h" 8 | 9 | namespace la3dm { 10 | 11 | /// Hash key to index OcTree nodes given depth and the index in that layer. 12 | typedef int OcTreeHashKey; 13 | 14 | /// Convert from node to hask key. 15 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index); 16 | 17 | /// Convert from hash key to node. 18 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index); 19 | 20 | /* 21 | * @brief A simple OcTree to organize occupancy data in one block. 22 | * 23 | * OcTree doesn't store positions of nodes in order to reduce memory usage. 24 | * The nodes in OcTrees are indexed by OcTreeHashKey which can be used to 25 | * retrieve positions later (See Block). 26 | * For the purpose of mapping, this OcTree has fixed depth which should be 27 | * set before using OcTrees. 28 | */ 29 | class OcTree { 30 | friend class BGKOctoMap; 31 | 32 | public: 33 | OcTree(); 34 | 35 | ~OcTree(); 36 | 37 | OcTree(const OcTree &other); 38 | 39 | OcTree &operator=(const OcTree &other); 40 | 41 | /* 42 | * @brief Rursively pruning OcTreeNodes with the same state. 43 | * 44 | * Prune nodes by setting nodes to PRUNED. 45 | * Delete the layer if all nodes are pruned. 46 | */ 47 | bool prune(); 48 | 49 | /// @return true if this node is a leaf node. 50 | bool is_leaf(OcTreeHashKey key) const; 51 | 52 | /// @return true if this node is a leaf node. 53 | bool is_leaf(unsigned short depth, unsigned short index) const; 54 | 55 | /// @return true if this node exists and is not pruned. 56 | bool search(OcTreeHashKey key) const; 57 | 58 | /// @return Occupancy of the node (without checking if it exists!) 59 | OcTreeNode &operator[](OcTreeHashKey key) const; 60 | 61 | /// Leaf iterator for OcTrees: iterate all leaf nodes not pruned. 62 | class LeafIterator : public std::iterator { 63 | public: 64 | LeafIterator() : tree(nullptr) { } 65 | 66 | LeafIterator(const OcTree *tree) 67 | : tree(tree != nullptr && tree->node_arr != nullptr ? tree : nullptr) { 68 | if (tree != nullptr) { 69 | stack.emplace(0, 0); 70 | stack.emplace(0, 0); 71 | ++(*this); 72 | } 73 | } 74 | 75 | LeafIterator(const LeafIterator &other) : tree(other.tree), stack(other.stack) { } 76 | 77 | LeafIterator &operator=(const LeafIterator &other) { 78 | tree = other.tree; 79 | stack = other.stack; 80 | return *this; 81 | } 82 | 83 | bool operator==(const LeafIterator &other) const { 84 | return (tree == other.tree) && 85 | (stack.size() == other.stack.size()) && 86 | (stack.size() == 0 || (stack.size() > 0 && 87 | (stack.top().depth == other.stack.top().depth) && 88 | (stack.top().index == other.stack.top().index))); 89 | } 90 | 91 | bool operator!=(const LeafIterator &other) const { 92 | return !(this->operator==(other)); 93 | } 94 | 95 | LeafIterator operator++(int) { 96 | LeafIterator result(*this); 97 | ++(*this); 98 | return result; 99 | } 100 | 101 | LeafIterator &operator++() { 102 | if (stack.empty()) { 103 | tree = nullptr; 104 | } else { 105 | stack.pop(); 106 | while (!stack.empty() && !tree->is_leaf(stack.top().depth, stack.top().index)) 107 | single_inc(); 108 | if (stack.empty()) 109 | tree = nullptr; 110 | } 111 | return *this; 112 | } 113 | 114 | inline OcTreeNode &operator*() const { 115 | return (*tree)[get_hash_key()]; 116 | } 117 | 118 | inline OcTreeNode &get_node() const { 119 | return operator*(); 120 | } 121 | 122 | inline OcTreeHashKey get_hash_key() const { 123 | OcTreeHashKey key = node_to_hash_key(stack.top().depth, stack.top().index); 124 | return key; 125 | } 126 | 127 | protected: 128 | void single_inc() { 129 | StackElement top(stack.top()); 130 | stack.pop(); 131 | 132 | for (int i = 0; i < 8; ++i) { 133 | stack.emplace(top.depth + 1, top.index * 8 + i); 134 | } 135 | } 136 | 137 | struct StackElement { 138 | unsigned short depth; 139 | unsigned short index; 140 | 141 | StackElement(unsigned short depth, unsigned short index) 142 | : depth(depth), index(index) { } 143 | }; 144 | 145 | const OcTree *tree; 146 | std::stack > stack; 147 | }; 148 | 149 | /// @return the beginning of leaf iterator 150 | inline LeafIterator begin_leaf() const { return LeafIterator(this); }; 151 | 152 | /// @return the end of leaf iterator 153 | inline LeafIterator end_leaf() const { return LeafIterator(nullptr); }; 154 | 155 | private: 156 | OcTreeNode **node_arr; 157 | static unsigned short max_depth; 158 | }; 159 | } 160 | 161 | #endif // LA3DM_BGK_OCTREE_H 162 | -------------------------------------------------------------------------------- /include/bgkloctomap/bgkloctree.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKL_OCTREE_H 2 | #define LA3DM_BGKL_OCTREE_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "bgkloctree_node.h" 8 | 9 | namespace la3dm { 10 | 11 | /// Hash key to index OcTree nodes given depth and the index in that layer. 12 | typedef int OcTreeHashKey; 13 | 14 | /// Convert from node to hask key. 15 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index); 16 | 17 | /// Convert from hash key to node. 18 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index); 19 | 20 | /* 21 | * @brief A simple OcTree to organize occupancy data in one block. 22 | * 23 | * OcTree doesn't store positions of nodes in order to reduce memory usage. 24 | * The nodes in OcTrees are indexed by OcTreeHashKey which can be used to 25 | * retrieve positions later (See Block). 26 | * For the purpose of mapping, this OcTree has fixed depth which should be 27 | * set before using OcTrees. 28 | */ 29 | class OcTree { 30 | friend class BGKLOctoMap; 31 | 32 | public: 33 | OcTree(); 34 | 35 | ~OcTree(); 36 | 37 | OcTree(const OcTree &other); 38 | 39 | OcTree &operator=(const OcTree &other); 40 | 41 | /* 42 | * @brief Rursively pruning OcTreeNodes with the same state. 43 | * 44 | * Prune nodes by setting nodes to PRUNED. 45 | * Delete the layer if all nodes are pruned. 46 | */ 47 | bool prune(); 48 | 49 | /// @return true if this node is a leaf node. 50 | bool is_leaf(OcTreeHashKey key) const; 51 | 52 | /// @return true if this node is a leaf node. 53 | bool is_leaf(unsigned short depth, unsigned short index) const; 54 | 55 | /// @return true if this node exists and is not pruned. 56 | bool search(OcTreeHashKey key) const; 57 | 58 | /// @return Occupancy of the node (without checking if it exists!) 59 | OcTreeNode &operator[](OcTreeHashKey key) const; 60 | 61 | /// Leaf iterator for OcTrees: iterate all leaf nodes not pruned. 62 | class LeafIterator : public std::iterator { 63 | public: 64 | LeafIterator() : tree(nullptr) { } 65 | 66 | LeafIterator(const OcTree *tree) 67 | : tree(tree != nullptr && tree->node_arr != nullptr ? tree : nullptr) { 68 | if (tree != nullptr) { 69 | stack.emplace(0, 0); 70 | stack.emplace(0, 0); 71 | ++(*this); 72 | } 73 | } 74 | 75 | LeafIterator(const LeafIterator &other) : tree(other.tree), stack(other.stack) { } 76 | 77 | LeafIterator &operator=(const LeafIterator &other) { 78 | tree = other.tree; 79 | stack = other.stack; 80 | return *this; 81 | } 82 | 83 | bool operator==(const LeafIterator &other) const { 84 | return (tree == other.tree) && 85 | (stack.size() == other.stack.size()) && 86 | (stack.size() == 0 || (stack.size() > 0 && 87 | (stack.top().depth == other.stack.top().depth) && 88 | (stack.top().index == other.stack.top().index))); 89 | } 90 | 91 | bool operator!=(const LeafIterator &other) const { 92 | return !(this->operator==(other)); 93 | } 94 | 95 | LeafIterator operator++(int) { 96 | LeafIterator result(*this); 97 | ++(*this); 98 | return result; 99 | } 100 | 101 | LeafIterator &operator++() { 102 | if (stack.empty()) { 103 | tree = nullptr; 104 | } else { 105 | stack.pop(); 106 | while (!stack.empty() && !tree->is_leaf(stack.top().depth, stack.top().index)) 107 | single_inc(); 108 | if (stack.empty()) 109 | tree = nullptr; 110 | } 111 | return *this; 112 | } 113 | 114 | inline OcTreeNode &operator*() const { 115 | return (*tree)[get_hash_key()]; 116 | } 117 | 118 | inline OcTreeNode &get_node() const { 119 | return operator*(); 120 | } 121 | 122 | inline OcTreeHashKey get_hash_key() const { 123 | OcTreeHashKey key = node_to_hash_key(stack.top().depth, stack.top().index); 124 | return key; 125 | } 126 | 127 | protected: 128 | void single_inc() { 129 | StackElement top(stack.top()); 130 | stack.pop(); 131 | 132 | for (int i = 0; i < 8; ++i) { 133 | stack.emplace(top.depth + 1, top.index * 8 + i); 134 | } 135 | } 136 | 137 | struct StackElement { 138 | unsigned short depth; 139 | unsigned short index; 140 | 141 | StackElement(unsigned short depth, unsigned short index) 142 | : depth(depth), index(index) { } 143 | }; 144 | 145 | const OcTree *tree; 146 | std::stack > stack; 147 | }; 148 | 149 | /// @return the beginning of leaf iterator 150 | inline LeafIterator begin_leaf() const { return LeafIterator(this); }; 151 | 152 | /// @return the end of leaf iterator 153 | inline LeafIterator end_leaf() const { return LeafIterator(nullptr); }; 154 | 155 | private: 156 | OcTreeNode **node_arr; 157 | static unsigned short max_depth; 158 | }; 159 | } 160 | 161 | #endif // LA3DM_BGKL_OCTREE_H 162 | -------------------------------------------------------------------------------- /include/bgklvoctomap/bgklvoctree.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKLV_OCTREE_H 2 | #define LA3DM_BGKLV_OCTREE_H 3 | 4 | #include 5 | #include 6 | #include "point3f.h" 7 | #include "bgklvoctree_node.h" 8 | 9 | namespace la3dm { 10 | 11 | /// Hash key to index OcTree nodes given depth and the index in that layer. 12 | typedef int OcTreeHashKey; 13 | 14 | /// Convert from node to hask key. 15 | OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned long index); 16 | 17 | /// Convert from hash key to node. 18 | void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned long &index); 19 | 20 | /* 21 | * @brief A simple OcTree to organize occupancy data in one block. 22 | * 23 | * OcTree doesn't store positions of nodes in order to reduce memory usage. 24 | * The nodes in OcTrees are indexed by OcTreeHashKey which can be used to 25 | * retrieve positions later (See Block). 26 | * For the purpose of mapping, this OcTree has fixed depth which should be 27 | * set before using OcTrees. 28 | */ 29 | class OcTree { 30 | friend class BGKLVOctoMap; 31 | 32 | public: 33 | OcTree(); 34 | 35 | ~OcTree(); 36 | 37 | OcTree(const OcTree &other); 38 | 39 | OcTree &operator=(const OcTree &other); 40 | 41 | /* 42 | * @brief Rursively pruning OcTreeNodes with the same state. 43 | * 44 | * Prune nodes by setting nodes to PRUNED. 45 | * Delete the layer if all nodes are pruned. 46 | */ 47 | bool prune(); 48 | 49 | /// @return true if this node is a leaf node. 50 | bool is_leaf(OcTreeHashKey key) const; 51 | 52 | /// @return true if this node is a leaf node. 53 | bool is_leaf(unsigned short depth, unsigned long index) const; 54 | 55 | /// @return true if this node exists and is not pruned. 56 | bool search(OcTreeHashKey key) const; 57 | 58 | /// @return Occupancy of the node (without checking if it exists!) 59 | OcTreeNode &operator[](OcTreeHashKey key) const; 60 | 61 | /// Leaf iterator for OcTrees: iterate all leaf nodes not pruned. 62 | class LeafIterator : public std::iterator { 63 | public: 64 | LeafIterator() : tree(nullptr) { } 65 | 66 | LeafIterator(const OcTree *tree) 67 | : tree(tree != nullptr && tree->node_arr != nullptr ? tree : nullptr) { 68 | if (tree != nullptr) { 69 | stack.emplace(0, 0); 70 | stack.emplace(0, 0); 71 | ++(*this); 72 | } 73 | } 74 | 75 | LeafIterator(const LeafIterator &other) : tree(other.tree), stack(other.stack) { } 76 | 77 | LeafIterator &operator=(const LeafIterator &other) { 78 | tree = other.tree; 79 | stack = other.stack; 80 | return *this; 81 | } 82 | 83 | bool operator==(const LeafIterator &other) const { 84 | return (tree == other.tree) && 85 | (stack.size() == other.stack.size()) && 86 | (stack.size() == 0 || (stack.size() > 0 && 87 | (stack.top().depth == other.stack.top().depth) && 88 | (stack.top().index == other.stack.top().index))); 89 | } 90 | 91 | bool operator!=(const LeafIterator &other) const { 92 | return !(this->operator==(other)); 93 | } 94 | 95 | LeafIterator operator++(int) { 96 | LeafIterator result(*this); 97 | ++(*this); 98 | return result; 99 | } 100 | 101 | LeafIterator &operator++() { 102 | if (stack.empty()) { 103 | tree = nullptr; 104 | } else { 105 | stack.pop(); 106 | while (!stack.empty() && !tree->is_leaf(stack.top().depth, stack.top().index)) 107 | single_inc(); 108 | if (stack.empty()) 109 | tree = nullptr; 110 | } 111 | return *this; 112 | } 113 | 114 | inline OcTreeNode &operator*() const { 115 | return (*tree)[get_hash_key()]; 116 | } 117 | 118 | inline OcTreeNode &get_node() const { 119 | return operator*(); 120 | } 121 | 122 | inline OcTreeHashKey get_hash_key() const { 123 | OcTreeHashKey key = node_to_hash_key(stack.top().depth, stack.top().index); 124 | return key; 125 | } 126 | 127 | protected: 128 | void single_inc() { 129 | StackElement top(stack.top()); 130 | stack.pop(); 131 | 132 | for (int i = 0; i < 8; ++i) { 133 | stack.emplace(top.depth + 1, top.index * 8 + i); 134 | } 135 | } 136 | 137 | struct StackElement { 138 | unsigned short depth; 139 | unsigned long index; 140 | 141 | StackElement(unsigned short depth, unsigned long index) 142 | : depth(depth), index(index) { } 143 | }; 144 | 145 | const OcTree *tree; 146 | std::stack > stack; 147 | }; 148 | 149 | /// @return the beginning of leaf iterator 150 | inline LeafIterator begin_leaf() const { return LeafIterator(this); }; 151 | 152 | /// @return the end of leaf iterator 153 | inline LeafIterator end_leaf() const { return LeafIterator(nullptr); }; 154 | 155 | private: 156 | OcTreeNode **node_arr; 157 | static unsigned short max_depth; 158 | }; 159 | } 160 | 161 | #endif // LA3DM_BGKLV_OCTREE_H 162 | -------------------------------------------------------------------------------- /rviz/sim_structured.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21/Autocompute Value Bounds1 10 | Splitter Ratio: 0.5 11 | Tree Height: 549 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 0 60 | Min Value: 4.300000190734863 61 | Value: false 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 255; 255; 255 66 | Color Transformer: AxisColor 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Min Color: 0; 0; 0 72 | Name: PointCloud2 73 | Position Transformer: XYZ 74 | Queue Size: 100 75 | Selectable: true 76 | Size (Pixels): 3 77 | Size (m): 0.10000000149011612 78 | Style: Points 79 | Topic: /raw_pc 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Class: rviz/MarkerArray 85 | Enabled: true 86 | Marker Topic: /occupied_cells_vis_array 87 | Name: MarkerArray 88 | Namespaces: 89 | map: true 90 | Queue Size: 100 91 | Value: true 92 | - Class: rviz/MarkerArray 93 | Enabled: true 94 | Marker Topic: /free_cells_vis_array 95 | Name: MarkerArray 96 | Namespaces: 97 | map: true 98 | Queue Size: 100 99 | Value: true 100 | Enabled: true 101 | Global Options: 102 | Background Color: 255; 255; 255 103 | Default Light: true 104 | Fixed Frame: map 105 | Frame Rate: 30 106 | Name: root 107 | Tools: 108 | - Class: rviz/Interact 109 | Hide Inactive Objects: true 110 | - Class: rviz/MoveCamera 111 | - Class: rviz/Select 112 | - Class: rviz/FocusCamera 113 | - Class: rviz/Measure 114 | - Class: rviz/SetInitialPose 115 | Theta std deviation: 0.2617993950843811 116 | Topic: /initialpose 117 | X std deviation: 0.5 118 | Y std deviation: 0.5 119 | - Class: rviz/SetGoal 120 | Topic: /move_base_simple/goal 121 | - Class: rviz/PublishPoint 122 | Single click: true 123 | Topic: /clicked_point 124 | Value: true 125 | Views: 126 | Current: 127 | Class: rviz/Orbit 128 | Distance: 24.44809913635254 129 | Enable Stereo Rendering: 130 | Stereo Eye Separation: 0.05999999865889549 131 | Stereo Focal Distance: 1 132 | Swap Stereo Eyes: false 133 | Value: false 134 | Field of View: 0.7853981852531433 135 | Focal Point: 136 | X: 10.838899612426758 137 | Y: 9.29736042022705 138 | Z: -8.657699584960938 139 | Focal Shape Fixed Size: true 140 | Focal Shape Size: 0.05000000074505806 141 | Invert Z Axis: false 142 | Name: Current View 143 | Near Clip Distance: 0.009999999776482582 144 | Pitch: 0.754800021648407 145 | Target Frame: 146 | Yaw: 3.6136300563812256 147 | Saved: ~ 148 | Window Geometry: 149 | Displays: 150 | collapsed: false 151 | Height: 845 152 | Hide Left Dock: false 153 | Hide Right Dock: true 154 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000002ac000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003dfc0100000002fb0000000800540069006d0065010000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d0000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 155 | Selection: 156 | collapsed: false 157 | Time: 158 | collapsed: false 159 | Tool Properties: 160 | collapsed: false 161 | Views: 162 | collapsed: true 163 | Width: 1600 164 | X: 72 165 | Y: 0 166 | -------------------------------------------------------------------------------- /rviz/sim_unstructured.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21/Autocompute Value Bounds1 10 | Splitter Ratio: 0.5 11 | Tree Height: 759 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 0 60 | Min Value: 4.300000190734863 61 | Value: false 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 0; 0; 0 66 | Color Transformer: AxisColor 67 | Decay Time: 9999 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Min Color: 0; 0; 0 72 | Name: PointCloud2 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 3 77 | Size (m): 0.10000000149011612 78 | Style: Points 79 | Topic: /raw_pc 80 | Unreliable: false 81 | Use Fixed Frame: false 82 | Use rainbow: true 83 | Value: true 84 | - Class: rviz/MarkerArray 85 | Enabled: true 86 | Marker Topic: /occupied_cells_vis_array 87 | Name: MarkerArray 88 | Namespaces: 89 | map: true 90 | Queue Size: 100 91 | Value: true 92 | - Class: rviz/MarkerArray 93 | Enabled: true 94 | Marker Topic: /free_cells_vis_array 95 | Name: MarkerArray 96 | Namespaces: 97 | map: true 98 | Queue Size: 100 99 | Value: true 100 | Enabled: true 101 | Global Options: 102 | Background Color: 255; 255; 255 103 | Default Light: true 104 | Fixed Frame: map 105 | Frame Rate: 30 106 | Name: root 107 | Tools: 108 | - Class: rviz/Interact 109 | Hide Inactive Objects: true 110 | - Class: rviz/MoveCamera 111 | - Class: rviz/Select 112 | - Class: rviz/FocusCamera 113 | - Class: rviz/Measure 114 | - Class: rviz/SetInitialPose 115 | Theta std deviation: 0.2617993950843811 116 | Topic: /initialpose 117 | X std deviation: 0.5 118 | Y std deviation: 0.5 119 | - Class: rviz/SetGoal 120 | Topic: /move_base_simple/goal 121 | - Class: rviz/PublishPoint 122 | Single click: true 123 | Topic: /clicked_point 124 | Value: true 125 | Views: 126 | Current: 127 | Class: rviz/Orbit 128 | Distance: 25.977399826049805 129 | Enable Stereo Rendering: 130 | Stereo Eye Separation: 0.05999999865889549 131 | Stereo Focal Distance: 1 132 | Swap Stereo Eyes: false 133 | Value: false 134 | Field of View: 0.7853981852531433 135 | Focal Point: 136 | X: 12.179200172424316 137 | Y: 8.235679626464844 138 | Z: -10.637200355529785 139 | Focal Shape Fixed Size: true 140 | Focal Shape Size: 0.05000000074505806 141 | Invert Z Axis: false 142 | Name: Current View 143 | Near Clip Distance: 0.009999999776482582 144 | Pitch: 0.8352029919624329 145 | Target Frame: 146 | Yaw: 3.5167899131774902 147 | Saved: ~ 148 | Window Geometry: 149 | Displays: 150 | collapsed: false 151 | Height: 1056 152 | Hide Left Dock: false 153 | Hide Right Dock: false 154 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000382fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000382000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000382fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000382000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004ce0000038200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 155 | Selection: 156 | collapsed: false 157 | Time: 158 | collapsed: false 159 | Tool Properties: 160 | collapsed: false 161 | Views: 162 | collapsed: false 163 | Width: 1855 164 | X: 72 165 | Y: 0 166 | -------------------------------------------------------------------------------- /src/bgkoctomap/bgkoctomap_static_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "bgkoctomap.h" 5 | #include "markerarray_pub.h" 6 | 7 | void load_pcd(std::string filename, la3dm::point3f &origin, la3dm::PCLPointCloud &cloud) { 8 | pcl::PCLPointCloud2 cloud2; 9 | Eigen::Vector4f _origin; 10 | Eigen::Quaternionf orientaion; 11 | pcl::io::loadPCDFile(filename, cloud2, _origin, orientaion); 12 | pcl::fromPCLPointCloud2(cloud2, cloud); 13 | origin.x() = _origin[0]; 14 | origin.y() = _origin[1]; 15 | origin.z() = _origin[2]; 16 | } 17 | 18 | int main(int argc, char **argv) { 19 | ros::init(argc, argv, "bgkoctomap_static_node"); 20 | ros::NodeHandle nh("~"); 21 | 22 | std::string dir; 23 | std::string prefix; 24 | int scan_num = 0; 25 | std::string map_topic("/occupied_cells_vis_array"); 26 | std::string map_topic2("/free_cells_vis_array"); 27 | double max_range = -1; 28 | double resolution = 0.1; 29 | int block_depth = 4; 30 | double sf2 = 1.0; 31 | double ell = 1.0; 32 | double free_resolution = 0.5; 33 | double ds_resolution = 0.1; 34 | double free_thresh = 0.3; 35 | double occupied_thresh = 0.7; 36 | double min_z = 0; 37 | double max_z = 0; 38 | bool original_size = false; 39 | float var_thresh = 1.0f; 40 | float prior_A = 1.0f; 41 | float prior_B = 1.0f; 42 | 43 | nh.param("dir", dir, dir); 44 | nh.param("prefix", prefix, prefix); 45 | nh.param("topic", map_topic, map_topic); 46 | nh.param("topic2", map_topic2, map_topic2); 47 | nh.param("scan_num", scan_num, scan_num); 48 | nh.param("max_range", max_range, max_range); 49 | nh.param("resolution", resolution, resolution); 50 | nh.param("block_depth", block_depth, block_depth); 51 | nh.param("sf2", sf2, sf2); 52 | nh.param("ell", ell, ell); 53 | nh.param("free_resolution", free_resolution, free_resolution); 54 | nh.param("ds_resolution", ds_resolution, ds_resolution); 55 | nh.param("free_thresh", free_thresh, free_thresh); 56 | nh.param("occupied_thresh", occupied_thresh, occupied_thresh); 57 | nh.param("min_z", min_z, min_z); 58 | nh.param("max_z", max_z, max_z); 59 | nh.param("original_size", original_size, original_size); 60 | nh.param("var_thresh", var_thresh, var_thresh); 61 | nh.param("prior_A", prior_A, prior_A); 62 | nh.param("prior_B", prior_B, prior_B); 63 | 64 | ROS_INFO_STREAM("Parameters:" << std::endl << 65 | "dir: " << dir << std::endl << 66 | "prefix: " << prefix << std::endl << 67 | "topic: " << map_topic << std::endl << 68 | "scan_sum: " << scan_num << std::endl << 69 | "max_range: " << max_range << std::endl << 70 | "resolution: " << resolution << std::endl << 71 | "block_depth: " << block_depth << std::endl << 72 | "sf2: " << sf2 << std::endl << 73 | "ell: " << ell << std::endl << 74 | "free_resolution: " << free_resolution << std::endl << 75 | "ds_resolution: " << ds_resolution << std::endl << 76 | "free_thresh: " << free_thresh << std::endl << 77 | "occupied_thresh: " << occupied_thresh << std::endl << 78 | "min_z: " << min_z << std::endl << 79 | "max_z: " << max_z << std::endl << 80 | "original_size: " << original_size << std::endl << 81 | "var_thresh: " << var_thresh << std::endl << 82 | "prior_A: " << prior_A << std::endl << 83 | "prior_B: " << prior_B 84 | ); 85 | 86 | la3dm::BGKOctoMap map(resolution, block_depth, sf2, ell, free_thresh, occupied_thresh, var_thresh, prior_A, prior_B); 87 | 88 | ros::Time start = ros::Time::now(); 89 | for (int scan_id = 1; scan_id <= scan_num; ++scan_id) { 90 | la3dm::PCLPointCloud cloud; 91 | la3dm::point3f origin; 92 | std::string filename(dir + "/" + prefix + "_" + std::to_string(scan_id) + ".pcd"); 93 | load_pcd(filename, origin, cloud); 94 | 95 | map.insert_pointcloud(cloud, origin, resolution, free_resolution, max_range); 96 | ROS_INFO_STREAM("Scan " << scan_id << " done"); 97 | } 98 | ros::Time end = ros::Time::now(); 99 | ROS_INFO_STREAM("Mapping finished in " << (end - start).toSec() << "s"); 100 | 101 | ///////// Publish Map ///////////////////// 102 | la3dm::MarkerArrayPub m_pub(nh, map_topic, resolution); 103 | la3dm::MarkerArrayPub m_pub2(nh, map_topic2, resolution); 104 | if (min_z == max_z) { 105 | la3dm::point3f lim_min, lim_max; 106 | map.get_bbox(lim_min, lim_max); 107 | min_z = lim_min.z(); 108 | max_z = lim_max.z(); 109 | } 110 | 111 | for (auto it = map.begin_leaf(); it != map.end_leaf(); ++it) { 112 | la3dm::point3f p = it.get_loc(); 113 | 114 | if (it.get_node().get_state() == la3dm::State::OCCUPIED) { 115 | if (original_size) { 116 | la3dm::point3f p = it.get_loc(); 117 | m_pub.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size()); 118 | } else { 119 | auto pruned = it.get_pruned_locs(); 120 | for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) 121 | m_pub.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution()); 122 | } 123 | } 124 | if (it.get_node().get_state() == la3dm::State::FREE) { 125 | if (original_size) { 126 | la3dm::point3f p = it.get_loc(); 127 | m_pub2.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size(), it.get_node().get_prob()); 128 | } 129 | else { 130 | auto pruned = it.get_pruned_locs(); 131 | for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) 132 | m_pub2.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution(), it.get_node().get_prob()); 133 | } 134 | 135 | } 136 | } 137 | 138 | m_pub.publish(); 139 | m_pub2.publish(); 140 | ros::spin(); 141 | 142 | return 0; 143 | } 144 | -------------------------------------------------------------------------------- /src/gpoctomap/gpoctomap_static_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "gpoctomap.h" 5 | #include "markerarray_pub.h" 6 | 7 | void load_pcd(std::string filename, la3dm::point3f &origin, la3dm::PCLPointCloud &cloud) { 8 | pcl::PCLPointCloud2 cloud2; 9 | Eigen::Vector4f _origin; 10 | Eigen::Quaternionf orientaion; 11 | pcl::io::loadPCDFile(filename, cloud2, _origin, orientaion); 12 | pcl::fromPCLPointCloud2(cloud2, cloud); 13 | origin.x() = _origin[0]; 14 | origin.y() = _origin[1]; 15 | origin.z() = _origin[2]; 16 | } 17 | 18 | int main(int argc, char **argv) { 19 | ros::init(argc, argv, "gpoctomap_static_node"); 20 | ros::NodeHandle nh("~"); 21 | 22 | std::string dir; 23 | std::string prefix; 24 | int scan_num = 0; 25 | std::string map_topic("/occupied_cells_vis_array"); 26 | std::string map_topic2("/free_cells_vis_array"); 27 | double max_range = -1; 28 | double resolution = 0.1; 29 | int block_depth = 4; 30 | double sf2 = 1.0; 31 | double ell = 1.0; 32 | double noise = 0.01; 33 | double l = 100; 34 | double min_var = 0.001; 35 | double max_var = 1000; 36 | double max_known_var = 0.02; 37 | double free_resolution = 0.5; 38 | double ds_resolution = 0.1; 39 | double free_thresh = 0.3; 40 | double occupied_thresh = 0.7; 41 | double min_z = 0; 42 | double max_z = 0; 43 | bool original_size = false; 44 | 45 | nh.param("dir", dir, dir); 46 | nh.param("prefix", prefix, prefix); 47 | nh.param("topic", map_topic, map_topic); 48 | nh.param("topic2", map_topic2, map_topic2); 49 | nh.param("scan_num", scan_num, scan_num); 50 | nh.param("max_range", max_range, max_range); 51 | nh.param("resolution", resolution, resolution); 52 | nh.param("block_depth", block_depth, block_depth); 53 | nh.param("sf2", sf2, sf2); 54 | nh.param("ell", ell, ell); 55 | nh.param("noise", noise, noise); 56 | nh.param("l", l, l); 57 | nh.param("min_var", min_var, min_var); 58 | nh.param("max_var", max_var, max_var); 59 | nh.param("max_known_var", max_known_var, max_known_var); 60 | nh.param("free_resolution", free_resolution, free_resolution); 61 | nh.param("ds_resolution", ds_resolution, ds_resolution); 62 | nh.param("free_thresh", free_thresh, free_thresh); 63 | nh.param("occupied_thresh", occupied_thresh, occupied_thresh); 64 | nh.param("min_z", min_z, min_z); 65 | nh.param("max_z", max_z, max_z); 66 | nh.param("original_size", original_size, original_size); 67 | 68 | ROS_INFO_STREAM("Parameters:" << std::endl << 69 | "dir: " << dir << std::endl << 70 | "prefix: " << prefix << std::endl << 71 | "topic: " << map_topic << std::endl << 72 | "scan_sum: " << scan_num << std::endl << 73 | "max_range: " << max_range << std::endl << 74 | "resolution: " << resolution << std::endl << 75 | "block_depth: " << block_depth << std::endl << 76 | "sf2: " << sf2 << std::endl << 77 | "ell: " << ell << std::endl << 78 | "l: " << l << std::endl << 79 | "min_var: " << min_var << std::endl << 80 | "max_var: " << max_var << std::endl << 81 | "max_known_var: " << max_known_var << std::endl << 82 | "free_resolution: " << free_resolution << std::endl << 83 | "ds_resolution: " << ds_resolution << std::endl << 84 | "free_thresh: " << free_thresh << std::endl << 85 | "occupied_thresh: " << occupied_thresh << std::endl << 86 | "min_z: " << min_z << std::endl << 87 | "max_z: " << max_z << std::endl << 88 | "original_size: " << original_size 89 | ); 90 | 91 | la3dm::GPOctoMap map(resolution, block_depth, sf2, ell, noise, l, min_var, max_var, max_known_var, free_thresh, occupied_thresh); 92 | 93 | ros::Time start = ros::Time::now(); 94 | for (int scan_id = 1; scan_id <= scan_num; ++scan_id) { 95 | la3dm::PCLPointCloud cloud; 96 | la3dm::point3f origin; 97 | std::string filename(dir + "/" + prefix + "_" + std::to_string(scan_id) + ".pcd"); 98 | load_pcd(filename, origin, cloud); 99 | 100 | map.insert_pointcloud(cloud, origin, resolution, free_resolution, max_range); 101 | ROS_INFO_STREAM("Scan " << scan_id << " done"); 102 | } 103 | ros::Time end = ros::Time::now(); 104 | ROS_INFO_STREAM("Mapping finished in " << (end - start).toSec() << "s"); 105 | 106 | ///////// Publish Map ///////////////////// 107 | la3dm::MarkerArrayPub m_pub(nh, map_topic, resolution); 108 | la3dm::MarkerArrayPub m_pub2(nh, map_topic2, resolution); 109 | if (min_z == max_z) { 110 | la3dm::point3f lim_min, lim_max; 111 | map.get_bbox(lim_min, lim_max); 112 | min_z = lim_min.z(); 113 | max_z = lim_max.z(); 114 | } 115 | 116 | for (auto it = map.begin_leaf(); it != map.end_leaf(); ++it) { 117 | la3dm::point3f p = it.get_loc(); 118 | 119 | if (it.get_node().get_state() == la3dm::State::OCCUPIED) { 120 | if (original_size) { 121 | m_pub.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size()); 122 | } else { 123 | auto pruned = it.get_pruned_locs(); 124 | for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) 125 | m_pub.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution()); 126 | } 127 | } 128 | else if (it.get_node().get_state() == la3dm::State::FREE) { 129 | if (original_size) { 130 | m_pub2.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size(), it.get_node().get_prob()); 131 | } else { 132 | auto pruned = it.get_pruned_locs(); 133 | for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) 134 | m_pub2.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution(), it.get_node().get_prob()); 135 | } 136 | 137 | } 138 | } 139 | 140 | m_pub.publish(); 141 | m_pub2.publish(); 142 | ros::spin(); 143 | 144 | return 0; 145 | } 146 | -------------------------------------------------------------------------------- /src/bgklvoctomap/bgklvoctomap_static_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "bgklvoctomap.h" 5 | #include "markerarray_pub.h" 6 | 7 | void load_pcd(std::string filename, la3dm::point3f &origin, la3dm::PCLPointCloud &cloud) { 8 | pcl::PCLPointCloud2 cloud2; 9 | Eigen::Vector4f _origin; 10 | Eigen::Quaternionf orientaion; 11 | pcl::io::loadPCDFile(filename, cloud2, _origin, orientaion); 12 | pcl::fromPCLPointCloud2(cloud2, cloud); 13 | ROS_INFO_STREAM("Pointcloud is of size: " << cloud.size()); 14 | origin.x() = _origin[0]; 15 | origin.y() = _origin[1]; 16 | origin.z() = _origin[2]; 17 | } 18 | 19 | int main(int argc, char **argv) { 20 | ros::init(argc, argv, "bgklvoctomap_static_node"); 21 | ros::NodeHandle nh("~"); 22 | 23 | std::string dir; 24 | std::string prefix; 25 | int scan_num = 0; 26 | std::string map_topic("/occupied_cells_vis_array"); 27 | std::string map_topic2("/free_cells_vis_array"); 28 | double max_range = -1; 29 | double resolution = 0.1; 30 | int block_depth = 4; 31 | double sf2 = 1.0; 32 | double ell = 1.0; 33 | double free_resolution = 0.5; 34 | double ds_resolution = 0.1; 35 | double free_thresh = 0.3; 36 | double occupied_thresh = 0.7; 37 | double min_z = 0; 38 | double max_z = 0; 39 | bool original_size = false; 40 | float var_thresh = 1.0f; 41 | float prior_A = 1.0f; 42 | float prior_B = 1.0f; 43 | float min_W = 0.1f; 44 | 45 | nh.param("dir", dir, dir); 46 | nh.param("prefix", prefix, prefix); 47 | nh.param("topic", map_topic, map_topic); 48 | nh.param("topic2", map_topic2, map_topic2); 49 | nh.param("scan_num", scan_num, scan_num); 50 | nh.param("max_range", max_range, max_range); 51 | nh.param("resolution", resolution, resolution); 52 | nh.param("block_depth", block_depth, block_depth); 53 | nh.param("sf2", sf2, sf2); 54 | nh.param("ell", ell, ell); 55 | nh.param("free_resolution", free_resolution, free_resolution); 56 | nh.param("ds_resolution", ds_resolution, ds_resolution); 57 | nh.param("free_thresh", free_thresh, free_thresh); 58 | nh.param("occupied_thresh", occupied_thresh, occupied_thresh); 59 | nh.param("min_z", min_z, min_z); 60 | nh.param("max_z", max_z, max_z); 61 | nh.param("original_size", original_size, original_size); 62 | nh.param("var_thresh", var_thresh, var_thresh); 63 | nh.param("prior_A", prior_A, prior_A); 64 | nh.param("prior_B", prior_B, prior_B); 65 | nh.param("min_W", min_W, min_W); 66 | 67 | ROS_INFO_STREAM("Parameters:" << std::endl << 68 | "dir: " << dir << std::endl << 69 | "prefix: " << prefix << std::endl << 70 | "topic: " << map_topic << std::endl << 71 | "scan_sum: " << scan_num << std::endl << 72 | "max_range: " << max_range << std::endl << 73 | "resolution: " << resolution << std::endl << 74 | "block_depth: " << block_depth << std::endl << 75 | "sf2: " << sf2 << std::endl << 76 | "ell: " << ell << std::endl << 77 | "free_resolution: " << free_resolution << std::endl << 78 | "ds_resolution: " << ds_resolution << std::endl << 79 | "free_thresh: " << free_thresh << std::endl << 80 | "occupied_thresh: " << occupied_thresh << std::endl << 81 | "min_z: " << min_z << std::endl << 82 | "max_z: " << max_z << std::endl << 83 | "original_size: " << original_size << std::endl << 84 | "var_thresh: " << var_thresh << std::endl << 85 | "prior_A: " << prior_A << std::endl << 86 | "prior_B: " << prior_B << std::endl << 87 | "min_W: " << min_W 88 | ); 89 | 90 | la3dm::BGKLVOctoMap map(resolution, block_depth, sf2, ell, free_thresh, occupied_thresh, var_thresh, prior_A, prior_B, original_size, min_W); 91 | 92 | ros::Time start = ros::Time::now(); 93 | for (int scan_id = 1; scan_id <= scan_num; ++scan_id) { 94 | la3dm::PCLPointCloud cloud; 95 | la3dm::point3f origin; 96 | std::string filename(dir + "/" + prefix + "_" + std::to_string(scan_id) + ".pcd"); 97 | load_pcd(filename, origin, cloud); 98 | ROS_INFO_STREAM("Scan " << scan_id << " loaded"); 99 | 100 | map.insert_pointcloud(cloud, origin, resolution, free_resolution, max_range); 101 | ROS_INFO_STREAM("Scan " << scan_id << " done"); 102 | } 103 | ros::Time end = ros::Time::now(); 104 | ROS_INFO_STREAM("Mapping finished in " << (end - start).toSec() << "s"); 105 | 106 | ///////// Publish Map ///////////////////// 107 | la3dm::MarkerArrayPub m_pub(nh, map_topic, resolution); 108 | la3dm::MarkerArrayPub m_pub2(nh, map_topic2, resolution); 109 | if (min_z == max_z) { 110 | la3dm::point3f lim_min, lim_max; 111 | map.get_bbox(lim_min, lim_max); 112 | min_z = lim_min.z(); 113 | max_z = lim_max.z(); 114 | } 115 | 116 | for (auto it = map.begin_leaf(); it != map.end_leaf(); ++it) { 117 | la3dm::point3f p = it.get_loc(); 118 | 119 | if(p.z() > 2.0) 120 | continue; 121 | 122 | if (it.get_node().get_state() == la3dm::State::OCCUPIED) { 123 | if (original_size) { 124 | m_pub.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size()); 125 | } 126 | else { 127 | auto pruned = it.get_pruned_locs(); 128 | for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) 129 | m_pub.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution()); 130 | } 131 | 132 | } 133 | else if (it.get_node().get_state() == la3dm::State::FREE) { 134 | if (original_size) { 135 | m_pub2.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size(), it.get_node().get_prob()); 136 | } 137 | else { 138 | auto pruned = it.get_pruned_locs(); 139 | for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) 140 | m_pub2.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution(), it.get_node().get_prob()); 141 | } 142 | } 143 | } 144 | 145 | m_pub.publish(); 146 | m_pub2.publish(); 147 | ros::spin(); 148 | 149 | return 0; 150 | } 151 | -------------------------------------------------------------------------------- /include/gpoctomap/gpregressor.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_GP_REGRESSOR_H 2 | #define LA3DM_GP_REGRESSOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace la3dm { 8 | 9 | /* 10 | * @brief A Simple Gaussian Process Regressor 11 | * @param dim dimension of data (2, 3, etc.) 12 | * @param T data type (float, double, etc.) 13 | */ 14 | template 15 | class GPRegressor { 16 | public: 17 | /// Eigen matrix type for training and test data and kernel 18 | using MatrixXType = Eigen::Matrix; 19 | using MatrixKType = Eigen::Matrix; 20 | using MatrixDKType = Eigen::Matrix; 21 | using MatrixYType = Eigen::Matrix; 22 | 23 | GPRegressor(T sf2, T ell, T noise) : sf2(sf2), ell(ell), noise(noise), trained(false) { } 24 | 25 | /* 26 | * @brief Train Gaussian Process 27 | * @param x input vector (3N, row major) 28 | * @param y target vector (N) 29 | */ 30 | void train(const std::vector &x, const std::vector &y) { 31 | assert(x.size() % dim == 0 && (int) (x.size() / dim) == y.size()); 32 | MatrixXType _x = Eigen::Map(x.data(), x.size() / dim, dim); 33 | MatrixYType _y = Eigen::Map(y.data(), y.size(), 1); 34 | train(_x, _y); 35 | } 36 | 37 | /* 38 | * @brief Train Gaussian Process 39 | * @param x input matrix (NX3) 40 | * @param y target matrix (NX1) 41 | */ 42 | void train(const MatrixXType &x, const MatrixYType &y) { 43 | this->x = MatrixXType(x); 44 | covMaterniso3(x, x, K); 45 | // covSparse(x, x, K); 46 | K = K + noise * MatrixKType::Identity(K.rows(), K.cols()); 47 | Eigen::LLT llt(K); 48 | alpha = llt.solve(y); 49 | L = llt.matrixL(); 50 | trained = true; 51 | } 52 | 53 | /* 54 | * @brief Predict with Gaussian Process 55 | * @param xs input vector (3M, row major) 56 | * @param m predicted mean vector (M) 57 | * @param var predicted variance vector (M) 58 | */ 59 | void predict(const std::vector &xs, std::vector &m, std::vector &var) const { 60 | assert(xs.size() % dim == 0); 61 | MatrixXType _xs = Eigen::Map(xs.data(), xs.size() / dim, dim); 62 | 63 | MatrixYType _m, _var; 64 | predict(_xs, _m, _var); 65 | 66 | m.resize(_m.rows()); 67 | var.resize(_var.rows()); 68 | for (int r = 0; r < _m.rows(); ++r) { 69 | m[r] = _m(r, 0); 70 | var[r] = _var(r, 0); 71 | } 72 | } 73 | 74 | /* 75 | * @brief Predict with Gaussian Process 76 | * @param xs input vector (MX3) 77 | * @param m predicted mean matrix (MX1) 78 | * @param var predicted variance matrix (MX1) 79 | */ 80 | void predict(const MatrixXType &xs, MatrixYType &m, MatrixYType &var) const { 81 | assert(trained == true); 82 | MatrixKType Ks; 83 | covMaterniso3(x, xs, Ks); 84 | // covSparse(x, xs, Ks); 85 | m = Ks.transpose() * alpha; 86 | 87 | MatrixKType v = L.template triangularView().solve(Ks); 88 | MatrixDKType Kss; 89 | covMaterniso3(xs, xs, Kss); 90 | // covSparse(xs, xs, Kss); 91 | var = Kss - (v.transpose() * v).diagonal(); 92 | } 93 | 94 | private: 95 | /* 96 | * @brief Compute Euclid distances between two vectors. 97 | * @param x input vector 98 | * @param z input vecotr 99 | * @return d distance matrix 100 | */ 101 | void dist(const MatrixXType &x, const MatrixXType &z, MatrixKType &d) const { 102 | d = MatrixKType::Zero(x.rows(), z.rows()); 103 | for (int i = 0; i < x.rows(); ++i) { 104 | d.row(i) = (z.rowwise() - x.row(i)).rowwise().norm(); 105 | } 106 | } 107 | 108 | /* 109 | * @brief Matern3 kernel. 110 | * @param x input vector 111 | * @param z input vector 112 | * @return Kxz covariance matrix 113 | */ 114 | void covMaterniso3(const MatrixXType &x, const MatrixXType &z, MatrixKType &Kxz) const { 115 | dist(1.73205 / ell * x, 1.73205 / ell * z, Kxz); 116 | Kxz = ((1 + Kxz.array()) * exp(-Kxz.array())).matrix() * sf2; 117 | } 118 | 119 | /* 120 | * @brief Diagonal of Matern3 kernel. 121 | * @return Kxz sf2 * I 122 | */ 123 | void covMaterniso3(const MatrixXType &x, const MatrixXType &z, MatrixDKType &Kxz) const { 124 | Kxz = MatrixDKType::Ones(x.rows()) * sf2; 125 | } 126 | 127 | /* 128 | * @brief Sparse kernel. 129 | * @param x input vector 130 | * @param z input vector 131 | * @return Kxz covariance matrix 132 | * @ref A sparse covariance function for exact gaussian process inference in large datasets. 133 | */ 134 | void covSparse(const MatrixXType &x, const MatrixXType &z, MatrixKType &Kxz) const { 135 | dist(x / ell, z / ell, Kxz); 136 | Kxz = ((2 + (Kxz * 2 * 3.1415926).array().cos()) * (1.0 - Kxz.array()) / 3.0 + 137 | (Kxz * 2 * 3.1415926).array().sin() / (2 * 3.1415926)).matrix() * sf2; 138 | for (int i = 0; i < Kxz.rows(); ++i) { 139 | for (int j = 0; j < Kxz.cols(); ++j) { 140 | if (Kxz(i,j) < 0.0) { 141 | Kxz(i,j) = 0.0f; 142 | } 143 | } 144 | } 145 | } 146 | 147 | /* 148 | * @brief Diagonal of sparse kernel. 149 | * @return Kxz sf2 * I 150 | */ 151 | void covSparse(const MatrixXType &x, const MatrixXType &z, MatrixDKType &Kxz) const { 152 | Kxz = MatrixDKType::Ones(x.rows()) * sf2; 153 | } 154 | 155 | T sf2; // signal variance 156 | T ell; // length-scale 157 | T noise; // noise variance 158 | 159 | MatrixXType x; // temporary storage of training data 160 | MatrixKType K; 161 | MatrixYType alpha; 162 | MatrixKType L; 163 | 164 | bool trained; // true if gpregressor has been trained 165 | }; 166 | 167 | typedef GPRegressor<3, float> GPR3f; 168 | } 169 | #endif // LA3DM_GP_REGRESSOR_H 170 | -------------------------------------------------------------------------------- /include/common/markerarray_pub.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace la3dm { 11 | 12 | std_msgs::ColorRGBA heightMapColor(double h) { 13 | 14 | std_msgs::ColorRGBA color; 15 | color.a = 1.0; 16 | // blend over HSV-values (more colors) 17 | 18 | double s = 1.0; 19 | double v = 1.0; 20 | 21 | h -= floor(h); 22 | h *= 6; 23 | int i; 24 | double m, n, f; 25 | 26 | i = floor(h); 27 | f = h - i; 28 | if (!(i & 1)) 29 | f = 1 - f; // if i is even 30 | m = v * (1 - s); 31 | n = v * (1 - s * f); 32 | 33 | switch (i) { 34 | case 6: 35 | case 0: 36 | color.r = v; 37 | color.g = n; 38 | color.b = m; 39 | break; 40 | case 1: 41 | color.r = n; 42 | color.g = v; 43 | color.b = m; 44 | break; 45 | case 2: 46 | color.r = m; 47 | color.g = v; 48 | color.b = n; 49 | break; 50 | case 3: 51 | color.r = m; 52 | color.g = n; 53 | color.b = v; 54 | break; 55 | case 4: 56 | color.r = n; 57 | color.g = m; 58 | color.b = v; 59 | break; 60 | case 5: 61 | color.r = v; 62 | color.g = m; 63 | color.b = n; 64 | break; 65 | default: 66 | color.r = 1; 67 | color.g = 0.5; 68 | color.b = 0.5; 69 | break; 70 | } 71 | 72 | return color; 73 | } 74 | 75 | class MarkerArrayPub { 76 | typedef pcl::PointXYZ PointType; 77 | typedef pcl::PointCloud PointCloud; 78 | public: 79 | MarkerArrayPub(ros::NodeHandle nh, std::string topic, float resolution) : nh(nh), 80 | msg(new visualization_msgs::MarkerArray), 81 | topic(topic), 82 | resolution(resolution), 83 | markerarray_frame_id("map") { 84 | pub = nh.advertise(topic, 1, true); 85 | 86 | msg->markers.resize(10); 87 | for (int i = 0; i < 10; ++i) { 88 | msg->markers[i].header.frame_id = markerarray_frame_id; 89 | msg->markers[i].ns = "map"; 90 | msg->markers[i].id = i; 91 | msg->markers[i].type = visualization_msgs::Marker::CUBE_LIST; 92 | msg->markers[i].scale.x = resolution * pow(2, i); 93 | msg->markers[i].scale.y = resolution * pow(2, i); 94 | msg->markers[i].scale.z = resolution * pow(2, i); 95 | std_msgs::ColorRGBA color; 96 | color.r = 0.0; 97 | color.g = 0.0; 98 | color.b = 1.0; 99 | color.a = 1.0; 100 | msg->markers[i].color = color; 101 | } 102 | } 103 | 104 | void insert_point3d(float x, float y, float z, float min_z, float max_z, float size) { 105 | geometry_msgs::Point center; 106 | center.x = x; 107 | center.y = y; 108 | center.z = z; 109 | 110 | int depth = 0; 111 | if (size > 0) 112 | depth = (int) log2(size /resolution); 113 | 114 | msg->markers[depth].points.push_back(center); 115 | 116 | if (min_z < max_z) { 117 | double h = (1.0 - std::min(std::max((z - min_z) / (max_z - min_z), 0.0f), 1.0f)) * 0.8; 118 | msg->markers[depth].colors.push_back(heightMapColor(h)); 119 | } 120 | } 121 | 122 | void insert_point3d(float x, float y, float z, float min_z, float max_z, float size, float prob) { 123 | geometry_msgs::Point center; 124 | center.x = x; 125 | center.y = y; 126 | center.z = z; 127 | 128 | int depth = 0; 129 | if (size > 0) 130 | depth = (int) log2(size / resolution); 131 | 132 | msg->markers[depth].points.push_back(center); 133 | 134 | std_msgs::ColorRGBA color; 135 | color.a = 1.0; 136 | 137 | if(prob < 0.5){ 138 | color.r = 0.8; 139 | color.g = 0.8; 140 | color.b = 0.8; 141 | } 142 | else{ 143 | color = heightMapColor(std::min(2.0-2.0*prob, 0.6)); 144 | } 145 | 146 | msg->markers[depth].colors.push_back(color); 147 | } 148 | 149 | void insert_point3d(float x, float y, float z, float min_z, float max_z) { 150 | insert_point3d(x, y, z, min_z, max_z, -1.0f); 151 | } 152 | 153 | void insert_point3d(float x, float y, float z) { 154 | insert_point3d(x, y, z, 1.0f, 0.0f, -1.0f); 155 | } 156 | 157 | void insert_color_point3d(float x, float y, float z, double min_v, double max_v, double v) { 158 | geometry_msgs::Point center; 159 | center.x = x; 160 | center.y = y; 161 | center.z = z; 162 | 163 | int depth = 0; 164 | msg->markers[depth].points.push_back(center); 165 | 166 | double h = (1.0 - std::min(std::max((v - min_v) / (max_v - min_v), 0.0), 1.0)) * 0.8; 167 | msg->markers[depth].colors.push_back(heightMapColor(h)); 168 | } 169 | 170 | void clear() { 171 | for (int i = 0; i < 10; ++i) { 172 | msg->markers[i].points.clear(); 173 | msg->markers[i].colors.clear(); 174 | } 175 | } 176 | 177 | void publish() const { 178 | msg->markers[0].header.stamp = ros::Time::now(); 179 | pub.publish(*msg); 180 | } 181 | 182 | private: 183 | ros::NodeHandle nh; 184 | ros::Publisher pub; 185 | visualization_msgs::MarkerArray::Ptr msg; 186 | std::string markerarray_frame_id; 187 | std::string topic; 188 | float resolution; 189 | }; 190 | 191 | } 192 | -------------------------------------------------------------------------------- /include/bgklvoctomap/bgklvinference.h: -------------------------------------------------------------------------------- 1 | #ifndef LA3DM_BGKLV_H 2 | #define LA3DM_BGKLV_H 3 | 4 | namespace la3dm { 5 | 6 | /* 7 | * @brief Bayesian Generalized Kernel Inference on Bernoulli distribution 8 | * @param dim dimension of data (2, 3, etc.) 9 | * @param T data type (float, double, etc.) 10 | * @ref Nonparametric Bayesian inference on multivariate exponential families 11 | */ 12 | template 13 | class BGKLVInference { 14 | public: 15 | /// Eigen matrix type for training and test data and kernel 16 | using MatrixXType = Eigen::Matrix; 17 | using MatrixPType = Eigen::Matrix; 18 | using MatrixKType = Eigen::Matrix; 19 | using MatrixDKType = Eigen::Matrix; 20 | using MatrixYType = Eigen::Matrix; 21 | 22 | float EPSILON = 0.0001; 23 | 24 | BGKLVInference(T sf2, T ell) : sf2(sf2), ell(ell), trained(false) { } 25 | 26 | /* 27 | * @brief Fit BGKLV Model 28 | * @param x input vector (3N, row major) 29 | * @param y target vector (N) 30 | */ 31 | void train(const std::vector &x, const std::vector &y) { 32 | assert(x.size() % (2*dim) == 0 && (int) (x.size() / (2*dim)) == y.size()); 33 | MatrixXType _x = Eigen::Map(x.data(), x.size() / (2*dim), 2*dim); 34 | MatrixYType _y = Eigen::Map(y.data(), y.size(), 1); 35 | train(_x, _y); 36 | } 37 | 38 | /* 39 | * @brief Fit BGKLV Model 40 | * @param x input matrix (NX3) 41 | * @param y target matrix (NX1) 42 | */ 43 | void train(const MatrixXType &x, const MatrixYType &y) { 44 | // std::cout << "training pt2" << std::endl; 45 | this->xt = MatrixXType(x); 46 | this->yt = MatrixYType(y); 47 | trained = true; 48 | } 49 | 50 | /* 51 | * @brief Predict with BGKLV Model 52 | * @param xs input vector (3M, row major) 53 | * @param ybar positive class kernel density estimate (\bar{y}) 54 | * @param kbar kernel density estimate (\bar{k}) 55 | */ 56 | void predict(const std::vector &xs, std::vector &ybar, std::vector &kbar) const { 57 | assert(xs.size() % dim == 0); 58 | MatrixPType _xs = Eigen::Map(xs.data(), xs.size() / dim, dim); 59 | 60 | MatrixYType _ybar, _kbar; 61 | predict(_xs, _ybar, _kbar); 62 | ybar.resize(_ybar.rows()); 63 | kbar.resize(_kbar.rows()); 64 | for (int r = 0; r < _kbar.rows(); ++r) { 65 | ybar[r] = _ybar(r, 0); 66 | kbar[r] = _kbar(r, 0); 67 | } 68 | } 69 | 70 | /* 71 | * @brief Predict with nonparametric Bayesian generalized kernel inference 72 | * @param xs input vector (M x 3) 73 | * @param ybar positive class kernel density estimate (M x 1) 74 | * @param kbar kernel density estimate (M x 1) 75 | */ 76 | void predict(const MatrixPType &xs, MatrixYType &ybar, MatrixYType &kbar) const { 77 | // std::cout << "second prediction step" << std::endl; 78 | assert(trained == true); 79 | MatrixKType Ks; 80 | covSparseLine(xs, xt, Ks); 81 | // std::cout << "computed covsparseline" << std::endl; 82 | ybar = (Ks * yt).array(); 83 | kbar = Ks.rowwise().sum().array(); 84 | } 85 | 86 | private: 87 | /* 88 | * @brief Compute Euclid distances between two vectors. 89 | * @param x input vector 90 | * @param z input vecotr 91 | * @return d distance matrix 92 | */ 93 | void dist(const MatrixXType &x, const MatrixXType &z, MatrixKType &d) const { 94 | d = MatrixKType::Zero(x.rows(), z.rows()); 95 | for (int i = 0; i < x.rows(); ++i) { 96 | d.row(i) = (z.rowwise() - x.row(i)).rowwise().norm(); 97 | } 98 | } 99 | 100 | void point_to_line_dist(const MatrixPType &x, const MatrixXType &z, MatrixKType &d) const { 101 | assert((x.cols() == 3) && (z.cols() == 6)); 102 | d = MatrixKType::Zero(x.rows(), z.rows()); 103 | float line_len; 104 | point3f p, p0, p1, v, w, line_vec, pnt_vec, nearest; 105 | float t; 106 | for (int i = 0; i < x.rows(); ++i) { 107 | p = point3f(x(i,0), x(i,1), x(i,2)); 108 | for (int j = 0; j < z.rows(); ++j) { 109 | p0 = point3f(z(j,0), z(j,1), z(j,2)); 110 | p1 = point3f(z(j,3), z(j,4), z(j,5)); 111 | line_vec = p1 - p0; 112 | line_len = line_vec.norm(); 113 | pnt_vec = p - p0; 114 | if (line_len < EPSILON) { 115 | d(i,j) = (p-p0).norm(); 116 | } 117 | else { 118 | double c1 = pnt_vec.dot(line_vec); 119 | double c2 = line_vec.dot(line_vec); 120 | if ( c1 <= 0) { 121 | d(i,j) = (p - p0).norm(); 122 | } 123 | else if (c2 <= c1) { 124 | d(i,j) = (p - p1).norm(); 125 | } 126 | else{ 127 | double b = c1 / c2; 128 | nearest = p0 + (line_vec*b); 129 | d(i,j) = (p - nearest).norm(); 130 | } 131 | } 132 | } 133 | } 134 | } 135 | 136 | /* 137 | * @brief Sparse kernel. 138 | * @param x input vector 139 | * @param z input vector 140 | * @return Kxz covariance matrix 141 | * @ref A sparse covariance function for exact gaussian process inference in large datasets. 142 | */ 143 | void covSparseLine(const MatrixPType &x, const MatrixXType &z, MatrixKType &Kxz) const { 144 | point_to_line_dist(x, z, Kxz); // Check on this 145 | Kxz /= ell; 146 | 147 | for (int i = 0; i < Kxz.rows(); ++i) 148 | { 149 | for (int j = 0; j < Kxz.cols(); ++j) 150 | if (Kxz(i,j) > 1.0) 151 | Kxz(i,j) = 1.0f; 152 | } 153 | 154 | //sparse kernel function 155 | Kxz = (((2.0f + (Kxz * 2.0f * 3.1415926f).array().cos()) * (1.0f - Kxz.array()) / 3.0f) + (Kxz * 2.0f * 3.1415926f).array().sin() / (2.0f * 3.1415926f)).matrix() * sf2; 156 | 157 | } 158 | 159 | T sf2; // signal variance 160 | T ell; // length-scale 161 | 162 | MatrixXType xt; // temporary storage of training data 163 | MatrixYType yt; // temporary storage of training labels 164 | 165 | bool trained; // true if bgklvinference stored training data 166 | }; 167 | 168 | typedef BGKLVInference<3, float> BGKLV3f; 169 | 170 | } 171 | #endif // LA3DM_BGKLV_H 172 | -------------------------------------------------------------------------------- /src/bgkoctomap/bgkblock.cpp: -------------------------------------------------------------------------------- 1 | #include "bgkblock.h" 2 | #include 3 | #include 4 | 5 | namespace la3dm { 6 | 7 | std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth) { 8 | std::unordered_map key_loc_map; 9 | 10 | std::queue center_q; 11 | center_q.push(point3f(0.0f, 0.0f, 0.0f)); 12 | 13 | for (unsigned short depth = 0; depth < max_depth; ++depth) { 14 | unsigned short q_size = (unsigned short) center_q.size(); 15 | float half_size = (float) (resolution * pow(2, max_depth - depth - 1) * 0.5f); 16 | for (unsigned short index = 0; index < q_size; ++index) { 17 | point3f center = center_q.front(); 18 | center_q.pop(); 19 | key_loc_map.emplace(node_to_hash_key(depth, index), center); 20 | 21 | if (depth == max_depth - 1) 22 | continue; 23 | for (unsigned short i = 0; i < 8; ++i) { 24 | float x = (float) (center.x() + half_size * (i & 4 ? 0.5 : -0.5)); 25 | float y = (float) (center.y() + half_size * (i & 2 ? 0.5 : -0.5)); 26 | float z = (float) (center.z() + half_size * (i & 1 ? 0.5 : -0.5)); 27 | center_q.emplace(x, y, z); 28 | } 29 | } 30 | } 31 | return key_loc_map; 32 | } 33 | 34 | std::unordered_map init_index_map( 35 | const std::unordered_map &key_loc_map, unsigned short max_depth) { 36 | std::vector> temp; 37 | for (auto it = key_loc_map.begin(); it != key_loc_map.end(); ++it) { 38 | unsigned short depth, index; 39 | hash_key_to_node(it->first, depth, index); 40 | if (depth == max_depth - 1) 41 | temp.push_back(std::make_pair(it->first, it->second)); 42 | } 43 | 44 | std::stable_sort(temp.begin(), temp.end(), 45 | [](const std::pair &p1, 46 | const std::pair &p2) { 47 | return p1.second.x() < p2.second.x(); 48 | }); 49 | std::stable_sort(temp.begin(), temp.end(), 50 | [](const std::pair &p1, 51 | const std::pair &p2) { 52 | return p1.second.y() < p2.second.y(); 53 | }); 54 | std::stable_sort(temp.begin(), temp.end(), 55 | [](const std::pair &p1, 56 | const std::pair &p2) { 57 | return p1.second.z() < p2.second.z(); 58 | }); 59 | 60 | std::unordered_map index_map; 61 | int index = 0; 62 | for (auto it = temp.cbegin(); it != temp.cend(); ++it, ++index) { 63 | index_map.insert(std::make_pair(index, it->first)); 64 | } 65 | 66 | return index_map; 67 | }; 68 | 69 | BlockHashKey block_to_hash_key(point3f center) { 70 | return block_to_hash_key(center.x(), center.y(), center.z()); 71 | } 72 | 73 | BlockHashKey block_to_hash_key(float x, float y, float z) { 74 | return (int64_t(x / (double) Block::size + 524288.5) << 40) | 75 | (int64_t(y / (double) Block::size + 524288.5) << 20) | 76 | (int64_t(z / (double) Block::size + 524288.5)); 77 | } 78 | 79 | point3f hash_key_to_block(BlockHashKey key) { 80 | return point3f(((key >> 40) - 524288) * Block::size, 81 | (((key >> 20) & 0xFFFFF) - 524288) * Block::size, 82 | ((key & 0xFFFFF) - 524288) * Block::size); 83 | } 84 | 85 | ExtendedBlock get_extended_block(BlockHashKey key) { 86 | ExtendedBlock blocks; 87 | point3f center = hash_key_to_block(key); 88 | float x = center.x(); 89 | float y = center.y(); 90 | float z = center.z(); 91 | blocks[0] = key; 92 | 93 | float ex, ey, ez; 94 | for (int i = 0; i < 6; ++i) { 95 | ex = (i / 2 == 0) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 96 | ey = (i / 2 == 1) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 97 | ez = (i / 2 == 2) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 98 | blocks[i + 1] = block_to_hash_key(ex + x, ey + y, ez + z); 99 | } 100 | return blocks; 101 | } 102 | 103 | float Block::resolution = 0.1f; 104 | float Block::size = 0.8f; 105 | unsigned short Block::cell_num = static_cast(round(Block::size / Block::resolution)); 106 | 107 | std::unordered_map Block::key_loc_map; 108 | std::unordered_map Block::index_map; 109 | 110 | Block::Block() : OcTree(), center(0.0f, 0.0f, 0.0f) { } 111 | 112 | Block::Block(point3f center) : OcTree(), center(center) { } 113 | 114 | ExtendedBlock Block::get_extended_block() const { 115 | ExtendedBlock blocks; 116 | float x = center.x(); 117 | float y = center.y(); 118 | float z = center.z(); 119 | blocks[0] = block_to_hash_key(x, y, z); 120 | 121 | float ex, ey, ez; 122 | for (int i = 0; i < 6; ++i) { 123 | ex = (i / 2 == 0) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 124 | ey = (i / 2 == 1) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 125 | ez = (i / 2 == 2) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 126 | blocks[i + 1] = block_to_hash_key(ex + x, ey + y, ez + z); 127 | } 128 | 129 | return blocks; 130 | } 131 | 132 | OcTreeHashKey Block::get_node(unsigned short x, unsigned short y, unsigned short z) const { 133 | unsigned short index = x + y * Block::cell_num + z * Block::cell_num * Block::cell_num; 134 | return Block::index_map[index]; 135 | } 136 | 137 | point3f Block::get_point(unsigned short x, unsigned short y, unsigned short z) const { 138 | return Block::key_loc_map[get_node(x, y, z)] + center; 139 | } 140 | 141 | void Block::get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const { 142 | int xx = static_cast((p.x() - center.x()) / resolution + Block::cell_num / 2); 143 | int yy = static_cast((p.y() - center.y()) / resolution + Block::cell_num / 2); 144 | int zz = static_cast((p.z() - center.z()) / resolution + Block::cell_num / 2); 145 | auto clip = [](int a) -> int { return std::max(0, std::min(a, Block::cell_num - 1)); }; 146 | x = static_cast(clip(xx)); 147 | y = static_cast(clip(yy)); 148 | z = static_cast(clip(zz)); 149 | } 150 | 151 | OcTreeNode& Block::search(float x, float y, float z) const { 152 | return search(point3f(x, y, z)); 153 | } 154 | 155 | OcTreeNode& Block::search(point3f p) const { 156 | unsigned short x, y, z; 157 | get_index(p, x, y, z); 158 | return operator[](get_node(x, y, z)); 159 | } 160 | } -------------------------------------------------------------------------------- /src/gpoctomap/gpblock.cpp: -------------------------------------------------------------------------------- 1 | #include "gpblock.h" 2 | #include 3 | #include 4 | 5 | namespace la3dm { 6 | std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth) { 7 | std::unordered_map key_loc_map; 8 | 9 | std::queue center_q; 10 | center_q.push(point3f(0.0f, 0.0f, 0.0f)); 11 | 12 | for (unsigned short depth = 0; depth < max_depth; ++depth) { 13 | unsigned short q_size = (unsigned short) center_q.size(); 14 | float half_size = (float) (resolution * pow(2, max_depth - depth - 1) * 0.5f); 15 | for (unsigned short index = 0; index < q_size; ++index) { 16 | point3f center = center_q.front(); 17 | center_q.pop(); 18 | key_loc_map.emplace(node_to_hash_key(depth, index), center); 19 | 20 | if (depth == max_depth - 1) 21 | continue; 22 | for (unsigned short i = 0; i < 8; ++i) { 23 | float x = (float) (center.x() + half_size * (i & 4 ? 0.5 : -0.5)); 24 | float y = (float) (center.y() + half_size * (i & 2 ? 0.5 : -0.5)); 25 | float z = (float) (center.z() + half_size * (i & 1 ? 0.5 : -0.5)); 26 | center_q.emplace(x, y, z); 27 | } 28 | } 29 | } 30 | return key_loc_map; 31 | } 32 | 33 | std::unordered_map init_index_map( 34 | const std::unordered_map &key_loc_map, unsigned short max_depth) { 35 | std::vector> temp; 36 | for (auto it = key_loc_map.begin(); it != key_loc_map.end(); ++it) { 37 | unsigned short depth, index; 38 | hash_key_to_node(it->first, depth, index); 39 | if (depth == max_depth - 1) 40 | temp.push_back(std::make_pair(it->first, it->second)); 41 | } 42 | 43 | std::stable_sort(temp.begin(), temp.end(), 44 | [](const std::pair &p1, 45 | const std::pair &p2) { 46 | return p1.second.x() < p2.second.x(); 47 | }); 48 | std::stable_sort(temp.begin(), temp.end(), 49 | [](const std::pair &p1, 50 | const std::pair &p2) { 51 | return p1.second.y() < p2.second.y(); 52 | }); 53 | std::stable_sort(temp.begin(), temp.end(), 54 | [](const std::pair &p1, 55 | const std::pair &p2) { 56 | return p1.second.z() < p2.second.z(); 57 | }); 58 | 59 | std::unordered_map index_map; 60 | int index = 0; 61 | for (auto it = temp.cbegin(); it != temp.cend(); ++it, ++index) { 62 | index_map.insert(std::make_pair(index, it->first)); 63 | } 64 | 65 | return index_map; 66 | }; 67 | 68 | BlockHashKey block_to_hash_key(point3f center) { 69 | return block_to_hash_key(center.x(), center.y(), center.z()); 70 | } 71 | 72 | BlockHashKey block_to_hash_key(float x, float y, float z) { 73 | return (int64_t(x / (double) Block::size + 524288.5) << 40) | 74 | (int64_t(y / (double) Block::size + 524288.5) << 20) | 75 | (int64_t(z / (double) Block::size + 524288.5)); 76 | } 77 | 78 | point3f hash_key_to_block(BlockHashKey key) { 79 | return point3f(((key >> 40) - 524288) * Block::size, 80 | (((key >> 20) & 0xFFFFF) - 524288) * Block::size, 81 | ((key & 0xFFFFF) - 524288) * Block::size); 82 | } 83 | 84 | ExtendedBlock get_extended_block(BlockHashKey key) { 85 | ExtendedBlock blocks; 86 | point3f center = hash_key_to_block(key); 87 | float x = center.x(); 88 | float y = center.y(); 89 | float z = center.z(); 90 | blocks[0] = key; 91 | 92 | float ex, ey, ez; 93 | for (int i = 0; i < 6; ++i) { 94 | ex = (i / 2 == 0) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 95 | ey = (i / 2 == 1) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 96 | ez = (i / 2 == 2) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 97 | blocks[i + 1] = block_to_hash_key(ex + x, ey + y, ez + z); 98 | } 99 | return blocks; 100 | } 101 | 102 | float Block::resolution = 0.1f; 103 | float Block::size = 0.8f; 104 | unsigned short Block::cell_num = static_cast(round(Block::size / Block::resolution)); 105 | 106 | std::unordered_map Block::key_loc_map; 107 | std::unordered_map Block::index_map; 108 | 109 | Block::Block() : OcTree(), center(0.0f, 0.0f, 0.0f) { } 110 | 111 | Block::Block(point3f center) : OcTree(), center(center) { } 112 | 113 | ExtendedBlock Block::get_extended_block() const { 114 | ExtendedBlock blocks; 115 | float x = center.x(); 116 | float y = center.y(); 117 | float z = center.z(); 118 | blocks[0] = block_to_hash_key(x, y, z); 119 | 120 | float ex, ey, ez; 121 | for (int i = 0; i < 6; ++i) { 122 | ex = (i / 2 == 0) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 123 | ey = (i / 2 == 1) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 124 | ez = (i / 2 == 2) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; 125 | blocks[i + 1] = block_to_hash_key(ex + x, ey + y, ez + z); 126 | } 127 | 128 | return blocks; 129 | } 130 | 131 | OcTreeHashKey Block::get_node(unsigned short x, unsigned short y, unsigned short z) const { 132 | unsigned short index = x + y * Block::cell_num + z * Block::cell_num * Block::cell_num; 133 | return Block::index_map[index]; 134 | } 135 | 136 | point3f Block::get_point(unsigned short x, unsigned short y, unsigned short z) const { 137 | return Block::key_loc_map[get_node(x, y, z)] + center; 138 | } 139 | 140 | void Block::get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const { 141 | int xx = static_cast((p.x() - center.x()) / resolution + Block::cell_num / 2); 142 | int yy = static_cast((p.y() - center.y()) / resolution + Block::cell_num / 2); 143 | int zz = static_cast((p.z() - center.z()) / resolution + Block::cell_num / 2); 144 | auto clip = [](int a) -> int { return std::max(0, std::min(a, Block::cell_num - 1)); }; 145 | x = static_cast(clip(xx)); 146 | y = static_cast(clip(yy)); 147 | z = static_cast(clip(zz)); 148 | } 149 | 150 | OcTreeNode& Block::search(float x, float y, float z) const { 151 | return search(point3f(x, y, z)); 152 | } 153 | 154 | OcTreeNode& Block::search(point3f p) const { 155 | unsigned short x, y, z; 156 | get_index(p, x, y, z); 157 | return operator[](get_node(x, y, z)); 158 | } 159 | } 160 | --------------------------------------------------------------------------------