├── .gitignore ├── CMakeLists.txt ├── README.md ├── RealResults.png ├── Screenshot.png ├── VirtualScan ├── VirtualScan.sln ├── VirtualScan.vcxproj ├── VirtualScan.vcxproj.filters ├── VirtualScan.vcxproj.user ├── shaders │ ├── phong.hlsl │ ├── test.hlsl │ └── ward.hlsl ├── src │ ├── GlobalAppState.h │ ├── Lighting.cpp │ ├── Lighting.h │ ├── Scene.cpp │ ├── Scene.h │ ├── VirtualScanCPU.h │ ├── Visualizer.cpp │ ├── Visualizer.h │ ├── mLibInclude.h │ ├── mLibSource.cpp │ ├── main.cpp │ ├── stdafx.cpp │ ├── stdafx.h │ └── targetver.h └── zParametersScan.txt ├── cmake.sh ├── cmake ├── CheckC11.cmake └── Modules │ ├── FindGStreamer.cmake │ ├── FindGStreamerWin.cmake │ └── FindTCLAP.cmake ├── cmake_jetson.sh ├── external ├── QGLViewer │ ├── .gitignore │ ├── CHANGELOG │ ├── GPL_EXCEPTION │ ├── INSTALL │ ├── ImageInterface.ui │ ├── LICENCE │ ├── Makefile │ ├── QGLViewer.pro │ ├── README │ ├── VRender │ │ ├── AxisAlignedBox.h │ │ ├── BSPSortMethod.cpp │ │ ├── BackFaceCullingOptimizer.cpp │ │ ├── EPSExporter.cpp │ │ ├── Exporter.cpp │ │ ├── Exporter.h │ │ ├── FIGExporter.cpp │ │ ├── NVector3.cpp │ │ ├── NVector3.h │ │ ├── Optimizer.h │ │ ├── ParserGL.cpp │ │ ├── ParserGL.h │ │ ├── Primitive.cpp │ │ ├── Primitive.h │ │ ├── PrimitivePositioning.cpp │ │ ├── PrimitivePositioning.h │ │ ├── SortMethod.h │ │ ├── TopologicalSortMethod.cpp │ │ ├── Types.h │ │ ├── VRender.cpp │ │ ├── VRender.h │ │ ├── Vector2.cpp │ │ ├── Vector2.h │ │ ├── Vector3.cpp │ │ ├── Vector3.h │ │ ├── VisibilityOptimizer.cpp │ │ ├── gpc.cpp │ │ └── gpc.h │ ├── VRenderInterface.ui │ ├── camera.cpp │ ├── camera.h │ ├── config.h │ ├── constraint.cpp │ ├── constraint.h │ ├── domUtils.h │ ├── frame.cpp │ ├── frame.h │ ├── keyFrameInterpolator.cpp │ ├── keyFrameInterpolator.h │ ├── manipulatedCameraFrame.cpp │ ├── manipulatedCameraFrame.h │ ├── manipulatedFrame.cpp │ ├── manipulatedFrame.h │ ├── mouseGrabber.cpp │ ├── mouseGrabber.h │ ├── qglviewer-icon.xpm │ ├── qglviewer.cpp │ ├── qglviewer.h │ ├── qglviewer.icns │ ├── qglviewer_fr.qm │ ├── qglviewer_fr.ts │ ├── quaternion.cpp │ ├── quaternion.h │ ├── saveSnapshot.cpp │ ├── vec.cpp │ └── vec.h ├── fastlz │ ├── 6pack.c │ ├── 6unpack.c │ ├── LICENSE │ ├── README.TXT │ ├── fastlz.c │ └── fastlz.h └── snappy │ ├── AUTHORS │ ├── COPYING │ ├── README │ ├── format_description.txt │ ├── framing_format.txt │ ├── snappy-internal.h │ ├── snappy-sinksource.cc │ ├── snappy-sinksource.h │ ├── snappy-stubs-internal.cc │ ├── snappy-stubs-internal.h │ ├── snappy-stubs-public.h │ ├── snappy.cc │ └── snappy.h ├── get_octomap.sh ├── include ├── ait │ ├── BoostNetworkClientTCP.h │ ├── BoostNetworkClientUDP.h │ ├── boost.h │ ├── boost_serialization_utils.h │ ├── color.h │ ├── common.h │ ├── cuda_math.h │ ├── cuda_utils.h │ ├── cv_utilities.h │ ├── dji │ │ ├── RosDjiDrone.h │ │ └── dji_drone.h │ ├── eigen.h │ ├── eigen_alignment.h │ ├── eigen_options.h │ ├── eigen_serialization.h │ ├── eigen_serialization_plugin.h │ ├── eigen_utils.h │ ├── eigen_with_serialization.h │ ├── filesystem.h │ ├── gps.h │ ├── graph_boost.h │ ├── mLib.h │ ├── mLibUtils.h │ ├── mLibZlib.h │ ├── math.h │ ├── math │ │ ├── continuous_grid3d.h │ │ ├── geometry.h │ │ ├── grid3d.h │ │ └── statistics.h │ ├── nn │ │ └── approximate_nearest_neighbor.h │ ├── options.h │ ├── pose.h │ ├── qt_utils.h │ ├── random.h │ ├── serializable.h │ ├── serialization.h │ ├── thread.h │ ├── utilities.h │ └── vision_utilities.h └── bh │ ├── aabb │ ├── aabb_tree.h │ └── aabb_tree.hxx │ ├── algorithm.h │ ├── boost.h │ ├── boost_serialization_utils.h │ ├── color.h │ ├── common.h │ ├── config_options.h │ ├── cuda_math.h │ ├── cuda_utils.h │ ├── eigen.h │ ├── eigen_options.h │ ├── eigen_serialization.h │ ├── eigen_utils.h │ ├── filesystem.h │ ├── gperf.h │ ├── gps.h │ ├── graph_boost.h │ ├── mLib │ ├── mLib.h │ ├── mLibUtils.h │ └── mLibZlib.h │ ├── math │ ├── continuous_grid3d.h │ ├── geometry.h │ ├── grid3d.h │ ├── histogram.h │ ├── statistics.h │ └── utilities.h │ ├── memory.h │ ├── mesh │ ├── triangle_mesh.h │ └── triangle_mesh.hxx │ ├── nn │ ├── approximate_nearest_neighbor.h │ └── approximate_nearest_neighbor.hxx │ ├── opencv │ ├── cameras.h │ ├── drawing.h │ ├── images.h │ └── matrix.h │ ├── opengl │ ├── offscreen_opengl.h │ ├── offscreen_opengl.hxx │ └── utils.h │ ├── pose.h │ ├── qt │ └── utils.h │ ├── random.h │ ├── se3_transform.h │ ├── sqlite │ └── sqlite3_wrapper.h │ ├── string_utils.h │ ├── thread.h │ ├── utilities.h │ └── vision │ ├── cameras.h │ ├── drawing_qt.h │ ├── geometry.h │ └── types.h ├── make_jetson.sh ├── plane_sweep_stereo ├── CMakeLists.txt └── src │ ├── plane_sweep_test.cpp │ ├── plane_sweep_test_orig.cpp │ └── plane_sweep_zed.cpp ├── quad_flight ├── CMakeLists.txt ├── cfg │ └── PositionController.cfg ├── config │ └── gimbal_control.yaml ├── launch │ ├── buildings_flight_gazebo.launch │ ├── buildings_world.launch │ ├── display_quad.launch │ ├── moveit_test.launch │ ├── outdoor_flight_gazebo.launch │ ├── position_controller.launch │ ├── position_controller_with_teleop.launch │ ├── position_teleop.launch │ ├── rolling_landscape_120m.launch │ ├── start_world.launch │ └── velocity_teleop.launch ├── package.xml ├── rviz_cfg │ ├── buildings_flight.rviz │ ├── outdoor_flight.rviz │ └── urdf.rviz ├── src │ ├── position_controller.cpp │ ├── position_teleop.cpp │ └── velocity_teleop.cpp ├── test │ ├── get_link_state.sh │ ├── pub_control.sh │ └── set_link_state.sh ├── urdf │ ├── camera_gimbal.urdf.xacro │ ├── camera_gimbal2.urdf.xacro │ ├── depth_camera.urdf.xacro │ ├── materials.xacro │ ├── quadrotor_base.urdf.xacro │ ├── quadrotor_simple.urdf.xacro │ ├── quadrotor_test.gazebo │ ├── quadrotor_test.xacro │ ├── quadrotor_with_depth_camera.gazebo.xacro │ ├── quadrotor_with_depth_camera.urdf.xacro │ ├── utils.xacro │ └── xacro_test.sh └── worlds │ ├── buildings.world │ ├── buildings.world.xacro │ ├── house1.sdf.xacro │ ├── house2.sdf.xacro │ ├── house3.sdf.xacro │ └── xacro.sh ├── quad_planner ├── CMakeLists.txt ├── LICENSE ├── cmake │ └── Modules │ │ └── Findglm.cmake ├── include │ └── quad_planner │ │ ├── optimizing_rrt_planner.h │ │ ├── quad_planner.h │ │ └── rendering │ │ ├── color.h │ │ ├── glm.h │ │ ├── linalg.h │ │ ├── lines.h │ │ ├── octomap_renderer.h │ │ ├── render_object.h │ │ ├── scene_object.h │ │ ├── shader_program.h │ │ ├── triangle_mesh.h │ │ ├── utilities.h │ │ └── visualizer.h ├── octomap_trajectory.txt ├── package.xml ├── scripts │ └── visualize_trajectory.py ├── src │ ├── octomap_trajectory.txt │ ├── optimizing_rrt_planner.cpp │ ├── quad_planner.cpp │ ├── quad_planner_app.cpp │ ├── rendering │ │ ├── lines.cpp │ │ ├── octomap_renderer.cpp │ │ ├── scene_object.cpp │ │ ├── shader_program.cpp │ │ ├── triangle_mesh.cpp │ │ ├── utilities.cpp │ │ └── visualizer.cpp │ └── shaders │ │ ├── octomap_shader.fragment │ │ ├── octomap_shader.geometry │ │ ├── octomap_shader.vertex │ │ ├── phong_shader.fragment │ │ ├── phong_shader.vertex │ │ ├── simple_fragment_shader.fragmentshader │ │ ├── simple_vertex_shader.vertexshader │ │ ├── trajectory_shader.fragment │ │ ├── trajectory_shader.geometry │ │ ├── trajectory_shader.vertex │ │ └── vertex.shader └── voxel_coordinate_system.jpeg ├── src ├── bh │ └── utilities.cpp ├── mLib.cpp ├── mLibSource.cpp ├── mLibZlib.cpp └── utilities.cpp ├── stereo ├── CMakeLists.txt ├── cmake.sh ├── include │ └── ait │ │ └── stereo │ │ ├── dense_stereo_matcher.h │ │ ├── sparse_stereo.h │ │ ├── sparse_stereo_matcher.h │ │ ├── sparse_stereo_matcher.hpp │ │ └── stereo_calibration.h └── src │ ├── sparse_stereo_test.cpp │ ├── sparse_stereo_zed.cpp │ ├── stereo_calibration.cpp │ ├── stereo_calibration_tool_zed.cpp │ └── stereo_capture_zed.cpp ├── video ├── CMakeLists.txt ├── cmake │ ├── CheckC11.cmake │ └── Modules │ │ ├── FindGStreamer.cmake │ │ ├── FindGStreamerWin.cmake │ │ └── FindTCLAP.cmake ├── include │ └── ait │ │ └── video │ │ ├── EncodingGstreamerPipeline.h │ │ ├── GstMetaCorrespondence.h │ │ ├── GstreamerPipeline.h │ │ ├── StereoNetworkSensorClient.h │ │ ├── StereoNetworkSensorManager.h │ │ ├── StereoNetworkSensorProtocol.h │ │ ├── video_source.h │ │ ├── video_source_opencv.h │ │ └── video_source_zed.h ├── src │ ├── GstMetaCorrespondence.cpp │ ├── video_capture.cpp │ ├── video_capture_zed.cpp │ ├── video_source.cpp │ ├── video_source_opencv.cpp │ ├── video_source_zed.cpp │ ├── video_streamer.cpp │ ├── video_streamer_bundlefusion.cpp │ └── video_streamer_bundlefusion_drone.cpp └── tools │ ├── make_all_pipeline_graphs.sh │ ├── make_pipeline_graph.sh │ ├── play_file.sh │ ├── receive_stream.sh │ ├── record_file.sh │ ├── run_with_graph.sh │ ├── send_stream.sh │ └── send_zed_stream.sh └── viewpoint_planner ├── CMakeLists.txt ├── src ├── bvh │ ├── bvh.cu │ ├── bvh.cuh │ └── bvh.h ├── exe │ ├── clip_mesh.cpp │ ├── clip_point_cloud.cpp │ ├── compute_ground_truth_mesh.cpp │ ├── create_baseline_viewpoint_path.cpp │ ├── create_colmap_mvs_files.cpp │ ├── evaluate_dense_points.cpp │ ├── fuse_point_cloud.cpp │ ├── image_match_invalidator.cpp │ ├── image_matcher.cpp │ ├── image_matcher_test.cpp │ ├── merge_viewpoint_paths.cpp │ ├── occupancy_map_from_colmap.cpp │ ├── occupancy_map_from_mesh.cpp │ ├── occupancy_map_from_sens.cpp │ ├── transform_mesh.cpp │ ├── viewpoint_planner_cmdline.cpp │ └── viewpoint_planner_gui.cpp ├── mLib │ ├── mLib.cpp │ └── mLib.h ├── octree │ ├── occupancy_map.cpp │ ├── occupancy_map.h │ ├── occupancy_map.hxx │ ├── occupancy_map_tree_navigator.hxx │ ├── occupancy_node.cpp │ └── occupancy_node.h ├── planner │ ├── motion_planner.h │ ├── occupied_tree.h │ ├── offscreen_octree_renderer.h │ ├── viewpoint.cpp │ ├── viewpoint.h │ ├── viewpoint_offscreen_renderer.cpp │ ├── viewpoint_offscreen_renderer.h │ ├── viewpoint_planner.cpp │ ├── viewpoint_planner.h │ ├── viewpoint_planner.hxx │ ├── viewpoint_planner_data.cpp │ ├── viewpoint_planner_data.h │ ├── viewpoint_planner_dump.cpp │ ├── viewpoint_planner_export.cpp │ ├── viewpoint_planner_graph.cpp │ ├── viewpoint_planner_graph.hxx │ ├── viewpoint_planner_motion.cpp │ ├── viewpoint_planner_opengl.cpp │ ├── viewpoint_planner_path.cpp │ ├── viewpoint_planner_path_tour.cpp │ ├── viewpoint_planner_raycast.cpp │ ├── viewpoint_planner_sampling.cpp │ ├── viewpoint_planner_scoring.cpp │ ├── viewpoint_planner_serialization.cpp │ ├── viewpoint_planner_serialization.h │ ├── viewpoint_planner_sparse_matching.cpp │ ├── viewpoint_planner_types.h │ ├── viewpoint_raycast.cpp │ ├── viewpoint_raycast.h │ ├── viewpoint_score.cpp │ └── viewpoint_score.h ├── reconstruction │ ├── colmap_database.h │ ├── dense_reconstruction.cpp │ ├── dense_reconstruction.h │ ├── sparse_reconstruction.cpp │ └── sparse_reconstruction.h ├── rendering │ ├── binned_octree_drawer.cpp │ ├── binned_octree_drawer.h │ ├── line_drawer.h │ ├── octree_drawer.cpp │ ├── octree_drawer.h │ ├── point_drawer.h │ ├── sparse_reconstruction_drawer.cpp │ ├── sparse_reconstruction_drawer.h │ ├── triangle_drawer.h │ ├── viewpoint_drawer.h │ ├── viewpoint_drawer.hxx │ ├── voxel_drawer.cpp │ └── voxel_drawer.h ├── shaders.qrc ├── shaders │ ├── lines.f.glsl │ ├── lines.g.glsl │ ├── lines.v.glsl │ ├── octomap_shader.f.glsl │ ├── octomap_shader.g.glsl │ ├── octomap_shader.v.glsl │ ├── phong_shader.f.glsl │ ├── phong_shader.v.glsl │ ├── points.f.glsl │ ├── points.v.glsl │ ├── trajectory_shader.f.glsl │ ├── trajectory_shader.g.glsl │ ├── trajectory_shader.v.glsl │ ├── triangles.f.glsl │ ├── triangles.v.glsl │ ├── triangles_depth.f.glsl │ ├── triangles_depth.v.glsl │ ├── triangles_indices.f.glsl │ ├── triangles_indices.v.glsl │ ├── triangles_normals.f.glsl │ ├── triangles_normals.v.glsl │ ├── voxels.f.glsl │ └── voxels.v.glsl ├── shaders_offscreen.qrc ├── ui │ ├── qt_utils.h │ ├── viewer_info_panel.h │ ├── viewer_info_panel.ui │ ├── viewer_planner_panel.h │ ├── viewer_planner_panel.ui │ ├── viewer_settings_panel.h │ ├── viewer_settings_panel.ui │ ├── viewer_widget.cpp │ ├── viewer_widget.h │ ├── viewer_window.cpp │ └── viewer_window.h ├── viewpoint_planner_window2.h ├── viewpoint_statistics_app.cpp └── web │ ├── .idea │ ├── modules.xml │ └── web.iml │ ├── external │ ├── bootstrap │ │ └── 3.3.7 │ │ │ ├── LICENSE │ │ │ ├── css │ │ │ ├── bootstrap-theme.css │ │ │ ├── bootstrap-theme.css.map │ │ │ ├── bootstrap-theme.min.css │ │ │ ├── bootstrap-theme.min.css.map │ │ │ ├── bootstrap.css │ │ │ ├── bootstrap.css.map │ │ │ ├── bootstrap.min.css │ │ │ └── bootstrap.min.css.map │ │ │ ├── fonts │ │ │ ├── glyphicons-halflings-regular.eot │ │ │ ├── glyphicons-halflings-regular.svg │ │ │ ├── glyphicons-halflings-regular.ttf │ │ │ ├── glyphicons-halflings-regular.woff │ │ │ └── glyphicons-halflings-regular.woff2 │ │ │ └── js │ │ │ ├── bootstrap.js │ │ │ ├── bootstrap.min.js │ │ │ └── npm.js │ └── jquery │ │ └── 3.1.1 │ │ ├── LICENSE │ │ ├── jquery-3.1.1.js │ │ └── jquery-3.1.1.min.js │ ├── map.html │ ├── src │ └── kml_utils.js │ ├── web_socket_server.cpp │ └── web_socket_server.h ├── test ├── CMakeLists.txt ├── test_ann.cpp ├── test_qt_image.cpp ├── test_se3_transform.cpp ├── test_vision.cpp └── test_vision_qt.cpp └── viewpoint_planner.cfg /.gitignore: -------------------------------------------------------------------------------- 1 | images/ 2 | workspace/ 3 | catkin_ws/ 4 | #catkin_ws/.catkin_tools/ 5 | #catkin_ws/build/ 6 | #catkin_ws/devel/ 7 | #catkin_ws/logs/ 8 | stereo/build_jetson/ 9 | video/build/ 10 | video/build_jetson/ 11 | *.mp4 12 | .qglviewer.xml 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Quad3DR 2 | 3 | *Quad3DR* enables planning of Quadrotor trajectories for dense 3D reconstruction of buildings and other structures. 4 | 5 | *Quad3DR* computes a volumetric map of the environment from an initial flight 6 | (using a modified version of [Octomap](https://github.com/OctoMap)). The volumetric map is used to **ensure that the 7 | planned trajectories are only in free space** and not in occupied or unknown space. 8 | Next a viewpoint graph is built by performing a **raycast on the GPU** to measure visible voxels and a viewpoint score is computed. 9 | The viewpoint score is computed based on distance and incidence angle of viewpoint and voxel 10 | (the angle is computed by using a rendered Poisson-reconstructed mesh of the initial flight). 11 | Connections in the viewpoint graph are found using **RRT***. The resulting viewpoint score is **submodular** and we compute 12 | a viewpoint path by using an adaptation of the recursive method in 13 | (Chekuri, Chandra, and Martin Pal. "A recursive greedy algorithm for walks in directed graphs." Foundations of Computer Science, 2005. FOCS 2005. 46th Annual IEEE Symposium on. IEEE, 2005.). 14 | 15 | The planned viewpoint paths were evaluated on synthetic scenes using Unreal Engine and on real scenes using a DJI Matrice 100. 16 | 17 | ## Screenshot 18 | ![Screenshot](https://raw.githubusercontent.com/bennihepp/Quad3DR/master/Screenshot.png?token=AA957YRPZYt7btvps5kmQbWGylK5uILMks5aZIikwA%3D%3D) 19 | 20 | ## Some results from real a scene 21 | ![Result](https://raw.githubusercontent.com/bennihepp/Quad3DR/master/RealResults.png?token=AA957eHcVO89NKMB-wPqV9EEQ3DbSj_Fks5aZIihwA%3D%3D) 22 | -------------------------------------------------------------------------------- /RealResults.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/RealResults.png -------------------------------------------------------------------------------- /Screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/Screenshot.png -------------------------------------------------------------------------------- /VirtualScan/VirtualScan.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2013 4 | VisualStudioVersion = 12.0.31101.0 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "VirtualScan", "VirtualScan.vcxproj", "{B7522A83-8AE0-4351-BE91-71C5989A6519}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|x64 = Debug|x64 11 | Release|x64 = Release|x64 12 | EndGlobalSection 13 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 14 | {B7522A83-8AE0-4351-BE91-71C5989A6519}.Debug|x64.ActiveCfg = Debug|x64 15 | {B7522A83-8AE0-4351-BE91-71C5989A6519}.Debug|x64.Build.0 = Debug|x64 16 | {B7522A83-8AE0-4351-BE91-71C5989A6519}.Release|x64.ActiveCfg = Release|x64 17 | {B7522A83-8AE0-4351-BE91-71C5989A6519}.Release|x64.Build.0 = Release|x64 18 | EndGlobalSection 19 | GlobalSection(SolutionProperties) = preSolution 20 | HideSolutionNode = FALSE 21 | EndGlobalSection 22 | EndGlobal 23 | -------------------------------------------------------------------------------- /VirtualScan/VirtualScan.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 6 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 7 | 8 | 9 | {8e0e7543-8329-42d8-a2e5-c872d029edc0} 10 | 11 | 12 | 13 | 14 | Shader 15 | 16 | 17 | Shader 18 | 19 | 20 | Shader 21 | 22 | 23 | 24 | 25 | Resource Files 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /VirtualScan/VirtualScan.vcxproj.user: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | -------------------------------------------------------------------------------- /VirtualScan/shaders/phong.hlsl: -------------------------------------------------------------------------------- 1 | 2 | cbuffer ConstantBufferCamera : register( b0 ) 3 | { 4 | matrix worldViewProj; 5 | matrix world; 6 | float4 eye; 7 | } 8 | 9 | Texture2D modelTexture : register(t0); 10 | SamplerState modelSampler : register(s0); 11 | 12 | cbuffer ConstantBufferMaterial : register( b1 ) 13 | { 14 | float4 ambient; 15 | float4 diffuse; 16 | float4 specular; 17 | float shiny; 18 | float3 dummyMaterial; 19 | } 20 | 21 | #define MAX_NUM_LIGHTS 10 22 | cbuffer ConstantBufferLight : register ( b2 ) 23 | { 24 | float4 lightDirs[MAX_NUM_LIGHTS]; 25 | unsigned int numLights; 26 | float3 dummy123; 27 | } 28 | 29 | 30 | 31 | struct VertexShaderOutput 32 | { 33 | float4 position : SV_POSITION; 34 | float4 normal : TEXCOORD0; 35 | float4 color : TEXCOORD1; 36 | float4 view : TEXCOORD2; 37 | float4 texCoord : TEXCOORD3; 38 | }; 39 | 40 | VertexShaderOutput vertexShaderMain( float4 position : position, 41 | float4 normal : normal, 42 | float4 color : color, 43 | float4 texCoord : texCoord ) 44 | { 45 | VertexShaderOutput output; 46 | output.position = mul(position, worldViewProj); 47 | float4 posWorld = mul(position, world); 48 | output.view = normalize(eye - posWorld); 49 | normal.w = 0.0f; 50 | output.normal = mul(normal, world); 51 | output.color = color; 52 | output.texCoord = texCoord; 53 | return output; 54 | } 55 | 56 | float3 phong(float3 normal, float3 viewDir, float3 lightDir) 57 | { 58 | normal = normalize(normal); 59 | lightDir = normalize(lightDir); 60 | viewDir = normalize(viewDir); 61 | 62 | //float4 diff = saturate(dot(normal, lightDir)); // diffuse component 63 | float3 diff = saturate(dot(normal, lightDir)); // diffuse component 64 | 65 | // R = 2 * (N.L) * N - L 66 | float3 reflect = normalize(2 * dot(normal, lightDir) * normal - lightDir); 67 | float3 spec = 0.0f; 68 | 69 | if (shiny > 0.0f) { 70 | spec = pow(saturate(dot(reflect, viewDir)), shiny); // R.V^n 71 | } 72 | 73 | return ambient.xyz + diffuse.xyz*diff.xyz + specular.xyz*spec.xyz; 74 | } 75 | 76 | float4 shade_phong( VertexShaderOutput input ) { 77 | float3 color = input.color.xyz; 78 | 79 | float3 res = 0.0f; 80 | for (unsigned int i = 0; i < numLights; i++) { 81 | float3 curr = phong(input.normal.xyz, input.view.xyz, lightDirs[i]) * color; 82 | res += curr; 83 | } 84 | 85 | return float4(res, 1.0f); 86 | } 87 | 88 | float4 pixelShaderMain( VertexShaderOutput input ) : SV_Target 89 | { 90 | return shade_phong(input); 91 | } 92 | 93 | float4 pixelShaderMain_textured(VertexShaderOutput input) : SV_Target 94 | { 95 | float4 shadeColor = shade_phong(input); 96 | float4 texColor = modelTexture.Sample(modelSampler, float2(input.texCoord.x, 1.0f - input.texCoord.y)); 97 | 98 | return float4(shadeColor.xyz * texColor.xyz, 1.0f); 99 | } 100 | -------------------------------------------------------------------------------- /VirtualScan/shaders/test.hlsl: -------------------------------------------------------------------------------- 1 | 2 | cbuffer ConstantBuffer : register( b0 ) 3 | { 4 | matrix worldViewProj; 5 | matrix world; 6 | float4 lightDir; 7 | float4 eye; 8 | } 9 | 10 | struct VertexShaderOutput 11 | { 12 | float4 position : SV_POSITION; 13 | float4 color : TEXCOORD0; 14 | }; 15 | 16 | VertexShaderOutput vertexShaderMain( float4 position : position, 17 | float4 normal : normal, 18 | float4 color : color, 19 | float4 texCoord : texCoord ) 20 | { 21 | VertexShaderOutput output; 22 | output.position = mul( position, worldViewProj ); 23 | output.color = color; 24 | return output; 25 | } 26 | 27 | float4 pixelShaderMain( VertexShaderOutput input ) : SV_Target 28 | { 29 | //return float4( input.color.x, input.color.y, input.color.z, 1.0f ); 30 | return float4( 0.8, 0.8, 0.8, 1.0); 31 | } 32 | -------------------------------------------------------------------------------- /VirtualScan/src/GlobalAppState.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stdafx.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | 11 | #define X_GLOBAL_APP_STATE_FIELDS \ 12 | X(std::vector, s_meshFilenames) \ 13 | X(unsigned int, s_imageWidth) \ 14 | X(unsigned int, s_imageHeight) \ 15 | X(float, s_cameraFov) \ 16 | X(bool, s_addNoiseToDepth) \ 17 | X(float, s_depthNoiseSigma) \ 18 | X(bool, s_filterDepthMap) \ 19 | X(float, s_depthSigmaD) \ 20 | X(float, s_depthSigmaR) \ 21 | X(float, s_voxelSize) \ 22 | X(float, s_depthMin) \ 23 | X(float, s_depthMax) \ 24 | X(unsigned int, s_numMaterialsDiffuse) \ 25 | X(unsigned int, s_numMaterialsSpecular) \ 26 | X(unsigned int, s_maxTrajectoryLength) \ 27 | X(unsigned int, s_maxVoxelSamples) \ 28 | X(unsigned int, s_randomizeLighting) \ 29 | X(bool, s_randomizeTrajectory) \ 30 | X(std::string, s_hostReconstructionName) \ 31 | X(unsigned int, s_hostReconstructionPort) \ 32 | X(unsigned int, s_listenControlPort) 33 | 34 | 35 | #ifndef VAR_NAME 36 | #define VAR_NAME(x) #x 37 | #endif 38 | 39 | #define checkSizeArray(a, d)( (((sizeof a)/(sizeof a[0])) >= d)) 40 | 41 | class GlobalAppState 42 | { 43 | public: 44 | 45 | #define X(type, name) type name; 46 | X_GLOBAL_APP_STATE_FIELDS 47 | #undef X 48 | 49 | //! sets the parameter file and reads 50 | void readMembers(const ParameterFile& parameterFile) { 51 | m_ParameterFile = parameterFile; 52 | readMembers(); 53 | } 54 | 55 | //! reads all the members from the given parameter file (could be called for reloading) 56 | void readMembers() { 57 | #define X(type, name) \ 58 | if (!m_ParameterFile.readParameter(std::string(#name), name)) {MLIB_WARNING(std::string(#name).append(" ").append("uninitialized")); name = type();} 59 | X_GLOBAL_APP_STATE_FIELDS 60 | #undef X 61 | 62 | 63 | m_bIsInitialized = true; 64 | } 65 | 66 | void print() const { 67 | #define X(type, name) \ 68 | std::cout << #name " = " << name << std::endl; 69 | X_GLOBAL_APP_STATE_FIELDS 70 | #undef X 71 | } 72 | 73 | static GlobalAppState& getInstance() { 74 | static GlobalAppState s; 75 | return s; 76 | } 77 | static GlobalAppState& get() { 78 | return getInstance(); 79 | } 80 | 81 | 82 | //! constructor 83 | GlobalAppState() { 84 | m_bIsInitialized = false; 85 | } 86 | 87 | //! destructor 88 | ~GlobalAppState() { 89 | } 90 | 91 | Timer s_Timer; 92 | 93 | private: 94 | bool m_bIsInitialized; 95 | ParameterFile m_ParameterFile; 96 | }; 97 | -------------------------------------------------------------------------------- /VirtualScan/src/Lighting.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "stdafx.h" 4 | #include "Lighting.h" -------------------------------------------------------------------------------- /VirtualScan/src/Scene.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "stdafx.h" 3 | #include "Scene.h" 4 | 5 | -------------------------------------------------------------------------------- /VirtualScan/src/Visualizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | #include 5 | #include 6 | 7 | class Visualizer : public ApplicationCallback 8 | { 9 | public: 10 | Visualizer() : ApplicationCallback() {} 11 | ~Visualizer(); 12 | 13 | void init(ApplicationData &app); 14 | void render(ApplicationData &app); 15 | 16 | void keyDown(ApplicationData &app, UINT key); 17 | void keyPressed(ApplicationData &app, UINT key); 18 | void mouseDown(ApplicationData &app, MouseButtonType button); 19 | void mouseMove(ApplicationData &app); 20 | void mouseWheel(ApplicationData &app, int wheelDelta); 21 | void resize(ApplicationData &app); 22 | 23 | private: 24 | FrameTimer m_timer; 25 | 26 | Scene m_scene; 27 | D3D11Font m_font; 28 | Cameraf m_camera; 29 | D3D11RenderTarget m_renderTarget; 30 | 31 | }; -------------------------------------------------------------------------------- /VirtualScan/src/mLibInclude.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef MLIB_INCLUDE_H 3 | #define MLIB_INCLUDE_H 4 | 5 | // 6 | // mLib config options 7 | // 8 | #define MLIB_ERROR_CHECK 9 | #define MLIB_BOUNDS_CHECK 10 | 11 | // 12 | // mLib includes 13 | // 14 | 15 | #include "mLibCore.h" 16 | #include "mLibDepthCamera.h" 17 | #include "mLibD3D11.h" 18 | #include "mLibD3D11Font.h" 19 | #include "mLibZLib.h" 20 | #include "mLibFreeImage.h" 21 | 22 | using namespace ml; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /VirtualScan/src/mLibSource.cpp: -------------------------------------------------------------------------------- 1 | 2 | //#include "stdafx.h" 3 | 4 | #include "mLibCore.cpp" 5 | #include "mLibD3D11.cpp" 6 | #include "mLibDepthCamera.cpp" 7 | #include "mlibZLib.cpp" -------------------------------------------------------------------------------- /VirtualScan/src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "stdafx.h" 4 | 5 | 6 | void loadGlobalAppState(const std::string& fileNameDescGlobalApp) { 7 | if (!util::fileExists(fileNameDescGlobalApp)) { 8 | throw MLIB_EXCEPTION("cannot find parameter filer " + fileNameDescGlobalApp); 9 | } 10 | 11 | std::cout << VAR_NAME(fileNameDescGlobalApp) << " = " << fileNameDescGlobalApp << std::endl; 12 | ParameterFile parameterFileGlobalApp(fileNameDescGlobalApp); 13 | GlobalAppState::get().readMembers(parameterFileGlobalApp); 14 | GlobalAppState::get().print(); 15 | } 16 | 17 | int _tmain(int argc, _TCHAR* argv[]) 18 | { 19 | try { 20 | const std::string fileNameDescGlobalApp = "zParametersScan.txt"; 21 | loadGlobalAppState(fileNameDescGlobalApp); 22 | 23 | Visualizer callback; 24 | ApplicationWin32 app(NULL, 640, 480, "Virtual Scan", GraphicsDeviceTypeD3D11, callback); 25 | app.messageLoop(); 26 | } 27 | catch (const std::exception& e) 28 | { 29 | MessageBoxA(NULL, e.what(), "Exception caught", MB_ICONERROR); 30 | exit(EXIT_FAILURE); 31 | } 32 | catch (...) 33 | { 34 | MessageBoxA(NULL, "UNKNOWN EXCEPTION", "Exception caught", MB_ICONERROR); 35 | exit(EXIT_FAILURE); 36 | } 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /VirtualScan/src/stdafx.cpp: -------------------------------------------------------------------------------- 1 | // stdafx.cpp : source file that includes just the standard includes 2 | // VirtualScan.pch will be the pre-compiled header 3 | // stdafx.obj will contain the pre-compiled type information 4 | 5 | #include "stdafx.h" 6 | 7 | #include "mLibSource.cpp" 8 | // TODO: reference any additional headers you need in STDAFX.H 9 | // and not in this file 10 | -------------------------------------------------------------------------------- /VirtualScan/src/stdafx.h: -------------------------------------------------------------------------------- 1 | // stdafx.h : include file for standard system include files, 2 | // or project specific include files that are used frequently, but 3 | // are changed infrequently 4 | // 5 | 6 | #pragma once 7 | 8 | #include "targetver.h" 9 | 10 | 11 | #include 12 | #include 13 | 14 | #include "mLibInclude.h" 15 | #include "Scene.h" 16 | #include "Visualizer.h" 17 | 18 | // TODO: reference additional headers your program requires here 19 | -------------------------------------------------------------------------------- /VirtualScan/src/targetver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Including SDKDDKVer.h defines the highest available Windows platform. 4 | 5 | // If you wish to build your application for a previous Windows platform, include WinSDKVer.h and 6 | // set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. 7 | 8 | #include 9 | -------------------------------------------------------------------------------- /VirtualScan/zParametersScan.txt: -------------------------------------------------------------------------------- 1 | 2 | s_meshFilenames[0] = "./data/dabrovic-sponza/sponza.obj"; 3 | 4 | s_imageWidth = 640; // render width 5 | s_imageHeight = 480; // render height 6 | s_cameraFov = 60.0f; // degrees 7 | 8 | //noise model for depth -- TODO implement 9 | s_addNoiseToDepth = false; 10 | s_depthNoiseSigma = 0.01f; 11 | 12 | s_filterDepthMap = true; //bilateral filter depth map 13 | s_depthSigmaD = 5.0f; //bilateral filter sigma domain 14 | s_depthSigmaR = 0.1f; //bilateral filter sigma range 15 | -------------------------------------------------------------------------------- /cmake.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | pushd . 4 | cd build || { echo "No such directory 'build'"; exit 1; } 5 | 6 | GENERATOR="Unix Makefiles" 7 | #GENERATOR="Eclipse CDT4 - Unix Makefiles" 8 | 9 | #BUILD_TYPE=Debug 10 | BUILD_TYPE=RelWithDebInfo 11 | #BUILD_TYPE=Release 12 | 13 | #OPENCV_VERSION=2.4 14 | OPENCV_VERSION=3.1 15 | 16 | WITH_ZED=False 17 | #WITH_ZED=True 18 | 19 | #CMAKE_SOURCE_DIR=${HOME}/Projects/Quad3DR/ 20 | CMAKE_SOURCE_DIR=.. 21 | 22 | #CXX_FLAGS="-D_GLIBCXX_USE_CXX11_ABI=0 -fno-strict-aliasing -march=native -msse2 -mavx" 23 | # Can't use -Werror=return-type because boost json_parser/standard_callbacks.hpp:132 does not comply 24 | CXX_FLAGS="-D_GLIBCXX_USE_CXX11_ABI=0 -O2 -g3" 25 | #CXX_FLAGS="-D_GLIBCXX_USE_CXX11_ABI=0 -O0 -g -pg" 26 | C_FLAGS="$CXX_FLAGS" 27 | 28 | rm -rf CMakeCache.txt CMakeFiles 29 | 30 | cmake \ 31 | -G"$GENERATOR" \ 32 | -DCMAKE_CXX_FLAGS="$CXX_FLAGS" \ 33 | -DCMAKE_C_FLAGS="$C_FLAGS" \ 34 | -DUSE_OPENCV_VERSION=$OPENCV_VERSION \ 35 | -DWITH_ZED=$WITH_ZED \ 36 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 37 | -DBUILD_PLANE_SWEEP_STEREO=OFF \ 38 | -DBUILD_STEREO=OFF \ 39 | -DBUILD_QUAD_PLANNER=OFF \ 40 | -DBUILD_VIDEO=OFF \ 41 | -DEIGEN3_INCLUDE_DIR=$HOME/Projects/Libraries/eigen-3.3.1 \ 42 | -DBOOST_ROOT=$HOME/Projects/Libraries/boost_1_60_0/ \ 43 | -Doctomap_DIR=$HOME/Projects/Libraries/octomap/lib/cmake/octomap \ 44 | -Doctovis_DIR=$HOME/Projects/Libraries/octomap/lib/cmake/octovis \ 45 | -DCMAKE_PREFIX_PATH=$HOME/Projects/Libraries/ompl \ 46 | -DOMPL_PREFIX=$HOME/Projects/Libraries/ompl \ 47 | $CMAKE_SOURCE_DIR \ 48 | $@ 49 | 50 | popd 51 | 52 | -------------------------------------------------------------------------------- /cmake/CheckC11.cmake: -------------------------------------------------------------------------------- 1 | # Check for C++11/0x support 2 | 3 | if(CMAKE_COMPILER_IS_GNUCXX) 4 | include(CheckCXXCompilerFlag) 5 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 6 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 7 | if(COMPILER_SUPPORTS_CXX11) 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 9 | elseif(COMPILER_SUPPORTS_CXX0X) 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 11 | else() 12 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 13 | endif() 14 | endif() 15 | -------------------------------------------------------------------------------- /cmake/Modules/FindTCLAP.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find TCLAP lib 2 | # 3 | # TCLAP_FOUND - system has TCLAP lib 4 | # TCLAP_INCLUDE_DIR - the TCLAP include directory 5 | 6 | # Copyright (c) 2015, Benjamin Hepp 7 | 8 | macro(_tclap_check_path) 9 | 10 | if(EXISTS "${TCLAP_INCLUDE_DIR}/tclap/CmdLine.h") 11 | set(TCLAP_OK TRUE) 12 | endif() 13 | 14 | if(NOT TCLAP_OK) 15 | message(STATUS "TCLAP include path was specified but no CmdLine.h file was found: ${TCLAP_INCLUDE_DIR}") 16 | endif() 17 | 18 | endmacro() 19 | 20 | if(NOT TCLAP_INCLUDE_DIR) 21 | 22 | find_path(TCLAP_INCLUDE_DIR NAMES tclap/CmdLine.h 23 | PATHS 24 | ${CMAKE_INSTALL_PREFIX}/include 25 | ${KDE4_INCLUDE_DIR} 26 | #PATH_SUFFIXES tclap 27 | ) 28 | 29 | endif() 30 | 31 | if(TCLAP_INCLUDE_DIR) 32 | _tclap_check_path() 33 | endif() 34 | 35 | include(FindPackageHandleStandardArgs) 36 | find_package_handle_standard_args(TCLAP DEFAULT_MSG TCLAP_INCLUDE_DIR TCLAP_OK) 37 | 38 | mark_as_advanced(TCLAP_INCLUDE_DIR) 39 | -------------------------------------------------------------------------------- /cmake_jetson.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | pushd . 4 | cd build_jetson || { echo "No such directory 'build_jetson'"; exit 1; } 5 | 6 | ROS_DIR=/opt/ros/kinetic 7 | DJI_DIR=$HOME/DJI 8 | DJI_CATKIN_DIR=$DJI_DIR/catkin_ws_jetson 9 | 10 | BUILD_TYPE=Release 11 | #BUILD_TYPE=Debug 12 | 13 | rm -r CMakeCache.txt CMakeFiles/ 14 | #cmake -DUSE_OPENCV_VERSION=2.4 .. 15 | cmake \ 16 | -DBUILD_PLANE_SWEEP_STEREO=OFF \ 17 | -DBUILD_QUAD_PLANNER=OFF \ 18 | -DBUILD_STEREO=OFF \ 19 | -DWITH_ROS=ON \ 20 | -DWITH_DJI=ON \ 21 | -DROS_INCLUDE_DIRS=$ROS_DIR/include \ 22 | -DROS_LIBRARY_DIR=$ROS_DIR/lib \ 23 | -DDJI_LIBRARY=$DJI_CATKIN_DIR/devel/lib/libdji_sdk_lib.a \ 24 | -DDJI_INCLUDE_DIRS="$DJI_CATKIN_DIR/src/dji_sdk/include;$DJI_CATKIN_DIR/src/dji_sdk_lib/include;$DJI_CATKIN_DIR/devel/include" \ 25 | -DCUDA_USE_STATIC_CUDA_RUNTIME=OFF \ 26 | -DUSE_OPENCV_VERSION=2.4 \ 27 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 28 | .. $@ 29 | 30 | popd 31 | 32 | -------------------------------------------------------------------------------- /external/QGLViewer/.gitignore: -------------------------------------------------------------------------------- 1 | .moc 2 | .obj 3 | *.so 4 | *.so.* 5 | ui_*.h 6 | libQGLViewer.prl 7 | 8 | 9 | -------------------------------------------------------------------------------- /external/QGLViewer/CHANGELOG: -------------------------------------------------------------------------------- 1 | This is libQGLViewer version 2.6.3. Packaged on July 10, 2015. 2 | 3 | The complete change log is available in doc/changeLog.html 4 | 5 | The latest releases and changeLog are available at: 6 | http://www.libqglviewer.com/changeLog.html 7 | -------------------------------------------------------------------------------- /external/QGLViewer/INSTALL: -------------------------------------------------------------------------------- 1 | 2 | l i b Q G L V i e w e r I n s t a l l a t i o n 3 | 4 | 5 | 6 | libQGLViewer requires the Qt library, available from Digia. 7 | 8 | In order to compile the library from its sources: 9 | 10 | - On UNIX platforms, simply type (see doc/installUnix.html for details): 11 | 12 | > qmake 13 | > make 14 | > make install [optional] 15 | 16 | - For Windows installation, see doc/installWindows.html. 17 | 18 | 19 | 20 | See doc/compilation.html for details on compiling programs that use libQGLViewer. 21 | -------------------------------------------------------------------------------- /external/QGLViewer/README: -------------------------------------------------------------------------------- 1 | 2 | l i b Q G L V i e w e r 3 | 4 | Version 2.6.3. Packaged on July 10, 2015 5 | 6 | 7 | Copyright (C) 2002-2014 Gilles Debunne. All rights reserved. 8 | http://www.libqglviewer.com 9 | Send e-mail to contact@libqglviewer.com 10 | 11 | 12 | libQGLViewer is a C++ library based on Qt that eases the creation of OpenGL 3D viewers. 13 | 14 | It provides some of the typical 3D viewer functionalities, such as the possibility to 15 | move the camera using the mouse, which lacks in most of the other APIs. Other features 16 | include mouse manipulated frames, interpolated keyFrames, object selection, stereo 17 | display, screenshot saving and much more. It can be used by OpenGL beginners as well as 18 | to create complex applications, being fully customizable and easy to extend. 19 | 20 | Based on the Qt toolkit, it compiles on any architecture (Unix-Linux, Mac, Windows). 21 | Full reference documentation and many examples are provided. libQGLViewer does not 22 | display 3D scenes in various formats, but it can be the base for the coding of such a 23 | viewer. 24 | 25 | libQGLViewer uses dual licensing: it is freely available under the terms of the GNU-GPL 26 | license for open source software development, while commercial applications can apply 27 | for a commercial license. 28 | 29 | 30 | The library's main functionalities are: 31 | 32 | - A camera trackball to intuitively move the camera in the scene. 33 | - Screenshot saving in different file formats (JPG, PNG, EPS...). 34 | - Easy scene object selection and manipulation using the mouse. 35 | - Definition and replay of keyFrame paths. 36 | - Stereo display (provided that your hardware supports it). 37 | - Clean, well-designed and easily extendable API. 38 | - Many examples and a complete reference documentation. 39 | 40 | See the doc/index.html page for details. 41 | 42 | -------------------------------------------------------------------------------- /external/QGLViewer/VRender/Types.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the VRender library. 3 | Copyright (C) 2005 Cyril Soler (Cyril.Soler@imag.fr) 4 | Version 1.0.0, released on June 27, 2005. 5 | 6 | http://artis.imag.fr/Members/Cyril.Soler/VRender 7 | 8 | VRender is free software; you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation; either version 2 of the License, or 11 | (at your option) any later version. 12 | 13 | VRender is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License 19 | along with VRender; if not, write to the Free Software Foundation, Inc., 20 | 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA. 21 | */ 22 | 23 | /**************************************************************************** 24 | 25 | Copyright (C) 2002-2014 Gilles Debunne. All rights reserved. 26 | 27 | This file is part of the QGLViewer library version 2.6.3. 28 | 29 | http://www.libqglviewer.com - contact@libqglviewer.com 30 | 31 | This file may be used under the terms of the GNU General Public License 32 | versions 2.0 or 3.0 as published by the Free Software Foundation and 33 | appearing in the LICENSE file included in the packaging of this file. 34 | In addition, as a special exception, Gilles Debunne gives you certain 35 | additional rights, described in the file GPL_EXCEPTION in this package. 36 | 37 | libQGLViewer uses dual licensing. Commercial/proprietary software must 38 | purchase a libQGLViewer Commercial License. 39 | 40 | This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE 41 | WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 42 | 43 | *****************************************************************************/ 44 | 45 | #ifndef _VRENDER_TYPES_H 46 | #define _VRENDER_TYPES_H 47 | 48 | #ifdef WIN32 49 | # include 50 | #endif 51 | 52 | #ifdef __APPLE__ 53 | # include 54 | #else 55 | # include 56 | #endif 57 | 58 | namespace vrender 59 | { 60 | typedef double FLOAT ; 61 | typedef GLdouble GLFLOAT ; 62 | 63 | #ifdef A_VOIR 64 | typedef T_Vect3 DVector3 ; 65 | typedef T_Vect2 Vector2 ; 66 | #endif 67 | 68 | class Primitive ; 69 | typedef Primitive *PtrPrimitive ; 70 | 71 | const float FLAT_POLYGON_EPS = 1e-5f ; 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /external/QGLViewer/qglviewer.icns: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/external/QGLViewer/qglviewer.icns -------------------------------------------------------------------------------- /external/QGLViewer/qglviewer_fr.qm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/external/QGLViewer/qglviewer_fr.qm -------------------------------------------------------------------------------- /external/fastlz/LICENSE: -------------------------------------------------------------------------------- 1 | FastLZ - lightning-fast lossless compression library 2 | 3 | Copyright (C) 2007 Ariya Hidayat (ariya@kde.org) 4 | Copyright (C) 2006 Ariya Hidayat (ariya@kde.org) 5 | Copyright (C) 2005 Ariya Hidayat (ariya@kde.org) 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in 15 | all copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 23 | THE SOFTWARE. 24 | 25 | -------------------------------------------------------------------------------- /external/fastlz/README.TXT: -------------------------------------------------------------------------------- 1 | FastLZ - lightning-fast lossless compression library 2 | 3 | Author: Ariya Hidayat 4 | Official website: http://www.fastlz.org 5 | 6 | FastLZ is distributed using the MIT license, see file LICENSE 7 | for details. 8 | 9 | FastLZ consists of two files: fastlz.h and fastlz.c. Just add these 10 | files to your project in order to use FastLZ. For information on 11 | compression and decompression routines, see fastlz.h. 12 | 13 | A simple file compressor called 6pack is included as an example 14 | on how to use FastLZ. The corresponding decompressor is 6unpack. 15 | 16 | To compile using GCC: 17 | 18 | gcc -o 6pack 6pack.c fastlz.c 19 | gcc -o 6unpack 6unpack.c fastlz.c 20 | 21 | To compile using MinGW: 22 | 23 | mingw32-gcc -o 6pack 6pack.c fastlz.c 24 | mingw32-gcc -o 6unpack 6unpack.c fastlz.c 25 | 26 | To compile using Microsoft Visual C++: 27 | 28 | cl 6pack.c fastlz.c 29 | cl 6unpack.c fastlz.c 30 | 31 | To compile using Borland C++: 32 | 33 | bcc32 6pack.c fastlz.c 34 | bcc32 6unpack.c fastlz.c 35 | 36 | To compile using OpenWatcom C/C++: 37 | 38 | cl386 6pack.c fastlz.c 39 | cl386 6unpack.c fastlz.c 40 | 41 | To compile using Intel C++ compiler for Windows: 42 | 43 | icl 6pack.c fastlz.c 44 | icl 6unpack.c fastlz.c 45 | 46 | To compile using Intel C++ compiler for Linux: 47 | 48 | icc -o 6pack 6pack.c fastlz.c 49 | icc -o 6unpack 6unpack.c fastlz.c 50 | 51 | To compile 6pack using LCC-Win32: 52 | 53 | lc 6pack.c fastlz.c 54 | lc 6unpack.c fastlz.c 55 | 56 | To compile 6pack using Pelles C: 57 | 58 | pocc 6pack.c 59 | pocc 6unpack.c 60 | pocc fastlz.c 61 | polink 6pack.obj fastlz.obj 62 | polink 6unpack.obj fastlz.obj 63 | 64 | For speed optimization, always use proper compile flags for optimization options. 65 | Typical compiler flags are given below: 66 | 67 | * GCC (pre 4.2): -march=pentium -O3 -fomit-frame-pointer -mtune=pentium 68 | * GCC 4.2 or later: -march=pentium -O3 -fomit-frame-pointer -mtune=generic 69 | * Digital Mars C/C++: -o+all -5 70 | * Intel C++ (Windows): /O3 /Qipo 71 | * Intel C++ (Linux): -O2 -march=pentium -mtune=pentium 72 | * Borland C++: -O2 -5 73 | * LCC-Win32: -O 74 | * Pelles C: /O2 75 | 76 | -------------------------------------------------------------------------------- /external/snappy/AUTHORS: -------------------------------------------------------------------------------- 1 | opensource@google.com 2 | -------------------------------------------------------------------------------- /external/snappy/COPYING: -------------------------------------------------------------------------------- 1 | Copyright 2011, Google Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above 11 | copyright notice, this list of conditions and the following disclaimer 12 | in the documentation and/or other materials provided with the 13 | distribution. 14 | * Neither the name of Google Inc. nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | === 31 | 32 | Some of the benchmark data in testdata/ is licensed differently: 33 | 34 | - fireworks.jpeg is Copyright 2013 Steinar H. Gunderson, and 35 | is licensed under the Creative Commons Attribution 3.0 license 36 | (CC-BY-3.0). See https://creativecommons.org/licenses/by/3.0/ 37 | for more information. 38 | 39 | - kppkn.gtb is taken from the Gaviota chess tablebase set, and 40 | is licensed under the MIT License. See 41 | https://sites.google.com/site/gaviotachessengine/Home/endgame-tablebases-1 42 | for more information. 43 | 44 | - paper-100k.pdf is an excerpt (bytes 92160 to 194560) from the paper 45 | “Combinatorial Modeling of Chromatin Features Quantitatively Predicts DNA 46 | Replication Timing in _Drosophila_” by Federico Comoglio and Renato Paro, 47 | which is licensed under the CC-BY license. See 48 | http://www.ploscompbiol.org/static/license for more ifnormation. 49 | 50 | - alice29.txt, asyoulik.txt, plrabn12.txt and lcet10.txt are from Project 51 | Gutenberg. The first three have expired copyrights and are in the public 52 | domain; the latter does not have expired copyright, but is still in the 53 | public domain according to the license information 54 | (http://www.gutenberg.org/ebooks/53). 55 | -------------------------------------------------------------------------------- /external/snappy/snappy-stubs-internal.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2011 Google Inc. All Rights Reserved. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are 5 | // met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above 10 | // copyright notice, this list of conditions and the following disclaimer 11 | // in the documentation and/or other materials provided with the 12 | // distribution. 13 | // * Neither the name of Google Inc. nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | 32 | #include "snappy-stubs-internal.h" 33 | 34 | namespace snappy { 35 | 36 | void Varint::Append32(string* s, uint32 value) { 37 | char buf[Varint::kMax32]; 38 | const char* p = Varint::Encode32(buf, value); 39 | s->append(buf, p - buf); 40 | } 41 | 42 | } // namespace snappy 43 | -------------------------------------------------------------------------------- /get_octomap.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosservice call /world/get_octomap '{bounding_box_origin: {x: 0, y: 0, z: 0}, bounding_box_lengths: {x: 25.0, y: 25.0, z: 15}, leaf_size: 0.1, filename: gazebo_octomap.bt}' 4 | 5 | -------------------------------------------------------------------------------- /include/ait/boost.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // boost.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Jan 18, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #ifndef BOOST_SYSTEM_NO_DEPRECATED 12 | #define BOOST_SYSTEM_NO_DEPRECATED 1 13 | #endif 14 | -------------------------------------------------------------------------------- /include/ait/boost_serialization_utils.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // boost_serialization_utils.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Feb 28, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // std::tuple serialization 14 | // by Christopher Allen Ogden (https://github.com/Sydius/serialize-tuple) 15 | // Copyright 2011 Christopher Allen Ogden. All rights reserved. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, are 18 | // permitted provided that the following conditions are met: 19 | // 20 | // 1. Redistributions of source code must retain the above copyright notice, this list of 21 | // conditions and the following disclaimer. 22 | // 23 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 24 | // of conditions and the following disclaimer in the documentation and/or other materials 25 | // provided with the distribution. 26 | // 27 | // THIS SOFTWARE IS PROVIDED BY CHRISTOPHER ALLEN OGDEN ``AS IS'' AND ANY EXPRESS OR IMPLIED 28 | // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 29 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CHRISTOPHER ALLEN OGDEN OR 30 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 33 | // ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 34 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 35 | // ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 36 | // 37 | // The views and conclusions contained in the software and documentation are those of the 38 | // authors and should not be interpreted as representing official policies, either expressed 39 | // or implied, of Christopher Allen Ogden. 40 | 41 | namespace boost { 42 | namespace serialization { 43 | 44 | template 45 | struct serialize_tuple { 46 | template 47 | static void serialize(Archive& ar, std::tuple& t, const unsigned int version) { 48 | ar & std::get(t); 49 | serialize_tuple::serialize(ar, t, version); 50 | } 51 | }; 52 | 53 | template<> 54 | struct serialize_tuple<0> { 55 | template 56 | static void serialize(Archive& ar, std::tuple& t, const unsigned int version) { 57 | } 58 | }; 59 | 60 | template 61 | void serialize(Archive& ar, std::tuple& t, const unsigned int version) 62 | { 63 | serialize_tuple::serialize(ar, t, version); 64 | } 65 | 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /include/ait/dji/RosDjiDrone.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // DjiRosSubscriber.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Nov 11, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace ait 17 | { 18 | namespace dji 19 | { 20 | 21 | class RosDjiDrone 22 | { 23 | public: 24 | RosDjiDrone() { 25 | dji_drone_ = new DJIDrone(node_handle_); 26 | } 27 | 28 | ~RosDjiDrone() { 29 | SAFE_DELETE(dji_drone_); 30 | } 31 | 32 | const DJIDrone& getDrone() const { 33 | return *dji_drone_; 34 | } 35 | 36 | DJIDrone& getDrone() { 37 | return *dji_drone_; 38 | } 39 | 40 | DJIDrone& operator()() { 41 | return *dji_drone_; 42 | } 43 | 44 | DJIDrone* operator->() { 45 | return dji_drone_; 46 | } 47 | 48 | private: 49 | ros::NodeHandle node_handle_; 50 | DJIDrone* dji_drone_; 51 | }; 52 | 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /include/ait/eigen.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // eigen.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 12, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "../bh/eigen.h" 11 | /* 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Eigen 18 | { 19 | // using Mat4f = Eigen::Matrix; 20 | // using Mat4d = Eigen::Matrix; 21 | // using Mat3f = Eigen::Matrix; 22 | // using Mat3d = Eigen::Matrix; 23 | 24 | using Matrix3x4d = Matrix; 25 | using Matrix3x4f = Matrix; 26 | using Vector2s = Matrix; 27 | using Vector3s = Matrix; 28 | using Vector4s = Matrix; 29 | } 30 | 31 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2f) 32 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) 33 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3f) 34 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) 35 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4f) 36 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4d) 37 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2f) 38 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2d) 39 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3f) 40 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d) 41 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4f) 42 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4d) 43 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3x4f) 44 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3x4d) 45 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Quaternionf) 46 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Quaterniond) 47 | 48 | #define EIGEN_ALIGNED_UNORDERED_MAP(Key, T) std::unordered_map, std::equal_to, \ 49 | Eigen::aligned_allocator>>; 50 | #define EIGEN_ALIGNED_UNORDERED_MAP2(Key, T, Hash) std::unordered_map, \ 51 | Eigen::aligned_allocator>>; 52 | #define EIGEN_ALIGNED_UNORDERED_MAP3(Key, T, Hash, EqualTo) std::unordered_map>>; 54 | 55 | #define USE_FIXED_EIGEN_TYPES(FloatType) \ 56 | using Vector2 = Eigen::Matrix; \ 57 | using Vector3 = Eigen::Matrix; \ 58 | using Vector4 = Eigen::Matrix; \ 59 | using Matrix4x4 = Eigen::Matrix; \ 60 | using Matrix3x3 = Eigen::Matrix; \ 61 | using Matrix3x4 = Eigen::Matrix; \ 62 | using Quaternion = Eigen::Quaternion; \ 63 | using AngleAxis = Eigen::AngleAxis; 64 | */ 65 | -------------------------------------------------------------------------------- /include/ait/eigen_alignment.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // eigen_alignment.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 8, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "eigen.h" 11 | -------------------------------------------------------------------------------- /include/ait/eigen_options.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // eigen_options.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Feb 21, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include "eigen.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace boost { 18 | 19 | template 20 | void validate(boost::any& v, 21 | const std::vector& values, 22 | Eigen::Matrix* target_type, int) { 23 | using namespace boost::program_options; 24 | 25 | // Make sure no previous assignment to 'v' was made. 26 | validators::check_first_occurrence(v); 27 | 28 | const std::string& s = validators::get_single_string(values); 29 | 30 | std::vector split_vec; 31 | boost::split(split_vec, s, boost::is_any_of(" ,"), boost::token_compress_on); 32 | 33 | if (split_vec.size() != Rows * Cols) { 34 | boost::throw_exception(validation_error(validation_error::invalid_option_value)); 35 | } 36 | 37 | typename Eigen::Matrix matrix; 38 | for (size_t i = 0; i < static_cast(matrix.size()); ++i) { 39 | matrix(i) = boost::lexical_cast(split_vec[i]); 40 | } 41 | v = boost::any(matrix); 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /include/ait/eigen_serialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * eigen_serialization.h 3 | * 4 | * Created on: Jan 2, 2017 5 | * Author: bhepp 6 | */ 7 | #pragma once 8 | 9 | #include "../bh/eigen_serialization.h" 10 | /* 11 | #include 12 | #include "eigen.h" 13 | 14 | namespace boost { 15 | //namespace serialization { 16 | 17 | // TODO: These don't work 18 | // template 19 | // inline void serialize( 20 | // Archive& ar, 21 | // Eigen::EigenBase& matrix, 22 | // const unsigned int version 23 | // ) { 24 | // ar & boost::serialization::make_array(matrix.data(), matrix.size()); 25 | // } 26 | // 27 | // template 28 | // inline void serialize( 29 | // Archive& ar, 30 | // const Eigen::EigenBase& matrix, 31 | // const unsigned int version 32 | // ) { 33 | // ar & boost::serialization::make_array(matrix.data(), matrix.size()); 34 | // } 35 | 36 | // template 37 | // inline void serialize( 38 | // Archive& ar, 39 | // Eigen::QuaternionBase& quat, 40 | // const unsigned int version 41 | // ) { 42 | // ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 43 | // } 44 | // 45 | // template 46 | // inline void serialize( 47 | // Archive& ar, 48 | // const Eigen::QuaternionBase& quat, 49 | // const unsigned int version 50 | // ) { 51 | // ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 52 | // } 53 | 54 | template 55 | inline void serialize( 56 | Archive& ar, 57 | Eigen::Quaternion& quat, 58 | const unsigned int version 59 | ) { 60 | ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 61 | } 62 | 63 | template 64 | inline void serialize( 65 | Archive& ar, 66 | const Eigen::Quaternion& quat, 67 | const unsigned int version 68 | ) { 69 | ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 70 | } 71 | 72 | template 73 | inline void serialize( 74 | Archive & ar, 75 | Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix, 76 | const unsigned int file_version) { 77 | ar & boost::serialization::make_array(matrix.data(), matrix.size()); 78 | } 79 | 80 | template 81 | inline void serialize( 82 | Archive & ar, 83 | const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix, 84 | const unsigned int file_version) { 85 | ar & boost::serialization::make_array(matrix.data(), matrix.size()); 86 | } 87 | 88 | //} 89 | } 90 | */ 91 | -------------------------------------------------------------------------------- /include/ait/eigen_serialization_plugin.h: -------------------------------------------------------------------------------- 1 | /* 2 | * eigen_serialization_plugin.h 3 | * 4 | * Created on: Jan 2, 2017 5 | * Author: bhepp 6 | */ 7 | #pragma once 8 | 9 | friend class boost::serialization::access; 10 | template 11 | void save(Archive & ar, const unsigned int version) const { 12 | derived().eval(); 13 | const Index rows = derived().rows(), cols = derived().cols(); 14 | ar & rows; 15 | ar & cols; 16 | for (Index j = 0; j < cols; ++j ) 17 | for (Index i = 0; i < rows; ++i ) 18 | ar & derived().coeff(i, j); 19 | } 20 | 21 | template 22 | void load(Archive & ar, const unsigned int version) { 23 | Index rows, cols; 24 | ar & rows; 25 | ar & cols; 26 | if (rows != derived().rows() || cols != derived().cols() ) 27 | derived().resize(rows, cols); 28 | ar & boost::serialization::make_array(derived().data(), derived().size()); 29 | } 30 | 31 | template 32 | void serialize(Archive & ar, const unsigned int file_version) { 33 | boost::serialization::split_member(ar, *this, file_version); 34 | } 35 | -------------------------------------------------------------------------------- /include/ait/eigen_utils.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // eigen_utils.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 14, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "../bh/eigen_utils.h" 11 | -------------------------------------------------------------------------------- /include/ait/eigen_with_serialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * eigen_serialization.h 3 | * 4 | * Created on: Jan 2, 2017 5 | * Author: bhepp 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #define EIGEN_DENSEBASE_PLUGIN "eigen_serialization_plugin.h" 11 | #include "eigen.h" 12 | -------------------------------------------------------------------------------- /include/ait/filesystem.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // boost_utilities.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 9, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "boost.h" 11 | #include 12 | 13 | namespace ait { 14 | 15 | //////////////////////// 16 | // File system utilities 17 | //////////////////////// 18 | 19 | template 20 | std::string joinPaths(T const&... paths) { 21 | boost::filesystem::path result; 22 | bool _[]{false, (result /= boost::filesystem::path(paths), false)...}; 23 | static_cast(_); 24 | return result.string(); 25 | } 26 | 27 | } 28 | -------------------------------------------------------------------------------- /include/ait/mLib.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // mLibInclude.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 14, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #if _WIN32 12 | #define NOMINMAX 13 | #include 14 | #include 15 | #include 16 | #endif 17 | 18 | // 19 | // mLib config options 20 | // 21 | 22 | #define MLIB_ERROR_CHECK 23 | #define MLIB_BOUNDS_CHECK 24 | #define MLIB_SOCKETS 25 | 26 | // 27 | // mLib includes 28 | // 29 | 30 | #include "mLibCore.h" 31 | #include "mLibFreeImage.h" 32 | #include "mLibDepthCamera.h" 33 | //#include "mLibLodePNG.h" 34 | //#include "mLibD3D11.h" 35 | 36 | //#include "mLibEigen.h" 37 | 38 | //move this to mlib (it's currently local) 39 | //#include "mLibCuda.h" 40 | -------------------------------------------------------------------------------- /include/ait/mLibZlib.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // mLibZlib.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 4, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "mLib.h" 11 | 12 | #include "mLibZLib.h" 13 | -------------------------------------------------------------------------------- /include/ait/math.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // math.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Nov 8, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include "eigen.h" 13 | 14 | namespace ait { 15 | 16 | using Mat4f = Eigen::Matrix; 17 | using Mat4d = Eigen::Matrix; 18 | using Mat3f = Eigen::Matrix; 19 | using Mat3d = Eigen::Matrix; 20 | 21 | template 22 | FloatType degreeToRadians(const FloatType degrees) { 23 | const FloatType pi = (FloatType)M_PI; 24 | return degrees * pi / 180; 25 | } 26 | 27 | template 28 | FloatType radiansToDegrees(const FloatType radians) { 29 | const FloatType pi = (FloatType)M_PI; 30 | return radians * 180 / pi; 31 | } 32 | 33 | template 34 | bool isApproxEqual(FloatType1 a, FloatType2 b, FloatType3 tolerance) { 35 | return std::abs(a - b) <= tolerance; 36 | } 37 | 38 | template 39 | bool isApproxGreater(FloatType1 a, FloatType2 b, FloatType3 tolerance) { 40 | return a > b + tolerance; 41 | } 42 | 43 | template 44 | bool isApproxGreaterEqual(FloatType1 a, FloatType2 b, FloatType3 tolerance) { 45 | return a >= b + tolerance; 46 | } 47 | 48 | template 49 | bool isApproxSmaller(FloatType1 a, FloatType2 b, FloatType3 tolerance) { 50 | return a < b - tolerance; 51 | } 52 | 53 | template 54 | bool isApproxSmallerEqual(FloatType1 a, FloatType2 b, FloatType3 tolerance) { 55 | return a <= b - tolerance; 56 | } 57 | 58 | } 59 | -------------------------------------------------------------------------------- /include/ait/math/statistics.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // statistics.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Nov 8, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "eigen.h" 14 | 15 | namespace ait { 16 | 17 | template 18 | class Histogram { 19 | public: 20 | using ValueType = ValueT; 21 | using CountType = CountT; 22 | 23 | // bin_edges_ contains the left and right edges of all bins. 24 | // i.e. if there are 2 bins, [0, 1) and [1, 2) bins_edges_ = { 0, 1, 2 }. 25 | 26 | Histogram(const size_t num_bins, const ValueT min, const ValueT max) { 27 | for (size_t i = 0; i <= num_bins; ++i) { 28 | const ValueT edge = min + i * (max - min) / num_bins; 29 | bin_edges_.push_back(edge); 30 | } 31 | counts_.resize(bin_edges_.size() - 1, 0); 32 | } 33 | 34 | template 35 | Histogram(const Iterator first, const Iterator last) { 36 | for (Iterator it = first; it != last; ++it) { 37 | bin_edges_.push_back(*it); 38 | } 39 | counts_.resize(bin_edges_.size() - 1, 0); 40 | } 41 | 42 | size_t numBins() const { 43 | return counts_.size(); 44 | } 45 | 46 | CountType operator()(const size_t bin) const { 47 | return counts_[bin]; 48 | } 49 | 50 | CountType totalCount() const { 51 | const CountT total_count = std::accumulate(counts_.begin(), counts_.end()); 52 | return total_count; 53 | } 54 | 55 | ValueT leftBinEdge(const size_t bin) const { 56 | return bin_edges_[bin]; 57 | } 58 | 59 | ValueT leftBinEdge(const size_t bin) const { 60 | return bin_edges_[bin + 1]; 61 | } 62 | 63 | template 64 | void accumulate(const ValueIterator first, const ValueIterator last) { 65 | for (ValueIterator it = first; it != last; ++it) { 66 | // find bin 67 | const size_t bin = 0; 68 | ++counts_[bin]; 69 | } 70 | } 71 | 72 | template ::value>> 73 | void normalize() { 74 | const CountT total_count = totalCount(); 75 | std::for_each(counts_.begin(), counts_.end(), [] (CountT& count) { 76 | count /= total_count; 77 | }); 78 | } 79 | 80 | private: 81 | std::vector bin_edges_; 82 | std::vector counts_; 83 | }; 84 | 85 | } 86 | -------------------------------------------------------------------------------- /include/ait/nn/approximate_nearest_neighbor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * approximate_nearest_neighbor.h 3 | * 4 | * Created on: Dec 29, 2016 5 | * Author: bhepp 6 | */ 7 | #pragma once 8 | 9 | #include "../../bh/nn/approximate_nearest_neighbor.h" 10 | -------------------------------------------------------------------------------- /include/ait/qt_utils.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // qt_utils.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Jan 18, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | std::ostream& operator<<(std::ostream& out, const QVector3D& vec); 15 | 16 | std::ostream& operator<<(std::ostream& out, const QMatrix4x4& mat); 17 | 18 | inline std::ostream& operator<<(std::ostream& out, const QVector3D& vec) { 19 | out << "(" << vec.x() << ", " << vec.y() << ", " << vec.z() << ")"; 20 | return out; 21 | } 22 | 23 | inline std::ostream& operator<<(std::ostream& out, const QMatrix4x4& mat) { 24 | for (int i = 0; i < 4; ++i) { 25 | for (int j = 0; j < 4; ++j) { 26 | if (j > 0) 27 | std::cout << ", "; 28 | std::cout << mat(i, j); 29 | } 30 | std::cout << std::endl; 31 | } 32 | std::cout << std::endl; 33 | return out; 34 | } 35 | -------------------------------------------------------------------------------- /include/ait/vision_utilities.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // vision_utilities.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 12, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "eigen.h" 11 | 12 | namespace ait { 13 | 14 | // ------------------- 15 | // Camera utilities 16 | //-------------------- 17 | 18 | /// Scale 3x3 intrinsics matrix 19 | template ::type = false> 20 | typename Derived::PlainObject getScaledIntrinsics(const Eigen::MatrixBase& intrinsics, typename Derived::Scalar scale_factor) { 21 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 22 | EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == Derived::ColsAtCompileTime, INTRINSICS_MATRIX_MUST_BE_SQUARE); 23 | typename Derived::PlainObject image_transform = Derived::PlainObject::Identity(); 24 | image_transform(0, 0) = scale_factor; 25 | image_transform(1, 1) = scale_factor; 26 | image_transform(0, 2) = scale_factor * 0.5 - 0.5; 27 | image_transform(1, 2) = scale_factor * 0.5 - 0.5; 28 | typename Derived::PlainObject scaled_intrinsics = image_transform * intrinsics; 29 | return scaled_intrinsics; 30 | } 31 | 32 | /// Scale 4x4 intrinsics matrix 33 | template ::type = false> 34 | typename Derived::PlainObject getScaledIntrinsics(const Eigen::DenseBase& intrinsics, typename Derived::Scalar scale_factor) { 35 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 36 | EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == Derived::ColsAtCompileTime, INTRINSICS_MATRIX_MUST_BE_SQUARE); 37 | typename Derived::PlainObject scaled_intrinsics = Derived::PlainObject::Identity(); 38 | scaled_intrinsics.template topLeftCorner<3, 3>() = getScaledIntrinsics(intrinsics.template topLeftCorner<3, 3>(), scale_factor); 39 | return scaled_intrinsics; 40 | } 41 | 42 | //template 43 | //Eigen::Vector2d projectPointIntoImage(const Eigen::MatrixBase& hom_point_camera, const Eigen::MatrixBase& intrinsics) const { 44 | // EIGEN_STATIC_ASSERT(Derived1::RowsAtCompileTime == 3, CAMERA_POINT_MUST_HAVE_3_ROWS); 45 | // Eigen::Matrix point_camera(hom_point_camera.hnormalized()); 46 | // Eigen::Matrix point_image; 47 | // point_image(0) = intrinsics(0, 0) * point_camera(0) + intrinsics(0, 2); 48 | // point_image(1) = intrinsics(1, 1) * point_camera(1) + intrinsics(1, 2); 49 | // return point_image; 50 | //} 51 | 52 | } 53 | -------------------------------------------------------------------------------- /include/bh/boost.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // boost.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Jan 18, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #ifndef BOOST_SYSTEM_NO_DEPRECATED 12 | #define BOOST_SYSTEM_NO_DEPRECATED 1 13 | #endif 14 | -------------------------------------------------------------------------------- /include/bh/boost_serialization_utils.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // boost_serialization_utils.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Feb 28, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // std::tuple serialization 14 | // by Christopher Allen Ogden (https://github.com/Sydius/serialize-tuple) 15 | // Copyright 2011 Christopher Allen Ogden. All rights reserved. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, are 18 | // permitted provided that the following conditions are met: 19 | // 20 | // 1. Redistributions of source code must retain the above copyright notice, this list of 21 | // conditions and the following disclaimer. 22 | // 23 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 24 | // of conditions and the following disclaimer in the documentation and/or other materials 25 | // provided with the distribution. 26 | // 27 | // THIS SOFTWARE IS PROVIDED BY CHRISTOPHER ALLEN OGDEN ``AS IS'' AND ANY EXPRESS OR IMPLIED 28 | // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 29 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CHRISTOPHER ALLEN OGDEN OR 30 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 33 | // ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 34 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 35 | // ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 36 | // 37 | // The views and conclusions contained in the software and documentation are those of the 38 | // authors and should not be interpreted as representing official policies, either expressed 39 | // or implied, of Christopher Allen Ogden. 40 | 41 | namespace boost { 42 | namespace serialization { 43 | 44 | template 45 | struct serialize_tuple { 46 | template 47 | static void serialize(Archive& ar, std::tuple& t, const unsigned int version) { 48 | ar & std::get(t); 49 | serialize_tuple::serialize(ar, t, version); 50 | } 51 | }; 52 | 53 | template<> 54 | struct serialize_tuple<0> { 55 | template 56 | static void serialize(Archive& ar, std::tuple& t, const unsigned int version) { 57 | } 58 | }; 59 | 60 | template 61 | void serialize(Archive& ar, std::tuple& t, const unsigned int version) 62 | { 63 | serialize_tuple::serialize(ar, t, version); 64 | } 65 | 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /include/bh/eigen_options.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // eigen_options.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Feb 21, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include "eigen.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace boost { 18 | 19 | template 20 | void validate(boost::any& v, 21 | const std::vector& values, 22 | Eigen::Matrix* target_type, int) { 23 | using namespace boost::program_options; 24 | 25 | // Make sure no previous assignment to 'v' was made. 26 | validators::check_first_occurrence(v); 27 | 28 | const std::string& s = validators::get_single_string(values); 29 | 30 | std::vector split_vec; 31 | boost::split(split_vec, s, boost::is_any_of(" ,"), boost::token_compress_on); 32 | 33 | if (split_vec.size() != Rows * Cols) { 34 | boost::throw_exception(validation_error(validation_error::invalid_option_value)); 35 | } 36 | 37 | typename Eigen::Matrix matrix; 38 | for (size_t i = 0; i < static_cast(matrix.size()); ++i) { 39 | matrix(i) = boost::lexical_cast(split_vec[i]); 40 | } 41 | v = boost::any(matrix); 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /include/bh/eigen_serialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * eigen_serialization.h 3 | * 4 | * Created on: Jan 2, 2017 5 | * Author: bhepp 6 | */ 7 | #pragma once 8 | 9 | #include 10 | #include "eigen.h" 11 | 12 | namespace boost { 13 | //namespace serialization { 14 | 15 | // TODO: These don't work 16 | // template 17 | // inline void serialize( 18 | // Archive& ar, 19 | // Eigen::EigenBase& matrix, 20 | // const unsigned int version 21 | // ) { 22 | // ar & boost::serialization::make_array(matrix.data(), matrix.size()); 23 | // } 24 | // 25 | // template 26 | // inline void serialize( 27 | // Archive& ar, 28 | // const Eigen::EigenBase& matrix, 29 | // const unsigned int version 30 | // ) { 31 | // ar & boost::serialization::make_array(matrix.data(), matrix.size()); 32 | // } 33 | 34 | // template 35 | // inline void serialize( 36 | // Archive& ar, 37 | // Eigen::QuaternionBase& quat, 38 | // const unsigned int version 39 | // ) { 40 | // ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 41 | // } 42 | // 43 | // template 44 | // inline void serialize( 45 | // Archive& ar, 46 | // const Eigen::QuaternionBase& quat, 47 | // const unsigned int version 48 | // ) { 49 | // ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 50 | // } 51 | 52 | template 53 | inline void serialize( 54 | Archive& ar, 55 | Eigen::Quaternion& quat, 56 | const unsigned int version 57 | ) { 58 | ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 59 | } 60 | 61 | template 62 | inline void serialize( 63 | Archive& ar, 64 | const Eigen::Quaternion& quat, 65 | const unsigned int version 66 | ) { 67 | ar & boost::serialization::make_array(quat.coeffs().data(), quat.coeffs().size()); 68 | } 69 | 70 | template 71 | inline void serialize( 72 | Archive & ar, 73 | Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix, 74 | const unsigned int file_version) { 75 | ar & boost::serialization::make_array(matrix.data(), matrix.size()); 76 | } 77 | 78 | template 79 | inline void serialize( 80 | Archive & ar, 81 | const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix, 82 | const unsigned int file_version) { 83 | ar & boost::serialization::make_array(matrix.data(), matrix.size()); 84 | } 85 | 86 | //} 87 | } 88 | -------------------------------------------------------------------------------- /include/bh/filesystem.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // boost_utilities.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 9, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "boost.h" 11 | #include 12 | 13 | namespace bh { 14 | 15 | namespace boostfs = boost::filesystem; 16 | 17 | //////////////////////// 18 | // File system utilities 19 | //////////////////////// 20 | 21 | template 22 | std::string joinPaths(T const&... paths) { 23 | boostfs::path result; 24 | bool _[]{false, (result /= boostfs::path(paths), false)...}; 25 | static_cast(_); 26 | return result.string(); 27 | } 28 | 29 | // 30 | // Thanks to http://stackoverflow.com/questions/10167382/boostfilesystem-get-relative-path. 31 | // 32 | inline boostfs::path pathRelativeTo(const boostfs::path& from, const boostfs::path& to, const bool make_absolute = true) { 33 | if (make_absolute) { 34 | const boostfs::path abs_from = boostfs::absolute(from); 35 | const boostfs::path abs_to = boostfs::absolute(to); 36 | return pathRelativeTo(abs_from, abs_to, false); 37 | } 38 | 39 | // Start at the root path and while they are the same then do nothing then when they first 40 | // diverge take the remainder of the two path and replace the entire from path with ".." 41 | // segments. 42 | boostfs::path::const_iterator from_iter = from.begin(); 43 | boostfs::path::const_iterator to_iter = to.begin(); 44 | 45 | // Loop through both while same 46 | while (from_iter != from.end() && to_iter != to.end() && (*to_iter) == (*from_iter)) { 47 | ++to_iter; 48 | ++from_iter; 49 | } 50 | 51 | boostfs::path final_path; 52 | while (from_iter != from.end()) { 53 | final_path /= ".."; 54 | ++from_iter; 55 | } 56 | 57 | while (to_iter != to.end()) { 58 | final_path /= *to_iter; 59 | ++to_iter; 60 | } 61 | 62 | return final_path; 63 | } 64 | 65 | } 66 | -------------------------------------------------------------------------------- /include/bh/gperf.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // gperf.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 21.03.17 7 | // 8 | 9 | #pragma once 10 | 11 | #if BH_WITH_PROFILING 12 | #include 13 | #endif 14 | 15 | namespace bh { 16 | 17 | inline void ProfilerStart(const std::string &filename) { 18 | #if BH_WITH_PROFILING 19 | ::ProfilerStart(filename.c_str()); 20 | #endif 21 | } 22 | 23 | inline void ProfilerStop() { 24 | #if BH_WITH_PROFILING 25 | ::ProfilerStop(); 26 | #endif 27 | } 28 | 29 | } 30 | -------------------------------------------------------------------------------- /include/bh/mLib/mLib.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // mLibInclude.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 14, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #if _WIN32 12 | #define NOMINMAX 13 | #include 14 | #include 15 | #include 16 | #endif 17 | 18 | // 19 | // mLib config options 20 | // 21 | 22 | #define MLIB_ERROR_CHECK 23 | #define MLIB_BOUNDS_CHECK 24 | #define MLIB_SOCKETS 25 | 26 | // 27 | // mLib includes 28 | // 29 | 30 | #include "mLibCore.h" 31 | #include "mLibFreeImage.h" 32 | #include "mLibDepthCamera.h" 33 | //#include "mLibLodePNG.h" 34 | //#include "mLibD3D11.h" 35 | 36 | //#include "mLibEigen.h" 37 | 38 | //move this to mlib (it's currently local) 39 | //#include "mLibCuda.h" 40 | -------------------------------------------------------------------------------- /include/bh/mLib/mLibZlib.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // mLibZlib.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 4, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "mLib.h" 11 | 12 | #include "mLibZLib.h" 13 | -------------------------------------------------------------------------------- /include/bh/math/histogram.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // histogram.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 08.05.17 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | 12 | namespace bh { 13 | 14 | template 15 | std::vector computeHistogram(const Iterator first, const Iterator last, 16 | const BinIterator first_bin, const BinIterator last_bin); 17 | 18 | /// Computes a histogram of values in range [first, last). 19 | /// The bins are given by the range [first_bin, last_bin), i.e. the first bin is [*first_bin, *(first_bin + 1)). 20 | /// Condition: last_bin - first_bin >= 2. 21 | /// Values below *first_bin are ignored. 22 | template 23 | std::vector computeHistogram(const Iterator first, const Iterator last, 24 | const BinIterator first_bin, const BinIterator last_bin) { 25 | BH_ASSERT(last_bin - first_bin >= 2); 26 | std::vector histogram(last_bin - first_bin, 0); 27 | for (Iterator it = first; it < last; ++it) { 28 | BinIterator it_bin = std::upper_bound(first_bin, last_bin, *it); 29 | if (it_bin != first_bin) { 30 | --it_bin; 31 | const N bin_index = it_bin - first_bin; 32 | ++histogram[bin_index]; 33 | } 34 | } 35 | return histogram; 36 | }; 37 | 38 | } -------------------------------------------------------------------------------- /include/bh/math/utilities.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // utilities.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Nov 8, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace bh { 14 | 15 | template 16 | FloatT degreeToRadians(const FloatT degrees) { 17 | const FloatT pi = (FloatT)M_PI; 18 | return degrees * pi / 180; 19 | } 20 | 21 | template 22 | FloatT radiansToDegrees(const FloatT radians) { 23 | const FloatT pi = (FloatT)M_PI; 24 | return radians * 180 / pi; 25 | } 26 | 27 | template 28 | FloatT wrapRadiansTo0And2Pi(const FloatT angle) { 29 | const FloatT two_pi = 2 * FloatT(M_PI); 30 | const FloatT wrapped_angle = angle - two_pi * std::floor(angle / two_pi); 31 | return wrapped_angle; 32 | } 33 | 34 | template 35 | FloatT wrapDegreesTo0And360(const FloatT angle) { 36 | const FloatT wrapped_angle = angle - 360 * std::floor(angle / 360); 37 | return wrapped_angle; 38 | } 39 | 40 | template 41 | FloatT wrapRadiansToMinusPiAndPi(const FloatT angle) { 42 | const FloatT angle_0_two_pi = wrapRadiansTo0And2Pi(angle); 43 | if (angle_0_two_pi >= FloatT(M_PI)) { 44 | return angle_0_two_pi - 2 * FloatT(M_PI); 45 | } 46 | return angle_0_two_pi; 47 | } 48 | 49 | template 50 | FloatT wrapDegreesToMinus180And180(const FloatT angle) { 51 | const FloatT angle_0_360 = wrapDegreesTo0And360(angle); 52 | if (angle_0_360 >= 180) { 53 | return angle_0_360 - 360; 54 | } 55 | return angle_0_360; 56 | } 57 | 58 | template 59 | bool isApproxEqual(const FloatT1 a, const FloatT2 b, 60 | const FloatT1 tolerance = std::numeric_limits::epsilon()) { 61 | return std::abs(a - b) <= tolerance; 62 | } 63 | 64 | template 65 | bool isApproxGreater(const FloatT1 a, const FloatT2 b, 66 | const FloatT1 tolerance = std::numeric_limits::epsilon()) { 67 | return a > b + tolerance; 68 | } 69 | 70 | template 71 | bool isApproxGreaterEqual(const FloatT1 a, const FloatT2 b, 72 | const FloatT1 tolerance = std::numeric_limits::epsilon()) { 73 | return a >= b + tolerance; 74 | } 75 | 76 | template 77 | bool isApproxSmaller(const FloatT1 a, const FloatT2 b, 78 | const FloatT1 tolerance = std::numeric_limits::epsilon()) { 79 | return a < b - tolerance; 80 | } 81 | 82 | template 83 | bool isApproxSmallerEqual(const FloatT1 a, const FloatT2 b, 84 | const FloatT1 tolerance = std::numeric_limits::epsilon()) { 85 | return a <= b - tolerance; 86 | } 87 | 88 | } 89 | -------------------------------------------------------------------------------- /include/bh/opencv/matrix.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // matrix.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 06.04.17 7 | //================================================== 8 | #pragma once 9 | 10 | #include "../common.h" 11 | #include "../eigen.h" 12 | #include 13 | 14 | namespace bh { 15 | namespace opencv { 16 | 17 | template 18 | EigenType convertCvToEigen(const cv::Mat& cv_mat) { 19 | EigenType eigen_mat(cv_mat.rows, cv_mat.cols); 20 | if (cv_mat.type() == CV_32F) { 21 | for (size_t row = 0; row < (size_t) cv_mat.rows; ++row) { 22 | for (size_t col = 0; col < (size_t) cv_mat.cols; ++col) { 23 | eigen_mat(row, col) = cv_mat.at(row, col); 24 | } 25 | } 26 | } 27 | else if (cv_mat.type() == CV_64F) { 28 | for (size_t row = 0; row < (size_t) cv_mat.rows; ++row) { 29 | for (size_t col = 0; col < (size_t) cv_mat.cols; ++col) { 30 | eigen_mat(row, col) = cv_mat.at(row, col); 31 | } 32 | } 33 | } 34 | else { 35 | throw bh::Error("OpenCV to Eigen conversion only supports floating types"); 36 | } 37 | return eigen_mat; 38 | } 39 | 40 | template 41 | cv::Mat convertEigenToCv32F(const Derived& eigen_mat) { 42 | cv::Mat cv_mat(eigen_mat.rows(), eigen_mat.cols(), CV_32F); 43 | for (size_t row = 0; row < (size_t) eigen_mat.rows(); ++row) { 44 | for (size_t col = 0; col < (size_t) eigen_mat.cols(); ++col) { 45 | cv_mat.at(row, col) = eigen_mat(row, col); 46 | } 47 | } 48 | return cv_mat; 49 | } 50 | 51 | template 52 | cv::Mat convertEigenToCv64F(const Derived& eigen_mat) { 53 | cv::Mat cv_mat(eigen_mat.rows(), eigen_mat.cols(), CV_64F); 54 | for (size_t row = 0; row < (size_t) eigen_mat.rows(); ++row) { 55 | for (size_t col = 0; col < (size_t) eigen_mat.cols(); ++col) { 56 | cv_mat.at(row, col) = eigen_mat(row, col); 57 | } 58 | } 59 | return cv_mat; 60 | } 61 | 62 | } 63 | } -------------------------------------------------------------------------------- /include/bh/qt/utils.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // qt_utils.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Jan 18, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | std::ostream& operator<<(std::ostream& out, const QSize& size); 19 | 20 | std::ostream& operator<<(std::ostream& out, const QVector3D& vec); 21 | 22 | std::ostream& operator<<(std::ostream& out, const QMatrix4x4& mat); 23 | 24 | inline std::ostream& operator<<(std::ostream& out, const QSize& size) { 25 | out << "(" << size.width() << ", " << size.height() << ")"; 26 | return out; 27 | } 28 | 29 | inline std::ostream& operator<<(std::ostream& out, const QVector3D& vec) { 30 | out << "(" << vec.x() << ", " << vec.y() << ", " << vec.z() << ")"; 31 | return out; 32 | } 33 | 34 | inline std::ostream& operator<<(std::ostream& out, const QMatrix4x4& mat) { 35 | for (int i = 0; i < 4; ++i) { 36 | for (int j = 0; j < 4; ++j) { 37 | if (j > 0) 38 | std::cout << ", "; 39 | std::cout << mat(i, j); 40 | } 41 | std::cout << std::endl; 42 | } 43 | std::cout << std::endl; 44 | return out; 45 | } 46 | 47 | namespace bh { 48 | namespace qt { 49 | 50 | template 51 | bh::Color3 color3FromQt(const QColor &color_qt); 52 | 53 | template 54 | bh::Color4 colorFromQt(const QColor &color_qt); 55 | 56 | template 57 | QColor colorToQt(const bh::Color3 &color); 58 | 59 | template 60 | QColor colorToQt(const bh::Color4 &color); 61 | 62 | template 63 | bh::Color3 color3FromQt(const QColor &color_qt) { 64 | return bh::Color3(color_qt.redF(), color_qt.greenF(), color_qt.blueF()); 65 | } 66 | 67 | template 68 | bh::Color4 colorFromQt(const QColor &color_qt) { 69 | return bh::Color4(color_qt.redF(), color_qt.greenF(), color_qt.blueF(), color_qt.alphaF()); 70 | } 71 | 72 | template 73 | QColor colorToQt(const bh::Color3 &color) { 74 | return QColor::fromRgbF(color.r(), color.g(), color.b(), 1); 75 | } 76 | 77 | template 78 | QColor colorToQt(const bh::Color4 &color) { 79 | return QColor::fromRgbF(color.r(), color.g(), color.b(), color.a()); 80 | } 81 | 82 | inline void setImageAlphaPremultiplied(QImage* image, const qreal alpha) { 83 | for (int y = 0; y < image->height(); ++y) { 84 | for (int x = 0; x < image->width(); ++x) { 85 | QRgb pixel_rgb = qUnpremultiply(image->pixel(x, y)); 86 | QColor pixel_color(pixel_rgb); 87 | pixel_color.setAlphaF(alpha); 88 | QRgb new_pixel_color = qPremultiply(pixel_color.rgba()); 89 | image->setPixel(x, y, new_pixel_color); 90 | } 91 | } 92 | } 93 | 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /include/bh/vision/types.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // types.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 05.04.17 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | 12 | namespace bh { 13 | namespace vision { 14 | 15 | template 16 | class Keypoint { 17 | public: 18 | Keypoint(); 19 | 20 | Keypoint(const FloatT x, const FloatT y, const FloatT scale, const FloatT orientation); 21 | 22 | Keypoint(const Keypoint& other) = default; 23 | 24 | FloatT x() const { return x_; } 25 | 26 | FloatT y() const { return y_; } 27 | 28 | FloatT scale() const { return scale_; } 29 | 30 | FloatT orientation() const { return orientation_; } 31 | 32 | private: 33 | FloatT x_; 34 | FloatT y_; 35 | FloatT scale_; 36 | FloatT orientation_; 37 | }; 38 | 39 | template 40 | class KeypointMatch { 41 | public: 42 | KeypointMatch(const Keypoint& keypoint1, const Keypoint& keypoint2); 43 | 44 | KeypointMatch(const KeypointMatch& keypoint_match) = default; 45 | 46 | const Keypoint& keypoint1() const { return keypoint1_; } 47 | 48 | const Keypoint& keypoint2() const { return keypoint2_; } 49 | 50 | private: 51 | Keypoint keypoint1_; 52 | Keypoint keypoint2_; 53 | }; 54 | 55 | 56 | using Keypointf = Keypoint; 57 | using Keypointd = Keypoint; 58 | 59 | template 60 | Keypoint::Keypoint() 61 | : x_(std::numeric_limits::quiet_NaN()), 62 | y_(std::numeric_limits::quiet_NaN()), 63 | scale_(std::numeric_limits::quiet_NaN()), 64 | orientation_(std::numeric_limits::quiet_NaN()) {} 65 | 66 | template 67 | Keypoint::Keypoint(const FloatT x, const FloatT y, const FloatT scale, const FloatT orientation) 68 | : x_(x), y_(y), scale_(scale), orientation_(orientation) {} 69 | 70 | template 71 | KeypointMatch::KeypointMatch(const Keypoint& keypoint1, const Keypoint& keypoint2) 72 | : keypoint1_(keypoint1), keypoint2_(keypoint2) {} 73 | 74 | } 75 | } -------------------------------------------------------------------------------- /make_jetson.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | pushd . 4 | cd build_jetson || { echo "No such directory 'build_jetson"; exit 1; } 5 | 6 | make -j4 video_streamer_bundlefusion_drone 7 | 8 | popd 9 | 10 | -------------------------------------------------------------------------------- /plane_sweep_stereo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( 2 | include/ 3 | ${PLANESWEEPLIB_INCLUDE_DIRS} 4 | ) 5 | 6 | add_executable(plane_sweep_test 7 | src/plane_sweep_test.cpp 8 | ../stereo/src/stereo_calibration.cpp 9 | ../src/utilities.cpp 10 | ) 11 | target_link_libraries(plane_sweep_test 12 | ${Boost_LIBRARIES} 13 | ${OpenCV_LIBRARIES} 14 | ${PLANESWEEPLIB_LIBRARIES} 15 | ) 16 | 17 | add_executable(plane_sweep_zed 18 | src/plane_sweep_zed.cpp 19 | ../stereo/src/stereo_calibration.cpp 20 | ../src/utilities.cpp 21 | ../video/src/video_source.cpp 22 | ../video/src/video_source_zed.cpp 23 | ) 24 | target_link_libraries(plane_sweep_zed 25 | ${Boost_LIBRARIES} 26 | ${OpenCV_LIBRARIES} 27 | ${PLANESWEEPLIB_LIBRARIES} 28 | ${ZED_LIBRARIES} 29 | ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_npp_LIBRARY} 30 | ) 31 | 32 | add_executable(plane_sweep_test_orig 33 | src/plane_sweep_test_orig.cpp 34 | ) 35 | target_link_libraries(plane_sweep_test_orig 36 | ${Boost_LIBRARIES} 37 | ${OpenCV_LIBRARIES} 38 | ${PLANESWEEPLIB_LIBRARIES} 39 | ) 40 | -------------------------------------------------------------------------------- /quad_flight/cfg/PositionController.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = "quad_flight" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("gain_position_p", double_t, 0, "Proportional position gain", 2.0, 0.0, 10.0) 10 | gen.add("gain_orientation_p", double_t, 0, "Proportional orientation gain", 2.0, 0.0, 10.0) 11 | gen.add("gain_position_i", double_t, 0, "Integral position gain", 0.002, 0.0, 10.0) 12 | gen.add("gain_orientation_i", double_t, 0, "Integral orientation gain", 0.002, 0.0, 10.0) 13 | gen.add("position_error_integral_lim", double_t, 0, "Integral limit for position", 2.0, 0.0, 10.0) 14 | gen.add("orientation_error_integral_lim", double_t, 0, "Integral limit for position", 2.0, 0.0, 10.0) 15 | 16 | exit(gen.generate(PACKAGE, "quad_flight", "PositionController")) 17 | 18 | -------------------------------------------------------------------------------- /quad_flight/config/gimbal_control.yaml: -------------------------------------------------------------------------------- 1 | quadrotor: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Position Controllers --------------------------------------- 8 | gimbal_yaw_controller: 9 | type: effort_controllers/JointPositionController 10 | joint: gimbal_yaw_joint 11 | pid: {p: 10, i: 0.01, d: 0} 12 | gimbal_pitch_controller: 13 | type: effort_controllers/JointPositionController 14 | joint: gimbal_pitch_joint 15 | pid: {p: 1, i: 0.0, d: 0.0} 16 | -------------------------------------------------------------------------------- /quad_flight/launch/buildings_flight_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /quad_flight/launch/buildings_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /quad_flight/launch/display_quad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /quad_flight/launch/moveit_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /quad_flight/launch/outdoor_flight_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /quad_flight/launch/position_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /quad_flight/launch/position_controller_with_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /quad_flight/launch/position_teleop.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 | 25 | -------------------------------------------------------------------------------- /quad_flight/launch/rolling_landscape_120m.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /quad_flight/launch/start_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /quad_flight/launch/velocity_teleop.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 | 25 | 26 | -------------------------------------------------------------------------------- /quad_flight/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | quad_flight 4 | 0.0.0 5 | Quadrotor tools for flying 6 | 7 | 8 | 9 | 10 | Benjamin Hepp 11 | 12 | 13 | 14 | 15 | 16 | ALv2 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Benjamin Hepp 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | roscpp 45 | gazebo_ros 46 | gazebo_plugins 47 | tf 48 | joy 49 | dynamic_reconfigure 50 | roscpp 51 | gazebo_ros 52 | gazebo_plugins 53 | tf 54 | joy 55 | dynamic_reconfigure 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /quad_flight/test/get_link_state.sh: -------------------------------------------------------------------------------- 1 | LINK="quadrotor::RGBD_gimbal_camera_link" 2 | REFERENCE="quadrotor::base_link" 3 | 4 | rosservice call /gazebo/get_link_state "{link_name: '${LINK}', reference_frame: '${REFERENCE}'}" 5 | 6 | -------------------------------------------------------------------------------- /quad_flight/test/pub_control.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub /quadrotor/gimbal_yaw_controller/command std_msgs/Float64 "{data: ${1}}" 4 | rostopic pub /quadrotor/gimbal_pitch_controller/command std_msgs/Float64 "{data: ${2}}" 5 | 6 | -------------------------------------------------------------------------------- /quad_flight/test/set_link_state.sh: -------------------------------------------------------------------------------- 1 | LINK="quadrotor::RGBD_gimbal_camera_link" 2 | #LINK="quadrotor::RGBD_gimbal_yaw_link" 3 | REFERENCE="quadrotor::base_link" 4 | 5 | X=0.10 6 | Y=0.0 7 | Z=-0.1 8 | 9 | OX=0.0 10 | OY=0.0 11 | OZ=0.2 12 | OW=-0.95 13 | 14 | rosservice call /gazebo/set_link_state "{link_state: { link_name: '${LINK}', pose: { position: {x: ${X}, y: ${Y}, z: ${Z}}, orientation: {x: ${OX}, y: ${OY}, z: ${OZ}, w: ${OW}} }, reference_frame: '${REFERENCE}' } }" 15 | 16 | -------------------------------------------------------------------------------- /quad_flight/urdf/depth_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 21 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | Gazebo/Yellow 49 | 50 | 20 51 | 52 | ${110 * PI/180.0} 53 | 54 | R8G8B8 55 | 640 56 | 480 57 | 58 | 59 | 0.01 60 | 100 61 | 62 | 63 | 64 | ${name}/rgb/image_raw 65 | ${name}/rgb/camera_info 66 | ${name}/depth/image_raw 67 | ${name}/depth/camera_info 68 | ${name}/depth/points 69 | ${name}_depth_optical_frame 70 | 0.7 71 | 20.0 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /quad_flight/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /quad_flight/urdf/quadrotor_base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /quad_flight/urdf/quadrotor_simple.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /quad_flight/urdf/quadrotor_test.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | /quadrotor 7 | gazebo_ros_control/DefaultRobotHWSim 8 | 9 | 10 | 11 | 12 | Gazebo/Orange 13 | 14 | 15 | 16 | 0.2 17 | 0.2 18 | Gazebo/Black 19 | 20 | 21 | 22 | 0.2 23 | 0.2 24 | Gazebo/Orange 25 | 26 | 27 | 28 | 0.2 29 | 0.2 30 | Gazebo/Red 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /quad_flight/urdf/quadrotor_with_depth_camera.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /quad_flight/urdf/quadrotor_with_depth_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | Gazebo/Blue 33 | 1.1 34 | 1.1 35 | 36 | 37 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /quad_flight/urdf/utils.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /quad_flight/urdf/xacro_test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun xacro xacro --inorder $1 4 | -------------------------------------------------------------------------------- /quad_flight/worlds/buildings.world.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 0.5 0.5 0.5 1 12 | 0.5 0.5 0.5 1 13 | false 14 | 15 | 16 | 0 0 -9.81 17 | 18 | 19 | quick 20 | 10 21 | 1.3 22 | 23 | 24 | 0.0 25 | 0.2 26 | 10 27 | 0.001 28 | 29 | 30 | 0.001 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /quad_flight/worlds/house1.sdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ${pose} 7 | 1 8 | 9 | 10 | 11 | 12 | model://house_1/meshes/house_1.dae 13 | 14 | 15 | 10 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://house_1/meshes/house_1.dae 30 | 31 | 32 | 33 | 38 | 39 | House_1_Normal.png 40 | 41 | 42 | 43 | 0 44 | 0 45 | 1 46 | 47 | 1 -1 0 0 -0 0 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /quad_flight/worlds/house2.sdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ${pose} 7 | 1 8 | 9 | 10 | 11 | 12 | model://house_2/meshes/house_2.dae 13 | 14 | 15 | 10 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://house_2/meshes/house_2.dae 30 | 31 | 32 | 33 | 39 | 40 | model://house_1/materials/textures/House_1_Normal.png 41 | 42 | 43 | 44 | 0 45 | 0 46 | 1 47 | 48 | 1 -1 0 0 -0 0 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /quad_flight/worlds/house3.sdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ${pose} 7 | 1 8 | 9 | 10 | 11 | 12 | model://house_3/meshes/house_3.dae 13 | 14 | 15 | 10 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://house_3/meshes/house_3.dae 30 | 31 | 32 | 33 | 39 | 40 | model://house_1/materials/textures/House_1_Normal.png 41 | 42 | 43 | 44 | 0 45 | 0 46 | 1 47 | 48 | 1 -1 0 0 -0 0 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /quad_flight/worlds/xacro.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun xacro xacro --inorder buildings.world.xacro $@ > buildings.world 4 | -------------------------------------------------------------------------------- /quad_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/") 2 | 3 | if("${EIGEN3_INCLUDE_DIR}" STREQUAL "") 4 | find_package(Eigen3 REQUIRED) 5 | endif() 6 | find_package(octomap REQUIRED) 7 | find_package(ompl REQUIRED) 8 | find_package(OpenGL REQUIRED) 9 | find_package(GLEW REQUIRED) 10 | find_package(glm REQUIRED) 11 | find_package(PkgConfig REQUIRED) 12 | pkg_search_module(GLFW REQUIRED glfw3) 13 | 14 | include_directories( 15 | include/ 16 | ${OMPL_INCLUDE_DIRS} 17 | ${EIGEN3_INCLUDE_DIR} 18 | ${OCTOMAP_INCLUDE_DIRS} 19 | ${OPENGL_INCLUDE_DIRS} 20 | ${GLFW_INCLUDE_DIRS} 21 | ${GLEW_INCLUDE_DIRS} 22 | ${GLM_INCLUDE_DIRS} 23 | ) 24 | 25 | add_executable(quad_planner 26 | src/quad_planner_app.cpp 27 | src/quad_planner.cpp 28 | src/optimizing_rrt_planner.cpp 29 | src/rendering/visualizer.cpp 30 | src/rendering/octomap_renderer.cpp 31 | src/rendering/shader_program.cpp 32 | src/rendering/triangle_mesh.cpp 33 | src/rendering/lines.cpp 34 | src/rendering/utilities.cpp 35 | src/rendering/scene_object.cpp 36 | ) 37 | target_link_libraries(quad_planner 38 | ${OMPL_LIBRARIES} 39 | ${OCTOMAP_LIBRARIES} 40 | ${OPENGL_LIBRARIES} 41 | ${GLEW_LIBRARIES} 42 | ${GLFW_LIBRARIES} 43 | ${GLM_LIBRARIES} 44 | ${Boost_LIBRARIES} 45 | ) 46 | -------------------------------------------------------------------------------- /quad_planner/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Benjamin Hepp. 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 | -------------------------------------------------------------------------------- /quad_planner/cmake/Modules/Findglm.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Find GLM 3 | # 4 | # Try to find GLM : OpenGL Mathematics. 5 | # This module defines 6 | # - GLM_INCLUDE_DIRS 7 | # - GLM_FOUND 8 | # 9 | # The following variables can be set as arguments for the module. 10 | # - GLM_ROOT_DIR : Root library directory of GLM 11 | # 12 | # References: 13 | # - https://github.com/Groovounet/glm/blob/master/util/FindGLM.cmake 14 | # - https://bitbucket.org/alfonse/gltut/src/28636298c1c0/glm-0.9.0.7/FindGLM.cmake 15 | # 16 | 17 | # Additional modules 18 | include(FindPackageHandleStandardArgs) 19 | 20 | if (WIN32) 21 | # Find include files 22 | find_path( 23 | GLM_INCLUDE_DIR 24 | NAMES glm/glm.hpp 25 | PATHS 26 | $ENV{PROGRAMFILES}/include 27 | ${GLM_ROOT_DIR}/include 28 | DOC "The directory where glm/glm.hpp resides") 29 | else() 30 | # Find include files 31 | find_path( 32 | GLM_INCLUDE_DIR 33 | NAMES glm/glm.hpp 34 | PATHS 35 | /usr/include 36 | /usr/local/include 37 | /sw/include 38 | /opt/local/include 39 | ${GLM_ROOT_DIR}/include 40 | DOC "The directory where glm/glm.hpp resides") 41 | endif() 42 | 43 | # Handle REQUIRD argument, define *_FOUND variable 44 | find_package_handle_standard_args(GLM DEFAULT_MSG GLM_INCLUDE_DIR) 45 | 46 | # Define GLM_INCLUDE_DIRS 47 | if (GLM_FOUND) 48 | set(GLM_INCLUDE_DIRS ${GLM_INCLUDE_DIR}) 49 | endif() 50 | 51 | # Hide some variables 52 | mark_as_advanced(GLM_INCLUDE_DIR) 53 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/quad_planner.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // quad_planner.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 21, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | 22 | namespace quad_planner 23 | { 24 | 25 | // Local namespace abbreviations 26 | namespace ob = ::ompl::base; 27 | namespace og = ::ompl::geometric; 28 | 29 | class QuadPlanner 30 | { 31 | private: 32 | const double OCCUPANCY_FREE_THRESHOLD = 0.2; 33 | 34 | using StateSpaceT = ob::SE3StateSpace; 35 | // using StateSpaceT = ob::RealVectorStateSpace; 36 | using PlannerT = OptimizingRRT; 37 | // using PlannerT = og::RRT; 38 | 39 | std::shared_ptr state_space_; 40 | std::shared_ptr space_info_; 41 | std::shared_ptr planner_; 42 | std::shared_ptr octomap_ptr_; 43 | 44 | 45 | public: 46 | QuadPlanner(double octomap_resolution); 47 | virtual ~QuadPlanner(); 48 | 49 | std::shared_ptr getOctomap() const; 50 | void loadOctomapFile(const std::string &filename); 51 | 52 | struct MotionValidator : public ob::MotionValidator 53 | { 54 | MotionValidator(const QuadPlanner* parent, ob::SpaceInformation* si); 55 | 56 | bool checkMotion(const ob::State* state1, const ob::State* state2, 57 | std::pair& lastValid) const; 58 | bool checkMotion(const ob::State* state1, const ob::State* state2) const; 59 | 60 | const QuadPlanner* planner_; 61 | }; 62 | 63 | bool isCoordinateValid(double x, double y, double z) const; 64 | bool isStateValid(const ob::State* state) const; 65 | 66 | std::shared_ptr run(); 67 | }; 68 | 69 | } 70 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/color.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // glm.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 21, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace quad_planner 14 | { 15 | namespace rendering 16 | { 17 | 18 | class ColorGlm 19 | { 20 | public: 21 | static glm::vec3 red() 22 | { 23 | return glm::vec3(1.0f, 0.0f, 0.0f); 24 | } 25 | static glm::vec3 green() 26 | { 27 | return glm::vec3(0.0f, 1.0f, 0.0f); 28 | } 29 | static glm::vec3 blue() 30 | { 31 | return glm::vec3(0.0f, 0.0f, 1.0f); 32 | } 33 | static glm::vec3 yellow() 34 | { 35 | return glm::vec3(1.0f, 1.0f, 0.0f); 36 | } 37 | static glm::vec3 magenta() 38 | { 39 | return glm::vec3(1.0f, 0.0f, 1.0f); 40 | } 41 | static glm::vec3 cyan() 42 | { 43 | return glm::vec3(0.0f, 1.0f, 1.0f); 44 | } 45 | static glm::vec3 orange() 46 | { 47 | return glm::vec3(1.0f, 0.5f, 0.0f); 48 | } 49 | static glm::vec3 violet() 50 | { 51 | return glm::vec3(0.5f, 0.0f, 1.0f); 52 | } 53 | static glm::vec3 grey() 54 | { 55 | return glm::vec3(0.5f, 0.5f, 0.5f); 56 | } 57 | static glm::vec3 white() 58 | { 59 | return glm::vec3(1.0f, 1.0f, 1.0f); 60 | } 61 | static glm::vec3 black() 62 | { 63 | return glm::vec3(0.0f, 0.0f, 0.0f); 64 | } 65 | }; 66 | 67 | class ColorEigen 68 | { 69 | public: 70 | static Eigen::Vector3d red() 71 | { 72 | return Eigen::Vector3d(1.0f, 0.0f, 0.0f); 73 | } 74 | static Eigen::Vector3d green() 75 | { 76 | return Eigen::Vector3d(0.0f, 1.0f, 0.0f); 77 | } 78 | static Eigen::Vector3d blue() 79 | { 80 | return Eigen::Vector3d(0.0f, 0.0f, 1.0f); 81 | } 82 | static Eigen::Vector3d yellow() 83 | { 84 | return Eigen::Vector3d(1.0f, 1.0f, 0.0f); 85 | } 86 | static Eigen::Vector3d magenta() 87 | { 88 | return Eigen::Vector3d(1.0f, 0.0f, 1.0f); 89 | } 90 | static Eigen::Vector3d cyan() 91 | { 92 | return Eigen::Vector3d(0.0f, 1.0f, 1.0f); 93 | } 94 | static Eigen::Vector3d orange() 95 | { 96 | return Eigen::Vector3d(1.0f, 0.5f, 0.0f); 97 | } 98 | static Eigen::Vector3d violet() 99 | { 100 | return Eigen::Vector3d(0.5f, 0.0f, 1.0f); 101 | } 102 | static Eigen::Vector3d grey() 103 | { 104 | return Eigen::Vector3d(0.5f, 0.5f, 0.5f); 105 | } 106 | static Eigen::Vector3d white() 107 | { 108 | return Eigen::Vector3d(1.0f, 1.0f, 1.0f); 109 | } 110 | static Eigen::Vector3d black() 111 | { 112 | return Eigen::Vector3d(0.0f, 0.0f, 0.0f); 113 | } 114 | }; 115 | 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/glm.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // color.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 21, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #define GLM_FORCE_RADIANS 12 | #include 13 | #include 14 | #include 15 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/linalg.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // linalg.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 21, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/render_object.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // render_object.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 23, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | namespace quad_planner 12 | { 13 | namespace rendering 14 | { 15 | 16 | class RenderObject 17 | { 18 | public: 19 | RenderObject() 20 | { 21 | } 22 | 23 | virtual ~RenderObject() 24 | { 25 | } 26 | 27 | virtual void render() const = 0; 28 | }; 29 | 30 | } /* namespace rendering */ 31 | } /* namespace quad_planner */ 32 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/scene_object.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // scene_object.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 22, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace quad_planner 17 | { 18 | namespace rendering 19 | { 20 | 21 | class SceneObject 22 | { 23 | std::shared_ptr obj_ptr_; 24 | std::shared_ptr shader_ptr_; 25 | glm::mat4 model_trans_; 26 | bool visible_; 27 | 28 | public: 29 | explicit SceneObject(); 30 | explicit SceneObject(const std::shared_ptr& obj_ptr, const std::shared_ptr& shader_ptr); 31 | virtual ~SceneObject(); 32 | 33 | std::shared_ptr getRenderObject(); 34 | std::shared_ptr getRenderObject() const; 35 | void setRenderObject(const std::shared_ptr& obj_ptr); 36 | void setShaderProgram(const std::shared_ptr& shader_ptr); 37 | bool isVisible() const; 38 | void setVisible(bool visible=true); 39 | 40 | void updateViewportSize(float width, float height); 41 | const glm::mat4& getTransformation() const; 42 | glm::mat4& getTransformation(); 43 | void setTransformation(const glm::mat4 &trans); 44 | 45 | void render(const Eigen::MatrixXf& view, const Eigen::MatrixXf& projection, float width, float height) const; 46 | void render(const Eigen::MatrixXd& view, const Eigen::MatrixXd& projection, float width, float height) const; 47 | void render(const glm::mat4& view, const glm::mat4& projection, float width, float height) const; 48 | }; 49 | 50 | } /* namespace rendering */ 51 | } /* namespace quad_planner */ 52 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/shader_program.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // shader_program.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 19, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace quad_planner 18 | { 19 | namespace rendering 20 | { 21 | 22 | class ShaderProgram 23 | { 24 | GLuint program_id_; 25 | std::vector shader_ids_; 26 | std::map uniform_locations_; 27 | 28 | void init(); 29 | void clear(); 30 | void ensureInitialized(); 31 | 32 | public: 33 | enum ShaderType 34 | { 35 | VERTEX = GL_VERTEX_SHADER, 36 | FRAGMENT = GL_FRAGMENT_SHADER, 37 | GEOMETRY = GL_GEOMETRY_SHADER, 38 | }; 39 | 40 | static std::string getShaderTypeString(ShaderType type) { 41 | switch (type) { 42 | case VERTEX: 43 | return "Vertex"; 44 | case FRAGMENT: 45 | return "Fragment"; 46 | case GEOMETRY: 47 | return "Geometry"; 48 | default: 49 | return "Unknown"; 50 | } 51 | } 52 | 53 | ShaderProgram(); 54 | virtual ~ShaderProgram(); 55 | 56 | void compileShader(const std::string &source, ShaderType type); 57 | void compileShaderFromStream(std::istream &input, ShaderType type); 58 | void compileShaderFromFile(const std::string &filename, ShaderType type); 59 | void link(); 60 | 61 | void use(); 62 | bool hasUniformLocation(const std::string &name); 63 | GLint getUniformLocation(const std::string &name, bool fail_if_not_found = true); 64 | void setUniform(const std::string &name, float value); 65 | void setUniform(const std::string &name, const glm::mat4 &matrix); 66 | void setUniform(const std::string &name, const Eigen::MatrixXd &matrix); 67 | void setUniform(const std::string &name, const Eigen::MatrixXf &matrix); 68 | }; 69 | 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /quad_planner/include/quad_planner/rendering/utilities.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // utilities.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 22, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace quad_planner 15 | { 16 | namespace rendering 17 | { 18 | 19 | class Utilities 20 | { 21 | public: 22 | Utilities() = delete; 23 | Utilities(const Utilities &) = delete; 24 | Utilities& operator=(const Utilities&) = delete; 25 | // virtual ~Utilities(); 26 | 27 | static Eigen::Vector3d glmToEigen(const glm::vec3 &vec); 28 | static glm::vec3 eigenToGlm(const Eigen::Vector3d &vec); 29 | 30 | static Eigen::MatrixXd glmToEigenMatrixXd(const glm::mat4 &mat); 31 | static Eigen::MatrixXf glmToEigenMatrixXf(const glm::mat4 &mat); 32 | static glm::mat4 eigenToGlm(const Eigen::MatrixXd &mat); 33 | static glm::mat4 eigenToGlm(const Eigen::MatrixXf &mat); 34 | static Eigen::Matrix glmToEigenMatrix44d(const glm::mat4 &mat); 35 | static Eigen::Matrix glmToEigenMatrix44f(const glm::mat4 &mat); 36 | static glm::mat4 eigenToGlm(const Eigen::Matrix &mat); 37 | static glm::mat4 eigenToGlm(const Eigen::Matrix &mat); 38 | 39 | template 40 | static void convertVectorToMatrix(const std::vector> &vector, 41 | Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> *matrix_ptr) 42 | { 43 | *matrix_ptr = Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(_Rows, vector.size()); 44 | for (int i = 0; i < vector.size(); ++i) 45 | { 46 | matrix_ptr->col(i) = vector[i]; 47 | } 48 | } 49 | 50 | template 51 | static void convertMatrixToVector(const Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> &matrix, 52 | std::vector> *vector_ptr) 53 | { 54 | *vector_ptr = std::vector>(matrix.cols()); 55 | for (int i = 0; i < matrix.cols(); ++i) 56 | { 57 | (*vector_ptr)[i] = matrix.col(i); 58 | } 59 | } 60 | }; 61 | 62 | } /* namespace rendering */ 63 | } /* namespace quad_planner */ 64 | -------------------------------------------------------------------------------- /quad_planner/octomap_trajectory.txt: -------------------------------------------------------------------------------- 1 | 0 0 0.15 0 0 0 1 2 | 2.2926 -2.08338 -3.71248 0.0473954 -0.00705948 0.00864811 0.998814 3 | 1.09137 -6.23137 -1.24333 0.022569 -0.00336163 0.00411811 0.999731 4 | 1.02526 -10.0875 1.81306 0.00941054 -0.0195388 0.0802605 0.996538 5 | 0 -10 1 0 0 0 1 6 | 7 | -------------------------------------------------------------------------------- /quad_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | quad_planner 4 | 0.0.0 5 | The quad_planner package 6 | 7 | 8 | 9 | 10 | bhepp 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | ompl 44 | roscpp 45 | tf 46 | cmake_modules 47 | ompl 48 | roscpp 49 | tf 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /quad_planner/scripts/visualize_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import numpy as np 5 | 6 | from mpl_toolkits.mplot3d import Axes3D 7 | import matplotlib.pyplot as plt 8 | 9 | 10 | filename = sys.argv[1] 11 | 12 | data = np.loadtxt(filename) 13 | fig = plt.figure() 14 | ax = fig.gca(projection='3d') 15 | cmap = plt.get_cmap('jet') 16 | plt.hold('on') 17 | for i in xrange(1, data.shape[0]): 18 | cmap_index = float(i) / data.shape[0] 19 | if i == 1: 20 | markersize = 16 21 | else: 22 | markersize = 6 23 | ax.plot(data[(i-1):(i+1),0], data[(i-1):(i+1),1], data[(i-1):(i+1),2], '.-', color=cmap(cmap_index), markersize=markersize) 24 | plt.hold('off') 25 | plt.show() 26 | -------------------------------------------------------------------------------- /quad_planner/src/octomap_trajectory.txt: -------------------------------------------------------------------------------- 1 | 0 0 0.15 0 0 0 1 2 | -3.18161 -2.936 2.55165 -0.0264461 0.0126309 0.039471 0.998791 3 | -2.58835 -6.90207 -0.317889 -0.0932384 0.00896351 0.0551726 0.994074 4 | 0.473869 -10.4249 -1.81823 -0.0907322 -0.00759171 -0.0403798 0.995027 5 | 0 -10 1 0 0 0 1 6 | 7 | -------------------------------------------------------------------------------- /quad_planner/src/quad_planner_app.cpp: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // quad_planner_app.cpp 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 21, 2016 7 | //================================================== 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | namespace quad_planner 16 | { 17 | 18 | class QuadPlannerApp 19 | { 20 | private: 21 | QuadPlanner *quad_planner_ptr_; 22 | rendering::Visualizer window; 23 | 24 | public: 25 | QuadPlannerApp(const std::string &octomap_filename, double octomap_resolution) 26 | { 27 | quad_planner_ptr_ = new QuadPlanner(octomap_resolution); 28 | if (!octomap_filename.empty()) 29 | { 30 | quad_planner_ptr_->loadOctomapFile(octomap_filename); 31 | } 32 | 33 | window.init(); 34 | window.getOctomapRenderer()->setOctomap(quad_planner_ptr_->getOctomap()); 35 | } 36 | 37 | ~QuadPlannerApp() 38 | { 39 | delete quad_planner_ptr_; 40 | } 41 | 42 | const QuadPlanner& getQuadPlanner() const 43 | { 44 | return *quad_planner_ptr_; 45 | } 46 | 47 | QuadPlanner& getQuadPlanner() 48 | { 49 | return *quad_planner_ptr_; 50 | } 51 | 52 | void run() 53 | { 54 | std::shared_ptr pdef = quad_planner_ptr_->run(); 55 | window.run(pdef); 56 | } 57 | }; 58 | 59 | } 60 | 61 | int main(int argc, char **argv) 62 | { 63 | namespace po = boost::program_options; 64 | try 65 | { 66 | std::string octomap_filename; 67 | double octomap_resolution; 68 | po::options_description desc("Allowed options"); 69 | desc.add_options() 70 | ("help", "Produce help message") 71 | ("octomap", po::value(&octomap_filename)->default_value("/home/bhepp/Projects/Quad3DR/gazebo_octomap.bt"), "Filename of octomap.") 72 | ("resolution", po::value(&octomap_resolution)->default_value(0.1), "Resolution of octomap.") 73 | ; 74 | 75 | po::variables_map vm; 76 | po::store(po::command_line_parser(argc, argv).options(desc).run(), vm); 77 | if (vm.count("help")) 78 | { 79 | std::cout << desc << std::endl; 80 | return 0; 81 | } 82 | 83 | po::notify(vm); 84 | 85 | quad_planner::QuadPlannerApp app(octomap_filename, octomap_resolution); 86 | app.run(); 87 | 88 | } 89 | catch (const po::required_option &err) 90 | { 91 | std::cerr << "Error parsing command line: Required option '" << err.get_option_name() << "' is missing" << std::endl; 92 | return 1; 93 | } 94 | catch (const po::error &err) 95 | { 96 | std::cerr << "Error parsing command line: " << err.what() << std::endl; 97 | return 2; 98 | } 99 | 100 | return 0; 101 | } 102 | 103 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/octomap_shader.vertex: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | layout(location = 0) in vec3 vertex_position_modelspace; 5 | layout(location = 1) in float voxel_size; 6 | 7 | // Uniform values 8 | uniform mat4 MVP; 9 | uniform mat4 M; 10 | uniform mat4 V; 11 | uniform float viewportWidth; 12 | uniform float viewportHeight; 13 | //uniform vec3 light_position_worldspace; 14 | 15 | // Outputs 16 | out vec4 fragment_color_vs; 17 | out vec3 position_modelspace_vs; 18 | out float voxel_size_vs; 19 | out vec3 light_position_worldspace_vs; 20 | 21 | void main() 22 | { 23 | voxel_size_vs = voxel_size; 24 | 25 | light_position_worldspace_vs = vec3(5, 5, 10); 26 | 27 | position_modelspace_vs = vertex_position_modelspace; 28 | 29 | gl_Position = MVP * vec4(vertex_position_modelspace, 1); 30 | 31 | // Position of the vertex, in worldspace : M * position 32 | vec3 position_worldspace = (M * vec4(vertex_position_modelspace, 1)).xyz; 33 | 34 | // Vector that goes from the vertex to the camera, in camera space. 35 | // In camera space, the camera is at the origin (0,0,0). 36 | //vec3 vertexPosition_cameraspace = (V * M * vec4(vertexPosition_modelspace,1)).xyz; 37 | //eyeDirection_cameraspace_vs = vec3(0,0,0) - vertexPosition_cameraspace; 38 | 39 | // Vector that goes from the vertex to the light, in camera space. M is ommited because it's identity. 40 | //vec3 lightPosition_cameraspace = (V * vec4(lightPosition_worldspace, 1)).xyz; 41 | //lightDirection_cameraspace_vs = lightPosition_cameraspace + eyeDirection_cameraspace_vs; 42 | 43 | // Normal of the vertex, in camera space 44 | //normal_cameraspace_vs = ( V * M * vec4(vertexNormal_modelspace,0)).xyz; // Only correct if ModelMatrix does not scale the model ! Use its inverse transpose if not. 45 | 46 | float color_level = 0.2f * position_worldspace[2]; 47 | float green = color_level < 0.5f ? 2 * color_level : 2 - 2 * color_level; 48 | fragment_color_vs = vec4(clamp(1.0f - color_level, 0, 1), green, clamp(color_level, 0, 1), 1.0f); 49 | } 50 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/phong_shader.vertex: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | layout(location = 0) in vec3 vertexPosition_modelspace; 5 | layout(location = 1) in vec3 vertexNormal_modelspace; 6 | layout(location = 2) in vec3 vertexColor; 7 | 8 | // Uniform values 9 | uniform mat4 MVP; 10 | uniform mat4 M; 11 | uniform mat4 V; 12 | uniform float viewportWidth; 13 | uniform float viewportHeight; 14 | //uniform vec3 lightPosition_worldspace; 15 | 16 | // Outputs 17 | out vec4 fragmentColor; 18 | out vec3 position_modelspace; 19 | out vec3 position_worldspace; 20 | out vec3 normal_cameraspace; 21 | out vec3 eyeDirection_cameraspace; 22 | out vec3 lightDirection_cameraspace; 23 | 24 | void main() 25 | { 26 | vec3 lightPosition_worldspace = vec3(5, 5, 10); 27 | 28 | position_modelspace = vertexPosition_modelspace; 29 | 30 | gl_Position = MVP * vec4(vertexPosition_modelspace, 1); 31 | 32 | // Position of the vertex, in worldspace : M * position 33 | position_worldspace = (M * vec4(vertexPosition_modelspace, 1)).xyz; 34 | 35 | // Vector that goes from the vertex to the camera, in camera space. 36 | // In camera space, the camera is at the origin (0,0,0). 37 | vec3 vertexPosition_cameraspace = (V * M * vec4(vertexPosition_modelspace,1)).xyz; 38 | eyeDirection_cameraspace = vec3(0,0,0) - vertexPosition_cameraspace; 39 | 40 | // Vector that goes from the vertex to the light, in camera space. M is ommited because it's identity. 41 | vec3 lightPosition_cameraspace = (V * vec4(lightPosition_worldspace, 1)).xyz; 42 | lightDirection_cameraspace = lightPosition_cameraspace + eyeDirection_cameraspace; 43 | 44 | // Normal of the vertex, in camera space 45 | normal_cameraspace = ( V * M * vec4(vertexNormal_modelspace,0)).xyz; // Only correct if ModelMatrix does not scale the model ! Use its inverse transpose if not. 46 | 47 | fragmentColor = vec4(vertexColor, 1.0f); 48 | } 49 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/simple_fragment_shader.fragmentshader: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | in vec4 fragmentColor; 4 | 5 | // Ouput data 6 | out vec4 color; 7 | 8 | void main() 9 | { 10 | 11 | // Output color = red 12 | color = fragmentColor; 13 | } 14 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/simple_vertex_shader.vertexshader: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | layout(location = 0) in vec3 vertexPosition_modelspace; 5 | layout(location = 1) in vec3 vertexColor; 6 | 7 | // Uniform values 8 | uniform mat4 mvp; 9 | 10 | out vec3 fragmentColor; 11 | 12 | void main(){ 13 | 14 | gl_Position = mvp * vec4(vertexPosition_modelspace, 1); 15 | fragmentColor = vertexColor; 16 | //fragmentColor = vec3(0.5f, 0.2f * vertexPosition_modelspace[2], 0.5f); 17 | } 18 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/trajectory_shader.fragment: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | 5 | // Inputs 6 | in vec4 line_vertex1; 7 | in vec4 line_normal; 8 | in vec4 line_direction; 9 | in vec3 fragmentColor; 10 | 11 | // Uniform values 12 | uniform mat4 MVP; 13 | uniform mat4 M; 14 | uniform mat4 V; 15 | uniform float viewportWidth; 16 | uniform float viewportHeight; 17 | uniform float lineWidth; 18 | 19 | // Ouput data 20 | out vec4 color; 21 | 22 | vec4 vec3_to_homogeneous(vec3 vec, float w) 23 | { 24 | vec4 vec_3d; 25 | vec_3d.xy = vec.xy; 26 | vec_3d.z = 0; 27 | vec_3d.w = 1; 28 | vec_3d *= w; 29 | return vec_3d; 30 | } 31 | 32 | vec3 vec3_from_homogeneous(vec4 vec) 33 | { 34 | vec3 vec_2d = vec.xyz; 35 | vec_2d.z = 0; 36 | vec_2d /= vec.w; 37 | return vec_2d; 38 | } 39 | 40 | vec3 convertDeviceCoordToViewCoord(vec4 fragCoord) 41 | { 42 | vec3 viewCoord = fragCoord.xyz; 43 | viewCoord.z = 0; 44 | viewCoord.x /= viewportWidth; 45 | viewCoord.y /= viewportHeight; 46 | viewCoord = viewCoord * 2.0 - 1.0; 47 | return viewCoord; 48 | } 49 | 50 | void main() 51 | { 52 | vec3 line_vertexTemp_vec3 = convertDeviceCoordToViewCoord(gl_FragCoord); 53 | vec3 line_vertex1_vec3 = line_vertex1.xyz / line_vertex1.w; 54 | vec3 dvertex = line_vertexTemp_vec3 - line_vertex1_vec3; 55 | 56 | vec3 line_normal_vec3 = line_normal.xyz; 57 | vec3 line_direction_vec3 = normalize(line_direction.xyz); 58 | float dist = abs(dot(dvertex, line_normal_vec3) / lineWidth); 59 | float weight = 1.0 - dist; 60 | weight = sqrt(1.0 - dist); 61 | 62 | color.xyz = fragmentColor; 63 | color.w = clamp(weight, 0, 1); 64 | } 65 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/trajectory_shader.vertex: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Inputs 4 | layout(location = 0) in vec3 vertexPosition_modelspace; 5 | layout(location = 1) in vec3 vertexColor; 6 | 7 | // Uniform values 8 | uniform mat4 MVP; 9 | uniform mat4 M; 10 | uniform mat4 V; 11 | uniform float viewportWidth; 12 | uniform float viewportHeight; 13 | uniform float lineWidth; 14 | 15 | // Outputs 16 | out vec3 fragmentColorGeom; 17 | 18 | void main() 19 | { 20 | gl_Position = MVP * vec4(vertexPosition_modelspace, 1); 21 | 22 | fragmentColorGeom = vertexColor; 23 | } 24 | -------------------------------------------------------------------------------- /quad_planner/src/shaders/vertex.shader: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | layout(location = 0) in vec3 vertexPosition_modelspace; 5 | layout(location = 1) in vec3 vertexNormal_modelspace; 6 | layout(location = 2) in vec3 vertexColor; 7 | 8 | // Uniform values 9 | uniform float lineWidth; 10 | uniform float viewportWidth; 11 | uniform float viewportHeight; 12 | uniform mat4 mvp; 13 | 14 | out vec4 fragmentColor; 15 | 16 | void main(){ 17 | 18 | gl_Position = mvp * vec4(vertexPosition_modelspace, 1); 19 | fragmentColor = vec4(0.5f, 0.2f * vertexPosition_modelspace[2], 0.5f, 1.0f); 20 | } 21 | -------------------------------------------------------------------------------- /quad_planner/voxel_coordinate_system.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/quad_planner/voxel_coordinate_system.jpeg -------------------------------------------------------------------------------- /src/mLib.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "mLibCore.cpp" 4 | #include "mLibDepthCamera.cpp" 5 | //#include "mLibLodePNG.cpp" 6 | //#include "mlibD3D11.cpp" 7 | -------------------------------------------------------------------------------- /src/mLibSource.cpp: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // mLibSource.cpp 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 14, 2016 7 | //================================================== 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace ml; 16 | -------------------------------------------------------------------------------- /src/mLibZlib.cpp: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // mLibZlib.cpp 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 4, 2016 7 | //================================================== 8 | 9 | 10 | #include 11 | 12 | #include "mLibZLib.cpp" 13 | -------------------------------------------------------------------------------- /stereo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( 2 | include/ 3 | ) 4 | 5 | add_executable(sparse_stereo_test 6 | src/sparse_stereo_test.cpp 7 | ../src/utilities.cpp 8 | src/stereo_calibration.cpp 9 | ) 10 | target_link_libraries(sparse_stereo_test 11 | ${OpenCV_LIBRARIES} 12 | ) 13 | 14 | if("${WITH_ZED}") 15 | add_executable(sparse_stereo_zed 16 | src/sparse_stereo_zed.cpp 17 | ../src/utilities.cpp 18 | src/stereo_calibration.cpp 19 | ../video/src/video_source.cpp 20 | ../video/src/video_source_zed.cpp 21 | ../src/mLibSource.cpp 22 | ) 23 | set_property(TARGET sparse_stereo_zed PROPERTY CXX_STANDARD 11) 24 | target_link_libraries(sparse_stereo_zed 25 | ${OpenCV_LIBRARIES} 26 | ${ZED_LIBRARIES} 27 | ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_npp_LIBRARY} 28 | ) 29 | 30 | add_executable(stereo_calibration_tool_zed 31 | src/stereo_calibration_tool_zed.cpp 32 | ../src/utilities.cpp 33 | src/stereo_calibration.cpp 34 | ../video/src/video_source.cpp 35 | ../video/src/video_source_zed.cpp 36 | ) 37 | set_property(TARGET stereo_calibration_tool_zed PROPERTY CXX_STANDARD 11) 38 | target_link_libraries(stereo_calibration_tool_zed 39 | ${OpenCV_LIBRARIES} 40 | ${ZED_LIBRARIES} 41 | ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_npp_LIBRARY} 42 | ) 43 | 44 | add_executable(stereo_capture_zed 45 | src/stereo_capture_zed.cpp 46 | ../src/utilities.cpp 47 | src/stereo_calibration.cpp 48 | ../video/src/video_source.cpp 49 | ../video/src/video_source_zed.cpp 50 | ) 51 | set_property(TARGET stereo_capture_zed PROPERTY CXX_STANDARD 11) 52 | target_link_libraries(stereo_capture_zed 53 | ${OpenCV_LIBRARIES} 54 | ${ZED_LIBRARIES} 55 | ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_npp_LIBRARY} 56 | ) 57 | endif() 58 | -------------------------------------------------------------------------------- /stereo/cmake.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rm -rf CMakeCache.txt CMakeFiles 4 | 5 | cmake -G"Eclipse CDT4 - Unix Makefiles" -DUSE_OPENCV_VERSION=3.1 -DCMAKE_BUILD_TYPE=Debug ../../stereo/ $@ 6 | #cmake -G"Eclipse CDT4 - Unix Makefiles" -DUSE_OPENCV_VERSION=3.1 -DCMAKE_BUILD_TYPE=RelWithDebInfo ../../stereo/ $@ 7 | #cmake -G"Eclipse CDT4 - Unix Makefiles" -DUSE_OPENCV_VERSION=3.1 -DCMAKE_BUILD_TYPE=Debug ../../stereo/ -DWITH_ZED=False $@ 8 | #cmake -G"Eclipse CDT4 - Unix Makefiles" -DUSE_OPENCV_VERSION=3.1 -DCMAKE_BUILD_TYPE=RelWithDebInfo ../../stereo/ -DWITH_ZED=False $@ 9 | 10 | #cmake -G"Eclipse CDT4 - Unix Makefiles" -DUSE_OPENCV_VERSION=2.4 -DCMAKE_BUILD_TYPE=Debug ../../stereo/ $@ 11 | #cmake -G"Eclipse CDT4 - Unix Makefiles" -DUSE_OPENCV_VERSION=2.4 -DCMAKE_BUILD_TYPE=RelWithDebInfo ../../stereo/ $@ 12 | 13 | -------------------------------------------------------------------------------- /stereo/include/ait/stereo/stereo_calibration.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // stereo_calibration.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Sep 6, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #if WITH_ZED 15 | #include 16 | #endif 17 | 18 | namespace ait 19 | { 20 | namespace stereo 21 | { 22 | 23 | struct CameraCalibration 24 | { 25 | // Matrices as returned from cv::calibrateCamera 26 | cv::Mat camera_matrix; 27 | cv::Mat dist_coeffs; 28 | 29 | 30 | CameraCalibration(); 31 | virtual ~CameraCalibration(); 32 | 33 | Eigen::Matrix3d getCameraMatrixEigen() const; 34 | Eigen::VectorXd getDistCoeffEigen() const; 35 | }; 36 | 37 | struct StereoCameraCalibration 38 | { 39 | #if WITH_ZED 40 | static CameraCalibration getCameraCalibrationFromZED(const sl::zed::CamParameters& params); 41 | static StereoCameraCalibration getStereoCalibrationFromZED(sl::zed::Camera* zed); 42 | #endif 43 | static StereoCameraCalibration getStereoCalibrationFromOpenCV(const std::string &filename); 44 | static StereoCameraCalibration readStereoCalibration(const std::string &filename); 45 | 46 | StereoCameraCalibration(); 47 | virtual ~StereoCameraCalibration(); 48 | 49 | Eigen::Matrix3d getRotationEigen() const; 50 | Eigen::Vector3d getTranslationEigen() const; 51 | 52 | Eigen::Matrix4d getLeftExtrinsicsEigen() const; 53 | Eigen::Matrix4d getRightExtrinsicsEigen() const; 54 | 55 | void computeProjectionMatrices(); 56 | 57 | CameraCalibration left; 58 | CameraCalibration right; 59 | 60 | cv::Size image_size; 61 | 62 | // Matrices as returned from cv::stereoCalibrate 63 | cv::Mat rotation; 64 | cv::Mat translation; 65 | cv::Mat essential_matrix; 66 | cv::Mat fundamental_matrix; 67 | 68 | // Matrices as returned from cv::stereoRectify 69 | // 70 | // 3x4 Projection matrices 71 | cv::Mat projection_matrix_left; 72 | cv::Mat projection_matrix_right; 73 | // 74 | // 3x3 Rectification matrices 75 | cv::Mat rectification_transform_left; 76 | cv::Mat rectification_transform_right; 77 | // 78 | // 4x4 Disparity to depth mapping (OpenCV) 79 | cv::Mat disparity_to_depth_map; 80 | }; 81 | 82 | } // namespace stereo 83 | } // namespace ait 84 | 85 | -------------------------------------------------------------------------------- /video/cmake/CheckC11.cmake: -------------------------------------------------------------------------------- 1 | # Check for C++11/0x support 2 | 3 | include(CheckCXXCompilerFlag) 4 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 5 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 6 | if(COMPILER_SUPPORTS_CXX11) 7 | set(CMAKE_CXX_FLAGS "-std=c++11") 8 | elseif(COMPILER_SUPPORTS_CXX0X) 9 | set(CMAKE_CXX_FLAGS "-std=c++0x") 10 | else() 11 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 12 | endif() 13 | 14 | -------------------------------------------------------------------------------- /video/cmake/Modules/FindTCLAP.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find TCLAP lib 2 | # 3 | # TCLAP_FOUND - system has TCLAP lib 4 | # TCLAP_INCLUDE_DIR - the TCLAP include directory 5 | 6 | # Copyright (c) 2015, Benjamin Hepp 7 | 8 | macro(_tclap_check_path) 9 | 10 | if(EXISTS "${TCLAP_INCLUDE_DIR}/tclap/CmdLine.h") 11 | set(TCLAP_OK TRUE) 12 | endif() 13 | 14 | if(NOT TCLAP_OK) 15 | message(STATUS "TCLAP include path was specified but no CmdLine.h file was found: ${TCLAP_INCLUDE_DIR}") 16 | endif() 17 | 18 | endmacro() 19 | 20 | if(NOT TCLAP_INCLUDE_DIR) 21 | 22 | find_path(TCLAP_INCLUDE_DIR NAMES tclap/CmdLine.h 23 | PATHS 24 | ${CMAKE_INSTALL_PREFIX}/include 25 | ${KDE4_INCLUDE_DIR} 26 | #PATH_SUFFIXES tclap 27 | ) 28 | 29 | endif() 30 | 31 | if(TCLAP_INCLUDE_DIR) 32 | _tclap_check_path() 33 | endif() 34 | 35 | include(FindPackageHandleStandardArgs) 36 | find_package_handle_standard_args(TCLAP DEFAULT_MSG TCLAP_INCLUDE_DIR TCLAP_OK) 37 | 38 | mark_as_advanced(TCLAP_INCLUDE_DIR) 39 | -------------------------------------------------------------------------------- /video/include/ait/video/GstMetaCorrespondence.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // GstMetaCorrespondence.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Nov 7, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | G_BEGIN_DECLS 14 | 15 | /** 16 | * GstCorrespondenceMeta: 17 | * @meta: parent #GstMeta 18 | * @id: Id to establish correspondence between appsrc and appsink buffers 19 | */ 20 | typedef struct { 21 | GstMeta meta; 22 | 23 | gint id; 24 | } GstCorrespondenceMeta; 25 | 26 | GType gst_correspondence_meta_api_get_type(); 27 | #define GST_CORRESPONDENCE_META_API_TYPE (gst_correspondence_meta_api_get_type()) 28 | const GstMetaInfo *gst_correspondence_meta_get_info(); 29 | #define GST_CORRESPONDENCE_META_INFO (gst_correspondence_meta_get_info()) 30 | 31 | #define gst_buffer_get_correspondence_meta(b) \ 32 | ((GstCorrespondenceMeta*)gst_buffer_get_meta((b), GST_CORRESPONDENCE_META_API_TYPE)) 33 | GstCorrespondenceMeta *gst_buffer_add_correspondence_meta(GstBuffer* buffer, gint id); 34 | 35 | bool gst_buffer_correspondence_meta_has(GstBuffer* buffer); 36 | gint gst_buffer_correspondence_meta_get_id(GstBuffer* buffer); 37 | void gst_buffer_correspondence_meta_set_id(GstBuffer* buffer, gint id); 38 | 39 | G_END_DECLS 40 | -------------------------------------------------------------------------------- /video/include/ait/video/video_source.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // video_source.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 19, 2016 7 | //================================================== 8 | 9 | #include 10 | #include 11 | 12 | #pragma once 13 | 14 | namespace ait 15 | { 16 | namespace video 17 | { 18 | 19 | class VideoSource 20 | { 21 | public: 22 | 23 | class Error : public std::runtime_error 24 | { 25 | public: 26 | Error(const std::string &str); 27 | }; 28 | 29 | virtual ~VideoSource(); 30 | 31 | virtual bool has_depth() const; 32 | virtual bool has_stereo() const; 33 | 34 | virtual int getWidth() const = 0; 35 | virtual int getHeight() const = 0; 36 | 37 | virtual bool grab(bool block=true) = 0; 38 | virtual bool retrieveMono(cv::Mat *mat) = 0; 39 | virtual bool retrieveLeft(cv::Mat *mat); 40 | virtual bool retrieveRight(cv::Mat *mat); 41 | virtual bool retrieveDepth(cv::Mat *mat); 42 | virtual bool retrieveDisparity(cv::Mat *mat); 43 | virtual bool retrieveConfidence(cv::Mat *mat); 44 | }; 45 | 46 | } // namespace video 47 | } // namespace ait 48 | -------------------------------------------------------------------------------- /video/include/ait/video/video_source_opencv.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // video_source_opencv.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 29, 2016 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace ait 14 | { 15 | namespace video 16 | { 17 | 18 | class VideoSourceOpenCV : public VideoSource 19 | { 20 | cv::VideoCapture capture_; 21 | 22 | void ensureOpened() const; 23 | 24 | public: 25 | VideoSourceOpenCV(); 26 | virtual ~VideoSourceOpenCV() override; 27 | 28 | void open(int device); 29 | void open(const std::string &filename); 30 | void close(); 31 | 32 | bool setFrameWidth(int frame_width); 33 | bool setFrameHeight(int frame_height); 34 | double getFPS() const; 35 | bool setFPS(double fps); 36 | 37 | int getWidth() const override; 38 | int getHeight() const override; 39 | 40 | bool grab(bool block=true) override; 41 | bool retrieveMono(cv::Mat *mat) override; 42 | }; 43 | 44 | } // namespace video 45 | } // namespace ait 46 | -------------------------------------------------------------------------------- /video/src/video_source.cpp: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // video_source.cpp 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 19, 2016 7 | //================================================== 8 | 9 | #include 10 | 11 | namespace ait 12 | { 13 | namespace video 14 | { 15 | 16 | VideoSource::Error::Error(const std::string &str) 17 | : std::runtime_error(str) 18 | { 19 | } 20 | 21 | VideoSource::~VideoSource() 22 | { 23 | } 24 | 25 | bool VideoSource::has_depth() const 26 | { 27 | return false; 28 | } 29 | 30 | bool VideoSource::has_stereo() const 31 | { 32 | return false; 33 | } 34 | 35 | bool VideoSource::retrieveLeft(cv::Mat *mat) 36 | { 37 | throw Error("Unable to grab right frame from mono camera"); 38 | } 39 | 40 | bool VideoSource::retrieveRight(cv::Mat *mat) 41 | { 42 | throw Error("Unable to grab right frame from mono camera"); 43 | } 44 | 45 | bool VideoSource::retrieveDepth(cv::Mat *mat) 46 | { 47 | throw Error("Unable to grab depth frame from non-depth camera"); 48 | } 49 | 50 | bool VideoSource::retrieveDisparity(cv::Mat *mat) 51 | { 52 | throw Error("Unable to grab disparity frame from non-depth camera"); 53 | } 54 | 55 | bool VideoSource::retrieveConfidence(cv::Mat *mat) 56 | { 57 | throw Error("Unable to grab confidence frame from non-depth camera"); 58 | } 59 | 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /video/src/video_source_opencv.cpp: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // video_source_opencv.cpp 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Aug 29, 2016 7 | //================================================== 8 | 9 | #include 10 | 11 | namespace ait 12 | { 13 | namespace video 14 | { 15 | 16 | VideoSourceOpenCV::VideoSourceOpenCV() 17 | { 18 | } 19 | 20 | VideoSourceOpenCV::~VideoSourceOpenCV() 21 | { 22 | close(); 23 | } 24 | 25 | void VideoSourceOpenCV::ensureOpened() const 26 | { 27 | if (!capture_.isOpened()) 28 | { 29 | throw VideoSource::Error("Video source has not been initialized"); 30 | } 31 | } 32 | 33 | void VideoSourceOpenCV::open(int device) 34 | { 35 | close(); 36 | if (!capture_.open(device)) 37 | { 38 | throw VideoSource::Error("Failed to open video device"); 39 | } 40 | } 41 | 42 | void VideoSourceOpenCV::open(const std::string &filename) 43 | { 44 | close(); 45 | if (!capture_.open(filename)) 46 | { 47 | throw VideoSource::Error("Failed to open video file"); 48 | } 49 | } 50 | 51 | void VideoSourceOpenCV::close() 52 | { 53 | if (capture_.isOpened()) 54 | { 55 | capture_.release(); 56 | } 57 | } 58 | 59 | bool VideoSourceOpenCV::setFrameWidth(int frame_width) 60 | { 61 | ensureOpened(); 62 | return capture_.set(CV_CAP_PROP_FRAME_WIDTH, frame_width); 63 | } 64 | bool VideoSourceOpenCV::setFrameHeight(int frame_height) 65 | { 66 | ensureOpened(); 67 | return capture_.set(CV_CAP_PROP_FRAME_HEIGHT, frame_height); 68 | } 69 | 70 | double VideoSourceOpenCV::getFPS() const 71 | { 72 | ensureOpened(); 73 | return const_cast(&capture_)->get(CV_CAP_PROP_FPS); 74 | } 75 | 76 | bool VideoSourceOpenCV::setFPS(double fps) 77 | { 78 | ensureOpened(); 79 | return capture_.set(CV_CAP_PROP_FPS, fps); 80 | } 81 | 82 | int VideoSourceOpenCV::getWidth() const 83 | { 84 | ensureOpened(); 85 | // const_cast is required for OpenCV 2.4 86 | return static_cast(const_cast(&capture_)->get(CV_CAP_PROP_FRAME_WIDTH)); 87 | } 88 | 89 | int VideoSourceOpenCV::getHeight() const 90 | { 91 | ensureOpened(); 92 | // const_cast is required for OpenCV 2.4 93 | return static_cast(const_cast(&capture_)->get(CV_CAP_PROP_FRAME_HEIGHT)); 94 | } 95 | 96 | bool VideoSourceOpenCV::grab(bool block) 97 | { 98 | ensureOpened(); 99 | return capture_.grab(); 100 | } 101 | 102 | bool VideoSourceOpenCV::retrieveMono(cv::Mat *mat) 103 | { 104 | ensureOpened(); 105 | return capture_.retrieve(*mat); 106 | } 107 | 108 | } // namespace video 109 | } // namespace ait 110 | -------------------------------------------------------------------------------- /video/tools/make_all_pipeline_graphs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | for dot_file in `ls *.dot`; do 4 | png_file=${dot_file%.dot}.png 5 | dot -Tpng ${dot_file} > ${png_file} 6 | done 7 | 8 | -------------------------------------------------------------------------------- /video/tools/make_pipeline_graph.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | png_file=${dot_file%.dot}.png 4 | dot -Tpng $dot_file > $png_file 5 | 6 | -------------------------------------------------------------------------------- /video/tools/play_file.sh: -------------------------------------------------------------------------------- 1 | gst-launch-1.0 filesrc location=test.bin ! \ 2 | h264parse ! omxh264dec ! \ 3 | videoconvert ! \ 4 | xvimagesink sync=false -e 5 | 6 | -------------------------------------------------------------------------------- /video/tools/record_file.sh: -------------------------------------------------------------------------------- 1 | gst-launch-1.0 videotestsrc num-buffers=1500 ! \ 2 | video/x-raw, format=(string)I420,width=(int)640, height=(int)480 ! \ 3 | omxh264enc ! video/x-h264, stream-format=(string)byte-stream ! \ 4 | filesink location=test.bin -e 5 | 6 | -------------------------------------------------------------------------------- /video/tools/run_with_graph.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | export GST_DEBUG=5 4 | export GST_DEBUG_DUMP_DOT_DIR="." 5 | ./video_streamer $@ 6 | 7 | -------------------------------------------------------------------------------- /viewpoint_planner/src/mLib/mLib.cpp: -------------------------------------------------------------------------------- 1 | #include "mLib.h" 2 | 3 | #include "mLibCore.cpp" 4 | #include "mLibDepthCamera.cpp" 5 | -------------------------------------------------------------------------------- /viewpoint_planner/src/mLib/mLib.h: -------------------------------------------------------------------------------- 1 | /* 2 | * mLib.h 3 | * 4 | * Created on: Dec 26, 2016 5 | * Author: bhepp 6 | */ 7 | 8 | #pragma once 9 | 10 | #if _WIN32 11 | #define NOMINMAX 12 | #include 13 | #include 14 | #include 15 | #endif 16 | 17 | // 18 | // mLib config options 19 | // 20 | 21 | #define MLIB_ERROR_CHECK 22 | #define MLIB_BOUNDS_CHECK 23 | #define MLIB_SOCKETS 24 | 25 | // 26 | // mLib includes 27 | // 28 | 29 | #include "mLibCore.h" 30 | #include "mLibFreeImage.h" 31 | #include "mLibDepthCamera.h" 32 | #include "mLibFLANN.h" 33 | -------------------------------------------------------------------------------- /viewpoint_planner/src/planner/occupied_tree.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // occupied_tree_node.h.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 27.03.17 7 | // 8 | 9 | #pragma once 10 | 11 | #include "viewpoint_planner_types.h" 12 | #include 13 | #include 14 | 15 | namespace viewpoint_planner { 16 | 17 | template 18 | struct NodeObject { 19 | FloatT occupancy; 20 | uint16_t observation_count; 21 | FloatT weight; 22 | Eigen::Matrix normal; 23 | 24 | private: 25 | // Boost serialization 26 | friend class boost::serialization::access; 27 | 28 | template 29 | void serialize(Archive &ar, const unsigned int version) { 30 | ar & occupancy; 31 | ar & observation_count; 32 | ar & weight; 33 | ar & normal; 34 | } 35 | }; 36 | 37 | using NodeObjectType = NodeObject; 38 | using OccupiedTreeType = bvh::Tree; 39 | using VoxelType = OccupiedTreeType::NodeType; 40 | 41 | /// Voxel node and it's corresponding amount of information 42 | struct VoxelWrapper { 43 | VoxelWrapper(const VoxelType* voxel) 44 | : voxel(voxel) {} 45 | 46 | const VoxelType* voxel; 47 | 48 | bool operator==(const VoxelWrapper& other) const { 49 | return voxel == other.voxel; 50 | } 51 | 52 | struct Hash { 53 | size_t operator()(const VoxelWrapper& voxel_with_information) const { 54 | size_t val { 0 }; 55 | boost::hash_combine(val, voxel_with_information.voxel); 56 | return val; 57 | } 58 | }; 59 | }; 60 | 61 | /// Voxel node and it's corresponding amount of information 62 | struct VoxelWithInformation { 63 | VoxelWithInformation(const VoxelType* voxel, const FloatType information) 64 | : voxel(voxel), information(information) {} 65 | 66 | const VoxelType* voxel; 67 | FloatType information; 68 | 69 | bool operator==(const VoxelWithInformation& other) const { 70 | return voxel == other.voxel && information == other.information; 71 | } 72 | 73 | struct VoxelHash { 74 | size_t operator()(const VoxelWithInformation& voxel_with_information) const { 75 | size_t val { 0 }; 76 | boost::hash_combine(val, voxel_with_information.voxel); 77 | return val; 78 | } 79 | }; 80 | 81 | struct Hash { 82 | size_t operator()(const VoxelWithInformation& voxel_with_information) const { 83 | size_t val { 0 }; 84 | boost::hash_combine(val, voxel_with_information.voxel); 85 | boost::hash_combine(val, voxel_with_information.information); 86 | return val; 87 | } 88 | }; 89 | }; 90 | 91 | using VoxelWithInformationSet = std::unordered_set; 92 | using VoxelMap = std::unordered_map; 93 | 94 | } 95 | -------------------------------------------------------------------------------- /viewpoint_planner/src/planner/offscreen_octree_renderer.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // offscreen_octree_renderer.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 19.04.17 7 | //================================================== 8 | #pragma once 9 | 10 | #include "viewpoint_planner_types.h" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "../rendering/voxel_drawer.h" 21 | 22 | namespace viewpoint_planner { 23 | 24 | class OffscreenOctreeRenderer { 25 | public: 26 | using OctreeType = viewpoint_planner::OccupancyMapType; 27 | 28 | struct Options : bh::ConfigOptions { 29 | Options() { 30 | addOption("dump_voxel_image", &dump_voxel_image); 31 | } 32 | 33 | ~Options() override {} 34 | 35 | // Whether to dump the voxel images after rendering 36 | bool dump_voxel_image = false; 37 | }; 38 | 39 | explicit OffscreenOctreeRenderer(const OctreeType* octree); 40 | 41 | explicit OffscreenOctreeRenderer(const Options& options, const OctreeType* octree); 42 | 43 | virtual ~OffscreenOctreeRenderer(); 44 | 45 | void initialize(); 46 | 47 | template 48 | void drawOctree(const QMatrix4x4& pvm_matrix, OpenGLFunctions&& opengl_functions) const; 49 | 50 | const rendering::VoxelDrawer& getVoxelDrawer() const { return voxel_drawer_; } 51 | 52 | rendering::VoxelDrawer& getVoxelDrawer() { return voxel_drawer_; } 53 | 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | 56 | private: 57 | Options options_; 58 | 59 | const OctreeType* octree_; 60 | rendering::VoxelDrawer voxel_drawer_; 61 | }; 62 | 63 | } -------------------------------------------------------------------------------- /viewpoint_planner/src/planner/viewpoint_planner_graph.hxx: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // viewpoint_planner_graph.hxx 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Jan 5, 2017 7 | //================================================== 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | template 14 | bool ViewpointPlanner::findAndAddShortestMotions(const ViewpointEntryIndex from_index, Iterator to_index_first, Iterator to_index_last) { 15 | std::cout << "Searching shortest path from new viewpoint " << from_index 16 | << " to " << (to_index_last - to_index_first) << " other viewpoints" << std::endl; 17 | for (Iterator it = to_index_first; it != to_index_last; ++it) { 18 | const ViewpointEntryIndex to_index = it->viewpoint_index; 19 | // std::cout << " searching path to viewpoint " << to_index << std::endl; 20 | bool found = findAndAddShortestMotion(from_index, to_index); 21 | if (!found) { 22 | return false; 23 | } 24 | } 25 | return true; 26 | } 27 | -------------------------------------------------------------------------------- /viewpoint_planner/src/planner/viewpoint_planner_opengl.cpp: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // viewpoint_planner_opengl.cpp 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Mar 5, 2017 7 | //================================================== 8 | 9 | #include 10 | #include "viewpoint_planner.h" 11 | 12 | std::unique_lock ViewpointPlanner::acquireOpenGLLock() const { 13 | return offscreen_renderer_->acquireOpenGLLock(); 14 | } 15 | 16 | QImage ViewpointPlanner::drawPoissonMesh(const ViewpointEntryIndex viewpoint_index) const { 17 | return offscreen_renderer_->drawPoissonMesh(viewpoint_entries_[viewpoint_index].viewpoint); 18 | } 19 | 20 | QImage ViewpointPlanner::drawPoissonMeshNormals(const ViewpointEntryIndex viewpoint_index) const { 21 | return offscreen_renderer_->drawPoissonMeshNormals(viewpoint_entries_[viewpoint_index].viewpoint); 22 | } 23 | 24 | QImage ViewpointPlanner::drawPoissonMeshDepth(const ViewpointEntryIndex viewpoint_index) const { 25 | return offscreen_renderer_->drawPoissonMeshDepth(viewpoint_entries_[viewpoint_index].viewpoint); 26 | } 27 | 28 | ViewpointPlanner::Vector3 ViewpointPlanner::computePoissonMeshNormalVector( 29 | const Viewpoint& viewpoint, const Vector3& position) const { 30 | return offscreen_renderer_->computePoissonMeshNormalVector(viewpoint, position); 31 | } 32 | 33 | ViewpointPlanner::Vector3 ViewpointPlanner::computePoissonMeshNormalVector( 34 | const Viewpoint& viewpoint, 35 | const std::size_t x, const std::size_t y) const { 36 | return offscreen_renderer_->computePoissonMeshNormalVector(viewpoint, x, y); 37 | } 38 | 39 | ViewpointPlanner::FloatType ViewpointPlanner::computePoissonMeshDepth( 40 | const Viewpoint& viewpoint, const Vector3& position) const { 41 | return offscreen_renderer_->computePoissonMeshDepth(viewpoint, position); 42 | } 43 | 44 | ViewpointPlanner::FloatType ViewpointPlanner::computePoissonMeshDepth( 45 | const Viewpoint& viewpoint, 46 | const std::size_t x, const std::size_t y) const { 47 | return offscreen_renderer_->computePoissonMeshDepth(viewpoint, x, y); 48 | } 49 | -------------------------------------------------------------------------------- /viewpoint_planner/src/planner/viewpoint_planner_types.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // viewpoint_planner_types.h.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: 27.03.17 7 | // 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "../octree/occupancy_map.h" 17 | #include "../bvh/bvh.h" 18 | 19 | namespace viewpoint_planner { 20 | 21 | using FloatType = float; 22 | 23 | USE_FIXED_EIGEN_TYPES(FloatType); 24 | 25 | using RayType = bh::Ray; 26 | using Pose = bh::Pose; 27 | 28 | using PointCloudIOType = ml::PointCloudIO; 29 | using PointCloudType = ml::PointCloud; 30 | using MeshIOType = ml::MeshIO; 31 | using MeshType = ml::MeshData; 32 | using BoundingBoxType = bh::BoundingBox3D; 33 | using Vector3 = Eigen::Vector3f; 34 | using Vector3i = Eigen::Vector3i; 35 | using DistanceFieldType = ml::DistanceField3f; 36 | using RegionType = bh::PolygonWithLowerAndUpperPlane; 37 | 38 | using RawOccupancyMapType = OccupancyMap; 39 | using OccupancyMapType = OccupancyMap; 40 | 41 | using TreeNavigatorType = TreeNavigator; 42 | using ConstTreeNavigatorType = TreeNavigator; 43 | using CounterType = OccupancyMapType::NodeType::CounterType; 44 | using WeightType = OccupancyMapType::NodeType::WeightType; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /viewpoint_planner/src/rendering/sparse_reconstruction_drawer.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // camera_drawer.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 8, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include "../reconstruction/sparse_reconstruction.h" 11 | #include "triangle_drawer.h" 12 | #include "line_drawer.h" 13 | 14 | namespace rendering { 15 | 16 | class SparseReconstructionDrawer { 17 | const float CAMERA_SIZE_SPEED = 0.1f; 18 | const float MIN_CAMERA_SIZE = 0.01f; 19 | const float MAX_CAMERA_SIZE = 10.0f; 20 | const float POINT_SIZE_SPEED = 0.1f; 21 | const float MIN_POINT_SIZE = 0.1f; 22 | const float MAX_POINT_SIZE = 100.0f; 23 | const float CAMERA_LINE_WIDTH = 1.0f; 24 | const double RENDER_MAX_POINT_ERROR = 2.0; 25 | const size_t RENDER_MIN_TRACK_LENGTH = 3; 26 | 27 | using SparseReconstruction = reconstruction::SparseReconstruction; 28 | using PinholeCameraColmap = reconstruction::PinholeCameraColmap; 29 | using ImageColmap = reconstruction::ImageColmap; 30 | 31 | public: 32 | SparseReconstructionDrawer(); 33 | 34 | ~SparseReconstructionDrawer(); 35 | 36 | void setSparseReconstruction(const SparseReconstruction *sparse_recon); 37 | 38 | void resetCameraWidth(); 39 | 40 | void overwriteCameraWidth(const float overwrite_camera_width); 41 | 42 | void changeCameraSize(const float delta); 43 | 44 | void changePointSize(const float delta); 45 | 46 | void setCameraSize(float camera_size); 47 | 48 | void setDrawCameras(bool draw_cameras); 49 | 50 | void setDrawSparsePoints(bool draw_sparse_points); 51 | 52 | void clear(); 53 | 54 | void init(); 55 | 56 | void upload(); 57 | 58 | void draw(const QMatrix4x4 &pvm_matrix, const int width, const int height); 59 | 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | private: 63 | const float IMAGE_R = 1.0f; 64 | const float IMAGE_G = 0.1f; 65 | const float IMAGE_B = 0.0f; 66 | const float IMAGE_A = 0.6f; 67 | 68 | void uploadCameraData(); 69 | 70 | void uploadPointData(); 71 | 72 | void generateImageModel(const PinholeCameraColmap &camera, const ImageColmap &image, 73 | const float camera_size, const float r, const float g, const float b, const float a, 74 | std::array &lines, std::array &triangles); 75 | 76 | const SparseReconstruction *sparse_recon_; 77 | float camera_size_; 78 | float overwrite_camera_width_; 79 | float point_size_; 80 | bool draw_cameras_; 81 | bool draw_sparse_points_; 82 | TriangleDrawer camera_triangle_drawer_; 83 | LineDrawer camera_line_drawer_; 84 | PointDrawer sparse_point_drawer_; 85 | }; 86 | 87 | } 88 | -------------------------------------------------------------------------------- /viewpoint_planner/src/rendering/viewpoint_drawer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * viewpoint_drawer.h 3 | * 4 | * Created on: Dec 24, 2016 5 | * Author: bhepp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "../reconstruction/sparse_reconstruction.h" 12 | #include "triangle_drawer.h" 13 | #include "line_drawer.h" 14 | 15 | namespace rendering { 16 | 17 | template 18 | class ViewpointDrawer { 19 | const FloatType CAMERA_SIZE_SPEED = 0.1f; 20 | const FloatType MIN_CAMERA_SIZE = 0.01f; 21 | const FloatType MAX_CAMERA_SIZE = 10.0f; 22 | const FloatType CAMERA_LINE_WIDTH = 1.0f; 23 | 24 | USE_FIXED_EIGEN_TYPES(FloatType) 25 | 26 | public: 27 | using Pose = bh::Pose; 28 | using Color4 = bh::Color4; 29 | 30 | ViewpointDrawer(); 31 | 32 | ~ViewpointDrawer(); 33 | 34 | void setCamera(const reconstruction::PinholeCamera &camera); 35 | 36 | void setViewpoints(const std::vector &poses); 37 | 38 | void setViewpoints(const std::vector &poses, const Color4 &color); 39 | 40 | void setViewpoints(const std::vector &poses, const std::vector &colors); 41 | 42 | void changeCameraSize(const FloatType delta); 43 | 44 | void setCameraSize(FloatType camera_size); 45 | 46 | void setDrawCameras(bool draw_cameras); 47 | 48 | void clear(); 49 | 50 | void init(); 51 | 52 | void upload(); 53 | 54 | void draw(const QMatrix4x4 &pvm_matrix, const int width, const int height); 55 | 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | 58 | private: 59 | void setColor(FloatType r, FloatType g, FloatType b, FloatType a); 60 | 61 | void setColor(const Color4 &color); 62 | 63 | void setColors(const std::vector &colors); 64 | 65 | void uploadCameraData(); 66 | 67 | void generateImageModel(const reconstruction::PinholeCamera &camera, const bh::Pose &pose, 68 | const FloatType camera_size, const Color4 &color, 69 | std::array &lines, std::array &triangles); 70 | 71 | reconstruction::PinholeCamera camera_; 72 | std::vector poses_; 73 | std::vector colors_; 74 | FloatType camera_size_; 75 | bool draw_cameras_; 76 | TriangleDrawer camera_triangle_drawer_; 77 | LineDrawer camera_line_drawer_; 78 | }; 79 | 80 | } 81 | 82 | #include "viewpoint_drawer.hxx" 83 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | shaders/octomap_shader.v.glsl 4 | shaders/octomap_shader.g.glsl 5 | shaders/octomap_shader.f.glsl 6 | shaders/triangles.v.glsl 7 | shaders/triangles.f.glsl 8 | shaders/lines.v.glsl 9 | shaders/lines.g.glsl 10 | shaders/lines.f.glsl 11 | shaders/points.v.glsl 12 | shaders/points.f.glsl 13 | shaders/voxels.v.glsl 14 | shaders/voxels.f.glsl 15 | shaders/triangles_normals.f.glsl 16 | shaders/triangles_normals.v.glsl 17 | shaders/triangles_depth.f.glsl 18 | shaders/triangles_depth.v.glsl 19 | shaders/triangles_indices.f.glsl 20 | shaders/triangles_indices.v.glsl 21 | 22 | 23 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/lines.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec4 g_color; 4 | out vec4 f_color; 5 | 6 | void main(void) { 7 | f_color = g_color; 8 | } 9 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/lines.g.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | layout(lines) in; 4 | layout(triangle_strip, max_vertices = 4) out; 5 | 6 | uniform vec2 u_inv_viewport; 7 | uniform float u_line_width; 8 | 9 | in vec4 v_pos[2]; 10 | in vec4 v_color[2]; 11 | out vec4 g_color; 12 | 13 | void main() { 14 | vec2 dir = normalize(v_pos[1].xy / v_pos[1].w - v_pos[0].xy / v_pos[0].w); 15 | vec2 normal_dir = vec2(-dir.y, dir.x); 16 | vec2 offset = (vec2(u_line_width) * u_inv_viewport) * normal_dir; 17 | 18 | gl_Position = vec4(v_pos[0].xy + offset * v_pos[0].w, v_pos[0].z, v_pos[0].w); 19 | g_color = v_color[0]; 20 | EmitVertex(); 21 | 22 | gl_Position = vec4(v_pos[1].xy + offset * v_pos[1].w, v_pos[1].z, v_pos[1].w); 23 | g_color = v_color[1]; 24 | EmitVertex(); 25 | 26 | gl_Position = vec4(v_pos[0].xy - offset * v_pos[0].w, v_pos[0].z, v_pos[0].w); 27 | g_color = v_color[0]; 28 | EmitVertex(); 29 | 30 | gl_Position = vec4(v_pos[1].xy - offset * v_pos[1].w, v_pos[1].z, v_pos[1].w); 31 | g_color = v_color[1]; 32 | EmitVertex(); 33 | 34 | EndPrimitive(); 35 | } 36 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/lines.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | uniform mat4 u_pvm_matrix; 4 | 5 | in vec3 a_pos; 6 | in vec4 a_color; 7 | out vec4 v_pos; 8 | out vec4 v_color; 9 | 10 | void main(void) { 11 | v_pos = u_pvm_matrix * vec4(a_pos, 1); 12 | v_color = a_color; 13 | } 14 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/octomap_shader.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | layout(location = 0) in vec3 vertex_position_modelspace; 5 | layout(location = 1) in float voxel_size; 6 | 7 | // Uniform values 8 | uniform mat4 MVP; 9 | uniform mat4 M; 10 | uniform mat4 V; 11 | uniform float viewportWidth; 12 | uniform float viewportHeight; 13 | //uniform vec3 light_position_worldspace; 14 | 15 | // Outputs 16 | out vec4 fragment_color_vs; 17 | out vec3 position_modelspace_vs; 18 | out float voxel_size_vs; 19 | out vec3 light_position_worldspace_vs; 20 | 21 | void main() 22 | { 23 | voxel_size_vs = voxel_size; 24 | 25 | light_position_worldspace_vs = vec3(5, 5, 10); 26 | 27 | position_modelspace_vs = vertex_position_modelspace; 28 | 29 | gl_Position = MVP * vec4(vertex_position_modelspace, 1); 30 | 31 | // Position of the vertex, in worldspace : M * position 32 | vec3 position_worldspace = (M * vec4(vertex_position_modelspace, 1)).xyz; 33 | 34 | // Vector that goes from the vertex to the camera, in camera space. 35 | // In camera space, the camera is at the origin (0,0,0). 36 | //vec3 vertexPosition_cameraspace = (V * M * vec4(vertexPosition_modelspace,1)).xyz; 37 | //eyeDirection_cameraspace_vs = vec3(0,0,0) - vertexPosition_cameraspace; 38 | 39 | // Vector that goes from the vertex to the light, in camera space. M is ommited because it's identity. 40 | //vec3 lightPosition_cameraspace = (V * vec4(lightPosition_worldspace, 1)).xyz; 41 | //lightDirection_cameraspace_vs = lightPosition_cameraspace + eyeDirection_cameraspace_vs; 42 | 43 | // Normal of the vertex, in camera space 44 | //normal_cameraspace_vs = ( V * M * vec4(vertexNormal_modelspace,0)).xyz; // Only correct if ModelMatrix does not scale the model ! Use its inverse transpose if not. 45 | 46 | float color_level = 0.2f * position_worldspace[2]; 47 | float green = color_level < 0.5f ? 2 * color_level : 2 - 2 * color_level; 48 | fragment_color_vs = vec4(clamp(1.0f - color_level, 0, 1), green, clamp(color_level, 0, 1), 1.0f); 49 | } 50 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/phong_shader.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | layout(location = 0) in vec3 vertexPosition_modelspace; 5 | layout(location = 1) in vec3 vertexNormal_modelspace; 6 | layout(location = 2) in vec3 vertexColor; 7 | 8 | // Uniform values 9 | uniform mat4 MVP; 10 | uniform mat4 M; 11 | uniform mat4 V; 12 | uniform float viewportWidth; 13 | uniform float viewportHeight; 14 | //uniform vec3 lightPosition_worldspace; 15 | 16 | // Outputs 17 | out vec4 fragmentColor; 18 | out vec3 position_modelspace; 19 | out vec3 position_worldspace; 20 | out vec3 normal_cameraspace; 21 | out vec3 eyeDirection_cameraspace; 22 | out vec3 lightDirection_cameraspace; 23 | 24 | void main() 25 | { 26 | vec3 lightPosition_worldspace = vec3(5, 5, 10); 27 | 28 | position_modelspace = vertexPosition_modelspace; 29 | 30 | gl_Position = MVP * vec4(vertexPosition_modelspace, 1); 31 | 32 | // Position of the vertex, in worldspace : M * position 33 | position_worldspace = (M * vec4(vertexPosition_modelspace, 1)).xyz; 34 | 35 | // Vector that goes from the vertex to the camera, in camera space. 36 | // In camera space, the camera is at the origin (0,0,0). 37 | vec3 vertexPosition_cameraspace = (V * M * vec4(vertexPosition_modelspace,1)).xyz; 38 | eyeDirection_cameraspace = vec3(0,0,0) - vertexPosition_cameraspace; 39 | 40 | // Vector that goes from the vertex to the light, in camera space. M is ommited because it's identity. 41 | vec3 lightPosition_cameraspace = (V * vec4(lightPosition_worldspace, 1)).xyz; 42 | lightDirection_cameraspace = lightPosition_cameraspace + eyeDirection_cameraspace; 43 | 44 | // Normal of the vertex, in camera space 45 | normal_cameraspace = ( V * M * vec4(vertexNormal_modelspace,0)).xyz; // Only correct if ModelMatrix does not scale the model ! Use its inverse transpose if not. 46 | 47 | fragmentColor = vec4(vertexColor, 1.0f); 48 | } 49 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/points.f.glsl: -------------------------------------------------------------------------------- 1 | #version 150 2 | 3 | in vec4 v_color; 4 | out vec4 f_color; 5 | 6 | void main(void) { 7 | f_color = v_color; 8 | } 9 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/points.v.glsl: -------------------------------------------------------------------------------- 1 | #version 150 2 | 3 | uniform float u_point_size; 4 | uniform mat4 u_pvm_matrix; 5 | 6 | in vec3 a_position; 7 | in vec4 a_color; 8 | out vec4 v_color; 9 | 10 | void main(void) { 11 | gl_Position = u_pvm_matrix * vec4(a_position, 1); 12 | gl_PointSize = u_point_size; 13 | v_color = a_color; 14 | } 15 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/trajectory_shader.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Input vertex data, different for all executions of this shader. 4 | 5 | // Inputs 6 | in vec4 line_vertex1; 7 | in vec4 line_normal; 8 | in vec4 line_direction; 9 | in vec3 fragmentColor; 10 | 11 | // Uniform values 12 | uniform mat4 MVP; 13 | uniform mat4 M; 14 | uniform mat4 V; 15 | uniform float viewportWidth; 16 | uniform float viewportHeight; 17 | uniform float lineWidth; 18 | 19 | // Ouput data 20 | out vec4 color; 21 | 22 | vec4 vec3_to_homogeneous(vec3 vec, float w) 23 | { 24 | vec4 vec_3d; 25 | vec_3d.xy = vec.xy; 26 | vec_3d.z = 0; 27 | vec_3d.w = 1; 28 | vec_3d *= w; 29 | return vec_3d; 30 | } 31 | 32 | vec3 vec3_from_homogeneous(vec4 vec) 33 | { 34 | vec3 vec_2d = vec.xyz; 35 | vec_2d.z = 0; 36 | vec_2d /= vec.w; 37 | return vec_2d; 38 | } 39 | 40 | vec3 convertDeviceCoordToViewCoord(vec4 fragCoord) 41 | { 42 | vec3 viewCoord = fragCoord.xyz; 43 | viewCoord.z = 0; 44 | viewCoord.x /= viewportWidth; 45 | viewCoord.y /= viewportHeight; 46 | viewCoord = viewCoord * 2.0 - 1.0; 47 | return viewCoord; 48 | } 49 | 50 | void main() 51 | { 52 | vec3 line_vertexTemp_vec3 = convertDeviceCoordToViewCoord(gl_FragCoord); 53 | vec3 line_vertex1_vec3 = line_vertex1.xyz / line_vertex1.w; 54 | vec3 dvertex = line_vertexTemp_vec3 - line_vertex1_vec3; 55 | 56 | vec3 line_normal_vec3 = line_normal.xyz; 57 | vec3 line_direction_vec3 = normalize(line_direction.xyz); 58 | float dist = abs(dot(dvertex, line_normal_vec3) / lineWidth); 59 | float weight = 1.0 - dist; 60 | weight = sqrt(1.0 - dist); 61 | 62 | color.xyz = fragmentColor; 63 | color.w = clamp(weight, 0, 1); 64 | } 65 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/trajectory_shader.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | // Inputs 4 | layout(location = 0) in vec3 vertexPosition_modelspace; 5 | layout(location = 1) in vec3 vertexColor; 6 | 7 | // Uniform values 8 | uniform mat4 MVP; 9 | uniform mat4 M; 10 | uniform mat4 V; 11 | uniform float viewportWidth; 12 | uniform float viewportHeight; 13 | uniform float lineWidth; 14 | 15 | // Outputs 16 | out vec3 fragmentColorGeom; 17 | 18 | void main() 19 | { 20 | gl_Position = MVP * vec4(vertexPosition_modelspace, 1); 21 | 22 | fragmentColorGeom = vertexColor; 23 | } 24 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec4 v_color; 4 | out vec4 f_color; 5 | 6 | void main(void) { 7 | f_color = v_color; 8 | } 9 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | uniform mat4 u_pvm_matrix; 4 | 5 | in vec3 a_position; 6 | in vec4 a_color; 7 | out vec4 v_color; 8 | 9 | void main(void) { 10 | gl_Position = u_pvm_matrix * vec4(a_position, 1); 11 | v_color = a_color; 12 | } 13 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles_depth.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in float v_dist_to_camera; 4 | out vec4 f_color; 5 | 6 | vec3 unpackColor3(float f) { 7 | vec3 color; 8 | color.r = floor(f / 256.); 9 | color.g = floor(f - color.r * 256.); 10 | color.b = floor((f - color.r * 256. - color.g) * 256.); 11 | return color / 256.; 12 | } 13 | 14 | void main(void) { 15 | f_color = vec4(unpackColor3(v_dist_to_camera), 1); 16 | } 17 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles_depth.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | uniform mat4 u_pvm_matrix; 4 | uniform mat4 u_vm_matrix; 5 | 6 | in vec3 a_position; 7 | in vec4 a_color; 8 | out float v_dist_to_camera; 9 | 10 | void main(void) { 11 | vec4 cs_position = u_vm_matrix * vec4(a_position, 1); 12 | v_dist_to_camera = -cs_position.z * cs_position.w; 13 | gl_Position = u_pvm_matrix * vec4(a_position, 1); 14 | } 15 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles_indices.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec4 v_color; 4 | out vec4 f_color; 5 | 6 | void main(void) { 7 | f_color = v_color; 8 | } 9 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles_indices.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | uniform mat4 u_pvm_matrix; 4 | uniform int u_index_offset; 5 | 6 | in vec3 a_position; 7 | in vec4 a_color; 8 | out vec4 v_color; 9 | 10 | const int VERTICES_PER_PRIMITIVE = 3; 11 | 12 | int getPrimitiveIndex() { 13 | int index = u_index_offset + gl_VertexID / VERTICES_PER_PRIMITIVE; 14 | return index; 15 | } 16 | 17 | vec4 indexToColor(int index) { 18 | int r = (index & 0x000000FF) >> 0; 19 | int g = (index & 0x0000FF00) >> 8; 20 | int b = (index & 0x00FF0000) >> 16; 21 | vec4 color; 22 | color.r = r / 255.0; 23 | color.g = g / 255.0; 24 | color.b = b / 255.0; 25 | color.a = 1; 26 | return color; 27 | } 28 | 29 | void main(void) { 30 | int index = getPrimitiveIndex(); 31 | 32 | gl_Position = u_pvm_matrix * vec4(a_position, 1); 33 | v_color = indexToColor(index); 34 | } 35 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles_normals.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | in vec3 v_normal; 4 | out vec4 f_color; 5 | 6 | void main(void) { 7 | f_color = vec4(0.5 * v_normal + vec3(0.5, 0.5, 0.5), 1); 8 | //mat3 normal_matrix = mat3(u_pvm_matrix); 9 | //normal_matrix = inverse(normal_matrix); 10 | //normal_matrix = transpose(normal_matrix); 11 | //vec3 transformed_normal = normal_matrix * a_normal; 12 | //f_color = vec4(-transformed_normal, 1); 13 | } 14 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/triangles_normals.v.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | uniform mat4 u_pvm_matrix; 4 | 5 | in vec3 a_position; 6 | in vec3 a_normal; 7 | in vec4 a_color; 8 | out vec3 v_normal; 9 | 10 | void main(void) { 11 | gl_Position = u_pvm_matrix * vec4(a_position, 1); 12 | v_normal = a_normal.xyz; 13 | } 14 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders/voxels.f.glsl: -------------------------------------------------------------------------------- 1 | #version 330 2 | 3 | // INPUTS 4 | in vec3 v_light_position_cameraspace; 5 | in vec4 v_color; 6 | in vec3 v_vertex_position_cameraspace; 7 | in vec3 v_vertex_normal_cameraspace; 8 | //in vec4 v_vertex_position; 9 | //in vec3 v_vertex_normal; 10 | //in vec3 v_light_direction; 11 | //in vec3 v_light_position; 12 | flat in int v_keep_flag; 13 | flat in int v_do_shading; 14 | 15 | // OUTPUTS 16 | out vec4 f_color; 17 | 18 | const vec3 light_color = vec3(1, 1, 1); 19 | 20 | void main() { 21 | // Light emission properties 22 | // float u_light_power = 5000.0f; 23 | 24 | if (v_keep_flag == 0) { 25 | discard; 26 | } 27 | 28 | if (v_do_shading != 0) { 29 | // Material properties 30 | vec3 material_color = v_color.xyz; 31 | vec3 u_ambient_color = 0.4 * material_color; 32 | vec3 u_diffuse_color = 0.7 * material_color; 33 | vec3 u_specular_color = 0.5 * material_color; 34 | float u_specular_alpha = 3; 35 | 36 | // Distance to the light 37 | vec3 light_direction_cameraspace = v_light_position_cameraspace - v_vertex_position_cameraspace; 38 | float distance = length(light_direction_cameraspace); 39 | // Direction of fragment 40 | vec3 eye_direction_cameraspace = - v_vertex_position_cameraspace; 41 | 42 | // Normal of the computed fragment, in camera space 43 | vec3 N = normalize(v_vertex_normal_cameraspace); 44 | // Direction of the light (from the fragment to the light) 45 | vec3 L = normalize(light_direction_cameraspace); 46 | // Cosine of the angle between the normal and the light direction, 47 | // clamped above 0 48 | // - light is at the vertical of the triangle -> 1 49 | // - light is perpendicular to the triangle -> 0 50 | // - light is behind the triangle -> 0 51 | float lambertian = clamp(dot(N, L), 0, 1); 52 | 53 | float specular = 0; 54 | if (lambertian > 0) { 55 | // Light ray vector (towards the camera) 56 | vec3 E = normalize(eye_direction_cameraspace); 57 | // Direction in which the triangle reflects the light 58 | vec3 R = reflect(-L, N); 59 | // Cosine of the angle between the Eye vector and the Reflect vector, 60 | // clamped to 0 61 | // - Looking into the reflection -> 1 62 | // - Looking elsewhere -> < 1 63 | float cos_alpha = clamp(dot(E, R), 0, 1); 64 | specular = pow(cos_alpha, u_specular_alpha); 65 | } 66 | //specular = 0; 67 | 68 | // float attenuated_light_power = u_light_power / (distance * distance); 69 | float attenuated_light_power = 1; 70 | f_color.rgb = 71 | u_ambient_color + 72 | lambertian * u_diffuse_color * light_color * attenuated_light_power + 73 | specular * u_specular_color * light_color * attenuated_light_power; 74 | f_color.a = v_color.a; 75 | } 76 | else { 77 | f_color = v_color; 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /viewpoint_planner/src/shaders_offscreen.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | shaders/triangles.v.glsl 4 | shaders/triangles.f.glsl 5 | shaders/triangles_normals.f.glsl 6 | shaders/triangles_normals.v.glsl 7 | shaders/triangles_depth.f.glsl 8 | shaders/triangles_depth.v.glsl 9 | shaders/triangles_indices.f.glsl 10 | shaders/triangles_indices.v.glsl 11 | shaders/voxels.v.glsl 12 | shaders/voxels.f.glsl 13 | 14 | 15 | -------------------------------------------------------------------------------- /viewpoint_planner/src/ui/qt_utils.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // qt_utils.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 6, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | template 18 | std::basic_ostream& operator<<(std::basic_ostream& out, const QVector3D& vec) 19 | { 20 | out << "(" << vec.x() << ", " << vec.y() << ", " << vec.z() << ")"; 21 | return out; 22 | } 23 | 24 | template 25 | std::basic_ostream& operator<<(std::basic_ostream& out, const QVector4D& vec) 26 | { 27 | out << "(" << vec.x() << ", " << vec.y() << ", " << vec.z() << ", " << vec.w() << ")"; 28 | return out; 29 | } 30 | 31 | template 32 | std::basic_ostream& operator<<(std::basic_ostream& out, const QMatrix3x3& mat) 33 | { 34 | out << "( " << mat(0, 0) << ", " << mat(0, 1) << ", " << mat(0, 2) << std::endl; 35 | out << " " << mat(1, 0) << ", " << mat(1, 1) << ", " << mat(1, 2) << std::endl; 36 | out << " " << mat(2, 0) << ", " << mat(2, 1) << ", " << mat(2, 2) << " )"; 37 | return out; 38 | } 39 | 40 | template 41 | std::basic_ostream& operator<<(std::basic_ostream& out, const QMatrix4x4& mat) 42 | { 43 | out << "( " << mat(0, 0) << ", " << mat(0, 1) << ", " << mat(0, 2) << ", " << mat(0, 3) << std::endl; 44 | out << " " << mat(1, 0) << ", " << mat(1, 1) << ", " << mat(1, 2) << ", " << mat(1, 3) << std::endl; 45 | out << " " << mat(2, 0) << ", " << mat(2, 1) << ", " << mat(2, 2) << ", " << mat(2, 3) << std::endl; 46 | out << " " << mat(3, 0) << ", " << mat(3, 1) << ", " << mat(3, 2) << ", " << mat(3, 3) << " )"; 47 | return out; 48 | } 49 | -------------------------------------------------------------------------------- /viewpoint_planner/src/ui/viewer_info_panel.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // viewer_info_panel.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 6, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "ui_viewer_info_panel.h" 14 | 15 | #define _TREE_MAX_DEPTH 20 16 | 17 | class ViewerInfoPanel : public QWidget 18 | { 19 | Q_OBJECT 20 | 21 | public: 22 | ViewerInfoPanel(QWidget *parent = 0) 23 | : QWidget(parent) { 24 | ui.setupUi(this); 25 | } 26 | 27 | ~ViewerInfoPanel() { 28 | } 29 | 30 | public slots: 31 | void setNumOfNodes(size_t num_of_nodes) { 32 | ui.numOfNodes->setText(QString::number(num_of_nodes)); 33 | } 34 | 35 | void setNumOfLeafNodes(size_t num_of_leaf_nodes) { 36 | ui.numOfLeafNodes->setText(QString::number(num_of_leaf_nodes)); 37 | } 38 | 39 | void setVoxelSize(double voxel_size) { 40 | ui.voxelSize->setText(QString::number(voxel_size)); 41 | } 42 | 43 | void setTreeDepth(size_t tree_depth) { 44 | ui.treeDepth->setText(QString::number(tree_depth)); 45 | } 46 | 47 | void setExtent(QVector3D extent) { 48 | ui.extent->setText(QString::number(extent.x()) + ", " + QString::number(extent.y()) + ", " + QString::number(extent.z())); 49 | } 50 | 51 | void setNumOfImages(size_t num_of_images) { 52 | ui.numOfImages->setText(QString::number(num_of_images)); 53 | } 54 | 55 | void setNumOfSparsePoints(size_t num_of_sparse_points) { 56 | ui.numOfSparsePoints->setText(QString::number(num_of_sparse_points)); 57 | } 58 | 59 | private: 60 | Ui::ViewerInfoPanelClass ui; 61 | }; 62 | -------------------------------------------------------------------------------- /viewpoint_planner/src/ui/viewer_window.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // viewpoint_planner_window.h 3 | // 4 | // Copyright (c) 2016 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Dec 6, 2016 7 | //================================================== 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "../planner/viewpoint_planner.h" 16 | #include "viewer_widget.h" 17 | #include "viewer_info_panel.h" 18 | #include "viewer_settings_panel.h" 19 | #include "viewer_planner_panel.h" 20 | 21 | class ViewerWindow : public QMainWindow { 22 | Q_OBJECT 23 | 24 | public: 25 | 26 | using Options = ViewerWidget::Options; 27 | 28 | ViewerWindow(const Options& options, ViewpointPlanner* planner, QWidget *parent = nullptr); 29 | 30 | ~ViewerWindow(); 31 | 32 | const ViewerWidget* getViewerWidget() const; 33 | 34 | ViewerWidget* getViewerWidget(); 35 | 36 | protected: 37 | ViewpointPlanner* planner_; 38 | ViewerWidget* viewer_widget_; 39 | QTabWidget* panel_tab_; 40 | ViewerInfoPanel* info_panel_; 41 | ViewerSettingsPanel* settings_panel_; 42 | ViewerPlannerPanel* planner_panel_; 43 | }; 44 | -------------------------------------------------------------------------------- /viewpoint_planner/src/web/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /viewpoint_planner/src/web/.idea/web.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/bootstrap/3.3.7/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2011-2016 Twitter, Inc. 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 13 | all 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 21 | THE SOFTWARE. 22 | -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.eot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.eot -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.ttf -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.woff: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.woff -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.woff2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bennihepp/Quad3DR/e86a64617aea4c71060552ef1215fdc35c755aa2/viewpoint_planner/src/web/external/bootstrap/3.3.7/fonts/glyphicons-halflings-regular.woff2 -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/bootstrap/3.3.7/js/npm.js: -------------------------------------------------------------------------------- 1 | // This file is autogenerated via the `commonjs` Grunt task. You can require() this file in a CommonJS environment. 2 | require('../../js/transition.js') 3 | require('../../js/alert.js') 4 | require('../../js/button.js') 5 | require('../../js/carousel.js') 6 | require('../../js/collapse.js') 7 | require('../../js/dropdown.js') 8 | require('../../js/modal.js') 9 | require('../../js/tooltip.js') 10 | require('../../js/popover.js') 11 | require('../../js/scrollspy.js') 12 | require('../../js/tab.js') 13 | require('../../js/affix.js') -------------------------------------------------------------------------------- /viewpoint_planner/src/web/external/jquery/3.1.1/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright JS Foundation and other contributors, https://js.foundation/ 2 | 3 | This software consists of voluntary contributions made by many 4 | individuals. For exact contribution history, see the revision history 5 | available at https://github.com/jquery/jquery 6 | 7 | The following license applies to all parts of this software except as 8 | documented below: 9 | 10 | ==== 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining 13 | a copy of this software and associated documentation files (the 14 | "Software"), to deal in the Software without restriction, including 15 | without limitation the rights to use, copy, modify, merge, publish, 16 | distribute, sublicense, and/or sell copies of the Software, and to 17 | permit persons to whom the Software is furnished to do so, subject to 18 | the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be 21 | included in all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 24 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 25 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 26 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 27 | LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 28 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | 31 | ==== 32 | 33 | All files located in the node_modules and external directories are 34 | externally maintained libraries used by this software which have their 35 | own licenses; we recommend you read them, as their terms may differ from 36 | the terms above. 37 | -------------------------------------------------------------------------------- /viewpoint_planner/src/web/web_socket_server.h: -------------------------------------------------------------------------------- 1 | //================================================== 2 | // websocket_server.h 3 | // 4 | // Copyright (c) 2017 Benjamin Hepp. 5 | // Author: Benjamin Hepp 6 | // Created on: Jan 14, 2017 7 | //================================================== 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | QT_FORWARD_DECLARE_CLASS(QWebSocketServer) 16 | QT_FORWARD_DECLARE_CLASS(QWebSocket) 17 | 18 | class WebSocketServer : public QObject { 19 | 20 | Q_OBJECT 21 | 22 | public: 23 | explicit WebSocketServer(quint16 port, QObject* parent = nullptr); 24 | ~WebSocketServer(); 25 | 26 | void sendTextMessage(const std::string& msg); 27 | 28 | void sendBinaryMessage(const std::string& msg); 29 | 30 | signals: 31 | void closed(); 32 | 33 | private Q_SLOTS: 34 | void onNewConnection(); 35 | void processTextMessage(QString message); 36 | void processBinaryMessage(QByteArray message); 37 | void socketDisconnected(); 38 | 39 | private: 40 | QWebSocketServer* web_socket_server_; 41 | std::vector clients_; 42 | 43 | std::string getPeerString(const QWebSocket* socket) const; 44 | }; 45 | -------------------------------------------------------------------------------- /viewpoint_planner/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #find_package(GTest REQUIRED) 2 | 3 | #include_directories(${GTEST_INCLUDE_DIRS}) 4 | include_directories(${OpenCV_INCLUDE_DIRS}) 5 | 6 | add_executable(test_ann 7 | # Executable 8 | test_ann.cpp 9 | ) 10 | target_link_libraries(test_ann 11 | #${GTEST_LIBRARIES} 12 | gtest 13 | gtest_main 14 | ) 15 | 16 | add_executable(test_vision 17 | # Executable 18 | test_vision.cpp 19 | ) 20 | target_link_libraries(test_vision 21 | #${GTEST_LIBRARIES} 22 | ${OpenCV_LIBRARIES} 23 | gtest 24 | gtest_main 25 | ) 26 | 27 | add_executable(test_vision_qt 28 | # Executable 29 | test_vision_qt.cpp 30 | ) 31 | target_link_libraries(test_vision_qt 32 | #${GTEST_LIBRARIES} 33 | ${OpenCV_LIBRARIES} 34 | gtest 35 | gtest_main 36 | ) 37 | target_link_libraries(test_vision_qt Qt5::Core Qt5::Gui) 38 | 39 | add_executable(test_se3_transform 40 | # Executable 41 | test_se3_transform.cpp 42 | ) 43 | target_link_libraries(test_se3_transform 44 | #${GTEST_LIBRARIES} 45 | gtest 46 | gtest_main 47 | ) 48 | 49 | add_executable(test_qt_image 50 | # Executable 51 | test_qt_image.cpp 52 | ) 53 | target_link_libraries(test_qt_image 54 | #${GTEST_LIBRARIES} 55 | gtest 56 | gtest_main 57 | ) 58 | target_link_libraries(test_qt_image Qt5::Core Qt5::Gui) 59 | -------------------------------------------------------------------------------- /viewpoint_planner/viewpoint_planner.cfg: -------------------------------------------------------------------------------- 1 | [viewpoint_planner] 2 | rng_seed = 0 3 | virtual_camera_scale = 0.05 4 | #virtual_camera_scale = 0.15 5 | 6 | drone_extent_x = 3 7 | drone_extent_y = 3 8 | drone_extent_z = 3 9 | 10 | sampling_roi_factor = 2 11 | pose_sample_num_trials = 100 12 | pose_sample_min_radius = 2 13 | pose_sample_max_radius = 5 14 | 15 | viewpoint_sample_count = 10 16 | viewpoint_min_voxel_count = 50 17 | viewpoint_min_information = 10 18 | 19 | viewpoint_discard_dist_knn = 20 20 | viewpoint_discard_dist_thres_square = 4 21 | viewpoint_discard_dist_count_thres = 3 22 | 23 | viewpoint_motion_max_neighbors = 20 24 | viewpoint_motion_max_dist_square = 100 25 | viewpoint_motion_min_connections = 3 26 | viewpoint_motion_densification_max_depth = 8 27 | 28 | viewpoint_path_branches = 1 29 | #viewpoint_path_branches = 20 30 | viewpoint_path_initial_distance = 6 31 | 32 | objective_parameter_alpha = 0 33 | objective_parameter_beta = 0 34 | 35 | viewpoint_path_2opt_max_k_length = 25 36 | 37 | #viewpoint_graph_filename = viewpoint_graph.bs 38 | #viewpoint_graph_filename = viewpoint_graph_with_motions.bs 39 | 40 | num_sampled_poses = 20 41 | num_planned_viewpoints = 1 42 | #num_sampled_poses = 250 43 | #num_planned_viewpoints = 25 44 | #num_sampled_poses = 1000 45 | #num_planned_viewpoints = 50 46 | 47 | [motion_planner] 48 | max_motion_range = 5 49 | max_time_per_solve = 0.01 50 | max_iterations_per_solve = 1000 51 | 52 | [viewpoint_planner.data] 53 | dense_reconstruction_path = /media/bhepp/MPData1/Stanford/luxo5/restroom2/dense_very_far_far_gps 54 | poisson_mesh_filename = /media/bhepp/MPData1/Stanford/luxo5/restroom2/dense_very_far_far_gps/meshed.ply 55 | raw_octree_filename = /media/bhepp/MPData1/Stanford/luxo5/restroom2/dense_very_far_far_gps.ot 56 | #raw_octree_filename = /home/bhepp/Desktop/restroom/dense_far3.ot 57 | #octree_filename = 58 | #bvh_filename = 59 | #distance_field_filename = 60 | 61 | #regenerate_augmented_octree = True 62 | #regenerate_bvh_tree = True 63 | #regenerate_distance_field = True 64 | 65 | grid_dimension = 200 66 | distance_field_cutoff = 5 67 | 68 | bvh_bbox_min_x = -100 69 | bvh_bbox_min_y = -100 70 | bvh_bbox_min_z = -100 71 | bvh_bbox_max_x = +100 72 | bvh_bbox_max_y = +100 73 | bvh_bbox_max_z = +100 74 | 75 | roi_bbox_min_x = +12.5 76 | roi_bbox_min_y = +5.0 77 | roi_bbox_min_z = -5 78 | roi_bbox_max_x = +27.5 79 | roi_bbox_max_y = +20.0 80 | roi_bbox_max_z = +10 81 | 82 | roi_falloff_distance = 10.0 83 | 84 | --------------------------------------------------------------------------------