├── .gitignore ├── README.md ├── calibrate_sweeps ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── action │ └── CalibrateSweeps.action ├── package.xml └── src │ └── calibrate_sweep_action_server.cpp ├── cloud_merge ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── action │ └── Sweep.action ├── include │ └── cloud_merge │ │ ├── cloud_merge.h │ │ └── cloud_merge_node.h ├── launch │ ├── cloud_merge.launch │ └── mapping.launch ├── package.xml ├── scripts │ └── do_sweep.py └── src │ ├── cloud_merge.cpp │ ├── cloud_merge_node.cpp │ └── main.cpp ├── dynamic_object_retrieval ├── README.md ├── convex_segmentation │ ├── CMakeLists.txt │ ├── include │ │ └── object_3d_retrieval │ │ │ └── supervoxel_segmentation.h │ ├── msg │ │ └── CloudArray.msg │ ├── package.xml │ └── src │ │ ├── convex_segmentation_node.cpp │ │ └── supervoxel_segmentation.cpp ├── dynamic_object_retrieval │ ├── CMakeLists.txt │ ├── cmake │ │ └── FindROS.cmake │ ├── include │ │ ├── dynamic_object_retrieval │ │ │ ├── definitions.h │ │ │ ├── dynamic_retrieval.h │ │ │ ├── extract_surfel_features.h │ │ │ ├── summary_iterators.h │ │ │ ├── summary_types.h │ │ │ ├── surfel_type.h │ │ │ └── visualize.h │ │ ├── eigen_cereal │ │ │ └── eigen_cereal.h │ │ ├── extract_sift │ │ │ └── extract_sift.h │ │ ├── object_3d_retrieval │ │ │ ├── pfhrgb_estimation.h │ │ │ ├── register_objects.h │ │ │ └── shot_estimation.h │ │ └── sift │ │ │ ├── sift.cpp │ │ │ └── sift.h │ ├── package.xml │ ├── scripts │ │ ├── build_surfel_maps.bash │ │ ├── clean_data_dir.bash │ │ ├── convert_surfel_maps.bash │ │ ├── menu.py │ │ ├── querying_menu.py │ │ ├── rename_complete_surfels.bash │ │ ├── summarize_segmentation_benchmark.py │ │ ├── training_menu.py │ │ └── verify_sweeps.bash │ ├── src │ │ ├── demo_convex_segmentation.cpp │ │ ├── demo_sweep_segmentation.cpp │ │ ├── dynamic_convex_segmentation.cpp │ │ ├── dynamic_create_subsegments.cpp │ │ ├── dynamic_extract_convex_features.cpp │ │ ├── dynamic_extract_sift.cpp │ │ ├── dynamic_extract_supervoxel_features.cpp │ │ ├── dynamic_init_folders.cpp │ │ ├── dynamic_init_vocabulary.cpp │ │ ├── dynamic_query_vocabulary.cpp │ │ ├── dynamic_retrieval.cpp │ │ ├── dynamic_supervoxel_convex_segmentation.cpp │ │ ├── dynamic_train_vocabulary.cpp │ │ ├── extract_sift.cpp │ │ ├── extract_surfel_features.cpp │ │ ├── pfhrgb_estimation.cpp │ │ ├── register_objects.cpp │ │ ├── shot_estimation.cpp │ │ └── visualize.cpp │ ├── stopwatch │ │ ├── .gitignore │ │ ├── README.md │ │ └── src │ │ │ ├── CMakeLists.txt │ │ │ ├── RingBuffer.h │ │ │ ├── Stopwatch.h │ │ │ ├── StopwatchDecoder.h │ │ │ ├── dataPlotWidget.cpp │ │ │ ├── dataPlotWidget.h │ │ │ ├── main.cpp │ │ │ ├── plotHolderWidget.cpp │ │ │ ├── plotHolderWidget.h │ │ │ ├── stopwatchviewer.cpp │ │ │ ├── stopwatchviewer.h │ │ │ └── stopwatchviewer.ui │ └── test │ │ ├── test_added_count.cpp │ │ ├── test_adjacencies.cpp │ │ ├── test_feature_keypoint_match.cpp │ │ ├── test_gt_labelled_data.cpp │ │ ├── test_query_keypoints.cpp │ │ ├── test_segmentation.cpp │ │ ├── test_supervoxel_convex_mapping.cpp │ │ ├── test_supervoxel_keypoints.cpp │ │ ├── test_surfel_segmentation.cpp │ │ ├── test_top_match_one_map.cpp │ │ └── test_visualize_keypoints.cpp ├── k_means_tree │ ├── CMakeLists.txt │ ├── README.md │ ├── cereal │ │ ├── .gitignore │ │ ├── .travis.yml │ │ ├── CMakeLists.txt │ │ ├── LICENSE │ │ ├── README.md │ │ ├── doc │ │ │ ├── DoxygenLayout.xml │ │ │ ├── doxygen.in │ │ │ ├── footer.html │ │ │ └── mainpage.dox │ │ ├── include │ │ │ └── cereal │ │ │ │ ├── access.hpp │ │ │ │ ├── archives │ │ │ │ ├── adapters.hpp │ │ │ │ ├── binary.hpp │ │ │ │ ├── json.hpp │ │ │ │ ├── portable_binary.hpp │ │ │ │ └── xml.hpp │ │ │ │ ├── cereal.hpp │ │ │ │ ├── details │ │ │ │ ├── helpers.hpp │ │ │ │ ├── polymorphic_impl.hpp │ │ │ │ ├── static_object.hpp │ │ │ │ ├── traits.hpp │ │ │ │ └── util.hpp │ │ │ │ ├── external │ │ │ │ ├── base64.hpp │ │ │ │ ├── rapidjson │ │ │ │ │ ├── document.h │ │ │ │ │ ├── filestream.h │ │ │ │ │ ├── genericstream.h │ │ │ │ │ ├── internal │ │ │ │ │ │ ├── pow10.h │ │ │ │ │ │ ├── stack.h │ │ │ │ │ │ └── strfunc.h │ │ │ │ │ ├── license.txt │ │ │ │ │ ├── prettywriter.h │ │ │ │ │ ├── rapidjson.h │ │ │ │ │ ├── reader.h │ │ │ │ │ ├── stringbuffer.h │ │ │ │ │ └── writer.h │ │ │ │ └── rapidxml │ │ │ │ │ ├── license.txt │ │ │ │ │ ├── manual.html │ │ │ │ │ ├── rapidxml.hpp │ │ │ │ │ ├── rapidxml_iterators.hpp │ │ │ │ │ ├── rapidxml_print.hpp │ │ │ │ │ └── rapidxml_utils.hpp │ │ │ │ ├── macros.hpp │ │ │ │ └── types │ │ │ │ ├── array.hpp │ │ │ │ ├── base_class.hpp │ │ │ │ ├── bitset.hpp │ │ │ │ ├── boost_variant.hpp │ │ │ │ ├── chrono.hpp │ │ │ │ ├── common.hpp │ │ │ │ ├── complex.hpp │ │ │ │ ├── deque.hpp │ │ │ │ ├── forward_list.hpp │ │ │ │ ├── list.hpp │ │ │ │ ├── map.hpp │ │ │ │ ├── memory.hpp │ │ │ │ ├── polymorphic.hpp │ │ │ │ ├── queue.hpp │ │ │ │ ├── set.hpp │ │ │ │ ├── stack.hpp │ │ │ │ ├── string.hpp │ │ │ │ ├── tuple.hpp │ │ │ │ ├── unordered_map.hpp │ │ │ │ ├── unordered_set.hpp │ │ │ │ ├── utility.hpp │ │ │ │ ├── valarray.hpp │ │ │ │ └── vector.hpp │ │ ├── sandbox │ │ │ ├── CMakeLists.txt │ │ │ ├── performance.cpp │ │ │ ├── sandbox.cpp │ │ │ ├── sandbox_json.cpp │ │ │ ├── sandbox_rtti.cpp │ │ │ ├── sandbox_shared_lib │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── base.cpp │ │ │ │ ├── base.hpp │ │ │ │ ├── derived.cpp │ │ │ │ └── derived.hpp │ │ │ └── sandbox_vs.cpp │ │ ├── scripts │ │ │ ├── renameincludes.sh │ │ │ ├── updatecoverage.sh │ │ │ └── updatedoc.in │ │ ├── unittests │ │ │ ├── CMakeLists.txt │ │ │ ├── array.cpp │ │ │ ├── basic_string.cpp │ │ │ ├── bitset.cpp │ │ │ ├── chrono.cpp │ │ │ ├── common.hpp │ │ │ ├── complex.cpp │ │ │ ├── deque.cpp │ │ │ ├── forward_list.cpp │ │ │ ├── list.cpp │ │ │ ├── load_construct.cpp │ │ │ ├── map.cpp │ │ │ ├── memory.cpp │ │ │ ├── memory_cycles.cpp │ │ │ ├── multimap.cpp │ │ │ ├── multiset.cpp │ │ │ ├── pair.cpp │ │ │ ├── pod.cpp │ │ │ ├── polymorphic.cpp │ │ │ ├── portability_test.cpp │ │ │ ├── portable_binary_archive.cpp │ │ │ ├── priority_queue.cpp │ │ │ ├── queue.cpp │ │ │ ├── run_portability_test.sh │ │ │ ├── run_valgrind.sh │ │ │ ├── set.cpp │ │ │ ├── stack.cpp │ │ │ ├── structs.cpp │ │ │ ├── structs_minimal.cpp │ │ │ ├── structs_specialized.cpp │ │ │ ├── tuple.cpp │ │ │ ├── unordered_loads.cpp │ │ │ ├── unordered_map.cpp │ │ │ ├── unordered_multimap.cpp │ │ │ ├── unordered_multiset.cpp │ │ │ ├── unordered_set.cpp │ │ │ ├── user_data_adapters.cpp │ │ │ ├── valarray.cpp │ │ │ ├── vector.cpp │ │ │ └── versioning.cpp │ │ └── vs2013 │ │ │ ├── .gitignore │ │ │ ├── performance │ │ │ ├── performance.vcxproj │ │ │ └── performance.vcxproj.filters │ │ │ ├── sandbox │ │ │ ├── sandbox.vcxproj │ │ │ └── sandbox.vcxproj.filters │ │ │ ├── sandbox_json │ │ │ ├── sandbox_json.vcxproj │ │ │ └── sandbox_json.vcxproj.filters │ │ │ ├── sandbox_rtti │ │ │ ├── sandbox_rtti.vcxproj │ │ │ └── sandbox_rtti.vcxproj.filters │ │ │ ├── sandbox_vs │ │ │ ├── sandbox_vs.vcxproj │ │ │ └── sandbox_vs.vcxproj.filters │ │ │ ├── sandbox_vs_dll │ │ │ ├── sandbox_vs_dll.vcxproj │ │ │ └── sandbox_vs_dll.vcxproj.filters │ │ │ ├── unittests │ │ │ ├── main.cpp │ │ │ ├── unittests.vcxproj │ │ │ └── unittests.vcxproj.filters │ │ │ └── vs2013.sln │ ├── impl │ │ ├── grouped_vocabulary_tree.hpp │ │ ├── k_means_tree.hpp │ │ └── vocabulary_tree.hpp │ ├── include │ │ ├── grouped_vocabulary_tree │ │ │ └── grouped_vocabulary_tree.h │ │ ├── k_means_tree │ │ │ └── k_means_tree.h │ │ └── vocabulary_tree │ │ │ └── vocabulary_tree.h │ ├── package.xml │ └── src │ │ ├── grouped_vocabulary_tree.cpp │ │ ├── k_means_tree.cpp │ │ ├── test.cpp │ │ ├── test_vocabulary_tree.cpp │ │ └── vocabulary_tree.cpp ├── object_3d_benchmark │ ├── CMakeLists.txt │ ├── include │ │ └── object_3d_benchmark │ │ │ ├── benchmark_overlap.h │ │ │ ├── benchmark_result.h │ │ │ ├── benchmark_retrieval.h │ │ │ ├── benchmark_segmentation.h │ │ │ ├── benchmark_visualization.h │ │ │ ├── surfel_renderer.h │ │ │ └── surfel_type.h │ ├── package.xml │ ├── src │ │ ├── benchmark_convex_segmentation.cpp │ │ ├── benchmark_incremental_segmentation.cpp │ │ ├── benchmark_overlap.cpp │ │ ├── benchmark_query_vocabulary.cpp │ │ ├── benchmark_result.cpp │ │ ├── benchmark_retrieval.cpp │ │ ├── benchmark_segmentation.cpp │ │ ├── benchmark_visualization.cpp │ │ └── surfel_renderer.cpp │ └── test │ │ ├── test_query_annotated_sweep.cpp │ │ ├── test_renderer.cpp │ │ ├── test_results_image.cpp │ │ ├── test_static_matching.cpp │ │ ├── test_summarize_benchmark.cpp │ │ └── test_visualize_query.cpp └── retrieval_tools │ ├── CMakeLists.txt │ ├── include │ └── retrieval_tools │ │ └── surfel_type.h │ ├── package.xml │ └── src │ ├── annotate_sweep_segment.cpp │ ├── create_convex_surfels.cpp │ ├── create_line_point_clouds.cpp │ ├── create_overlap_cloud.cpp │ ├── create_surfel_overlap_cloud.cpp │ ├── create_sweep_illustration.cpp │ └── transform_surfel_cloud.cpp ├── ekz-public-lib ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.txt ├── include │ ├── FeatureDescriptor │ │ ├── FeatureDescriptor.h │ │ ├── FloatHistogramFeatureDescriptor.h │ │ ├── OrbFeatureDescriptor.h │ │ ├── SurfFeatureDescriptor128.h │ │ └── SurfFeatureDescriptor64.h │ ├── FeatureExtractor │ │ ├── FeatureExtractor.h │ │ ├── OrbExtractor.h │ │ └── SurfExtractor.h │ ├── FrameMatcher │ │ ├── AICK.h │ │ ├── BowAICK.h │ │ └── FrameMatcher.h │ ├── Map │ │ ├── Map3D.h │ │ └── Map3Dbow.h │ ├── RGBDSegmentation │ │ └── RGBDSegmentation.h │ ├── TransformationFilter │ │ └── TransformationFilter.h │ ├── core │ │ ├── Calibration.h │ │ ├── FrameInput.h │ │ ├── RGBDFrame.h │ │ ├── Transformation.h │ │ └── core.h │ ├── ekz.h │ └── mygeometry │ │ ├── HasDistance.h │ │ ├── KeyPoint.h │ │ ├── KeyPointChain.h │ │ ├── KeyPointSet.h │ │ ├── Line.h │ │ ├── Line2D.h │ │ ├── Plane.h │ │ ├── PlaneChain.h │ │ ├── Point.h │ │ └── mygeometry.h ├── package.xml └── src │ ├── FeatureDescriptor │ ├── FeatureDescriptor.cpp │ ├── FloatHistogramFeatureDescriptor.cpp │ ├── OrbFeatureDescriptor.cpp │ ├── SurfFeatureDescriptor128.cpp │ └── SurfFeatureDescriptor64.cpp │ ├── FeatureExtractor │ ├── .goutputstream-MIEO6W │ ├── FeatureExtractor.cpp │ ├── OrbExtractor.cpp │ └── SurfExtractor.cpp │ ├── FrameMatcher │ ├── AICK.cpp │ ├── BowAICK.cpp │ └── FrameMatcher.cpp │ ├── Map │ ├── Map3D.cpp │ └── Map3Dbow.cpp │ ├── RGBDSegmentation │ └── RGBDSegmentation.cpp │ ├── TransformationFilter │ └── TransformationFilter.cpp │ ├── apps │ ├── example_bow_images.cpp │ ├── example_register_images_fast_map.cpp │ ├── example_register_images_map.cpp │ ├── example_register_images_standalone.cpp │ ├── example_register_pcd_map.cpp │ ├── example_register_pcd_standalone.cpp │ ├── image_recorder.cpp │ └── pcd_recorder.cpp │ ├── core │ ├── FrameInput.cpp │ ├── RGBDFrame.cpp │ └── Transformation.cpp │ └── mygeometry │ ├── HasDistance.cpp │ ├── KeyPoint.cpp │ ├── KeyPointChain.cpp │ ├── KeyPointSet.cpp │ ├── Line.cpp │ ├── Line2D.cpp │ ├── Plane.cpp │ ├── PlaneChain.cpp │ └── Point.cpp ├── learn_objects_action ├── CMakeLists.txt ├── README.md ├── action │ └── LearnObject.action ├── launch │ └── learn_objects_dependencies.launch ├── package.xml ├── scripts │ ├── call_action_server.py │ ├── server.py │ └── server_y3.py ├── setup.py └── src │ └── learn_objects_action │ ├── __init__.py │ ├── control.py │ ├── machine.py │ ├── machine_y3.py │ ├── metric_sweep.py │ ├── ptu_track.py │ ├── record.py │ ├── util.py │ ├── vision.py │ └── vision_y3.py ├── metaroom_xml_parser ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── apps │ ├── load_additional_views_main.cpp │ ├── load_labelled_data.cpp │ ├── load_multiple_files_main.cpp │ ├── load_single_file_main.cpp │ ├── print_objects_with_views.cpp │ ├── print_sweep_xmls.cpp │ ├── print_sweep_xmls_at_waypoint.cpp │ └── test_dynamic_object_parser.cpp ├── include │ └── metaroom_xml_parser │ │ ├── load_utilities.h │ │ ├── load_utilities.hpp │ │ ├── simple_dynamic_object_parser.h │ │ ├── simple_summary_parser.h │ │ └── simple_xml_parser.h ├── package.xml └── src │ ├── load_utilities.cpp │ ├── simple_dynamic_object_parser.cpp │ ├── simple_summary_parser.cpp │ └── simple_xml_parser.cpp ├── nbv_planning ├── CMakeLists.txt ├── README.md ├── include │ └── nbv_planning │ │ ├── NBVFinder.h │ │ ├── NBVFinderROS.h │ │ ├── Ray.h │ │ ├── SensorModel.h │ │ ├── TargetVolume.h │ │ └── conversions.h ├── notes.rst ├── package.xml ├── scripts │ ├── capture_some_clouds.py │ └── test_rays.py ├── src │ ├── NBVFinder.cpp │ ├── NBVFinderROS.cpp │ ├── Ray.cpp │ ├── SensorModel.cpp │ ├── TargetVolume.cpp │ ├── nbv_planner_pcds.cpp │ ├── nbv_planner_server.cpp │ └── test_model.cpp ├── srv │ ├── SelectNextView.srv │ ├── SetTarget.srv │ ├── SetViews.srv │ └── Update.srv └── test_files │ ├── out.yaml │ ├── out0.pcd │ ├── out1.pcd │ ├── out2.pcd │ ├── out3.pcd │ ├── out4.pcd │ ├── out5.pcd │ ├── view0.pcd │ ├── view1.pcd │ ├── view10.pcd │ ├── view11.pcd │ ├── view12.pcd │ ├── view2.pcd │ ├── view3.pcd │ ├── view4.pcd │ ├── view5.pcd │ ├── view6.pcd │ ├── view7.pcd │ ├── view8.pcd │ ├── view9.pcd │ └── views.yaml ├── object_manager ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── object_manager │ │ ├── dynamic_object.h │ │ ├── dynamic_object_mongodb_interface.h │ │ ├── dynamic_object_utilities.h │ │ ├── dynamic_object_xml_parser.h │ │ └── object_manager.h ├── msg │ ├── DynamicObjectTrackingData.msg │ └── DynamicObjectTracks.msg ├── package.xml ├── src │ ├── dynamic_object.cpp │ ├── dynamic_object_compute_mask_server.cpp │ ├── dynamic_object_mongodb_interface.cpp │ ├── dynamic_object_utilities.cpp │ ├── dynamic_object_xml_parser.cpp │ ├── load_objects_from_mongo.cpp │ ├── object_manager.cpp │ └── object_manager_main.cpp └── srv │ ├── DynamicObjectComputeMaskService.srv │ ├── DynamicObjectsService.srv │ ├── GetDynamicObjectService.srv │ └── ProcessDynamicObjectService.srv ├── object_view_generator ├── CMakeLists.txt ├── README.md ├── package.xml ├── scripts │ ├── test_with_rviz.py │ └── view_points_service.py └── srv │ └── GetTrajectoryPoints.srv ├── observation_registration ├── README.md ├── additional_view_registration_server │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── additional_view_registration_server │ │ │ ├── additional_view_registration_optimizer.h │ │ │ ├── additional_view_registration_residual.h │ │ │ └── sift_wrapper.h │ ├── package.xml │ ├── src │ │ ├── additional_view_registration_main.cpp │ │ ├── additional_view_registration_optimizer.cpp │ │ └── sift_wrapper.cpp │ └── test │ │ └── test_additional_view_registration.cpp ├── observation_registration_launcher │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ └── observation_registration.launch │ └── package.xml ├── observation_registration_server │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── observation_registration_server │ │ │ ├── observation_registration_optimizer.h │ │ │ ├── observation_residual.h │ │ │ └── sift_wrapper.h │ ├── package.xml │ ├── src │ │ ├── observation_registration_main.cpp │ │ ├── observation_registration_optimizer.cpp │ │ └── sift_wrapper.cpp │ └── test │ │ └── test_observation_registration.cpp ├── observation_registration_services │ ├── CMakeLists.txt │ ├── README.md │ ├── package.xml │ └── srv │ │ ├── AdditionalViewRegistrationService.srv │ │ ├── GetLastAdditionalViewRegistrationResultService.srv │ │ ├── ObjectAdditionalViewRegistrationService.srv │ │ ├── ObservationRegistrationService.srv │ │ └── ProcessRegisteredViews.srv └── siftgpu │ ├── CMakeLists.txt │ ├── README.txt │ ├── include │ └── siftgpu │ │ ├── CuTexImage.h │ │ ├── FrameBufferObject.h │ │ ├── GLTexImage.h │ │ ├── GlobalUtil.h │ │ ├── LiteWindow.h │ │ ├── ProgramCU.h │ │ ├── ProgramGLSL.h │ │ ├── ProgramGPU.h │ │ ├── PyramidCU.h │ │ ├── PyramidGL.h │ │ ├── ShaderMan.h │ │ ├── SiftGPU.h │ │ ├── SiftMatch.h │ │ ├── SiftMatchCU.h │ │ └── SiftPyramid.h │ ├── license.txt │ ├── package.xml │ └── src │ ├── CuTexImage.cpp │ ├── FrameBufferObject.cpp │ ├── GLTexImage.cpp │ ├── GlobalUtil.cpp │ ├── ProgramCU.cu │ ├── ProgramGLSL.cpp │ ├── ProgramGPU.cpp │ ├── PyramidCU.cpp │ ├── PyramidGL.cpp │ ├── ShaderMan.cpp │ ├── SiftGPU.cpp │ ├── SiftMatch.cpp │ ├── SiftMatchCU.cpp │ └── SiftPyramid.cpp ├── quasimodo ├── README.md ├── quasimodo_brain │ ├── CMakeLists.txt │ ├── launch │ │ ├── brain.launch │ │ ├── modelserver.launch │ │ └── robot_listener.launch │ ├── package.xml │ └── src │ │ ├── MeshTest.cpp │ │ ├── ModelDatabase │ │ ├── ModelDatabase.cpp │ │ ├── ModelDatabase.h │ │ ├── ModelDatabaseBasic.cpp │ │ ├── ModelDatabaseBasic.h │ │ ├── ModelDatabaseRGBHistogram.cpp │ │ ├── ModelDatabaseRGBHistogram.h │ │ ├── ModelDatabaseRetrieval.cpp │ │ └── ModelDatabaseRetrieval.h │ │ ├── bag_reader.cpp │ │ ├── build_dataset_node.cpp │ │ ├── fb_node.cpp │ │ ├── graph_part.cpp │ │ ├── kinect_node.cpp │ │ ├── load_dataset_node.cpp │ │ ├── massregPCD.cpp │ │ ├── modelserver.cpp │ │ ├── pcd_node.cpp │ │ ├── preload_chair_data.cpp │ │ ├── preload_data.cpp │ │ ├── preload_object_data.cpp │ │ ├── preload_object_data2.cpp │ │ ├── preload_test_data.cpp │ │ ├── preloaderv2.cpp │ │ ├── robot_listener.cpp │ │ ├── superpixel_test.cpp │ │ ├── test_ModelDatabase.cpp │ │ ├── test_align.cpp │ │ ├── test_msgs.cpp │ │ ├── test_noise.cpp │ │ ├── usepreload.cpp │ │ └── velodyne2.cpp ├── quasimodo_conversions │ ├── CATKIN_IGNORE │ ├── CMakeLists.txt │ ├── include │ │ └── quasimodo_conversions │ │ │ └── conversions.h │ ├── package.xml │ └── src │ │ └── quasimodo_conversions │ │ └── conversions.cpp ├── quasimodo_launch │ ├── CMakeLists.txt │ ├── launch │ │ └── quasimodo.launch │ └── package.xml ├── quasimodo_models │ ├── CMakeLists.txt │ ├── include │ │ ├── core │ │ │ ├── Camera.h │ │ │ ├── RGBDFrame.h │ │ │ ├── Util.h │ │ │ └── nanoflann.hpp │ │ ├── mesher │ │ │ └── Mesh.h │ │ ├── model │ │ │ ├── Model.h │ │ │ └── ModelMask.h │ │ ├── modelupdater │ │ │ ├── ModelUpdater.h │ │ │ ├── ModelUpdaterBasic.h │ │ │ └── ModelUpdaterBasicFuse.h │ │ ├── modelupdater2 │ │ │ └── ModelUpdater2.h │ │ ├── registration │ │ │ ├── MassRegistration.h │ │ │ ├── MassRegistrationPPR.h │ │ │ ├── Registration.h │ │ │ ├── RegistrationRandom.h │ │ │ ├── RegistrationRefinement.h │ │ │ └── nanoflann.hpp │ │ └── weightfunctions │ │ │ ├── DistanceWeightFunction2.h │ │ │ ├── DistanceWeightFunction2PPR.h │ │ │ └── DistanceWeightFunction2PPR2.h │ ├── package.xml │ └── src │ │ ├── core │ │ ├── Camera.cpp │ │ ├── RGBDFrame.cpp │ │ └── Util.cpp │ │ ├── mesher │ │ └── Mesh.cpp │ │ ├── model │ │ ├── Model.cpp │ │ └── ModelMask.cpp │ │ ├── modelupdater │ │ ├── ModelUpdater.cpp │ │ ├── ModelUpdaterBasic.cpp │ │ └── ModelUpdaterBasicFuse.cpp │ │ ├── registration │ │ ├── MassRegistration.cpp │ │ ├── MassRegistrationPPR.cpp │ │ ├── Registration.cpp │ │ ├── RegistrationRandom.cpp │ │ └── RegistrationRefinement.cpp │ │ └── weightfunctions │ │ ├── DistanceWeightFunction2.cpp │ │ ├── DistanceWeightFunction2PPR.cpp │ │ └── DistanceWeightFunction2PPR2.cpp ├── quasimodo_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── SOMA2Object.msg │ │ ├── fused_world_state_object.msg │ │ ├── image_array.msg │ │ ├── int_array.msg │ │ ├── model.msg │ │ ├── model_array.msg │ │ ├── retrieval_query.msg │ │ ├── retrieval_query_array.msg │ │ ├── retrieval_query_result.msg │ │ ├── retrieval_result.msg │ │ ├── rgbd_frame.msg │ │ └── string_array.msg │ ├── package.xml │ └── srv │ │ ├── cloud_from_model.srv │ │ ├── fuse_models.srv │ │ ├── get_model.srv │ │ ├── index_cloud.srv │ │ ├── index_frame.srv │ │ ├── insert_model.srv │ │ ├── mask_pointclouds.srv │ │ ├── model_from_frame.srv │ │ ├── model_to_retrieval_query.srv │ │ ├── query_cloud.srv │ │ ├── simple_query_cloud.srv │ │ ├── transform_cloud.srv │ │ └── visualize_query.srv ├── quasimodo_object_search │ ├── CMakeLists.txt │ ├── launch │ │ └── object_search.launch │ ├── package.xml │ └── scripts │ │ ├── create_object_search_observation.py │ │ ├── insert_object_server.py │ │ └── retrieve_object_search.py ├── quasimodo_optimization │ ├── CMakeLists.txt │ ├── action │ │ └── rosbag_play.action │ ├── launch │ │ └── optimizer.launch │ ├── package.xml │ └── scripts │ │ ├── optimizer.py │ │ ├── plot_values.py │ │ └── rosbag_player.py ├── quasimodo_retrieval │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ └── parameters.cfg │ ├── launch │ │ └── retrieval.launch │ ├── package.xml │ ├── scripts │ │ └── analyze_retrieval_queries.py │ └── src │ │ ├── quasimodo_logging_server.cpp │ │ ├── quasimodo_retrieval_node.cpp │ │ ├── quasimodo_retrieval_publisher.cpp │ │ ├── quasimodo_retrieval_server.cpp │ │ ├── quasimodo_retrieve_observation.cpp │ │ ├── quasimodo_visualization_server.cpp │ │ ├── quasimodo_visualize_model.cpp │ │ └── quasimodo_visualize_retrieval_cloud.cpp └── retrieval_processing │ ├── CMakeLists.txt │ ├── launch │ └── processing.launch │ ├── package.xml │ └── src │ ├── retrieval_features.cpp │ ├── retrieval_segmentation.cpp │ ├── retrieval_simulate_observations.cpp │ └── retrieval_vocabulary.cpp ├── scitos_ptu_sweep ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ └── PTUSweep.action ├── launch │ └── ptu_sweep.launch ├── package.xml ├── scripts │ ├── PTU_sweep.py │ ├── PTU_test.py │ └── sweep_save.py └── src │ └── transform_pc2.cpp ├── semantic_map ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── semantic_map │ │ ├── constants.h │ │ ├── metaroom.h │ │ ├── metaroom.hpp │ │ ├── metaroom_update_iteration.h │ │ ├── metaroom_update_iteration.hpp │ │ ├── metaroom_xml_parser.h │ │ ├── metaroom_xml_parser.hpp │ │ ├── mongodb_interface.h │ │ ├── ndt_registration.h │ │ ├── occlusion_checker.h │ │ ├── reg_features.h │ │ ├── reg_transforms.h │ │ ├── room.h │ │ ├── room.hpp │ │ ├── room_utilities.h │ │ ├── room_xml_parser.h │ │ ├── room_xml_parser.hpp │ │ ├── roombase.h │ │ ├── roombase.hpp │ │ ├── semantic_map_node.h │ │ ├── semantic_map_summary_parser.h │ │ └── sweep_parameters.h ├── launch │ └── semantic_map.launch ├── msg │ └── RoomObservation.msg ├── package.xml ├── scripts │ ├── ftp_upload_task_client.py │ └── metric_map_task_client.py ├── src │ ├── add_to_mongo.cpp │ ├── load_from_mongo.cpp │ ├── metaroom.cpp │ ├── metaroom_update_iteration.cpp │ ├── metaroom_xml_parser.cpp │ ├── mongodb_interface.cpp │ ├── ndt_registration.cpp │ ├── occlusion_checker.cpp │ ├── reg_features.cpp │ ├── reg_transforms.cpp │ ├── room.cpp │ ├── room_utilities.cpp │ ├── room_xml_parser.cpp │ ├── roombase.cpp │ ├── semantic_map_main.cpp │ ├── semantic_map_node.cpp │ ├── semantic_map_summary_parser.cpp │ └── sweep_parameters.cpp └── srv │ ├── ClearMetaroomService.srv │ ├── DynamicClusterService.srv │ ├── MetaroomService.srv │ └── ObservationService.srv ├── semantic_map_launcher ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── launch │ └── semantic_map.launch └── package.xml ├── semantic_map_publisher ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── semantic_map_publisher │ │ └── semantic_map_publisher.h ├── package.xml ├── src │ ├── semantic_map_publisher.cpp │ └── semantic_map_publisher_main.cpp └── srv │ ├── MetaroomService.srv │ ├── ObservationInstanceService.srv │ ├── ObservationOctomapInstanceService.srv │ ├── ObservationOctomapService.srv │ ├── ObservationService.srv │ ├── SensorOriginService.srv │ ├── WaypointInfoService.srv │ └── WaypointTimestampService.srv ├── semantic_map_to_2d ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── semantic_map_to_2d │ │ └── SemanticMap2dServer.h ├── package.xml ├── src │ ├── SemanticMap2DServer.cpp │ └── semantic_map_2d_server.cpp └── srv │ └── ChangeWaypoint.srv ├── strands_sweep_registration ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── strands_sweep_registration │ │ ├── Camera.h │ │ ├── Frame.h │ │ ├── PixelFunction.h │ │ ├── ProblemFrameConnection.h │ │ ├── RobotContainer.h │ │ ├── Sweep.h │ │ ├── camera_parameters.h │ │ ├── pair3DError.h │ │ └── util.h ├── package.xml └── src │ ├── Camera.cpp │ ├── Frame.cpp │ ├── PixelFunction.cpp │ ├── ProblemFrameConnection.cpp │ ├── RobotContainer.cpp │ ├── Sweep.cpp │ ├── apps │ ├── popcnt.cpp │ ├── reader.cpp │ └── reader_rares.cpp │ ├── pair3DError.cpp │ └── util.cpp └── world_state ├── .gitignore ├── CMakeLists.txt ├── package.xml ├── scripts ├── manage_object_master.py ├── observation_trial.py └── test_data.py ├── setup.py ├── src └── world_state │ ├── __init__.py │ ├── exceptions.py │ ├── geometry.py │ ├── identification.py │ ├── mongo.py │ ├── objectmaster.py │ ├── observation.py │ ├── report.py │ └── state.py └── unittests ├── test_objectmaster.py └── test_world.py /.gitignore: -------------------------------------------------------------------------------- 1 | Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | *.pyc 10 | 11 | # Packages # 12 | ############ 13 | # it's better to unpack these files and commit the raw source 14 | # git has its own built in compression methods 15 | *.7z 16 | *.dmg 17 | *.gz 18 | *.iso 19 | *.jar 20 | *.rar 21 | *.tar 22 | *.zip 23 | 24 | # Logs and databases # 25 | ###################### 26 | *.log 27 | *.sql 28 | *.sqlite 29 | 30 | # OS generated files # 31 | ###################### 32 | .DS_Store 33 | .DS_Store? 34 | ._* 35 | .Spotlight-V100 36 | .Trashes 37 | ehthumbs.db 38 | Thumbs.db 39 | *~ -------------------------------------------------------------------------------- /calibrate_sweeps/action/CalibrateSweeps.action: -------------------------------------------------------------------------------- 1 | #goal 2 | int32 min_num_sweeps 3 | int32 max_num_sweeps 4 | string sweep_location 5 | string save_location 6 | --- 7 | #result 8 | string calibration_file 9 | --- 10 | #feedback 11 | int32 percentage_done 12 | -------------------------------------------------------------------------------- /cloud_merge/action/Sweep.action: -------------------------------------------------------------------------------- 1 | #goal 2 | string type 3 | --- 4 | #result 5 | bool success 6 | --- 7 | #feedback 8 | sensor_msgs/JointState feedback_ptu_pose 9 | 10 | -------------------------------------------------------------------------------- /cloud_merge/src/cloud_merge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "cloud_merge/cloud_merge.h" 4 | 5 | 6 | template class CloudMerge; 7 | -------------------------------------------------------------------------------- /cloud_merge/src/cloud_merge_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "cloud_merge/cloud_merge_node.h" 4 | 5 | template class CloudMergeNode; 6 | -------------------------------------------------------------------------------- /cloud_merge/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "cloud_merge/cloud_merge_node.h" 2 | 3 | void callback(const std_msgs::String& controlString) 4 | { 5 | ROS_INFO_STREAM("Received control string "< aCloudMergeNode(aRosNode); 17 | 18 | ros::Rate loop_rate(10); 19 | while (ros::ok()) 20 | { 21 | ros::spinOnce(); 22 | loop_rate.sleep(); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/convex_segmentation/msg/CloudArray.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2[] clouds 2 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/cmake/FindROS.cmake: -------------------------------------------------------------------------------- 1 | # Find ROS library 2 | # 3 | # ROS_INCLUDE_DIR where to find the include files 4 | # ROS_LIBRARY_DIR where to find the libraries 5 | # ROS_LIBRARIES list of libraries to link 6 | # ROS_FOUND true if ROS was found 7 | 8 | SET( ROS_LIBRARYDIR / CACHE PATH "Alternative library directory" ) 9 | SET( ROS_INCLUDEDIR / CACHE PATH "Alternative include directory" ) 10 | MARK_AS_ADVANCED( ROS_LIBRARYDIR ROS_INCLUDEDIR ) 11 | 12 | FIND_LIBRARY(ROS_LIBRARY_DIR 13 | NAMES rviz rosconsole roscpp rqt_rviz message_store roscpp_serialization rostime 14 | PATHS ${ROS_LIBRARYDIR}) 15 | FIND_PATH(ROS_INCLUDE_DIR 16 | NAMES ros/ros.h ros/time.h rqt_rviz/rviz.h 17 | PATHS ${ROS_INCLUDEDIR}) 18 | 19 | GET_FILENAME_COMPONENT( ROS_LIBRARY_DIR ${ROS_LIBRARY_DIR} PATH ) 20 | 21 | IF( ROS_INCLUDE_DIR AND ROS_LIBRARY_DIR ) 22 | SET( ROS_LIBRARIES image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser pcl_ros_tf) 23 | SET( ROS_FOUND TRUE ) 24 | ELSE( ROS_INCLUDE_DIR AND ROS_LIBRARY_DIR ) 25 | SET( ROS_FOUND FALSE ) 26 | ENDIF( ROS_INCLUDE_DIR AND ROS_LIBRARY_DIR ) 27 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/definitions.h: -------------------------------------------------------------------------------- 1 | #ifndef DEFINITIONS_H 2 | #define DEFINITIONS_H 3 | 4 | #define VT_PRECOMPILE 5 | 6 | static const int N = 250; 7 | //static const int N = 1344; 8 | 9 | //namespace dynamic_object_retrieval { 10 | // 11 | //} // namespace dynamic_object_retrieval 12 | 13 | #endif // DEFINITIONS_H 14 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/visualize.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNAMIC_VISUALIZE_H 2 | #define DYNAMIC_VISUALIZE_H 3 | 4 | #include 5 | #include 6 | 7 | #define VT_PRECOMPILE 8 | #include 9 | #include 10 | #include 11 | #include "dynamic_object_retrieval/definitions.h" 12 | 13 | using PointT = pcl::PointXYZRGB; 14 | using CloudT = pcl::PointCloud; 15 | using HistT = pcl::Histogram; 16 | using HistCloudT = pcl::PointCloud; 17 | 18 | namespace dynamic_object_retrieval { 19 | 20 | void visualize(CloudT::Ptr& cloud); 21 | void visualize(CloudT::Ptr& cloud, float subsample_size); 22 | void save_vocabulary(vocabulary_tree& vt, const boost::filesystem::path& vocabulary_path); 23 | void load_vocabulary(vocabulary_tree& vt, const boost::filesystem::path& vocabulary_path); 24 | void save_vocabulary(grouped_vocabulary_tree& vt, const boost::filesystem::path& vocabulary_path); 25 | void load_vocabulary(grouped_vocabulary_tree& vt, const boost::filesystem::path& vocabulary_path); 26 | 27 | } 28 | 29 | #endif // DYNAMIC_VISUALIZE_H 30 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/include/extract_sift/extract_sift.h: -------------------------------------------------------------------------------- 1 | #ifndef EXTRACT_SIFT_H 2 | #define EXTRACT_SIFT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using PointT = pcl::PointXYZRGB; 10 | using CloudT = pcl::PointCloud; 11 | using SiftT = pcl::Histogram<128>; 12 | using SiftCloudT = pcl::PointCloud; 13 | 14 | namespace extract_sift { 15 | 16 | void extract_sift_for_sweep(const boost::filesystem::path& xml_path, bool visualize = false); 17 | std::pair extract_sift_for_image(cv::Mat& image, cv::Mat& depth, const Eigen::Matrix3f& K); 18 | std::pair extract_sift_for_cloud(CloudT::Ptr& cloud, const Eigen::Matrix3f& K); 19 | // optionally provide the cloud as well 20 | std::tuple get_sift_for_cloud_path(const boost::filesystem::path& cloud_path); 21 | std::pair get_sift_for_cloud_path(const boost::filesystem::path& cloud_path, CloudT::Ptr& cloud); 22 | std::tuple get_sift_for_cloud_path(const std::vector& cloud_path); 23 | 24 | } // namespace extract_sift 25 | 26 | #endif // EXTRACT_SIFT_H 27 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/include/object_3d_retrieval/pfhrgb_estimation.h: -------------------------------------------------------------------------------- 1 | #ifndef PFHRGB_ESTIMATION_H 2 | #define PFHRGB_ESTIMATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace pfhrgb_estimation { 8 | 9 | using PointT = pcl::PointXYZRGB; 10 | using CloudT = pcl::PointCloud; 11 | using PfhRgbT = pcl::Histogram<250>; 12 | using PfhRgbCloudT = pcl::PointCloud; 13 | using NormalT = pcl::Normal; 14 | using NormalCloudT = pcl::PointCloud; 15 | 16 | void visualize_keypoints(CloudT::Ptr& cloud, CloudT::Ptr& keypoints); 17 | void compute_query_features(PfhRgbCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, bool visualize_features = false); 18 | void compute_regularized_query_features(PfhRgbCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, bool visualize_features = false); 19 | void compute_features(PfhRgbCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, bool visualize_features = false); 20 | void compute_surfel_features(PfhRgbCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, bool visualize_features = false, bool is_query = false); 21 | void split_descriptor_points(std::vector& split_features, std::vector& split_keypoints, 22 | PfhRgbCloudT::Ptr& features, CloudT::Ptr& keypoints, int expected_cluster_size = 30, bool verbose = false); 23 | void visualize_split_keypoints(std::vector& split_keypoints); 24 | 25 | } 26 | 27 | #endif // PFHRGB_ESTIMATION_H 28 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/include/object_3d_retrieval/shot_estimation.h: -------------------------------------------------------------------------------- 1 | #ifndef SHOT_ESTIMATION_H 2 | #define SHOT_ESTIMATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace shot_estimation { 8 | 9 | using PointT = pcl::PointXYZRGB; 10 | using CloudT = pcl::PointCloud; 11 | using ShotT = pcl::Histogram<1344>; 12 | using ShotCloudT = pcl::PointCloud; 13 | using NormalT = pcl::Normal; 14 | using NormalCloudT = pcl::PointCloud; 15 | 16 | void visualize_keypoints(CloudT::Ptr& cloud, CloudT::Ptr& keypoints); 17 | void compute_query_features(ShotCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, bool visualize_features = false); 18 | void compute_features(ShotCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, bool visualize_features = false); 19 | void split_descriptor_points(std::vector& split_features, std::vector& split_keypoints, 20 | ShotCloudT::Ptr& features, CloudT::Ptr& keypoints, int expected_cluster_size = 30, bool verbose = false); 21 | void visualize_split_keypoints(std::vector& split_keypoints); 22 | 23 | 24 | } // namespace shot_estimation 25 | 26 | #endif // SHOT_ESTIMATION_H 27 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/scripts/build_surfel_maps.bash: -------------------------------------------------------------------------------- 1 | for f in $(find 'KTH_Objects' -name '*room.xml'); do 2 | rosrun surfelize_it surfelize_it -l "$f" 3 | done 4 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/scripts/clean_data_dir.bash: -------------------------------------------------------------------------------- 1 | rm -rf ./*/*/*/subsegments && rm -rf ./*/*/*/convex_segments && rm -rf ./*/*/*/sift_features.pcd && rm -rf ./*/*/*/sift_keypoints.pcd && rm segments_summary.json 2 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/scripts/convert_surfel_maps.bash: -------------------------------------------------------------------------------- 1 | for f in $(find . -name '*surfel_map.pcd'); do 2 | rosrun surfel_publisher surfel_exporter _cloud_name:=$f _threshold:=0.3 3 | done 4 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/scripts/rename_complete_surfels.bash: -------------------------------------------------------------------------------- 1 | for f in $(find . -wholename '*backup_complete_cloud.pcd'); do 2 | DIR=$(dirname "${f}") 3 | mv $f $DIR/complete_cloud.pcd 4 | done 5 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/scripts/summarize_segmentation_benchmark.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import os 4 | import matplotlib 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | import sys 8 | 9 | def get_values(folder): 10 | rootdir = os.path.abspath(folder) 11 | 12 | print rootdir 13 | 14 | topdirs = [ os.path.join(rootdir, name) for name in os.listdir(rootdir) if os.path.isdir(os.path.join(rootdir, name)) ] 15 | for objectdir in topdirs: 16 | #print subdir 17 | overlaps = [] 18 | for subdir, dirs, files in os.walk(objectdir): 19 | for somefile in files: 20 | if somefile != "overlaps.txt": 21 | continue 22 | with open(os.path.join(subdir, somefile)) as f: 23 | for line in f: 24 | overlaps.append(float(line)) 25 | #print overlaps 26 | meanoverlap = np.mean(np.array(overlaps)) 27 | print "Mean for %s: %f" % (objectdir, meanoverlap) 28 | 29 | 30 | 31 | if __name__ == "__main__": 32 | if len(sys.argv) < 2: 33 | sys.exit("Please provide directory to check!") 34 | get_values(sys.argv[1]) 35 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/scripts/verify_sweeps.bash: -------------------------------------------------------------------------------- 1 | for f in $(find . -name '*rgb_*_label_0.jpg'); do 2 | p=$(dirname "${f}") 3 | c=$(find "${p}" -name 'intermediate*.pcd' | wc -l) 4 | if [ $c -lt 17 ]; then 5 | echo $p 6 | fi 7 | done 8 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/src/dynamic_extract_sift.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | using namespace std; 7 | 8 | using PointT = pcl::PointXYZRGB; 9 | 10 | int main(int argc, char** argv) 11 | { 12 | if (argc < 2) { 13 | cout << "Please supply the path containing the sweeps..." << endl; 14 | return -1; 15 | } 16 | 17 | boost::filesystem::path data_path(argv[1]); 18 | 19 | vector folder_xmls = semantic_map_load_utilties::getSweepXmls(data_path.string(), true); 20 | 21 | for (const string& xml : folder_xmls) { 22 | std::cout << "Analyzing sweep: " << xml << std::endl; 23 | extract_sift::extract_sift_for_sweep(boost::filesystem::path(xml)); 24 | } 25 | 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/src/dynamic_retrieval.cpp: -------------------------------------------------------------------------------- 1 | #include "dynamic_object_retrieval/dynamic_retrieval.h" 2 | #include "dynamic_object_retrieval/summary_iterators.h" 3 | #include "dynamic_object_retrieval/definitions.h" 4 | 5 | using namespace std; 6 | 7 | POINT_CLOUD_REGISTER_POINT_STRUCT (HistT, 8 | (float[N], histogram, histogram) 9 | ) 10 | 11 | namespace dynamic_object_retrieval { 12 | 13 | } 14 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/stopwatch/.gitignore: -------------------------------------------------------------------------------- 1 | build/* 2 | .project 3 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/stopwatch/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6.0) 2 | project(StopwatchViewer) 3 | 4 | find_package(Qt4 REQUIRED) 5 | include(${QT_USE_FILE}) 6 | 7 | set(QT_USE_QTNETWORK TRUE) 8 | 9 | QT4_WRAP_UI(view_UIS_H stopwatchviewer.ui) 10 | 11 | qt4_wrap_cpp(view_moc_srcs 12 | dataPlotWidget.h 13 | plotHolderWidget.h 14 | stopwatchviewer.h 15 | ) 16 | 17 | add_executable(StopwatchViewer 18 | main.cpp 19 | ${view_moc_srcs} 20 | ${view_UIS_H} 21 | dataPlotWidget.h 22 | dataPlotWidget.cpp 23 | plotHolderWidget.h 24 | plotHolderWidget.cpp 25 | stopwatchviewer.cpp 26 | stopwatchviewer.h 27 | RingBuffer.h 28 | StopwatchDecoder.h 29 | ) 30 | 31 | target_link_libraries(StopwatchViewer 32 | ${QT_LIBRARIES} 33 | ${QT_QTNETWORK_LIBRARY}) 34 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/stopwatch/src/StopwatchDecoder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * StopwatchDecoder.h 3 | * 4 | * Created on: 23 Nov 2011 5 | * Author: thomas 6 | */ 7 | 8 | #ifndef STOPWATCHDECODER_H_ 9 | #define STOPWATCHDECODER_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | class StopwatchDecoder 17 | { 18 | public: 19 | static std::pair > > decodePacket(unsigned char * data, int size) 20 | { 21 | const char * stringData = (const char *)&data[sizeof(int) + sizeof(unsigned long long int)]; 22 | 23 | int totalLength = sizeof(int) + sizeof(unsigned long long int); 24 | 25 | std::pair > > values; 26 | 27 | while(totalLength < size) 28 | { 29 | std::pair nextTiming; 30 | 31 | nextTiming.first = std::string(stringData); 32 | stringData += nextTiming.first.length() + 1; 33 | 34 | nextTiming.second = *((float *)stringData); 35 | stringData += sizeof(float); 36 | 37 | totalLength += nextTiming.first.length() + 1 + sizeof(float); 38 | 39 | values.second.push_back(nextTiming); 40 | } 41 | 42 | values.first = *((unsigned long long int *)&data[sizeof(int)]); 43 | 44 | return values; 45 | } 46 | }; 47 | 48 | #endif /* STOPWATCHDECODER_H_ */ 49 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/stopwatch/src/dataPlotWidget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author fiachra & tom 3 | */ 4 | 5 | #ifndef DATA_PLOT_WIDGET_H 6 | #define DATA_PLOT_WIDGET_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | class QPaintEvent; 19 | 20 | class DataPlotWidget : public QWidget 21 | { 22 | Q_OBJECT; 23 | 24 | public: 25 | DataPlotWidget(QWidget * parent = 0); 26 | virtual ~DataPlotWidget(); 27 | 28 | void updatePlot(float * value, bool * enabled); 29 | void resetPlot(); 30 | void setDataLength(int length); 31 | 32 | static const int DEFAULT_DATA_LENGTH = 100; 33 | static const int NUM_PLOTS = 4; 34 | static const int PLOT_WIDTH = 640; 35 | static const int PLOT_HEIGHT = 240; 36 | static const int PEN_WIDTH = 1; 37 | static const int PEN_HIGHLIGHT_WIDTH = 2; 38 | private: 39 | QPainter painter; 40 | 41 | void paintEvent(QPaintEvent * event); 42 | int getPlotY(float val, float dataMin, float dataMax); 43 | void drawDataPlot(); 44 | 45 | int lastLimit; 46 | int dataLength; 47 | float ** dataArray; 48 | int currentCount; 49 | int currentIndex; 50 | }; 51 | 52 | #endif 53 | 54 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/stopwatch/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "stopwatchviewer.h" 2 | 3 | #include 4 | #include 5 | 6 | int main(int argc, char *argv[]) 7 | { 8 | QApplication a(argc, argv); 9 | StopwatchViewer w; 10 | w.show(); 11 | return a.exec(); 12 | } 13 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/stopwatch/src/plotHolderWidget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author fiachra & tom 3 | */ 4 | 5 | #ifndef PLOT_HOLDER_WIDGET_H 6 | #define PLOT_HOLDER_WIDGET_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "RingBuffer.h" 21 | #include "dataPlotWidget.h" 22 | 23 | class QPaintEvent; 24 | 25 | class PlotHolderWidget : public QWidget 26 | { 27 | Q_OBJECT; 28 | 29 | public: 30 | PlotHolderWidget(QWidget * parent = 0); 31 | void update(float * values, bool * enabled); 32 | 33 | DataPlotWidget * dataPlotWidget; 34 | QSlider * plotLength; 35 | QLabel * currentDataLength; 36 | QPushButton * resetButton; 37 | QVBoxLayout * mainLayout; 38 | QVBoxLayout * plotLayout; 39 | QHBoxLayout * lengthLayout; 40 | QHBoxLayout * buttonLayout; 41 | 42 | public slots: 43 | void resetPlot(); 44 | void changeDataLength(int len); 45 | }; 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/dynamic_object_retrieval/test/test_query_keypoints.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | using namespace std; 11 | 12 | using PointT = pcl::PointXYZRGB; 13 | using CloudT = pcl::PointCloud; 14 | using LabelT = semantic_map_load_utilties::LabelledData; 15 | 16 | int main(int argc, char** argv) 17 | { 18 | if (argc < 2) { 19 | cout << "Please supply the path to the data..." << endl; 20 | return -1; 21 | } 22 | 23 | boost::filesystem::path data_path(argv[1]); 24 | 25 | vector folder_xmls = semantic_map_load_utilties::getSweepXmls(data_path.string(), true); 26 | 27 | for (const string& sweep_xml : folder_xmls) { 28 | LabelT labels = semantic_map_load_utilties::loadLabelledDataFromSingleSweep(sweep_xml); 29 | 30 | for (CloudT::Ptr& c : labels.objectClouds) { 31 | HistCloudT::Ptr query_features(new HistCloudT); 32 | CloudT::Ptr keypoints(new CloudT); 33 | pfhrgb_estimation::compute_regularized_query_features(query_features, keypoints, c); 34 | for (PointT p : keypoints->points) { 35 | p.r = 255; p.g = 0; p.b = 0; 36 | c->push_back(p); 37 | } 38 | dynamic_object_retrieval::visualize(c); 39 | } 40 | } 41 | 42 | return 0; 43 | } 44 | 45 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/README.md: -------------------------------------------------------------------------------- 1 | k_means_tree 2 | ============ 3 | 4 | Hierarchical tree organization determined by k means segmentation at each level 5 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | # Visual studio cruft 16 | *.opensdf 17 | *.sdf 18 | *.suo 19 | *.user 20 | */x64 21 | */Debug* 22 | */Release* 23 | *.log 24 | *.tlog* 25 | 26 | # misc files mostly used for testing 27 | out.txt 28 | ptr.txt 29 | test.txt 30 | boost_serialize 31 | arr.txt 32 | performance 33 | include_renamed 34 | .ycm_extra_conf.py* 35 | doc/html 36 | rtti.txt 37 | doc/latex 38 | portability64 39 | portability32 40 | file.json 41 | out.xml 42 | cereal_version.out 43 | xml_ordering.out 44 | build 45 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | compiler: 4 | # TODO: Clang is currently giving issues 5 | #- clang 6 | - gcc 7 | 8 | before_install: 9 | # Always install g++4.8.1 10 | - sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test 11 | 12 | # Install recent version of Boost 13 | - sudo add-apt-repository -y ppa:boost-latest/ppa 14 | 15 | # clang 3.3 16 | - if [ "$CXX" == "clang++" ]; then sudo add-apt-repository -y ppa:h-rayflood/llvm; fi 17 | 18 | - sudo apt-get update -qq 19 | 20 | install: 21 | - sudo apt-get install cmake 22 | - sudo apt-get install libboost1.54-all-dev 23 | 24 | # Always install valgrind 25 | - sudo apt-get install valgrind 26 | 27 | # Always install g++4.8.1 28 | - sudo apt-get install -qq g++-4.8 29 | - sudo apt-get install -qq g++-4.8-multilib 30 | - if [ "$CXX" = "g++" ]; then export CMAKE_CXX_COMPILER="g++-4.8"; fi 31 | - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8"; fi 32 | 33 | # clang 3.3 34 | - if [ "$CXX" == "clang++" ]; then sudo apt-get install --allow-unauthenticated -qq clang-3.3; fi 35 | - if [ "$CXX" == "clang++" ]; then export CMAKE_CXX_COMPILER="clang++-3.3"; fi 36 | - if [ "$CXX" == "clang++" ]; then export CXX="clang++-3.3"; fi 37 | 38 | script: 39 | - mkdir build 40 | - cd build 41 | - cmake .. 42 | - make 43 | 44 | after_script: 45 | - ctest . 46 | # - make valgrind 47 | 48 | branches: 49 | only: 50 | - develop 51 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6.2) 2 | project (cereal) 3 | 4 | option(SKIP_PORTABILITY_TEST "Skip portability tests" OFF) 5 | 6 | set(CMAKE_CXX_FLAGS "-std=c++11 -Wall -Werror -g -Wextra -Wshadow -pedantic ${CMAKE_CXX_FLAGS}") 7 | 8 | include_directories(./include) 9 | 10 | find_package(Boost COMPONENTS serialization unit_test_framework) 11 | 12 | if(Boost_FOUND) 13 | include_directories(${Boost_INCLUDE_DIRS}) 14 | enable_testing() 15 | add_subdirectory(unittests) 16 | endif(Boost_FOUND) 17 | 18 | add_subdirectory(sandbox) 19 | 20 | find_package(Doxygen) 21 | if(DOXYGEN_FOUND) 22 | 23 | configure_file("${CMAKE_CURRENT_SOURCE_DIR}/doc/doxygen.in" "${CMAKE_CURRENT_BINARY_DIR}/doxygen.cfg" @ONLY) 24 | add_custom_target(doc 25 | COMMAND ${DOXYGEN_EXECUTABLE} "${CMAKE_CURRENT_BINARY_DIR}/doxygen.cfg" 26 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" 27 | COMMENT "Generating API documentation with Doxygen" VERBATIM 28 | ) 29 | 30 | configure_file("${CMAKE_CURRENT_SOURCE_DIR}/scripts/updatedoc.in" "${CMAKE_CURRENT_BINARY_DIR}/updatedoc.sh" @ONLY) 31 | add_custom_target(update-doc 32 | COMMAND "${CMAKE_CURRENT_BINARY_DIR}/updatedoc.sh" 33 | DEPENDS doc 34 | COMMENT "Copying documentation to gh-pages branch" VERBATIM 35 | ) 36 | 37 | endif(DOXYGEN_FOUND) 38 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Randolph Voorhies, Shane Grant 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 met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of cereal nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL RANDOLPH VOORHIES OR SHANE GRANT BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/doc/footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | 13 | 14 |
15 | $generatedby   16 | doxygen 17 | $doxygenversion 18 |
19 | 20 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/include/cereal/external/rapidjson/filestream.h: -------------------------------------------------------------------------------- 1 | #ifndef RAPIDJSON_FILESTREAM_H_ 2 | #define RAPIDJSON_FILESTREAM_H_ 3 | 4 | #include 5 | 6 | namespace rapidjson { 7 | 8 | //! Wrapper of C file stream for input or output. 9 | /*! 10 | This simple wrapper does not check the validity of the stream. 11 | \implements Stream 12 | */ 13 | class FileStream { 14 | public: 15 | typedef char Ch; //!< Character type. Only support char. 16 | 17 | FileStream(FILE* fp) : fp_(fp), count_(0) { Read(); } 18 | 19 | char Peek() const { return current_; } 20 | char Take() { char c = current_; Read(); return c; } 21 | size_t Tell() const { return count_; } 22 | void Put(char c) { fputc(c, fp_); } 23 | 24 | // Not implemented 25 | char* PutBegin() { return 0; } 26 | size_t PutEnd(char*) { return 0; } 27 | 28 | private: 29 | void Read() { 30 | RAPIDJSON_ASSERT(fp_ != 0); 31 | int c = fgetc(fp_); 32 | if (c != EOF) { 33 | current_ = (char)c; 34 | count_++; 35 | } 36 | else 37 | current_ = '\0'; 38 | } 39 | 40 | FILE* fp_; 41 | char current_; 42 | size_t count_; 43 | }; 44 | 45 | } // namespace rapidjson 46 | 47 | #endif // RAPIDJSON_FILESTREAM_H_ 48 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/include/cereal/external/rapidjson/internal/strfunc.h: -------------------------------------------------------------------------------- 1 | #ifndef RAPIDJSON_INTERNAL_STRFUNC_H_ 2 | #define RAPIDJSON_INTERNAL_STRFUNC_H_ 3 | 4 | namespace rapidjson { 5 | namespace internal { 6 | 7 | //! Custom strlen() which works on different character types. 8 | /*! \tparam Ch Character type (e.g. char, wchar_t, short) 9 | \param s Null-terminated input string. 10 | \return Number of characters in the string. 11 | \note This has the same semantics as strlen(), the return value is not number of Unicode codepoints. 12 | */ 13 | template 14 | inline SizeType StrLen(const Ch* s) { 15 | const Ch* p = s; 16 | while (*p != '\0') 17 | ++p; 18 | return SizeType(p - s); 19 | } 20 | 21 | } // namespace internal 22 | } // namespace rapidjson 23 | 24 | #endif // RAPIDJSON_INTERNAL_STRFUNC_H_ 25 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/include/cereal/external/rapidjson/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (C) 2011 Milo Yip 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWARE. -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/sandbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(sandbox_shared_lib) 2 | 3 | add_executable(sandbox sandbox.cpp) 4 | add_executable(sandbox_json sandbox_json.cpp) 5 | add_executable(sandbox_rtti sandbox_rtti.cpp) 6 | 7 | add_executable(sandbox_vs sandbox_vs.cpp) 8 | target_link_libraries(sandbox_vs sandbox_vs_dll) 9 | include_directories(sandbox_shared_lib) 10 | 11 | if(Boost_FOUND) 12 | add_executable(performance performance.cpp) 13 | target_link_libraries(performance ${Boost_LIBRARIES}) 14 | endif(Boost_FOUND) 15 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/sandbox/sandbox_shared_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sandbox_vs_dll SHARED base.cpp derived.cpp) 2 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/sandbox/sandbox_shared_lib/base.cpp: -------------------------------------------------------------------------------- 1 | #ifndef CEREAL_DLL_USE 2 | #define CEREAL_DLL_MAKE 3 | #endif 4 | #include "base.hpp" 5 | 6 | template void Base::serialize 7 | ( cereal::XMLOutputArchive & ar, std::uint32_t const version ); 8 | template void Base::serialize 9 | ( cereal::XMLInputArchive & ar, std::uint32_t const version ); 10 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/sandbox/sandbox_shared_lib/base.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #if defined (_WINDLL) 9 | #define DECLSPECIFIER __declspec(dllexport) 10 | #elif defined(MSC_VER) 11 | #define DECLSPECIFIER __declspec(dllimport) 12 | #else 13 | #define DECLSPECIFIER 14 | #endif 15 | 16 | int doit(); 17 | 18 | class VersionTest 19 | { 20 | public: 21 | int x; 22 | template 23 | void serialize( Archive & ar, const std::uint32_t /* version */ ) 24 | { ar( x ); } 25 | }; 26 | 27 | class Base 28 | { 29 | public: 30 | friend class cereal::access; 31 | 32 | template < class Archive > 33 | void serialize(Archive &, std::uint32_t const) {} 34 | virtual ~Base() {} 35 | }; 36 | 37 | extern template DECLSPECIFIER void Base::serialize 38 | ( cereal::XMLInputArchive & ar, std::uint32_t const version ); 39 | 40 | extern template DECLSPECIFIER void Base::serialize 41 | ( cereal::XMLOutputArchive & ar, std::uint32_t const version ); 42 | 43 | CEREAL_CLASS_VERSION(VersionTest, 1) 44 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/sandbox/sandbox_shared_lib/derived.cpp: -------------------------------------------------------------------------------- 1 | #ifndef CEREAL_DLL_USE 2 | #define CEREAL_DLL_MAKE 3 | #endif 4 | #include "derived.hpp" 5 | 6 | template void Derived::serialize 7 | ( cereal::XMLOutputArchive & ar, std::uint32_t const version ); 8 | 9 | template void Derived::serialize 10 | ( cereal::XMLInputArchive & ar, std::uint32_t const version ); 11 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/sandbox/sandbox_shared_lib/derived.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base.hpp" 3 | class Derived : public Base 4 | { 5 | private: 6 | friend class cereal::access; 7 | template 8 | void serialize(Archive & ar, std::uint32_t const) 9 | { 10 | ar(cereal::base_class(this)); 11 | } 12 | }; 13 | 14 | extern template DECLSPECIFIER void Derived::serialize 15 | ( cereal::XMLOutputArchive & ar, std::uint32_t const version ); 16 | extern template DECLSPECIFIER void Derived::serialize 17 | ( cereal::XMLInputArchive & ar, std::uint32_t const version ); 18 | 19 | CEREAL_REGISTER_TYPE(Derived) 20 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/scripts/renameincludes.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | destdir="include_renamed" 4 | 5 | echo -n "New prefix: " 6 | read newprefix 7 | 8 | cp -r include ${destdir} 9 | 10 | newprefix=$(echo ${newprefix} | sed -e 's/\//\\\//') 11 | 12 | find ${destdir} -name '*.hpp' -exec sed -i "s/#include 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/vs2013/sandbox/sandbox.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/vs2013/sandbox_json/sandbox_json.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/vs2013/sandbox_rtti/sandbox_rtti.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/vs2013/sandbox_vs/sandbox_vs.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/cereal/vs2013/sandbox_vs_dll/sandbox_vs_dll.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | Source Files 23 | 24 | 25 | 26 | 27 | Source Files 28 | 29 | 30 | Source Files 31 | 32 | 33 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/src/grouped_vocabulary_tree.cpp: -------------------------------------------------------------------------------- 1 | #include "grouped_vocabulary_tree/grouped_vocabulary_tree.h" 2 | 3 | #include 4 | #include 5 | 6 | template class grouped_vocabulary_tree; 7 | template class grouped_vocabulary_tree, 8>; 8 | template class grouped_vocabulary_tree, 8>; 9 | template class grouped_vocabulary_tree, 8>; 10 | template class grouped_vocabulary_tree, 8>; 11 | template class grouped_vocabulary_tree, 8>; 12 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/src/k_means_tree.cpp: -------------------------------------------------------------------------------- 1 | #include "k_means_tree/k_means_tree.h" 2 | 3 | //#define PCL_NO_PRECOMPILE 4 | #include 5 | #include 6 | 7 | /*POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::Histogram<33>, 8 | (float, histogram, histogram) 9 | )*/ 10 | 11 | template <> 12 | typename map_proxy::map_type eig(pcl::PointXYZ& v) 13 | { 14 | return v.getVector3fMap(); 15 | } 16 | 17 | template <> 18 | typename map_proxy::const_map_type eig(const pcl::PointXYZ& v) 19 | { 20 | return v.getVector3fMap(); 21 | } 22 | 23 | template <> 24 | typename map_proxy::map_type eig(pcl::PointXYZRGB& v) 25 | { 26 | return v.getVector3fMap(); 27 | } 28 | 29 | template <> 30 | typename map_proxy::const_map_type eig(const pcl::PointXYZRGB& v) 31 | { 32 | return v.getVector3fMap(); 33 | } 34 | 35 | //template class k_means_tree; 36 | template class k_means_tree; 37 | template class k_means_tree, 8>; 38 | template class k_means_tree, 8>; 39 | template class k_means_tree, 8>; 40 | template class k_means_tree, 8>; 41 | template class k_means_tree, 8>; 42 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/k_means_tree/src/vocabulary_tree.cpp: -------------------------------------------------------------------------------- 1 | #include "vocabulary_tree/vocabulary_tree.h" 2 | 3 | #include 4 | #include 5 | 6 | //template class vocabulary_tree; 7 | template class vocabulary_tree; 8 | template class vocabulary_tree, 8>; 9 | template class vocabulary_tree, 8>; 10 | template class vocabulary_tree, 8>; 11 | template class vocabulary_tree, 8>; 12 | template class vocabulary_tree, 8>; 13 | template class k_means_tree, 8, inverted_file>; 14 | //template void serialize(cereal::BinaryOutputArchive& archive, vocabulary_tree, 8>& vt); 15 | //template void serialize(cereal::BinaryInputArchive& archive, vocabulary_tree, 8>& vt); 16 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/object_3d_benchmark/include/object_3d_benchmark/benchmark_overlap.h: -------------------------------------------------------------------------------- 1 | #ifndef BENCHMARK_OVERLAP_H 2 | #define BENCHMARK_OVERLAP_H 3 | 4 | #include 5 | #include 6 | 7 | using PointT = pcl::PointXYZRGB; 8 | using CloudT = pcl::PointCloud; 9 | 10 | namespace benchmark_retrieval { 11 | 12 | double compute_overlap(CloudT::Ptr& A, CloudT::Ptr& B, float resolution = 0.08); 13 | 14 | } // namespace benchmark_retrieval 15 | 16 | #endif // BENCHMARK_OVERLAP_H 17 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/object_3d_benchmark/src/benchmark_result.cpp: -------------------------------------------------------------------------------- 1 | #include "object_3d_benchmark/benchmark_result.h" 2 | 3 | #include 4 | 5 | namespace benchmark_retrieval { 6 | 7 | void save_benchmark(const benchmark_result& benchmark, const boost::filesystem::path& benchmark_path) 8 | { 9 | std::ofstream out((benchmark_path / "benchmark.json").string()); 10 | { 11 | cereal::JSONOutputArchive archive_o(out); 12 | archive_o(benchmark); 13 | } 14 | } 15 | 16 | void load_benchmark(benchmark_result& benchmark, const boost::filesystem::path& benchmark_path) 17 | { 18 | std::ifstream in((benchmark_path / "benchmark.json").string()); 19 | { 20 | cereal::JSONInputArchive archive_i(in); 21 | archive_i(benchmark); 22 | } 23 | } 24 | 25 | } // namespace benchmark_retrieval 26 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/object_3d_benchmark/test/test_renderer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | int main(int argc, char** argv) 8 | { 9 | if (argc < 2) { 10 | cout << "Please provide path to sweep data..." << endl; 11 | return -1; 12 | } 13 | 14 | boost::filesystem::path data_path(argv[1]); 15 | vector folder_xmls = semantic_map_load_utilties::getSweepXmls(data_path.string(), true); 16 | 17 | for (const string& xml : folder_xmls) { 18 | boost::filesystem::path xml_path = xml; 19 | boost::filesystem::path surfels_path = xml_path.parent_path() / "surfel_map.pcd"; 20 | Eigen::Matrix3f K; 21 | vector > transforms; 22 | tie(K, transforms) = benchmark_retrieval::get_camera_matrix_and_transforms(xml); 23 | 24 | SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); 25 | pcl::io::loadPCDFile(surfels_path.string(), *surfel_cloud); 26 | 27 | // here, I just want to set the file, position and camera 28 | // parameters (and image size) and render to a cv mat 29 | size_t height, width; 30 | height = 480; 31 | width = 640; 32 | 33 | for (const Eigen::Matrix4f& T : transforms) { 34 | cv::Mat image = benchmark_retrieval::render_surfel_image(surfel_cloud, T, K, height, width); 35 | cv::imshow("Surfel Image", image); 36 | cv::waitKey(); 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /dynamic_object_retrieval/retrieval_tools/src/transform_surfel_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include "retrieval_tools/surfel_type.h" 2 | #include 3 | 4 | using SurfelT = SurfelType; 5 | using SurfelCloudT = pcl::PointCloud; 6 | 7 | using namespace std; 8 | 9 | int main(int argc, char** argv) 10 | { 11 | if (argc < 6) { 12 | cout << "Please provide the path to the cloud and a quaternion w x y z..." << endl; 13 | return 0; 14 | } 15 | boost::filesystem::path cloud_path(argv[1]); 16 | float w = atof(argv[2]); 17 | float x = atof(argv[3]); 18 | float y = atof(argv[4]); 19 | float z = atof(argv[5]); 20 | 21 | Eigen::Quaternionf quat(w, x, y, z); 22 | Eigen::Matrix3f R = quat.matrix(); 23 | 24 | SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); 25 | pcl::io::loadPCDFile(cloud_path.string(), *surfel_cloud); 26 | 27 | // now, associate each point in segment with a surfel in the surfel cloud! 28 | for (SurfelT& p : surfel_cloud->points) { 29 | if (!pcl::isFinite(p)) { 30 | continue; 31 | } 32 | p.getVector3fMap() = R*p.getVector3fMap(); 33 | p.getNormalVector3fMap() = R*p.getNormalVector3fMap(); 34 | } 35 | 36 | boost::filesystem::path rotated_surfel_path = cloud_path.parent_path() / (cloud_path.stem().string() + "_rotated.pcd"); 37 | pcl::io::savePCDFileBinary(rotated_surfel_path.string(), *surfel_cloud); 38 | 39 | return 0; 40 | } 41 | 42 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureDescriptor/FeatureDescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef FeatureDescriptor_H_ 2 | #define FeatureDescriptor_H_ 3 | //#include 4 | #include 5 | #include 6 | 7 | #include 8 | using namespace std; 9 | 10 | enum DescriptorType { unitiated, base, surf128, surf64, rgb, orb, FPFH, IntegerHistogram, FloatHistogram }; 11 | 12 | class FeatureDescriptor { 13 | public: 14 | DescriptorType type; 15 | virtual double distance(FeatureDescriptor * other_descriptor); 16 | FeatureDescriptor(); 17 | virtual ~FeatureDescriptor(); 18 | virtual void print(); 19 | virtual void normalize(); 20 | virtual FeatureDescriptor * add(FeatureDescriptor * feature); 21 | virtual FeatureDescriptor * mul(float val); 22 | virtual FeatureDescriptor * clone(); 23 | virtual void update(vector * input); 24 | virtual void store(string path); 25 | }; 26 | #include "OrbFeatureDescriptor.h" 27 | #include "SurfFeatureDescriptor64.h" 28 | #include "SurfFeatureDescriptor128.h" 29 | #endif 30 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureDescriptor/FloatHistogramFeatureDescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef FloatHistogramFeatureDescriptor_H_ 2 | #define FloatHistogramFeatureDescriptor_H_ 3 | #include "FeatureDescriptor.h" 4 | using namespace std; 5 | class FloatHistogramFeatureDescriptor : public FeatureDescriptor 6 | { 7 | public: 8 | float * descriptor; 9 | int length; 10 | 11 | FloatHistogramFeatureDescriptor(); 12 | FloatHistogramFeatureDescriptor(string path); 13 | FloatHistogramFeatureDescriptor(float * feature_descriptor, int feature_length); 14 | double distance(FeatureDescriptor * other_descriptor); 15 | void print(); 16 | void store(string path); 17 | FloatHistogramFeatureDescriptor * clone(); 18 | void update(vector * input); 19 | ~FloatHistogramFeatureDescriptor(); 20 | }; 21 | #endif 22 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureDescriptor/OrbFeatureDescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef OrbFeatureDescriptor_H_ 2 | #define OrbFeatureDescriptor_H_ 3 | #include "FeatureDescriptor.h" 4 | using namespace std; 5 | class OrbFeatureDescriptor : public FeatureDescriptor 6 | { 7 | public: 8 | int * descriptor; 9 | 10 | OrbFeatureDescriptor(); 11 | OrbFeatureDescriptor(string path); 12 | OrbFeatureDescriptor(int * feature_descriptor); 13 | double distance(OrbFeatureDescriptor * other_descriptor); 14 | void print(); 15 | void store(string path); 16 | OrbFeatureDescriptor * clone(); 17 | void update(vector * input); 18 | ~OrbFeatureDescriptor(); 19 | }; 20 | #endif 21 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureDescriptor/SurfFeatureDescriptor128.h: -------------------------------------------------------------------------------- 1 | #ifndef SurfFeatureDescriptor128_H_ 2 | #define SurfFeatureDescriptor128_H_ 3 | #include "FeatureDescriptor.h" 4 | using namespace std; 5 | class SurfFeatureDescriptor128 : public FeatureDescriptor 6 | { 7 | public: 8 | float stabilety; 9 | int laplacian; 10 | 11 | float * descriptor; 12 | unsigned int descriptor_length; 13 | 14 | SurfFeatureDescriptor128(); 15 | SurfFeatureDescriptor128(float * feature_descriptor); 16 | SurfFeatureDescriptor128(float * feature_descriptor, int feature_laplacian); 17 | SurfFeatureDescriptor128(string path); 18 | double distance(SurfFeatureDescriptor128 * other_descriptor); 19 | void print(); 20 | void store(string path); 21 | void update(vector * input); 22 | SurfFeatureDescriptor128 * clone(); 23 | ~SurfFeatureDescriptor128(); 24 | }; 25 | #endif 26 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureDescriptor/SurfFeatureDescriptor64.h: -------------------------------------------------------------------------------- 1 | #ifndef SurfFeatureDescriptor64_H_ 2 | #define SurfFeatureDescriptor64_H_ 3 | #include "FeatureDescriptor.h" 4 | using namespace std; 5 | class SurfFeatureDescriptor64 : public FeatureDescriptor 6 | { 7 | public: 8 | float stabilety; 9 | int laplacian; 10 | 11 | float * descriptor; 12 | unsigned int descriptor_length; 13 | 14 | SurfFeatureDescriptor64(); 15 | SurfFeatureDescriptor64(float * feature_descriptor); 16 | SurfFeatureDescriptor64(float * feature_descriptor, int feature_laplacian); 17 | SurfFeatureDescriptor64(string path); 18 | double distance(SurfFeatureDescriptor64 * other_descriptor); 19 | void print(); 20 | void store(string path); 21 | void update(vector * input); 22 | SurfFeatureDescriptor64 * clone(); 23 | ~SurfFeatureDescriptor64(); 24 | }; 25 | #endif 26 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureExtractor/FeatureExtractor.h: -------------------------------------------------------------------------------- 1 | #ifndef FeatureExtractor_H_ 2 | #define FeatureExtractor_H_ 3 | 4 | //OpenCV 5 | #include "cv.h" 6 | #include "highgui.h" 7 | #include 8 | //#include "highgui.h" 9 | #include "KeyPointSet.h" 10 | #include 11 | #include 12 | #include 13 | //#include 14 | #include "FrameInput.h" 15 | 16 | //bool comparison_f (KeyPoint * i,KeyPoint * j) { return (i->stabilety>j->stabilety); } 17 | 18 | class Calibration; 19 | class FeatureExtractor 20 | { 21 | public: 22 | bool debugg; 23 | bool verbose; 24 | void setVerbose(bool b); 25 | void setDebugg(bool b); 26 | 27 | virtual KeyPointSet * getKeyPointSet(FrameInput * fi); 28 | FeatureExtractor(); 29 | virtual ~FeatureExtractor(); 30 | }; 31 | 32 | #include "OrbExtractor.h" 33 | #include "SurfExtractor.h" 34 | #endif 35 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureExtractor/OrbExtractor.h: -------------------------------------------------------------------------------- 1 | #ifndef OrbExtractor_H_ 2 | #define OrbExtractor_H_ 3 | //OpenCV 4 | #include "FeatureExtractor.h" 5 | 6 | class OrbExtractor: public FeatureExtractor 7 | { 8 | public: 9 | int nr_features; 10 | KeyPointSet * getKeyPointSet(FrameInput * fi); 11 | OrbExtractor(); 12 | ~OrbExtractor(); 13 | }; 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FeatureExtractor/SurfExtractor.h: -------------------------------------------------------------------------------- 1 | #ifndef SurfExtractor_H_ 2 | #define SurfExtractor_H_ 3 | //OpenCV 4 | #include "FeatureExtractor.h" 5 | 6 | class SurfExtractor: public FeatureExtractor 7 | { 8 | public: 9 | double hessianThreshold; 10 | int nOctaves; 11 | int nOctaveLayers; 12 | bool extended; 13 | bool upright; 14 | 15 | bool gpu; 16 | 17 | KeyPointSet * getKeyPointSet(FrameInput * fi); 18 | SurfExtractor(); 19 | ~SurfExtractor(); 20 | }; 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FrameMatcher/AICK.h: -------------------------------------------------------------------------------- 1 | #ifndef AICK_H_ 2 | #define AICK_H_ 3 | //OpenCV 4 | //#include "cv.h" 5 | //#include "highgui.h" 6 | //#include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | //#include 14 | #include 15 | #include 16 | //#include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "FrameMatcher.h" 26 | 27 | using namespace std; 28 | 29 | class AICK: public FrameMatcher 30 | { 31 | public: 32 | int nr_iter; 33 | float feature_scale; 34 | float distance_threshold; 35 | float feature_threshold; 36 | float shrinking; 37 | float stabilety_threshold; 38 | int max_points; 39 | AICK(); 40 | AICK(int max_points_); 41 | AICK(int max_points_, int nr_iter, float shrinking_, float distance_threshold_, float feature_threshold_); 42 | ~AICK(); 43 | Transformation * getTransformation(RGBDFrame * src, RGBDFrame * dst); 44 | float getAlpha(int iteration); 45 | }; 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FrameMatcher/BowAICK.h: -------------------------------------------------------------------------------- 1 | #ifndef BowAICK_H_ 2 | #define BowAICK_H_ 3 | //OpenCV 4 | //#include "cv.h" 5 | //#include "highgui.h" 6 | //#include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | //#include 15 | #include 16 | #include 17 | //#include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "FrameMatcher.h" 27 | 28 | using namespace std; 29 | 30 | class BowAICK: public FrameMatcher 31 | { 32 | public: 33 | int nr_iter; 34 | float feature_scale; 35 | float distance_threshold; 36 | float feature_threshold; 37 | float bow_threshold; 38 | float shrinking; 39 | float stabilety_threshold; 40 | 41 | float converge; 42 | bool iteration_shrinking; 43 | bool fitness_shrinking; 44 | float fitness_constant; 45 | 46 | int max_points; 47 | BowAICK(); 48 | BowAICK(int max_points_); 49 | BowAICK(int max_points_, int nr_iter_, float shrinking_,float bow_threshold_, float distance_threshold_,float feature_threshold_); 50 | ~BowAICK(); 51 | Transformation * getTransformation(RGBDFrame * src, RGBDFrame * dst); 52 | float getAlpha(int iteration); 53 | float getAlpha(float avg_d2, int iteration); 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /ekz-public-lib/include/FrameMatcher/FrameMatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef FrameMatcher_H_ 2 | #define FrameMatcher_H_ 3 | //OpenCV 4 | //#include "cv.h" 5 | //#include "highgui.h" 6 | //#include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | //#include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "RGBDFrame.h" 20 | #include "Transformation.h" 21 | 22 | using namespace std; 23 | 24 | //class RGBDFrame; 25 | //class Transformation; 26 | 27 | class FrameMatcher 28 | { 29 | public: 30 | boost::shared_ptr viewer; 31 | string name; 32 | bool debugg; 33 | bool verbose; 34 | void setVerbose(bool b); 35 | void setDebugg(bool b); 36 | virtual ~FrameMatcher(); 37 | virtual Transformation * getTransformation(RGBDFrame * src, RGBDFrame * dst); 38 | virtual void print(); 39 | void setVisualization(boost::shared_ptr view); 40 | }; 41 | 42 | #include "AICK.h" 43 | #include "BowAICK.h" 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /ekz-public-lib/include/Map/Map3Dbow.h: -------------------------------------------------------------------------------- 1 | #ifndef Map3Dbow_H_ 2 | #define Map3Dbow_H_ 3 | 4 | #include "Map3D.h" 5 | 6 | using namespace std; 7 | 8 | class Map3Dbow: public Map3D 9 | { 10 | public: 11 | string path; 12 | int nr_restarts_; 13 | int iterations_; 14 | int nr_clusters_; 15 | Map3Dbow(string file_path); 16 | Map3Dbow(string file_path, int nr_restarts, int iterations, int nr_clusters); 17 | ~Map3Dbow(); 18 | vector estimate(); 19 | vector * kmeans(vector input, int nr_restarts, int iterations, int nr_clusters); 20 | }; 21 | #endif 22 | -------------------------------------------------------------------------------- /ekz-public-lib/include/TransformationFilter/TransformationFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef TransformationFilter_H_ 2 | #define TransformationFilter_H_ 3 | //OpenCV 4 | //#include "cv.h" 5 | //#include "highgui.h" 6 | //#include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | //#include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | //#include 22 | #include 23 | #include 24 | //#include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "RGBDFrame.h" 34 | #include "Transformation.h" 35 | 36 | using namespace std; 37 | 38 | class TransformationFilter 39 | { 40 | public: 41 | boost::shared_ptr viewer; 42 | string name; 43 | virtual ~TransformationFilter(); 44 | virtual Transformation * filterTransformation(Transformation * input); 45 | virtual void print(); 46 | void setVisualization(boost::shared_ptr view); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /ekz-public-lib/include/core/Calibration.h: -------------------------------------------------------------------------------- 1 | #ifndef Calibration_H_ 2 | #define Calibration_H_ 3 | #include 4 | #include 5 | #include "../FeatureDescriptor/FeatureDescriptor.h" 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | class Calibration{ 17 | public: 18 | float fx; 19 | float fy; 20 | float cx; 21 | float cy; 22 | float ds; 23 | float scale; 24 | vector words; 25 | boost::shared_ptr viewer; 26 | pthread_mutex_t viewer_mutex; 27 | 28 | void loadWord(string path){ 29 | printf("loadWord(%s)\n",path.c_str()); 30 | string type = path.substr(path.find_last_of(".")+1); 31 | if( type.compare("orb") == 0){ //orb type feature 32 | words.push_back(new OrbFeatureDescriptor(path)); 33 | }else if( type.compare("surf64") == 0){ //Surf64 feature 34 | words.push_back(new SurfFeatureDescriptor64(path)); 35 | }else if( type.compare("surf128") == 0){ //Surf128 feature 36 | words.push_back(new SurfFeatureDescriptor128(path)); 37 | }else{ 38 | printf("couldnt load word, type not in calibration class\n"); 39 | } 40 | } 41 | 42 | void loadWords(string path,string type, int nr_words){ 43 | for(int i = 0; i < nr_words; i++){ 44 | char buf[1024]; 45 | sprintf(buf,"%s_%i.%s",path.c_str(),i,type.c_str()); 46 | loadWord(string(buf)); 47 | } 48 | } 49 | }; 50 | #endif 51 | -------------------------------------------------------------------------------- /ekz-public-lib/include/core/RGBDFrame.h: -------------------------------------------------------------------------------- 1 | #ifndef RGBDFrame_H_ 2 | #define RGBDFrame_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "KeyPointSet.h" 18 | #include "Plane.h" 19 | #include "Point.h" 20 | 21 | #include "FeatureExtractor.h" 22 | #include "RGBDSegmentation.h" 23 | #include "FloatHistogramFeatureDescriptor.h" 24 | 25 | #include "FrameInput.h" 26 | 27 | using namespace std; 28 | class RGBDFrame{ 29 | public: 30 | int id; 31 | FrameInput * input; 32 | 33 | FeatureDescriptor * image_descriptor; 34 | KeyPointSet * keypoints; 35 | 36 | Segments * segments; 37 | 38 | RGBDFrame(); 39 | RGBDFrame(FrameInput * fi, FeatureExtractor * extractor, RGBDSegmentation * segmenter, bool verbose = false); 40 | ~RGBDFrame(); 41 | 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | }; 45 | #endif 46 | -------------------------------------------------------------------------------- /ekz-public-lib/include/ekz.h: -------------------------------------------------------------------------------- 1 | #ifndef ekz_H_ 2 | #define ekz_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "FrameInput.h" 17 | #include "core/core.h" 18 | #include "FeatureExtractor/FeatureExtractor.h" 19 | #include "RGBDSegmentation/RGBDSegmentation.h" 20 | #include "Map/Map3D.h" 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/HasDistance.h: -------------------------------------------------------------------------------- 1 | #ifndef HasDistance_H_ 2 | #define HasDistance_H_ 3 | class HasDistance 4 | { 5 | public: 6 | HasDistance(); 7 | ~HasDistance(); 8 | virtual float distance(float x, float y, float z); 9 | }; 10 | #endif 11 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/KeyPoint.h: -------------------------------------------------------------------------------- 1 | #ifndef KeyPoint_H_ 2 | #define KeyPoint_H_ 3 | #include "FeatureDescriptor.h" 4 | #include 5 | #include 6 | #include 7 | #include "Point.h" 8 | #include "KeyPointChain.h" 9 | 10 | 11 | class KeyPointChain; 12 | class KeyPoint { 13 | public: 14 | int index_number; 15 | FeatureDescriptor * descriptor; 16 | Point * point; 17 | bool valid; 18 | int r; 19 | int g; 20 | int b; 21 | 22 | float w; 23 | float h; 24 | 25 | float stabilety; 26 | vector< pair > cluster_distance_pairs; 27 | KeyPointChain * chain; 28 | int frame_id; 29 | KeyPoint(); 30 | ~KeyPoint(); 31 | void print(); 32 | void sortDistances(); 33 | }; 34 | #endif 35 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/KeyPointChain.h: -------------------------------------------------------------------------------- 1 | #ifndef KeyPointChain_H_ 2 | #define KeyPointChain_H_ 3 | #include "KeyPoint.h" 4 | #include 5 | 6 | using namespace std; 7 | 8 | class KeyPoint; 9 | class KeyPointChain { 10 | public: 11 | vector key_points; 12 | int id; 13 | 14 | KeyPointChain(KeyPoint * kp); 15 | ~KeyPointChain(); 16 | 17 | void merge(KeyPointChain * chain); 18 | }; 19 | 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/KeyPointSet.h: -------------------------------------------------------------------------------- 1 | #ifndef KeyPointSet_H_ 2 | #define KeyPointSet_H_ 3 | #include "KeyPoint.h" 4 | #include 5 | 6 | using namespace std; 7 | 8 | class KeyPointSet { 9 | public: 10 | double stabilety_threshold; 11 | vector valid_key_points; 12 | vector invalid_key_points; 13 | 14 | KeyPointSet(); 15 | ~KeyPointSet(); 16 | 17 | void sortKeyPoints(); 18 | }; 19 | 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/Line.h: -------------------------------------------------------------------------------- 1 | #ifndef Line_H_ 2 | #define Line_H_ 3 | #include 4 | #include 5 | #include "Point.h" 6 | #include "Plane.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace Eigen; 17 | 18 | class Line 19 | { 20 | public: 21 | int id; 22 | float dir_x; 23 | float dir_y; 24 | float dir_z; 25 | float point_x; 26 | float point_y; 27 | float point_z; 28 | 29 | float dir_length; 30 | Eigen::Vector3f dir; 31 | Eigen::Vector3f point_a; 32 | Eigen::Vector3f point_b; 33 | 34 | Line(Vector3f & p1, Vector3f & p2); 35 | Line(float x1, float y1, float z1, float x2, float y2, float z2); 36 | Line(Point * a, Point * b); 37 | Line(Plane * a, Plane * b); 38 | Line(); 39 | ~Line(); 40 | 41 | void init(Vector3f & p1, Vector3f & p2); 42 | void init(float x1, float y1, float z1, float x2, float y2, float z2); 43 | 44 | float distance(Point * point); 45 | float distance(Vector3f & point); 46 | float distance(float x, float y, float z); 47 | 48 | void distance(float & de, float & t1, float & t2, Line * line); 49 | 50 | void closestPoints(Vector3f & src_point,Vector3f & dst_point,float & s ,float & t, Line * line); 51 | 52 | void closestPoint(Vector3f & point, float & t, Vector3f & Q); 53 | }; 54 | #endif 55 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/Line2D.h: -------------------------------------------------------------------------------- 1 | #ifndef Line2D_H_ 2 | #define Line2D_H_ 3 | #include 4 | #include 5 | #include "Point.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "HasDistance.h" 15 | 16 | using namespace Eigen; 17 | using namespace std; 18 | 19 | class Line2DChain; 20 | class Line2D : HasDistance 21 | { 22 | public: 23 | int id; 24 | float weight; 25 | 26 | float normal_w; 27 | float normal_h; 28 | float normal2_w; 29 | float normal2_h; 30 | float point_w; 31 | float point_h; 32 | 33 | Line2D(vector * p_width, vector * p_height, vector * p_weight); 34 | ~Line2D(); 35 | 36 | float distance(float w, float h); 37 | }; 38 | #endif 39 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/PlaneChain.h: -------------------------------------------------------------------------------- 1 | #ifndef PlaneChain_H_ 2 | #define PlaneChain_H_ 3 | #include "Plane.h" 4 | #include 5 | 6 | using namespace std; 7 | 8 | class Plane; 9 | class PlaneChain { 10 | public: 11 | int id; 12 | vector planes; 13 | int r; 14 | int g; 15 | int b; 16 | PlaneChain(Plane * kp); 17 | ~PlaneChain(); 18 | 19 | void merge(PlaneChain * chain); 20 | }; 21 | 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/Point.h: -------------------------------------------------------------------------------- 1 | #ifndef Point_H_ 2 | #define Point_H_ 3 | #include 4 | class KeyPoint; 5 | class Point 6 | { 7 | 8 | public: 9 | KeyPoint * keypoint; 10 | float x; 11 | float y; 12 | float z; 13 | float w; 14 | float h; 15 | Eigen::Vector3f pos; 16 | Point(float x_, float y_, float z_, float w_,float h_); 17 | Point(); 18 | ~Point(); 19 | void print(); 20 | float distance(Point * point); 21 | }; 22 | #endif 23 | -------------------------------------------------------------------------------- /ekz-public-lib/include/mygeometry/mygeometry.h: -------------------------------------------------------------------------------- 1 | #ifndef mygeometry_H_ 2 | #define mygeometry_H_ 3 | 4 | #include "HasDistance.h" 5 | #include "Point.h" 6 | #include "Plane.h" 7 | #include "Line.h" 8 | #include "KeyPoint.h" 9 | #include "KeyPointSet.h" 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /ekz-public-lib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ekz_public_lib 4 | 0.0.12 5 | The ekz_public_lib package 6 | Johan Ekekrantz 7 | BSD 8 | catkin 9 | cv_bridge 10 | 11 | 12 | libpcl-all-dev 13 | pcl_ros 14 | roscpp 15 | rospy 16 | sensor_msgs 17 | std_msgs 18 | cv_bridge 19 | 20 | 21 | libpcl-all 22 | pcl_ros 23 | roscpp 24 | rospy 25 | sensor_msgs 26 | std_msgs 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ekz-public-lib/src/FeatureExtractor/.goutputstream-MIEO6W: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/ekz-public-lib/src/FeatureExtractor/.goutputstream-MIEO6W -------------------------------------------------------------------------------- /ekz-public-lib/src/FeatureExtractor/FeatureExtractor.cpp: -------------------------------------------------------------------------------- 1 | #include "FeatureExtractor.h" 2 | 3 | using namespace std; 4 | 5 | FeatureExtractor::FeatureExtractor(){} 6 | 7 | FeatureExtractor::~FeatureExtractor(){} 8 | 9 | void FeatureExtractor::setVerbose(bool b){verbose = b;} 10 | void FeatureExtractor::setDebugg(bool b){debugg = b;} 11 | 12 | KeyPointSet * FeatureExtractor::getKeyPointSet(FrameInput * fi){ 13 | //printf("Extracting unknown feature\n"); 14 | return new KeyPointSet(); 15 | } 16 | -------------------------------------------------------------------------------- /ekz-public-lib/src/FrameMatcher/FrameMatcher.cpp: -------------------------------------------------------------------------------- 1 | #include "FrameMatcher.h" 2 | 3 | using namespace std; 4 | 5 | void FrameMatcher::setVerbose(bool b){verbose = b;} 6 | void FrameMatcher::setDebugg(bool b){debugg = b;} 7 | 8 | FrameMatcher::~FrameMatcher(){} 9 | Transformation * FrameMatcher::getTransformation(RGBDFrame * src, RGBDFrame * dst){ 10 | 11 | Transformation * transformation = new Transformation(); 12 | transformation->transformationMatrix = Eigen::Matrix4f::Identity(); 13 | transformation->src = src; 14 | transformation->dst = dst; 15 | transformation->weight = 0; 16 | return transformation; 17 | } 18 | void FrameMatcher::print(){printf("%s\n",name.c_str());} 19 | void FrameMatcher::setVisualization(boost::shared_ptr view){viewer = view;} 20 | -------------------------------------------------------------------------------- /ekz-public-lib/src/TransformationFilter/TransformationFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "TransformationFilter.h" 2 | 3 | using namespace std; 4 | 5 | TransformationFilter::~TransformationFilter(){} 6 | Transformation * TransformationFilter::filterTransformation(Transformation * input){ 7 | return input; 8 | } 9 | void TransformationFilter::print(){printf("%s\n",name.c_str());} 10 | void TransformationFilter::setVisualization(boost::shared_ptr view){viewer = view;} 11 | -------------------------------------------------------------------------------- /ekz-public-lib/src/apps/example_bow_images.cpp: -------------------------------------------------------------------------------- 1 | #include "ekz.h" 2 | 3 | using namespace std; 4 | 5 | int main(int argc, char **argv){ 6 | printf("starting testing software2\n"); 7 | printf("give path to files as input\n"); 8 | string input = argv[1]; //Folder to load data from 9 | string output = argv[2]; //path to save output and initial name 10 | int nr_files = atoi(argv[3]); //max number of files to investigate 11 | int step = atoi(argv[4]); //See how many files to step 12 | Map3D * m = new Map3Dbow(output); //Create a bow map object 13 | m->setVerbose(true); //Set the map to give text output 14 | 15 | vector< RGBDFrame * > frames; 16 | for(int i = step; i <= nr_files; i+=step){ 17 | printf("adding a new frame\n"); 18 | 19 | //Get paths to image files 20 | char rgbbuf[512]; 21 | char depthbuf[512]; 22 | sprintf(rgbbuf,"%s/RGB%.10i.png",input.c_str(),i); 23 | sprintf(depthbuf,"%s/Depth%.10i.png",input.c_str(),i); 24 | 25 | printf("%i \n",i); 26 | //Add frame to map 27 | m->addFrame(string(rgbbuf) , string(depthbuf)); 28 | } 29 | 30 | m->estimate(); //Estimate and save bag of words model. 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /ekz-public-lib/src/apps/example_register_images_map.cpp: -------------------------------------------------------------------------------- 1 | #include "ekz.h" 2 | 3 | using namespace std; 4 | 5 | int main(int argc, char **argv){ 6 | printf("starting testing software2\n"); 7 | printf("give path to files as input\n"); 8 | string input = argv[1]; 9 | 10 | Map3D * m = new Map3D(); //Create a standard map object 11 | m->setVerbose(true); //Set the map to give text output 12 | 13 | vector< RGBDFrame * > frames; 14 | for(int i = 25; i <= 95; i+=5){ 15 | printf("adding a new frame\n"); 16 | 17 | //Get paths to image files 18 | char rgbbuf[512]; 19 | char depthbuf[512]; 20 | printf("%i\n",i); 21 | sprintf(rgbbuf,"%s/RGB%.10i.png",input.c_str(),i); 22 | sprintf(depthbuf,"%s/Depth%.10i.png",input.c_str(),i); 23 | 24 | //Add frame to map 25 | m->addFrame(string(rgbbuf) , string(depthbuf)); 26 | } 27 | 28 | vector poses = m->estimate(); //Estimate poses for the frames using the map object. 29 | m->savePCD("test.pcd"); //Saves a downsampled pointcloud with aligned data. 30 | 31 | //Print poses 32 | cout << "Poses:" << endl; 33 | for(unsigned int i = 0; i < poses.size(); i++){ 34 | cout << poses.at(i) << endl << endl; 35 | } 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /ekz-public-lib/src/apps/example_register_images_standalone.cpp: -------------------------------------------------------------------------------- 1 | #include "ekz.h" 2 | 3 | using namespace std; 4 | 5 | int main(int argc, char **argv){ 6 | printf("starting testing software2\n"); 7 | string input = argv[1]; 8 | 9 | Calibration * cal = new Calibration(); 10 | cal->fx = 525.0; 11 | cal->fy = 525.0; 12 | cal->cx = 319.5; 13 | cal->cy = 239.5; 14 | cal->ds = 1; 15 | cal->scale = 5000; 16 | 17 | FrameMatcher * matcher = new AICK(); 18 | RGBDSegmentation * seg = new RGBDSegmentation(); 19 | FeatureExtractor * fe = new SurfExtractor(); 20 | 21 | vector< RGBDFrame * > frames; 22 | for(int i = 25; i <= 95; i+=5){ 23 | printf("adding a new frame\n"); 24 | char rgbbuf[512]; 25 | char depthbuf[512]; 26 | sprintf(rgbbuf,"%s/RGB%.10i.png",input.c_str(),i); 27 | sprintf(depthbuf,"%s/Depth%.10i.png",input.c_str(),i); 28 | FrameInput * fi = new FrameInput(cal, string(rgbbuf) , string(depthbuf)); 29 | frames.push_back(new RGBDFrame(fi,fe,seg)); 30 | } 31 | 32 | for(unsigned int i = 1; i < frames.size(); i++){ 33 | Transformation * t = matcher->getTransformation(frames.at(i-1),frames.at(i)); 34 | t->show(false); 35 | } 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /ekz-public-lib/src/apps/example_register_pcd_map.cpp: -------------------------------------------------------------------------------- 1 | #include "ekz.h" //Include the library 2 | 3 | #include "core/RGBDFrame.h" 4 | 5 | using namespace std; 6 | 7 | int main(int argc, char **argv){ 8 | printf("starting testing software for pcd files\n"); 9 | printf("sequentally matches frames in form of pcd files given as input\n"); 10 | 11 | Map3D * m = new Map3D(); //Create a standard map object 12 | m->setVerbose(true); //Set the map to give text output 13 | for(int i = 1; i < argc; i++){ 14 | string input = string(argv[i]); 15 | printf("input: %s\n",input.c_str()); 16 | 17 | //Read cloud from a the user defined input 18 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 19 | if (pcl::io::loadPCDFile (input.c_str(), *cloud) == -1){continue;} 20 | 21 | //Add cloud to map 22 | m->addFrame(cloud); 23 | } 24 | 25 | vector poses = m->estimate();//Estimate poses for the frames using the map object. 26 | m->savePCD("test.pcd"); 27 | //Print poses 28 | cout << "Poses:" << endl; 29 | for(unsigned int i = 0; i < poses.size(); i++){ 30 | cout << poses.at(i) << endl << endl; 31 | } 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /ekz-public-lib/src/apps/pcd_recorder.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include 4 | #include 5 | 6 | 7 | // PCL specific includes 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | //Opencv 16 | #include "cv.h" 17 | #include "highgui.h" 18 | #include 19 | 20 | #include 21 | 22 | 23 | using namespace std; 24 | 25 | string path; 26 | int counter = 0; 27 | 28 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){ 29 | counter++; 30 | ROS_INFO("pointcloud in %i",counter); 31 | 32 | pcl::PointCloud input_cloud; 33 | pcl::fromROSMsg (*input, input_cloud); 34 | 35 | char buf[1024]; 36 | sprintf(buf,"%s%.10i.pcd",path.c_str(),counter); 37 | pcl::io::savePCDFileBinary (string(buf), input_cloud); 38 | 39 | } 40 | 41 | 42 | int main(int argc, char **argv) 43 | { 44 | printf("starting pcd_recording software\n"); 45 | if(argc == 1){ 46 | printf("Run program with second argument to define path and start of filename for the .pcd files\n"); 47 | return -1; 48 | } 49 | path = string(argv[1]); 50 | 51 | ros::init(argc, argv, "ekz_record"); 52 | ros::NodeHandle n; 53 | ros::Subscriber sub2 = n.subscribe ("/camera/depth_registered/points", 1, cloud_cb); 54 | ros::spin(); 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /ekz-public-lib/src/mygeometry/HasDistance.cpp: -------------------------------------------------------------------------------- 1 | #include "HasDistance.h" 2 | 3 | HasDistance::HasDistance(){} 4 | HasDistance::~HasDistance(){} 5 | float HasDistance::distance(float x, float y, float z){return -1;} 6 | -------------------------------------------------------------------------------- /ekz-public-lib/src/mygeometry/KeyPointChain.cpp: -------------------------------------------------------------------------------- 1 | #include "KeyPointChain.h" 2 | #include 3 | #include 4 | 5 | int kpc = 0; 6 | 7 | KeyPointChain::KeyPointChain(KeyPoint * kp){ 8 | id = kpc++; 9 | key_points.push_back(kp); 10 | }; 11 | 12 | KeyPointChain::~KeyPointChain(){}; 13 | 14 | void KeyPointChain::merge(KeyPointChain * chain){ 15 | for(unsigned int i = 0; i < chain->key_points.size(); i++){ 16 | //printf("chain->key_points.at(i)->frame->id: %i \n",chain->key_points.at(i)->frame_id); 17 | key_points.push_back(chain->key_points.at(i)); 18 | chain->key_points.at(i)->chain = this; 19 | } 20 | delete chain; 21 | } 22 | -------------------------------------------------------------------------------- /ekz-public-lib/src/mygeometry/KeyPointSet.cpp: -------------------------------------------------------------------------------- 1 | #include "KeyPointSet.h" 2 | #include 3 | #include 4 | 5 | KeyPointSet::KeyPointSet(){stabilety_threshold = 0;}; 6 | 7 | KeyPointSet::~KeyPointSet(){ 8 | for(unsigned int i = 0; i < valid_key_points.size(); i++) {delete valid_key_points.at(i);} 9 | for(unsigned int i = 0; i < invalid_key_points.size(); i++) {delete invalid_key_points.at(i);} 10 | }; 11 | 12 | //bool comparison(KeyPoint * a, KeyPoint * b) { return (a->stabilety > b->stabilety); } 13 | 14 | void KeyPointSet::sortKeyPoints(){ 15 | //sort (valid_key_points.begin() , valid_key_points.end() , comparison); 16 | //sort (invalid_key_points.begin(), invalid_key_points.end() , comparison); 17 | } 18 | -------------------------------------------------------------------------------- /ekz-public-lib/src/mygeometry/PlaneChain.cpp: -------------------------------------------------------------------------------- 1 | #include "PlaneChain.h" 2 | #include 3 | #include 4 | int plane_chain_id_counter = 0; 5 | 6 | PlaneChain::PlaneChain(Plane * kp){ 7 | id = plane_chain_id_counter++; 8 | planes.push_back(kp); 9 | r = rand()%256; 10 | g = rand()%256; 11 | b = rand()%256; 12 | }; 13 | 14 | PlaneChain::~PlaneChain(){}; 15 | 16 | void PlaneChain::merge(PlaneChain * chain){ 17 | if(chain != this){ 18 | for(unsigned int i = 0; i < chain->planes.size(); i++){ 19 | planes.push_back(chain->planes.at(i)); 20 | chain->planes.at(i)->chain = this; 21 | } 22 | printf("merge size: %i\n",(int)planes.size()); 23 | delete chain; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /ekz-public-lib/src/mygeometry/Point.cpp: -------------------------------------------------------------------------------- 1 | #include "Point.h" 2 | Point::Point(float x_, float y_, float z_, float w_,float h_) 3 | { 4 | x = x_; 5 | y = y_; 6 | z = z_; 7 | w = w_; 8 | h = h_; 9 | pos(0) = x; 10 | pos(1) = y; 11 | pos(2) = z; 12 | keypoint = 0; 13 | } 14 | Point::Point() 15 | { 16 | x = 0; 17 | y = 0; 18 | z = 0; 19 | w = 0; 20 | h = 0; 21 | pos(0) = 0; 22 | pos(1) = 0; 23 | pos(2) = 0; 24 | keypoint = 0; 25 | } 26 | Point::~Point() 27 | { 28 | } 29 | 30 | float Point::distance(Point * p) 31 | { 32 | float dx = x - p->x; 33 | float dy = y - p->y; 34 | float dz = z - p->z; 35 | return sqrt(dx*dx+dy*dy+dz*dz); 36 | } 37 | 38 | void Point::print() 39 | { 40 | //printf("Point w:%i,h:%i,x:%f,y:%f,z:%f\n",w,h,x,y,z); 41 | } 42 | -------------------------------------------------------------------------------- /learn_objects_action/action/LearnObject.action: -------------------------------------------------------------------------------- 1 | # goal definition 2 | string waypoint 3 | --- 4 | # result definition 5 | int32 number_of_clusters 6 | string object_name 7 | --- 8 | # feedback 9 | -------------------------------------------------------------------------------- /learn_objects_action/scripts/call_action_server.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import rospy 3 | import actionlib 4 | from learn_objects_action.msg import LearnObjectAction, LearnObjectGoal 5 | 6 | def learn_object(waypoint): 7 | client = actionlib.SimpleActionClient('learn_object', LearnObjectAction) 8 | 9 | client.wait_for_server() 10 | goal = LearnObjectGoal(waypoint=waypoint) 11 | client.send_goal(goal) 12 | client.wait_for_result() 13 | 14 | return client.get_result() 15 | 16 | if __name__ == '__main__': 17 | try: 18 | rospy.init_node('learn_object_client') 19 | result = learn_object("WayPoint9") 20 | print "Result:", ', ', result 21 | except rospy.ROSInterruptException: 22 | print "program interrupted before completion" 23 | -------------------------------------------------------------------------------- /learn_objects_action/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['learn_objects_action'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /learn_objects_action/src/learn_objects_action/__init__.py: -------------------------------------------------------------------------------- 1 | #import vision 2 | import machine 3 | #import ptu_track 4 | #import control 5 | #import util 6 | #import metric_sweep 7 | -------------------------------------------------------------------------------- /metaroom_xml_parser/apps/load_additional_views_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | typedef pcl::PointXYZRGB PointType; 7 | typedef semantic_map_load_utilties::DynamicObjectData ObjectData; 8 | 9 | typedef pcl::PointCloud Cloud; 10 | typedef typename Cloud::Ptr CloudPtr; 11 | 12 | using namespace std; 13 | 14 | int main(int argc, char** argv) 15 | { 16 | string folder; 17 | 18 | 19 | if (argc > 1){ 20 | folder = argv[1]; 21 | } else { 22 | cout<<"Please specify the folder from where to load the data."< sweep_xmls = semantic_map_load_utilties::getSweepXmls(folder); 28 | cout< objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep(sweep); 35 | cout<<"Found "< simple_parser; 28 | SimpleXMLParser::RoomData roomData; 29 | 30 | std::vector allSweeps = summary_parser.getRooms(); 31 | 32 | for (size_t i=0; ipoints.size()<points.size()< 2 | #include 3 | #include 4 | #include 5 | 6 | typedef pcl::PointXYZRGB PointType; 7 | typedef semantic_map_load_utilties::DynamicObjectData ObjectData; 8 | 9 | typedef pcl::PointCloud Cloud; 10 | typedef typename Cloud::Ptr CloudPtr; 11 | 12 | using namespace std; 13 | 14 | int main(int argc, char** argv) 15 | { 16 | string folder; 17 | string waypoint; 18 | 19 | 20 | if (argc > 1){ 21 | folder = argv[1]; 22 | } else { 23 | cout<<"Please specify the folder from where to load the data."< sweep_xmls = semantic_map_load_utilties::getSweepXmls(folder); 29 | cout< 2 | #include 3 | #include 4 | #include 5 | 6 | typedef pcl::PointXYZRGB PointType; 7 | typedef semantic_map_load_utilties::DynamicObjectData ObjectData; 8 | 9 | typedef pcl::PointCloud Cloud; 10 | typedef typename Cloud::Ptr CloudPtr; 11 | 12 | using namespace std; 13 | 14 | int main(int argc, char** argv) 15 | { 16 | string folder; 17 | string waypoint; 18 | 19 | 20 | if (argc > 2){ 21 | folder = argv[1]; 22 | waypoint = argv[2]; 23 | } else { 24 | cout<<"Please specify the folder from where to load the data and sweep waypoint."< sweep_xmls = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(folder, waypoint); 30 | cout< 5 | //#include 6 | 7 | #include "ros/time.h" 8 | #include "ros/serialization.h" 9 | #include 10 | #include "tf/tf.h" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | class SimpleSummaryParser { 17 | 18 | public: 19 | struct EntityStruct{ 20 | std::string roomXmlFile; 21 | 22 | EntityStruct() 23 | { 24 | roomXmlFile=""; 25 | } 26 | }; 27 | 28 | private: 29 | QString m_XMLFile; 30 | std::vector m_vAllRooms; 31 | int m_maxFolderDepth; 32 | 33 | public: 34 | 35 | SimpleSummaryParser(std::string xmlFile=""); 36 | 37 | ~SimpleSummaryParser(); 38 | 39 | std::vector getRooms(); 40 | 41 | bool createSummaryXML(std::string rootFolder="", bool verbose = false); 42 | 43 | 44 | private: 45 | 46 | void saveSemanticRooms(QXmlStreamWriter* xmlWriter, QString qrootFolder, bool verbose); 47 | 48 | std::vector listXmlInFolder(QString qrootFolder, int depth); 49 | 50 | 51 | }; 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /metaroom_xml_parser/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | metaroom_xml_parser 4 | 0.0.12 5 | Metaroom XML parser 6 | 7 | Rares Ambrus 8 | MIT 9 | 10 | catkin 11 | rospy 12 | roscpp 13 | libqt4-dev 14 | qt_build 15 | image_geometry 16 | libpcl-all-dev 17 | tf 18 | std_msgs 19 | sensor_msgs 20 | pcl_ros 21 | 22 | rospy 23 | roscpp 24 | message_runtime 25 | libqt4-dev 26 | qt_build 27 | image_geometry 28 | libpcl-all 29 | tf 30 | std_msgs 31 | sensor_msgs 32 | pcl_ros 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /metaroom_xml_parser/src/simple_dynamic_object_parser.cpp: -------------------------------------------------------------------------------- 1 | #include "metaroom_xml_parser/simple_dynamic_object_parser.h" 2 | -------------------------------------------------------------------------------- /metaroom_xml_parser/src/simple_xml_parser.cpp: -------------------------------------------------------------------------------- 1 | #include "metaroom_xml_parser/simple_xml_parser.h" 2 | 3 | template class SimpleXMLParser; 4 | -------------------------------------------------------------------------------- /nbv_planning/include/nbv_planning/conversions.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by chris on 25/11/15. 3 | // 4 | 5 | #ifndef NBV_PLANNING_CONVERSIONS_H 6 | #define NBV_PLANNING_CONVERSIONS_H 7 | 8 | //#include 2 | nbv_planning 3 | 0.0.10 4 | 5 | This package provides code for NBV planning. 6 | 7 | Chris Burbridge 8 | Chris Burbridge 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | sensor_msgs 15 | nav_msgs 16 | std_msgs 17 | std_srvs 18 | octomap 19 | octomap_msgs 20 | octomap_ros 21 | message_generation 22 | tf_conversions 23 | eigen_conversions 24 | visualization_msgs 25 | cmake_modules 26 | yaml-cpp 27 | 28 | roscpp 29 | sensor_msgs 30 | nav_msgs 31 | std_msgs 32 | std_srvs 33 | octomap 34 | octomap_msgs 35 | octomap_ros 36 | message_runtime 37 | tf_conversions 38 | eigen_conversions 39 | visualization_msgs 40 | yaml-cpp 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /nbv_planning/scripts/capture_some_clouds.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | import python_pcd.io as pcdio 5 | import tf 6 | from sensor_msgs.msg import PointCloud2 7 | 8 | IN_FRAME = "/odom" 9 | 10 | rospy.init_node("capture_some") 11 | more=True 12 | i=0 13 | tfs = tf.TransformListener(400) 14 | #rospy.sleep(10) 15 | while more: 16 | inp=raw_input("Capture? 'q' to stop.") 17 | if inp=="q": 18 | more=False 19 | else: 20 | cloud = rospy.wait_for_message("/head_xtion/depth_registered/points", 21 | PointCloud2, 22 | 10) 23 | try: 24 | tfs.waitForTransform(IN_FRAME, cloud.header.frame_id, cloud.header.stamp, rospy.Duration(1)) 25 | frame = tfs.lookupTransform(IN_FRAME, cloud.header.frame_id, cloud.header.stamp) 26 | 27 | print "Capured a cloud at " 28 | print frame 29 | pcdio.write_pcd("view%d.pcd"%i,cloud,overwrite=True,viewpoint=frame[0]+(frame[1][3],)+frame[1][0:3]) 30 | i+=1 31 | except Exception, e: 32 | print "TF time error! No capture." 33 | print e 34 | 35 | -------------------------------------------------------------------------------- /nbv_planning/src/Ray.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by chris on 30/11/15. 3 | // 4 | 5 | #include "nbv_planning/Ray.h" 6 | -------------------------------------------------------------------------------- /nbv_planning/src/TargetVolume.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by chris on 24/11/15. 3 | // 4 | 5 | #include "nbv_planning/TargetVolume.h" 6 | 7 | namespace nbv_planning { 8 | std::ostream& operator<<(std::ostream &os, const TargetVolume &volume) { 9 | os << "Origin " << volume.m_origin << std::endl; 10 | os << "Extents: " << volume.m_extents << std::endl; 11 | os << "Scale: " << volume.m_scale; 12 | return os; 13 | } 14 | } -------------------------------------------------------------------------------- /nbv_planning/srv/SelectNextView.srv: -------------------------------------------------------------------------------- 1 | bool disable_view 2 | --- 3 | bool success 4 | geometry_msgs/Pose selected_view 5 | uint32 selected_view_index 6 | float64 view_score -------------------------------------------------------------------------------- /nbv_planning/srv/SetTarget.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point target_centroid 2 | geometry_msgs/Point target_extents 3 | --- 4 | bool success -------------------------------------------------------------------------------- /nbv_planning/srv/SetViews.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose[] candidate_views 2 | --- 3 | bool success -------------------------------------------------------------------------------- /nbv_planning/srv/Update.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 view 2 | geometry_msgs/Pose view_pose 3 | --- 4 | bool success -------------------------------------------------------------------------------- /nbv_planning/test_files/out.yaml: -------------------------------------------------------------------------------- 1 | sensor_model: 2 | width: 640 3 | height: 480 4 | max_range: 4.0 5 | min_range: 0.3 6 | sub_sample: 20 7 | projection_matrix: [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0] 8 | 9 | target_volume: 10 | scale: 0.05 11 | origin: [-2.8, -3.75, 1.0] 12 | extents: [0.5,0.5,0.3] 13 | 14 | views: 15 | - out0.pcd 16 | - out1.pcd 17 | - out2.pcd 18 | - out3.pcd 19 | - out4.pcd 20 | - out5.pcd 21 | -------------------------------------------------------------------------------- /nbv_planning/test_files/out0.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/out0.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/out1.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/out1.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/out2.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/out2.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/out3.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/out3.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/out4.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/out4.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/out5.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/out5.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view0.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view0.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view1.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view1.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view10.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view10.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view11.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view11.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view12.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view12.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view2.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view2.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view3.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view3.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view4.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view4.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view5.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view5.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view6.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view6.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view7.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view7.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view8.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view8.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/view9.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/nbv_planning/test_files/view9.pcd -------------------------------------------------------------------------------- /nbv_planning/test_files/views.yaml: -------------------------------------------------------------------------------- 1 | sensor_model: 2 | width: 640 3 | height: 480 4 | max_range: 4.0 5 | min_range: 0.3 6 | sub_sample: 20 7 | projection_matrix: [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0] 8 | 9 | target_volume: 10 | scale: 0.05 11 | origin: [-0.26, -2.72, 1.15] 12 | extents: [0.15,0.15,0.1] 13 | 14 | views: 15 | - view0.pcd 16 | - view1.pcd 17 | - view2.pcd 18 | - view3.pcd 19 | - view4.pcd 20 | - view5.pcd 21 | - view6.pcd 22 | - view7.pcd 23 | - view8.pcd 24 | -------------------------------------------------------------------------------- /object_manager/include/object_manager/dynamic_object_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef __DYNAMIC_OBJECT_UTILITIES__ 2 | #define __DYNAMIC_OBJECT_UTILITIES__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace dynamic_object_utilities 14 | { 15 | std::vector loadDynamicObjects(std::string folder, bool verbose = false); 16 | } 17 | 18 | 19 | #endif // __DYNAMIC_OBJECT_UTILITIES__ 20 | -------------------------------------------------------------------------------- /object_manager/include/object_manager/dynamic_object_xml_parser.h: -------------------------------------------------------------------------------- 1 | #ifndef __DYNAMIC_OBJECT_XML_PARSER__H 2 | #define __DYNAMIC_OBJECT_XML_PARSER__H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include "object_manager/dynamic_object.h" 11 | 12 | 13 | class DynamicObjectXMLParser { 14 | public: 15 | 16 | typedef pcl::PointXYZRGB PointType; 17 | typedef pcl::PointCloud Cloud; 18 | typedef typename Cloud::Ptr CloudPtr; 19 | 20 | 21 | DynamicObjectXMLParser(std::string rootFolder = "", bool verbose = false); 22 | ~DynamicObjectXMLParser(); 23 | 24 | std::string saveAsXML(DynamicObject::Ptr object, std::string xml_filename = "", std::string cloud_filename = ""); 25 | DynamicObject::Ptr loadFromXML(std::string filename, bool load_cloud = true); 26 | 27 | 28 | void saveObjectTrackToXml(tf::Transform pose, CloudPtr cloud, QXmlStreamWriter* xmlWriter, std::string nodeName, std::string cloudFilename); 29 | DynamicObject::ObjectTrack readObjectTrackFromXml(QXmlStreamReader* xmlReader, std::string nodeName, std::string objectFolder, bool& errorReading, bool load_cloud = true); 30 | 31 | void saveTfStampedTransfromToXml(tf::StampedTransform transform, QXmlStreamWriter* xmlWriter, std::string nodeName); 32 | tf::StampedTransform readTfStampedTransformFromXml(QXmlStreamReader* xmlReader, std::string nodeName, bool& errorReading); 33 | 34 | std::string m_rootFolderPath; 35 | bool m_verbose; 36 | 37 | }; 38 | 39 | #endif // __DYNAMIC_OBJECT_XML_PARSER__H 40 | -------------------------------------------------------------------------------- /object_manager/msg/DynamicObjectTrackingData.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Transform[] additional_view_transforms 2 | sensor_msgs/PointCloud2[] additional_views 3 | int32[] object_mask # in first view 4 | -------------------------------------------------------------------------------- /object_manager/msg/DynamicObjectTracks.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Transform[] poses 2 | sensor_msgs/PointCloud2[] clouds 3 | -------------------------------------------------------------------------------- /object_manager/src/dynamic_object_utilities.cpp: -------------------------------------------------------------------------------- 1 | #include "object_manager/dynamic_object_utilities.h" 2 | #include 3 | 4 | using namespace std; 5 | 6 | std::vector dynamic_object_utilities::loadDynamicObjects(std::string folder, bool verbose) 7 | { 8 | std::vector objects; 9 | 10 | folder+=std::string("/"); 11 | QString qfolder = folder.c_str(); 12 | if (!QDir(qfolder).exists()) 13 | { 14 | if (verbose) 15 | { 16 | cout<<"Folder "< aObjectManager(aRosNode); 12 | 13 | ros::Rate loop_rate(10); 14 | while (ros::ok()) 15 | { 16 | ros::spinOnce(); 17 | loop_rate.sleep(); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /object_manager/srv/DynamicObjectComputeMaskService.srv: -------------------------------------------------------------------------------- 1 | string object_xml 2 | string observation_xml 3 | --- 4 | sensor_msgs/PointCloud2 segmented_object 5 | -------------------------------------------------------------------------------- /object_manager/srv/DynamicObjectsService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | string[] object_id 4 | sensor_msgs/PointCloud2[] objects 5 | geometry_msgs/Point[] centroids 6 | -------------------------------------------------------------------------------- /object_manager/srv/GetDynamicObjectService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | string object_id 3 | --- 4 | sensor_msgs/PointCloud2 object_cloud 5 | int32[] object_mask 6 | geometry_msgs/Transform transform_to_map 7 | int32 pan_angle 8 | int32 tilt_angle 9 | -------------------------------------------------------------------------------- /object_manager/srv/ProcessDynamicObjectService.srv: -------------------------------------------------------------------------------- 1 | string observation_xml 2 | string object_xml 3 | --- 4 | -------------------------------------------------------------------------------- /object_view_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(object_view_generator) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | message_generation 7 | nav_msgs 8 | rospy 9 | tf 10 | std_msgs 11 | ) 12 | 13 | add_service_files( 14 | FILES 15 | GetTrajectoryPoints.srv 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES 20 | std_msgs 21 | geometry_msgs 22 | ) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | geometry_msgs 27 | message_generation 28 | nav_msgs 29 | rospy 30 | tf 31 | std_msgs 32 | ) 33 | 34 | include_directories( 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | 38 | install( 39 | PROGRAMS scripts/view_points_service.py scripts/test_with_rviz.py 40 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | 43 | #install( 44 | # DIRECTORY launch 45 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 46 | #) 47 | -------------------------------------------------------------------------------- /object_view_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object_view_generator 4 | 0.0.0 5 | 6 | This package provides a node supplying a service to generate a set of view 7 | points aound a given object. 8 | 9 | 10 | Chris Burbridge 11 | Chris Burbridge 12 | MIT 13 | 14 | catkin 15 | 16 | geometry_msgs 17 | message_generation 18 | nav_msgs 19 | rospy 20 | tf 21 | std_msgs 22 | 23 | geometry_msgs 24 | message_generation 25 | nav_msgs 26 | rospy 27 | tf 28 | std_msgs 29 | 30 | 31 | -------------------------------------------------------------------------------- /object_view_generator/srv/GetTrajectoryPoints.srv: -------------------------------------------------------------------------------- 1 | float32 min_dist 2 | float32 max_dist 3 | int16 number_views 4 | float32 inflation_radius 5 | geometry_msgs/Pose target_pose 6 | bool return_as_trajectory 7 | string SOMA_region 8 | --- 9 | geometry_msgs/PoseArray goals 10 | -------------------------------------------------------------------------------- /observation_registration/README.md: -------------------------------------------------------------------------------- 1 | # observation_registration 2 | 3 | This contains packages (nodes) for the registration of: 4 | * [observations](observation_registration_server/README.md) 5 | * [additional views](additional_view_registration_server/README.md) 6 | 7 | These packages should be called using the services defined [here](observation_registration_services). 8 | -------------------------------------------------------------------------------- /observation_registration/additional_view_registration_server/include/additional_view_registration_server/sift_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef __SIFT_WRAPPER__ 2 | #define __SIFT_WRAPPER__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class SIFTWrapper{ 13 | public: 14 | SIFTWrapper(); 15 | ~SIFTWrapper(); 16 | 17 | void extractSIFT(const cv::Mat& image, int& num_desc, std::vector& desc, std::vector& keypoints); 18 | void matchSIFT(const int& num_desc1, const int& num_desc2, 19 | const std::vector& desc1, const std::vector& desc2, 20 | const std::vector& keypoints1, const std::vector& keypoints2, 21 | std::vector>& matches); 22 | 23 | private: 24 | bool initialized; 25 | SiftGPU *sift; 26 | SiftMatchGPU *matcher; 27 | }; 28 | 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /observation_registration/additional_view_registration_server/src/additional_view_registration_optimizer.cpp: -------------------------------------------------------------------------------- 1 | #include "additional_view_registration_server/additional_view_registration_optimizer.h" 2 | 3 | AdditionalViewRegistrationOptimizer::AdditionalViewRegistrationOptimizer(bool verbose) : m_bVerbose(verbose){ 4 | 5 | } 6 | 7 | AdditionalViewRegistrationOptimizer::~AdditionalViewRegistrationOptimizer(){ 8 | 9 | } 10 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_launcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(observation_registration_launcher) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp metaroom_xml_parser qt_build additional_view_registration_server observation_registration_server observation_registration_services siftgpu) 5 | 6 | set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}") 7 | 8 | find_package(PCL 1.7 REQUIRED) 9 | include_directories(${PCL_INCLUDE_DIRS}) 10 | link_directories(${PCL_LIBRARY_DIRS}) 11 | add_definitions(${PCL_DEFINITIONS}) 12 | 13 | rosbuild_prepare_qt4(QtCore QtXml) 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS roscpp metaroom_xml_parser additional_view_registration_server observation_registration_server observation_registration_services siftgpu 17 | DEPENDS 18 | ) 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | 25 | ############################# INSTALL TARGETS 26 | 27 | install(DIRECTORY launch/ 28 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 29 | ) 30 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_launcher/README.md: -------------------------------------------------------------------------------- 1 | # observation_registration_launcher 2 | 3 | This launches the metric mapping / semantic mapping nodes required for the registration of meta-rooms and additional views. 4 | 5 | ```roslaunch observation_registration_launcher observation_registration.launch``` 6 | 7 | ## Nodes started 8 | 9 | * observation_registration_server 10 | * additional_view_registration_server 11 | 12 | 13 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_launcher/launch/observation_registration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_launcher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | observation_registration_launcher 4 | 0.0.0 5 | The observation_registration_launcher package 6 | Rares Ambrus 7 | MIT 8 | catkin 9 | roscpp 10 | metaroom_xml_parser 11 | additional_view_registration_server 12 | observation_registration_server 13 | observation_registration_services 14 | siftgpu 15 | 16 | 17 | roscpp 18 | metaroom_xml_parser 19 | additional_view_registration_server 20 | observation_registration_server 21 | observation_registration_services 22 | siftgpu 23 | 24 | 25 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_server/README.md: -------------------------------------------------------------------------------- 1 | Observation registration server 2 | ================================ 3 | 4 | This node provides a service which registers two observations. [The service used](../observation_registration_services/srv/ObservationRegistrationService.srv) is defined [here](../observation_registration_services): 5 | 6 | ``` 7 | 8 | string source_observation_xml 9 | string target_observation_xml 10 | --- 11 | geometry_msgs/Transform transform 12 | int32 total_correspondences 13 | ``` 14 | 15 | The service returns the transform which aligns the source observation to the target observation. The underlying registration computes correspondences between pairs of images from the source and target observations from which the registration transformation is computed. 16 | 17 | 18 | #### Test observation registration server 19 | 20 | For convenience, a test routine is provided in this package which takes as input two observation xml files, calls the `observation_registration_server` and visualizes the registration results. To run execute: 21 | 22 | ``` 23 | rosrun observation_registration_server test_observation_registration XML_1 XML_2 24 | ``` 25 | 26 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_server/include/observation_registration_server/sift_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef __SIFT_WRAPPER__ 2 | #define __SIFT_WRAPPER__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class SIFTWrapper{ 13 | public: 14 | SIFTWrapper(); 15 | ~SIFTWrapper(); 16 | 17 | void extractSIFT(const cv::Mat& image, int& num_desc, std::vector& desc, std::vector& keypoints); 18 | void matchSIFT(const int& num_desc1, const int& num_desc2, 19 | const std::vector& desc1, const std::vector& desc2, 20 | const std::vector& keypoints1, const std::vector& keypoints2, 21 | std::vector>& matches); 22 | 23 | private: 24 | bool initialized; 25 | SiftGPU *sift; 26 | SiftMatchGPU *matcher; 27 | }; 28 | 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_server/src/observation_registration_optimizer.cpp: -------------------------------------------------------------------------------- 1 | #include "observation_registration_server/observation_registration_optimizer.h" 2 | 3 | ObservationRegistrationOptimizer::ObservationRegistrationOptimizer(bool verbose) : m_bVerbose(verbose){ 4 | 5 | } 6 | 7 | ObservationRegistrationOptimizer::~ObservationRegistrationOptimizer(){ 8 | 9 | } 10 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(observation_registration_services) 3 | 4 | set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}") 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp rospy sensor_msgs std_msgs geometry_msgs message_generation) 7 | 8 | add_service_files( 9 | FILES 10 | ObservationRegistrationService.srv 11 | AdditionalViewRegistrationService.srv 12 | ObjectAdditionalViewRegistrationService.srv 13 | GetLastAdditionalViewRegistrationResultService.srv 14 | ProcessRegisteredViews.srv 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | std_msgs 20 | sensor_msgs 21 | geometry_msgs 22 | ) 23 | 24 | 25 | catkin_package( 26 | INCLUDE_DIRS 27 | CATKIN_DEPENDS std_msgs geometry_msgs sensor_msgs 28 | DEPENDS 29 | LIBRARIES 30 | ) 31 | 32 | 33 | 34 | 35 | include_directories(${catkin_INCLUDE_DIRS} 36 | ) 37 | 38 | 39 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | observation_registration_services 4 | 0.0.12 5 | The observation_registration_services package 6 | 7 | Rares Ambrus 8 | BSD 9 | 10 | catkin 11 | std_msgs 12 | sensor_msgs 13 | geometry_msgs 14 | rospy 15 | roscpp 16 | message_generation 17 | 18 | std_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | message_runtime 22 | rospy 23 | roscpp 24 | message_runtime 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/srv/AdditionalViewRegistrationService.srv: -------------------------------------------------------------------------------- 1 | string observation_xml # can be blank 2 | sensor_msgs/PointCloud2[] additional_views 3 | geometry_msgs/Transform[] additional_views_odometry_transforms 4 | --- 5 | geometry_msgs/Transform[] additional_view_transforms 6 | int32[] additional_view_correspondences 7 | geometry_msgs/Transform observation_transform 8 | int32 observation_correspondences 9 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/srv/GetLastAdditionalViewRegistrationResultService.srv: -------------------------------------------------------------------------------- 1 | --- 2 | sensor_msgs/PointCloud2[] additional_views 3 | geometry_msgs/Transform[] additional_view_transforms 4 | int32[] additional_view_correspondences 5 | geometry_msgs/Transform observation_transform 6 | int32 observation_correspondences 7 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/srv/ObjectAdditionalViewRegistrationService.srv: -------------------------------------------------------------------------------- 1 | string observation_xml # can be blank 2 | string object_xml 3 | --- 4 | geometry_msgs/Transform[] additional_view_transforms 5 | int32[] additional_view_correspondences 6 | geometry_msgs/Transform observation_transform 7 | int32 observation_correspondences 8 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/srv/ObservationRegistrationService.srv: -------------------------------------------------------------------------------- 1 | string source_observation_xml 2 | string target_observation_xml 3 | --- 4 | geometry_msgs/Transform transform 5 | int32 total_correspondences 6 | 7 | -------------------------------------------------------------------------------- /observation_registration/observation_registration_services/srv/ProcessRegisteredViews.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2[] registered_views 2 | geometry_msgs/Transform[] registered_views_transforms 3 | std_msgs/Float32[] intrinsics # can be empty 4 | --- 5 | sensor_msgs/PointCloud2 processed_cloud 6 | -------------------------------------------------------------------------------- /observation_registration/siftgpu/license.txt: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Copyright (c) 2007 University of North Carolina at Chapel Hill 4 | // All Rights Reserved 5 | // 6 | // Permission to use, copy, modify and distribute this software and its 7 | // documentation for educational, research and non-profit purposes, without 8 | // fee, and without a written agreement is hereby granted, provided that the 9 | // above copyright notice and the following paragraph appear in all copies. 10 | // 11 | // The University of North Carolina at Chapel Hill make no representations 12 | // about the suitability of this software for any purpose. It is provided 13 | // 'as is' without express or implied warranty. 14 | // 15 | // Please send BUG REPORTS to ccwu@cs.unc.edu 16 | // 17 | //////////////////////////////////////////////////////////////////////////// -------------------------------------------------------------------------------- /observation_registration/siftgpu/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | siftgpu 4 | 0.1.0 5 | Catkinized SiftGPU (CUDA version) 6 | non-profit license University of North Carolina 7 | http://cs.unc.edu/~ccwu/siftgpu/ 8 | Changchang Wu 9 | Karl Pauwels 10 | 11 | catkin 12 | libglew-dev 13 | nvidia-cuda 14 | libglew-dev 15 | nvidia-cuda 16 | 17 | -------------------------------------------------------------------------------- /observation_registration/siftgpu/src/ProgramGPU.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // File: ProgramGPU.cpp 3 | // Author: Changchang Wu 4 | // Description : Implementation of ProgramGPU and FilterProgram 5 | // This part is independent of GPU language 6 | // 7 | // 8 | // 9 | // Copyright (c) 2007 University of North Carolina at Chapel Hill 10 | // All Rights Reserved 11 | // 12 | // Permission to use, copy, modify and distribute this software and its 13 | // documentation for educational, research and non-profit purposes, without 14 | // fee, and without a written agreement is hereby granted, provided that the 15 | // above copyright notice and the following paragraph appear in all copies. 16 | // 17 | // The University of North Carolina at Chapel Hill make no representations 18 | // about the suitability of this software for any purpose. It is provided 19 | // 'as is' without express or implied warranty. 20 | // 21 | // Please send BUG REPORTS to ccwu@cs.unc.edu 22 | // 23 | //////////////////////////////////////////////////////////////////////////// 24 | 25 | 26 | #include "GL/glew.h" 27 | #include 28 | #include 29 | #include 30 | using namespace std; 31 | 32 | #include "siftgpu/GlobalUtil.h" 33 | #include "siftgpu/GLTexImage.h" 34 | #include "siftgpu/ShaderMan.h" 35 | #include "siftgpu/ProgramGPU.h" 36 | #include "siftgpu/ProgramGLSL.h" 37 | #include "siftgpu/SiftGPU.h" 38 | 39 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/launch/brain.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/launch/modelserver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/launch/robot_listener.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp: -------------------------------------------------------------------------------- 1 | #include "ModelDatabase.h" 2 | 3 | ModelDatabase::ModelDatabase(){} 4 | ModelDatabase::~ModelDatabase(){} 5 | 6 | //Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching 7 | void ModelDatabase::add(reglib::Model * model){ 8 | 9 | } 10 | 11 | // return true if successfull 12 | // return false if fail 13 | bool ModelDatabase::remove(reglib::Model * model){ 14 | return false; 15 | } 16 | 17 | //Find the number_of_matches closest matches in dabase to the pointcloud for index 18 | std::vector ModelDatabase::search(reglib::Model * model, int number_of_matches){ 19 | return std::vector(); 20 | } 21 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h: -------------------------------------------------------------------------------- 1 | #ifndef ModelDatabase_H 2 | #define ModelDatabase_H 3 | 4 | #include 5 | 6 | // PCL specific includes 7 | #include 8 | #include 9 | 10 | #include "model/Model.h" 11 | 12 | class ModelDatabase{ 13 | public: 14 | std::vector models; 15 | 16 | //Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching 17 | virtual void add(reglib::Model * model); 18 | 19 | // return true if successfull 20 | // return false if fail 21 | virtual bool remove(reglib::Model * model); 22 | 23 | //Find the number_of_matches closest matches in dabase to the pointcloud for index 24 | virtual std::vector search(reglib::Model * model, int number_of_matches); 25 | 26 | ModelDatabase(); 27 | ~ModelDatabase(); 28 | }; 29 | 30 | #include "ModelDatabaseBasic.h" 31 | #include "ModelDatabaseRGBHistogram.h" 32 | //#include "ModelDatabaseRetrieval.h" 33 | #endif // ModelDatabase_H 34 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp: -------------------------------------------------------------------------------- 1 | #include "ModelDatabaseBasic.h" 2 | 3 | ModelDatabaseBasic::ModelDatabaseBasic(){} 4 | ModelDatabaseBasic::~ModelDatabaseBasic(){} 5 | 6 | 7 | void ModelDatabaseBasic::add(reglib::Model * model){ 8 | models.push_back(model); 9 | //printf("number of models: %i\n",models.size()); 10 | } 11 | 12 | bool ModelDatabaseBasic::remove(reglib::Model * model){ 13 | for(unsigned int i = 0; i < models.size(); i++){ 14 | if(models[i] == model){ 15 | models[i] = models.back(); 16 | models.pop_back(); 17 | return true; 18 | } 19 | } 20 | return false; 21 | } 22 | 23 | std::vector ModelDatabaseBasic::search(reglib::Model * model, int number_of_matches){ 24 | std::vector ret; 25 | printf("when searching my database contains %i models\n",models.size()); 26 | for(unsigned int i = 0; i < models.size(); i++){ 27 | if(models[i] != model){ 28 | ret.push_back(models[i]); 29 | } 30 | if(ret.size() == number_of_matches){break;} 31 | } 32 | return ret; 33 | } 34 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.h: -------------------------------------------------------------------------------- 1 | #ifndef ModelDatabaseBasic_H 2 | #define ModelDatabaseBasic_H 3 | 4 | #include "ModelDatabase.h" 5 | 6 | 7 | class ModelDatabaseBasic: public ModelDatabase{ 8 | public: 9 | 10 | virtual void add(reglib::Model * model); 11 | virtual bool remove(reglib::Model * model); 12 | virtual std::vector search(reglib::Model * model, int number_of_matches); 13 | 14 | ModelDatabaseBasic(); 15 | ~ModelDatabaseBasic(); 16 | }; 17 | 18 | 19 | 20 | #endif // ModelDatabaseBasic_H 21 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.h: -------------------------------------------------------------------------------- 1 | #ifndef ModelDatabaseRGBHistogram_H 2 | #define ModelDatabaseRGBHistogram_H 3 | 4 | #include "ModelDatabase.h" 5 | 6 | 7 | class ModelDatabaseRGBHistogram: public ModelDatabase{ 8 | public: 9 | 10 | int res; 11 | std::vector< std::vector< double > > descriptors; 12 | 13 | 14 | virtual void add(reglib::Model * model); 15 | virtual bool remove(reglib::Model * model); 16 | virtual std::vector search(reglib::Model * model, int number_of_matches); 17 | 18 | ModelDatabaseRGBHistogram(int res_); 19 | ~ModelDatabaseRGBHistogram(); 20 | }; 21 | 22 | 23 | 24 | #endif // ModelDatabaseRGBHistogram_H 25 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h: -------------------------------------------------------------------------------- 1 | #ifndef MODELDATABASERETRIEVAL_H 2 | #define MODELDATABASERETRIEVAL_H 3 | 4 | #include "ModelDatabase.h" 5 | #include 6 | #include 7 | 8 | using HistT = pcl::Histogram<250>; 9 | using HistCloudT = pcl::PointCloud; 10 | 11 | class ModelDatabaseRetrieval: public ModelDatabase{ 12 | private: 13 | 14 | vocabulary_tree vt; 15 | vector vt_features; 16 | map added_indices; 17 | int training_indices; 18 | set removed_indices; 19 | 20 | public: 21 | 22 | 23 | virtual void add(reglib::Model * model); 24 | virtual bool remove(reglib::Model * model); 25 | virtual std::vector search(reglib::Model * model, int number_of_matches); 26 | 27 | virtual int add(pcl::PointCloud::Ptr cloud, double weight = 1); 28 | 29 | // return true if successfull 30 | // return false if fail 31 | virtual bool remove(int index); 32 | 33 | //Find the number_of_matches closest matches in dabase to the pointcloud for index 34 | virtual std::vector search(int index, int number_of_matches); 35 | 36 | ModelDatabaseRetrieval(std::string vpath = "/media/johane/SSDstorage/vocabulary_johan/"); 37 | ~ModelDatabaseRetrieval(); 38 | }; 39 | 40 | #endif // MODELDATABASERETRIEVAL_H 41 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_conversions/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/quasimodo/quasimodo_conversions/CATKIN_IGNORE -------------------------------------------------------------------------------- /quasimodo/quasimodo_launch/launch/quasimodo.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 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/core/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef reglibCamera_H 2 | #define reglibCamera_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | #include 12 | #include 13 | 14 | namespace reglib 15 | { 16 | class Camera{ 17 | public: 18 | int id; 19 | 20 | double idepth_scale; 21 | 22 | double fx; 23 | double fy; 24 | double cx; 25 | double cy; 26 | 27 | double bias; 28 | 29 | double * pixel_weights; 30 | double * pixel_sums; 31 | unsigned int width; 32 | unsigned int height; 33 | 34 | Camera(); 35 | ~Camera(); 36 | 37 | void save(std::string path = ""); 38 | void print(); 39 | static Camera * load(std::string path); 40 | }; 41 | 42 | } 43 | 44 | #endif // reglibCamera_H 45 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/core/RGBDFrame.h: -------------------------------------------------------------------------------- 1 | #ifndef reglibRGBDFrame_H 2 | #define reglibRGBDFrame_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | // PCL specific includes 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include "Util.h" 20 | #include "Camera.h" 21 | 22 | namespace reglib 23 | { 24 | class RGBDFrame{ 25 | public: 26 | Camera * camera; 27 | unsigned long id; 28 | double capturetime; 29 | Eigen::Matrix4d pose; 30 | int sweepid; 31 | 32 | cv::Mat rgb; 33 | cv::Mat depth; 34 | cv::Mat normals; 35 | cv::Mat depthedges; 36 | int * labels; 37 | int nr_labels; 38 | 39 | std::vector< std::vector > connections; 40 | std::vector< std::vector > intersections; 41 | 42 | RGBDFrame(); 43 | RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true); 44 | ~RGBDFrame(); 45 | 46 | void show(bool stop = false); 47 | pcl::PointCloud::Ptr getPCLcloud(); 48 | void savePCD(std::string path = "cloud.pcd"); 49 | 50 | void save(std::string path = ""); 51 | static RGBDFrame * load(Camera * cam, std::string path); 52 | }; 53 | 54 | } 55 | 56 | #endif // reglibRGBDFrame_H 57 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/mesher/Mesh.h: -------------------------------------------------------------------------------- 1 | #ifndef reglibMesh_H 2 | #define reglibMesh_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | #include 12 | #include 13 | 14 | #include "../model/Model.h" 15 | #include 16 | 17 | namespace reglib 18 | { 19 | 20 | class Triangle; 21 | class Vertex{ 22 | public: 23 | float x; 24 | float y; 25 | float z; 26 | std::vector triangles; 27 | 28 | Vertex(float x_, float y_, float z_); 29 | ~Vertex(); 30 | }; 31 | 32 | class Triangle{ 33 | public: 34 | Vertex * a; 35 | Vertex * b; 36 | Vertex * c; 37 | Triangle(Vertex * a_, Vertex * b_, Vertex * c_); 38 | ~Triangle(); 39 | }; 40 | 41 | 42 | 43 | class Mesh{ 44 | public: 45 | std::vector vertexes; 46 | std::vector triangles; 47 | 48 | Mesh(); 49 | ~Mesh(); 50 | 51 | void build(Model * model, int type); 52 | void show(pcl::visualization::PCLVisualizer * viewer); 53 | //void save(std::string path = ""); 54 | //void print(); 55 | //static Camera * load(std::string path); 56 | }; 57 | 58 | } 59 | 60 | #endif // reglibMesh_H 61 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/model/ModelMask.h: -------------------------------------------------------------------------------- 1 | #ifndef reglibModelMask_H 2 | #define reglibModelMask_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | // PCL specific includes 13 | #include 14 | #include 15 | #include "../core/RGBDFrame.h" 16 | 17 | namespace reglib 18 | { 19 | class ModelMask{ 20 | public: 21 | int id; 22 | cv::Mat mask; 23 | int width; 24 | int height; 25 | bool * maskvec; 26 | std::vector testw; 27 | std::vector testh; 28 | int sweepid; 29 | 30 | ModelMask(cv::Mat mask_); 31 | ~ModelMask(); 32 | cv::Mat getMask(); 33 | }; 34 | 35 | } 36 | 37 | #endif // reglibModelMask_H 38 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/modelupdater/ModelUpdaterBasic.h: -------------------------------------------------------------------------------- 1 | #ifndef reglibModelUpdaterBasic_H 2 | #define reglibModelUpdaterBasic_H 3 | 4 | #include "ModelUpdater.h" 5 | 6 | namespace reglib 7 | { 8 | class ModelUpdaterBasic: public ModelUpdater{ 9 | public: 10 | 11 | Registration * registration; 12 | 13 | ModelUpdaterBasic(Registration * registration_ = new Registration()); 14 | ModelUpdaterBasic(Model * model_,Registration * registration_ = new Registration()); 15 | ~ModelUpdaterBasic(); 16 | 17 | virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); 18 | 19 | virtual void setRegistration( Registration * registration_); 20 | virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); 21 | virtual void refine(); 22 | }; 23 | 24 | } 25 | 26 | #endif // reglibModelUpdaterBasic_H 27 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/modelupdater/ModelUpdaterBasicFuse.h: -------------------------------------------------------------------------------- 1 | #ifndef reglibModelUpdaterBasicFuse_H 2 | #define reglibModelUpdaterBasicFuse_H 3 | 4 | #include "ModelUpdater.h" 5 | 6 | #include "registration/nanoflann.hpp" 7 | 8 | namespace reglib 9 | { 10 | class ModelUpdaterBasicFuse: public ModelUpdater{ 11 | public: 12 | 13 | std::vector fusedmodel; 14 | Registration * registration; 15 | 16 | //ModelGraph * graph; 17 | 18 | ModelUpdaterBasicFuse(Registration * registration_ = new Registration()); 19 | ModelUpdaterBasicFuse(Model * model_,Registration * registration_ = new Registration()); 20 | ~ModelUpdaterBasicFuse(); 21 | 22 | virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); 23 | //virtual void addFrame(RGBDFrame * frame, Eigen::Matrix4d pose); 24 | virtual void setRegistration( Registration * registration_); 25 | virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); 26 | virtual UpdatedModels fuseData(FusionResults * f, Model * model1, Model * model2); 27 | //virtual void refine(); 28 | virtual void computeMassRegistration(std::vector current_poses, std::vector current_frames,std::vector current_masks); 29 | }; 30 | 31 | } 32 | 33 | #endif // reglibModelUpdaterBasicFuse_H 34 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/registration/RegistrationRandom.h: -------------------------------------------------------------------------------- 1 | #ifndef RegistrationRandom_H 2 | #define RegistrationRandom_H 3 | 4 | #include "Registration.h" 5 | #include 6 | 7 | #include "RegistrationRefinement.h" 8 | 9 | namespace reglib 10 | { 11 | class RegistrationRandom : public Registration 12 | { 13 | public: 14 | 15 | Registration * refinement; 16 | 17 | virtual void setSrc(CloudData * src_); 18 | virtual void setDst(CloudData * dst_); 19 | 20 | 21 | RegistrationRandom(); 22 | ~RegistrationRandom(); 23 | 24 | FusionResults getTransform(Eigen::MatrixXd guess); 25 | }; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h: -------------------------------------------------------------------------------- 1 | #ifndef RegistrationRefinement_H 2 | #define RegistrationRefinement_H 3 | 4 | #include "Registration.h" 5 | #include 6 | #include "nanoflann.hpp" 7 | 8 | namespace reglib 9 | { 10 | class RegistrationRefinement : public Registration 11 | { 12 | public: 13 | 14 | MatchType type; 15 | bool use_PPR_weight; 16 | bool use_features; 17 | bool normalize_matchweights; 18 | 19 | Eigen::Matrix Y; 20 | Eigen::Matrix N; 21 | std::vector total_dweight; 22 | unsigned int ycols; 23 | 24 | Eigen::VectorXd DST_INORMATION; 25 | 26 | DistanceWeightFunction2PPR2 * func; 27 | 28 | int nr_arraypoints; 29 | double * arraypoints; 30 | Tree3d * trees3d; 31 | ArrayData3D * a3d; 32 | 33 | RegistrationRefinement(); 34 | ~RegistrationRefinement(); 35 | 36 | void setDst(CloudData * dst_); 37 | 38 | FusionResults getTransform(Eigen::MatrixXd guess); 39 | }; 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasic.cpp: -------------------------------------------------------------------------------- 1 | #include "modelupdater/ModelUpdaterBasic.h" 2 | 3 | namespace reglib 4 | { 5 | 6 | ModelUpdaterBasic::ModelUpdaterBasic(Registration * registration_){ 7 | registration = registration_; 8 | model = new reglib::Model(); 9 | } 10 | 11 | ModelUpdaterBasic::ModelUpdaterBasic(Model * model_, Registration * registration_){ 12 | registration = registration_; 13 | model = model_; 14 | } 15 | 16 | ModelUpdaterBasic::~ModelUpdaterBasic(){} 17 | 18 | FusionResults ModelUpdaterBasic::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){ 19 | printf("registerModel\n"); 20 | return FusionResults(); 21 | } 22 | 23 | void ModelUpdaterBasic::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){} 24 | void ModelUpdaterBasic::refine(){}//No refinement behaviour added yet 25 | 26 | void ModelUpdaterBasic::setRegistration( Registration * registration_){ 27 | if(registration != 0){delete registration;} 28 | registration = registration; 29 | } 30 | 31 | } 32 | 33 | 34 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 surfel_cloud 2 | sensor_msgs/PointCloud2 features 3 | sensor_msgs/PointCloud2 keypoints 4 | string object_id 5 | uint64 vocabulary_id 6 | sensor_msgs/Image[] images 7 | sensor_msgs/Image[] depths 8 | sensor_msgs/Image[] masks 9 | geometry_msgs/Pose[] transforms 10 | string inserted_at 11 | string removed_at 12 | string associated_mongodb_fields_map 13 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/image_array.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image[] images 2 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/int_array.msg: -------------------------------------------------------------------------------- 1 | uint32[] ints 2 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/model.msg: -------------------------------------------------------------------------------- 1 | uint64 model_id 2 | geometry_msgs/Pose[] local_poses 3 | rgbd_frame[] frames 4 | sensor_msgs/Image[] masks 5 | sensor_msgs/PointCloud2[] clouds 6 | geometry_msgs/Pose global_pose 7 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/model_array.msg: -------------------------------------------------------------------------------- 1 | quasimodo_msgs/model[] models 2 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/retrieval_query.msg: -------------------------------------------------------------------------------- 1 | uint64 ALL_QUERY = 0 2 | uint64 MONGODB_QUERY = 1 3 | uint64 METAROOM_QUERY = 2 4 | sensor_msgs/PointCloud2 cloud 5 | sensor_msgs/Image image 6 | sensor_msgs/Image depth 7 | sensor_msgs/Image mask 8 | geometry_msgs/Pose global_pose 9 | sensor_msgs/CameraInfo camera 10 | int32 number_query 11 | geometry_msgs/Transform room_transform 12 | uint64 query_kind 13 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg: -------------------------------------------------------------------------------- 1 | quasimodo_msgs/retrieval_query[] queries 2 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/retrieval_query_result.msg: -------------------------------------------------------------------------------- 1 | retrieval_query query 2 | retrieval_result result 3 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/retrieval_result.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2[] retrieved_clouds 2 | geometry_msgs/PoseArray[] retrieved_initial_poses 3 | image_array[] retrieved_images 4 | image_array[] retrieved_depths 5 | image_array[] retrieved_masks 6 | string_array[] retrieved_image_paths 7 | float64[] retrieved_distance_scores 8 | int_array[] segment_indices 9 | geometry_msgs/Pose[] global_poses 10 | string[] vocabulary_ids 11 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/rgbd_frame.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/CameraInfo camera 2 | time capture_time 3 | geometry_msgs/Pose pose 4 | sensor_msgs/Image rgb 5 | sensor_msgs/Image depth 6 | uint64 frame_id 7 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/msg/string_array.msg: -------------------------------------------------------------------------------- 1 | string[] strings 2 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/cloud_from_model.srv: -------------------------------------------------------------------------------- 1 | model model 2 | --- 3 | sensor_msgs/PointCloud2 cloud 4 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/fuse_models.srv: -------------------------------------------------------------------------------- 1 | uint64 model1 2 | uint64 model2 3 | geometry_msgs/Pose pose 4 | --- 5 | uint64 fused_model 6 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/get_model.srv: -------------------------------------------------------------------------------- 1 | uint64 model_id 2 | --- 3 | model model 4 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/index_cloud.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 cloud 2 | string object_id 3 | --- 4 | uint64 id 5 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/index_frame.srv: -------------------------------------------------------------------------------- 1 | rgbd_frame frame 2 | --- 3 | uint64 frame_id 4 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/insert_model.srv: -------------------------------------------------------------------------------- 1 | uint64 INSERT=1 2 | uint64 REMOVE=2 3 | uint64 action 4 | quasimodo_msgs/model model 5 | string object_id 6 | --- 7 | uint64 vocabulary_id 8 | string object_id 9 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2[] clouds 2 | sensor_msgs/Image[] masks 3 | --- 4 | sensor_msgs/PointCloud2[] clouds 5 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/model_from_frame.srv: -------------------------------------------------------------------------------- 1 | uint64 frame_id 2 | uint64 isnewmodel 3 | sensor_msgs/Image mask 4 | --- 5 | uint64 model_id 6 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv: -------------------------------------------------------------------------------- 1 | quasimodo_msgs/model model 2 | --- 3 | quasimodo_msgs/retrieval_query query 4 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/query_cloud.srv: -------------------------------------------------------------------------------- 1 | retrieval_query query 2 | --- 3 | retrieval_result result 4 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/simple_query_cloud.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 query_cloud 2 | --- 3 | retrieval_result result 4 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/transform_cloud.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 cloud 2 | --- 3 | sensor_msgs/PointCloud2 cloud1 4 | sensor_msgs/PointCloud2 cloud2 5 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_msgs/srv/visualize_query.srv: -------------------------------------------------------------------------------- 1 | retrieval_query query 2 | retrieval_result result 3 | --- 4 | sensor_msgs/Image image 5 | quasimodo_msgs/image_array images 6 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_object_search/launch/object_search.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 | 27 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_optimization/action/rosbag_play.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | string command 3 | string file 4 | --- 5 | #result definition 6 | --- 7 | #feedback 8 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_optimization/launch/optimizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_optimization/scripts/plot_values.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import itertools 4 | import os.path 5 | import json 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | 9 | def plot_values(): 10 | 11 | iss_res = 7 # run2 # 7 # run1 12 | pfh_res = 1 # run2 # 6 # run1 13 | 14 | mat = np.zeros((iss_res, pfh_res)) 15 | iss = np.zeros((iss_res, pfh_res)) 16 | pfh = np.zeros((iss_res, pfh_res)) 17 | for i in itertools.count(): 18 | fpath = os.path.abspath(str(i) + '.json') 19 | if not os.path.exists(fpath): 20 | break 21 | 22 | with open(fpath) as f: 23 | rates = json.load(f) 24 | y, x = divmod(i, pfh_res) 25 | mat[y, x] = rates[2]['total'][2] 26 | iss[y, x] = rates[0] 27 | pfh[y, x] = rates[1] 28 | print mat 29 | print iss 30 | print pfh 31 | plt.matshow(mat) 32 | plt.show() 33 | 34 | 35 | if __name__ == '__main__': 36 | plot_values() 37 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_retrieval/cfg/parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "quasimodo_retrieval" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | #gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100) 9 | #gen.add("double_param", double_t, 0, "A double parameter", .5, 0, 1) 10 | #gen.add("str_param", str_t, 0, "A string parameter", "Hello World") 11 | #gen.add("bool_param", bool_t, 0, "A Boolean parameter", True) 12 | 13 | #size_enum = gen.enum([ gen.const("Small", int_t, 0, "A small constant"), 14 | # gen.const("Medium", int_t, 1, "A medium constant"), 15 | # gen.const("Large", int_t, 2, "A large constant"), 16 | # gen.const("ExtraLarge", int_t, 3, "An extra large constant")], 17 | # "An enum to set size") 18 | 19 | #gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum) 20 | 21 | gen.add("iss_model_resolution", double_t, 0, "A double parameter", 0.008, 0.001, 0.02) 22 | gen.add("pfhrgb_radius_search", double_t, 0, "A double parameter", 0.06, 0.03, 0.08) 23 | 24 | exit(gen.generate(PACKAGE, "quasimodo_retrieval", "parameters")) 25 | -------------------------------------------------------------------------------- /quasimodo/quasimodo_retrieval/launch/retrieval.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /scitos_ptu_sweep/action/PTUSweep.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float32 max_pan #Maximum translation velocity. 3 | float32 max_tilt #Maximum rotation velocity. 4 | float32 min_pan #Maximum distance of detected humans. Everything above will be ignored and the max_vel_x value will be used. 5 | float32 min_tilt #The distance to a human at which the robot will stop (have 0.0 velocity). 6 | float32 pan_step #The time it takes while no human is detected to resume to 'max_vel_x' speed. 7 | float32 tilt_step #The time it takes while no human is detected to resume to 'max_vel_x' speed. 8 | --- 9 | #result definition 10 | bool success #Returns true when the time 'seconds' is up. 11 | --- 12 | #feedback 13 | float32 current_pan #Maximum translation velocity. 14 | float32 current_tilt #Maximum rotation velocity. 15 | 16 | -------------------------------------------------------------------------------- /scitos_ptu_sweep/launch/ptu_sweep.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /scitos_ptu_sweep/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | scitos_ptu_sweep 4 | 0.0.12 5 | The scitos PTU sweep package 6 | 7 | Jaime Pulido Fentanes 8 | 9 | MIT 10 | 11 | Jaime Pulido Fentanes 12 | 13 | catkin 14 | actionlib 15 | actionlib_msgs 16 | rospy 17 | sensor_msgs 18 | message_generation 19 | pcl_ros 20 | tf 21 | message_filters 22 | flir_pantilt_d46 23 | 24 | actionlib 25 | actionlib_msgs 26 | rospy 27 | sensor_msgs 28 | pcl_ros 29 | tf 30 | message_filters 31 | flir_pantilt_d46 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /scitos_ptu_sweep/scripts/PTU_test.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import sys 5 | # Brings in the SimpleActionClient 6 | import actionlib 7 | import scitos_ptu_sweep.msg 8 | 9 | 10 | def ptu_sweep_client(): 11 | 12 | client = actionlib.SimpleActionClient('PTUSweep', scitos_ptu_sweep.msg.PTUSweepAction) 13 | 14 | client.wait_for_server() 15 | rospy.loginfo(" ... Init done") 16 | 17 | ptugoal = scitos_ptu_sweep.msg.PTUSweepGoal() 18 | 19 | ptugoal.max_pan = float(sys.argv[1]) 20 | ptugoal.max_tilt = float(sys.argv[2]) 21 | ptugoal.min_pan = float(sys.argv[3]) 22 | ptugoal.min_tilt = float(sys.argv[4]) 23 | ptugoal.pan_step = float(sys.argv[5]) 24 | ptugoal.tilt_step = float(sys.argv[6]) 25 | 26 | # Sends the goal to the action server. 27 | client.send_goal(ptugoal) 28 | 29 | # Waits for the server to finish performing the action. 30 | client.wait_for_result() 31 | 32 | # Prints out the result of executing the action 33 | return client.get_result() # A FibonacciResult 34 | 35 | if __name__ == '__main__': 36 | print 'Argument List:',str(sys.argv) 37 | if len(sys.argv) < 7 : 38 | sys.exit(2) 39 | rospy.init_node('ptu_sweep_test_py') 40 | ps = ptu_sweep_client() 41 | print ps 42 | -------------------------------------------------------------------------------- /semantic_map/include/semantic_map/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef __SEMANTIC_MAP_CONSTANTS__H 2 | #define __SEMANTIC_MAP_CONSTANTS__H 3 | 4 | const double ROOM_CENTROID_DISTANCE = 1.5; // meters 5 | 6 | enum SEMANTIC_MAP_ENTITY_TYPE 7 | { 8 | SEMANTIC_MAP_METAROOM = 0, 9 | SEMANTIC_MAP_ROOM = 1, 10 | SEMANTIC_MAP_OBJECT = 2 11 | }; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /semantic_map/include/semantic_map/metaroom_xml_parser.h: -------------------------------------------------------------------------------- 1 | #ifndef __META_ROOM_XML_PARSER__H 2 | #define __META_ROOM_XML_PARSER__H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "ros/time.h" 10 | #include "ros/serialization.h" 11 | #include 12 | #include "tf/tf.h" 13 | 14 | #include "constants.h" 15 | #include "metaroom_update_iteration.h" 16 | 17 | // QT 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | template 24 | class MetaRoom; 25 | 26 | template 27 | class MetaRoomXMLParser { 28 | public: 29 | 30 | typedef pcl::PointCloud Cloud; 31 | typedef typename Cloud::Ptr CloudPtr; 32 | 33 | 34 | MetaRoomXMLParser(std::string rootFolder="home"); 35 | ~MetaRoomXMLParser(); 36 | 37 | std::string saveMetaRoomAsXML(MetaRoom& aMetaRoom, std::string xmlFile="metaroom.xml"); 38 | 39 | static boost::shared_ptr> loadMetaRoomFromXML(const std::string& xmlFile, bool deepLoad=true); 40 | 41 | QString findMetaRoomLocation(MetaRoom* aMetaRoom); 42 | private: 43 | 44 | QString m_RootFolder; 45 | }; 46 | 47 | #include "metaroom_xml_parser.hpp" 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /semantic_map/launch/semantic_map.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 | 27 | -------------------------------------------------------------------------------- /semantic_map/msg/RoomObservation.msg: -------------------------------------------------------------------------------- 1 | string xml_file_name 2 | -------------------------------------------------------------------------------- /semantic_map/src/metaroom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "semantic_map/metaroom.h" 4 | 5 | //template class MetaRoom; 6 | template class MetaRoom; 7 | -------------------------------------------------------------------------------- /semantic_map/src/metaroom_update_iteration.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/metaroom_update_iteration.h" 2 | 3 | template class MetaRoomUpdateIteration; 4 | template class MetaRoomUpdateIteration; 5 | -------------------------------------------------------------------------------- /semantic_map/src/metaroom_xml_parser.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/metaroom_xml_parser.h" 2 | 3 | template class MetaRoomXMLParser; 4 | template class MetaRoomXMLParser; 5 | 6 | -------------------------------------------------------------------------------- /semantic_map/src/mongodb_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | -------------------------------------------------------------------------------- /semantic_map/src/ndt_registration.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/ndt_registration.h" 2 | 3 | template class NdtRegistration; 4 | template class NdtRegistration; 5 | -------------------------------------------------------------------------------- /semantic_map/src/occlusion_checker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "semantic_map/occlusion_checker.h" 4 | 5 | template class OcclusionChecker; 6 | template class OcclusionChecker; 7 | -------------------------------------------------------------------------------- /semantic_map/src/reg_features.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | RegistrationFeatures::RegistrationFeatures(bool verbose, std::string data_filename) 4 | { 5 | m_verbose = verbose; 6 | m_dataFilename = data_filename; 7 | } 8 | 9 | RegistrationFeatures::~RegistrationFeatures() 10 | { 11 | 12 | } 13 | -------------------------------------------------------------------------------- /semantic_map/src/room.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/room.h" 2 | 3 | template class SemanticRoom; 4 | template class SemanticRoom; 5 | 6 | 7 | -------------------------------------------------------------------------------- /semantic_map/src/room_utilities.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/room_utilities.h" 2 | -------------------------------------------------------------------------------- /semantic_map/src/room_xml_parser.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/room_xml_parser.h" 2 | 3 | template class SemanticRoomXMLParser; 4 | template class SemanticRoomXMLParser; 5 | -------------------------------------------------------------------------------- /semantic_map/src/roombase.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "semantic_map/roombase.h" 4 | 5 | template class RoomBase; 6 | template class RoomBase; 7 | -------------------------------------------------------------------------------- /semantic_map/src/semantic_map_main.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/semantic_map_node.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | // Set up ROS. 6 | ros::init(argc, argv, "Semantic_map_node"); 7 | ros::NodeHandle n; 8 | 9 | ros::NodeHandle aRosNode("~"); 10 | 11 | SemanticMapNode aSemanticMapNode(aRosNode); 12 | 13 | ros::Rate loop_rate(10); 14 | while (ros::ok()) 15 | { 16 | ros::spinOnce(); 17 | loop_rate.sleep(); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /semantic_map/src/semantic_map_node.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/semantic_map_node.h" 2 | 3 | //template class SemanticMapNode; 4 | template class SemanticMapNode; 5 | -------------------------------------------------------------------------------- /semantic_map/src/semantic_map_summary_parser.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map/semantic_map_summary_parser.h" 2 | #include "semantic_map/room.h" 3 | #include "semantic_map/room_xml_parser.h" 4 | #include "semantic_map/metaroom.h" 5 | -------------------------------------------------------------------------------- /semantic_map/srv/ClearMetaroomService.srv: -------------------------------------------------------------------------------- 1 | string[] waypoint_id 2 | bool initialize 3 | --- 4 | -------------------------------------------------------------------------------- /semantic_map/srv/DynamicClusterService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | string reply 4 | -------------------------------------------------------------------------------- /semantic_map/srv/MetaroomService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | string reply 4 | -------------------------------------------------------------------------------- /semantic_map/srv/ObservationService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | string reply 4 | -------------------------------------------------------------------------------- /semantic_map_launcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_launcher) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp semantic_map calibrate_sweeps cloud_merge object_manager semantic_map_publisher) 5 | 6 | set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}") 7 | 8 | find_package(PCL 1.7 REQUIRED) 9 | include_directories(${PCL_INCLUDE_DIRS}) 10 | link_directories(${PCL_LIBRARY_DIRS}) 11 | add_definitions(${PCL_DEFINITIONS}) 12 | 13 | rosbuild_prepare_qt4(QtCore QtXml) 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS roscpp semantic_map calibrate_sweeps cloud_merge object_manager semantic_map_publisher 17 | ) 18 | 19 | include_directories( 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | 23 | 24 | ############################# INSTALL TARGETS 25 | 26 | install(DIRECTORY launch/ 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 28 | ) 29 | -------------------------------------------------------------------------------- /semantic_map_launcher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_map_launcher 4 | 0.0.12 5 | The semantic_map_launcher package 6 | Rares Ambrus 7 | MIT 8 | catkin 9 | roscpp 10 | semantic_map 11 | calibrate_sweeps 12 | cloud_merge 13 | object_manager 14 | semantic_map_publisher 15 | roscpp 16 | semantic_map 17 | calibrate_sweeps 18 | cloud_merge 19 | object_manager 20 | semantic_map_publisher 21 | 22 | -------------------------------------------------------------------------------- /semantic_map_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_map_publisher 4 | 0.0.12 5 | The semantic map publisher package 6 | 7 | Rares Ambrus 8 | 9 | 10 | MIT 11 | 12 | 13 | catkin 14 | roscpp 15 | std_msgs 16 | sensor_msgs 17 | message_generation 18 | libqt4-dev 19 | qt_build 20 | qt_ros 21 | libpcl-all-dev 22 | pcl_ros 23 | semantic_map 24 | metaroom_xml_parser 25 | octomap 26 | octomap_ros 27 | octomap_msgs 28 | 29 | roscpp 30 | std_msgs 31 | sensor_msgs 32 | message_runtime 33 | libqt4-dev 34 | qt_ros 35 | libpcl-all 36 | pcl_ros 37 | semantic_map 38 | metaroom_xml_parser 39 | octomap 40 | octomap_ros 41 | octomap_msgs 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /semantic_map_publisher/src/semantic_map_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map_publisher/semantic_map_publisher.h" 2 | -------------------------------------------------------------------------------- /semantic_map_publisher/src/semantic_map_publisher_main.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_map_publisher/semantic_map_publisher.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | // Set up ROS. 6 | ros::init(argc, argv, "Semantic_map_publisher_node"); 7 | ros::NodeHandle n; 8 | 9 | ros::NodeHandle aRosNode("~"); 10 | 11 | SemanticMapPublisher aSemanticMapPublisherNode(aRosNode); 12 | 13 | ros::Rate loop_rate(10); 14 | while (ros::ok()) 15 | { 16 | ros::spinOnce(); 17 | loop_rate.sleep(); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/MetaroomService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | string reply 4 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/ObservationInstanceService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | int64 instance_number # convention 0 - oldest available 3 | float64 resolution 4 | --- 5 | sensor_msgs/PointCloud2 cloud 6 | string observation_timestamp 7 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/ObservationOctomapInstanceService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | int64 instance_number # convention 0 - oldest available 3 | float64 resolution 4 | --- 5 | octomap_msgs/Octomap octomap 6 | string observation_timestamp 7 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/ObservationOctomapService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | float64 resolution 3 | --- 4 | octomap_msgs/Octomap octomap 5 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/ObservationService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | float64 resolution 3 | --- 4 | sensor_msgs/PointCloud2 cloud 5 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/SensorOriginService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | geometry_msgs/Vector3 origin 4 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/WaypointInfoService.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string[] waypoint_id 3 | int32[] observation_count 4 | -------------------------------------------------------------------------------- /semantic_map_publisher/srv/WaypointTimestampService.srv: -------------------------------------------------------------------------------- 1 | string waypoint_id 2 | --- 3 | string[] waypoint_timestamps 4 | -------------------------------------------------------------------------------- /semantic_map_to_2d/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package semantic_map_to_2d 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.12 (2015-08-21) 6 | ------------------- 7 | 8 | 0.0.11 (2015-08-21) 9 | ------------------- 10 | 11 | 0.0.10 (2015-08-13) 12 | ------------------- 13 | * updated changelogs 14 | * Remove bad install target. 15 | * Add depend on semantic_map_publisher. 16 | * Add depend on semantic_map_publisher. 17 | * Fix gencpp dependency. 18 | * Add node to publish 2d map of waypoint sweeps. 19 | * Contributors: Chris Burbridge, Marc Hanheide 20 | 21 | * Remove bad install target. 22 | * Add depend on semantic_map_publisher. 23 | * Add depend on semantic_map_publisher. 24 | * Fix gencpp dependency. 25 | * Add node to publish 2d map of waypoint sweeps. 26 | * Contributors: Chris Burbridge 27 | 28 | * Remove bad install target. 29 | * Add depend on semantic_map_publisher. 30 | * Add depend on semantic_map_publisher. 31 | * Fix gencpp dependency. 32 | * Add node to publish 2d map of waypoint sweeps. 33 | * Contributors: Chris Burbridge 34 | 35 | 0.0.9 (2014-11-23) 36 | ------------------ 37 | 38 | 0.0.8 (2014-11-22) 39 | ------------------ 40 | 41 | 0.0.7 (2014-11-20) 42 | ------------------ 43 | 44 | 0.0.6 (2014-11-19) 45 | ------------------ 46 | 47 | 0.0.5 (2014-11-12) 48 | ------------------ 49 | 50 | 0.0.3 (2014-11-11 20:34) 51 | ------------------------ 52 | 53 | 0.0.2 (2014-11-11 19:46) 54 | ------------------------ 55 | 56 | 0.0.1 (2014-11-11 17:30) 57 | ------------------------ 58 | -------------------------------------------------------------------------------- /semantic_map_to_2d/README.md: -------------------------------------------------------------------------------- 1 | This package contains a single map server node that publishes a downprojected metric map. 2 | A service is provided for chaning which waypoint is currently being published. 3 | 4 | # Usage 5 | 6 | Start it with 7 | 8 | `rosrun semantic_map_to_2d semantic_map_2d_server` 9 | 10 | This then provides the service `/set_waypoint` with the following format [TODO: This should be moved to the nodes namespace...] 11 | 12 | Request: 13 | - `string waypoint` : The name of the waypoint to switch the map to. 14 | 15 | Response: 16 | - `bool is_ok` : If the map was switched or not 17 | - `string response` : Some textual description of what when wrong if not ok. 18 | 19 | The map server publishes the map as a `nav_msgs::OccupancyGrid` on the topic `/waypoint_map`. 20 | -------------------------------------------------------------------------------- /semantic_map_to_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_to_2d 3 | 0.0.12 4 | 5 | This package provides a node that publishes a 2d version of the latest semantic map for 6 | a fiven waypoint. A service is provided for changing the currently being published 7 | waypoint. 8 | 9 | Chris Burbridge 10 | Chris Burbridge 11 | BSD 12 | 13 | catkin 14 | 15 | roscpp 16 | sensor_msgs 17 | nav_msgs 18 | std_msgs 19 | std_srvs 20 | octomap 21 | octomap_msgs 22 | octomap_ros 23 | message_generation 24 | semantic_map_publisher 25 | 26 | 27 | roscpp 28 | sensor_msgs 29 | nav_msgs 30 | std_msgs 31 | std_srvs 32 | octomap 33 | octomap_msgs 34 | octomap_ros 35 | message_runtime 36 | semantic_map_publisher 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /semantic_map_to_2d/src/semantic_map_2d_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "semtic_map_2d_server"); 6 | 7 | SemanticMap2DServer server; 8 | ros::spinOnce(); 9 | 10 | 11 | try{ 12 | ros::spin(); 13 | }catch(std::runtime_error& e){ 14 | ROS_ERROR("semantic_map_2d_server exception: %s", e.what()); 15 | return -1; 16 | } 17 | 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /semantic_map_to_2d/srv/ChangeWaypoint.srv: -------------------------------------------------------------------------------- 1 | string waypoint 2 | --- 3 | bool is_ok 4 | string response 5 | 6 | -------------------------------------------------------------------------------- /strands_sweep_registration/README.md: -------------------------------------------------------------------------------- 1 | # strands_sweep_registration 2 | 3 | Component that calibrates the intermediate positions of the point clouds making up a sweep. Currently only accepts sweeps of type `complete`, as recorded by the `do_sweep.py` action server from the `cloud_merge` package, i.e. with 51 intermediate positions. 4 | 5 | Internally it uses the Ceres optimization engine. 6 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef Camera_H_ 2 | #define Camera_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "PixelFunction.h" 9 | 10 | class Camera { 11 | public: 12 | float fx; 13 | float fy; 14 | float cx; 15 | float cy; 16 | 17 | int width; 18 | int height; 19 | 20 | int version; 21 | 22 | PixelFunction ** pixelsFunctions; 23 | 24 | Camera( float fx_, float fy_, float cx_, float cy_, int width_, int height_); 25 | virtual ~Camera(); 26 | 27 | virtual void save(std::string path); 28 | virtual void load(std::string path); 29 | virtual void show(); 30 | virtual void print(); 31 | }; 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/Frame.h: -------------------------------------------------------------------------------- 1 | #ifndef Frame_H_ 2 | #define Frame_H_ 3 | 4 | #include 5 | 6 | #include "Camera.h" 7 | 8 | #include "opencv2/core/core.hpp" 9 | #include "opencv2/highgui/highgui.hpp" 10 | #include "opencv/cv.hpp" 11 | #include "opencv/highgui.h" 12 | 13 | #include 14 | 15 | class Camera; 16 | 17 | class Frame 18 | { 19 | public: 20 | Camera * camera; 21 | 22 | int id; 23 | int framepos; 24 | 25 | int featuretype; 26 | std::vector keypoints; 27 | cv::Mat descriptors; 28 | std::vector keypoint_depth; 29 | 30 | std::vector keypoint_location; 31 | 32 | Frame(Camera * camera_, float * rgb_data_, float * depth_data_); 33 | Frame(Camera * camera_, std::vector k, std::vector depth, cv::Mat d); // initialize from ORB features driectly 34 | ~Frame(); 35 | 36 | void recalculateFullPoints(); 37 | }; 38 | #endif 39 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/PixelFunction.h: -------------------------------------------------------------------------------- 1 | #ifndef PixelFunction_H_ 2 | #define PixelFunction_H_ 3 | #include 4 | 5 | class PixelFunction 6 | { 7 | public: 8 | float r_mul; 9 | float g_mul; 10 | float b_mul; 11 | float d_mul; 12 | PixelFunction(); 13 | ~PixelFunction(); 14 | 15 | void getValues(float * pixel); 16 | void update(std::vector data, float bias = 2, float random = 0, float step = 1); 17 | void addOutput( std::vector & pixeldata_char, std::vector & pixeldata_counter); 18 | void load(char * buffer,int & pos); 19 | }; 20 | #endif 21 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h: -------------------------------------------------------------------------------- 1 | #ifndef ProblemFrameConnection_H_ 2 | #define ProblemFrameConnection_H_ 3 | 4 | #include "pair3DError.h" 5 | #include "Frame.h" 6 | 7 | class ProblemFrameConnection { 8 | public: 9 | Frame * src; 10 | Frame * dst; 11 | 12 | double * params; 13 | double * src_variable; 14 | double * dst_variable; 15 | 16 | std::vector< Eigen::Vector3f > full_src_points; 17 | std::vector< Eigen::Vector3f > full_dst_points; 18 | 19 | 20 | std::vector< Eigen::Vector3f > src_points; 21 | std::vector< Eigen::Vector3f > dst_points; 22 | 23 | std::vector< float > possible_matches_fdistance; 24 | std::vector< float > possible_matches_edistance; 25 | std::vector src_possible_matches_id; 26 | std::vector dst_possible_matches_id; 27 | 28 | std::vector src_matches; 29 | std::vector dst_matches; 30 | 31 | ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight = 1, bool show = false); 32 | void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions); 33 | void addMatchesToProblem(ceres::Problem & problem, float weight = 1); 34 | void findPossibleMatches(float di = 1, float pi = 0, Eigen::Matrix4f pose = Eigen::Matrix4f::Identity()); 35 | void recalculatePoints(); 36 | ~ProblemFrameConnection(); 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h: -------------------------------------------------------------------------------- 1 | #ifndef RobotContainer_H_ 2 | #define RobotContainer_H_ 3 | 4 | #include 5 | #include "Frame.h" 6 | #include "Camera.h" 7 | #include "Sweep.h" 8 | 9 | #include "pair3DError.h" 10 | #include "ProblemFrameConnection.h" 11 | 12 | #include 13 | 14 | class RobotContainer 15 | { 16 | public: 17 | 18 | unsigned int width; 19 | unsigned int height; 20 | 21 | unsigned int gx; 22 | unsigned int todox; 23 | unsigned int gy; 24 | unsigned int todoy; 25 | unsigned int ** inds; 26 | 27 | float * rgb; 28 | float * depth; 29 | 30 | double * shared_params; 31 | 32 | double *** poses; 33 | 34 | Camera * camera; 35 | std::vector sweeps; 36 | std::vector alignedSweep; 37 | 38 | RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_); 39 | ~RobotContainer(); 40 | 41 | void initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h); 42 | void addToTraining(std::string path); // this loads a whole sweep and extracts ORB features 43 | bool addToTrainingORBFeatures(std::string path); // this loads previously comptued ORB features 44 | 45 | std::vector runInitialTraining(); 46 | void refineTraining(); 47 | std::vector train(); 48 | bool isCalibrated(); 49 | 50 | std::vector alignAndStoreSweeps(); 51 | 52 | void saveAllSweeps(std::string path); 53 | 54 | private: 55 | void saveSweep(Sweep*, std::string path); 56 | 57 | }; 58 | #endif 59 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/Sweep.h: -------------------------------------------------------------------------------- 1 | #ifndef Sweep_H_ 2 | #define Sweep_H_ 3 | #include 4 | #include 5 | #include "Frame.h" 6 | 7 | class Sweep 8 | { 9 | public: 10 | std::string idtag; 11 | std::string xmlpath; 12 | 13 | int width; 14 | int height; 15 | Frame *** frames; 16 | Eigen::Matrix4f ** poses; 17 | 18 | Sweep(int width_, int height_, std::vector framesvec); 19 | Sweep(); 20 | virtual ~Sweep(); 21 | 22 | Eigen::Matrix4f align(Sweep * sweep, float threshold = 0.02, int ransac_iter = 20000, int nr_points = 3); 23 | std::vector getPoseVector(); 24 | }; 25 | #endif 26 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/pair3DError.h: -------------------------------------------------------------------------------- 1 | #ifndef pair3DError_H_ 2 | #define pair3DError_H_ 3 | #include "ceres/ceres.h" 4 | #include "gflags/gflags.h" 5 | #include "glog/logging.h" 6 | #include "ceres/rotation.h" 7 | #include "ceres/iteration_callback.h" 8 | 9 | #include "util.h" 10 | 11 | 12 | 13 | using ceres::NumericDiffCostFunction; 14 | using ceres::SizedCostFunction; 15 | using ceres::CENTRAL; 16 | using ceres::AutoDiffCostFunction; 17 | using ceres::CostFunction; 18 | using ceres::Problem; 19 | using ceres::Solver; 20 | using ceres::Solve; 21 | using ceres::Solve; 22 | 23 | class pair3DError : public SizedCostFunction<3, 6, 6, 4> { 24 | public: 25 | static bool optimizeCameraParams; 26 | pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight); 27 | ~pair3DError(); 28 | bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ; 29 | 30 | int id; 31 | double sw; 32 | double sh; 33 | double sz; 34 | double dw; 35 | double dh; 36 | double dz; 37 | double weight; 38 | //bool optimizeCameraParams; 39 | double information; 40 | }; 41 | #endif 42 | -------------------------------------------------------------------------------- /strands_sweep_registration/include/strands_sweep_registration/util.h: -------------------------------------------------------------------------------- 1 | #ifndef myutil_H_ 2 | #define myutil_H_ 3 | 4 | // PCL specific includes 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | int popcount_lauradoux(uint64_t *buf, uint32_t size); 12 | template Eigen::Matrix getMat(const T* const camera, int mode = 0); 13 | template void transformPoint(const T* const camera, T * point, int mode = 0); 14 | void getMat(const double* const camera, double * mat); 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /strands_sweep_registration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | strands_sweep_registration 4 | 0.0.12 5 | The strands_room package 6 | 7 | Johan Ekekrantz 8 | BSD 9 | 10 | catkin 11 | libpcl-all-dev 12 | pcl_ros 13 | qt_build 14 | metaroom_xml_parser 15 | image_geometry 16 | cpp_common 17 | roscpp 18 | rosconsole 19 | tf_conversions 20 | libceres-dev 21 | suitesparse 22 | 23 | qt_build 24 | libpcl-all-dev 25 | pcl_ros 26 | metaroom_xml_parser 27 | image_geometry 28 | cpp_common 29 | roscpp 30 | rosconsole 31 | tf_conversions 32 | libceres-dev 33 | suitesparse 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /strands_sweep_registration/src/PixelFunction.cpp: -------------------------------------------------------------------------------- 1 | #include "strands_sweep_registration/PixelFunction.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | PixelFunction::PixelFunction(){ 8 | r_mul = 1; 9 | g_mul = 1; 10 | b_mul = 1; 11 | d_mul = 1; 12 | }; 13 | PixelFunction::~PixelFunction(){}; 14 | 15 | void PixelFunction::getValues(float * pixel){//Go through pixel and calculate new values 16 | pixel[0] *= r_mul; 17 | pixel[1] *= g_mul; 18 | pixel[2] *= b_mul; 19 | pixel[3] *= d_mul; 20 | } 21 | 22 | void PixelFunction::update(std::vector data, float bias, float random, float step){ 23 | float meanrel = bias; 24 | int nr = data.size(); 25 | for(int i = 0; i < nr; i++){ 26 | meanrel += step*((data.at(i)[7]/data.at(i)[3])-1)+1; 27 | } 28 | 29 | d_mul *= meanrel / float(bias+nr); 30 | d_mul += random * (2.0f*float(rand()%10000)/float(10000) - 1.0f); 31 | } 32 | 33 | void PixelFunction::addOutput( std::vector & pixeldata_char, std::vector & pixeldata_counter){ 34 | int length = 4*sizeof(float); 35 | char * buffer = new char[length]; 36 | float * buffer_float = (float *)buffer; 37 | buffer_float[0] = r_mul; 38 | buffer_float[1] = g_mul; 39 | buffer_float[2] = b_mul; 40 | buffer_float[3] = d_mul; 41 | pixeldata_char.push_back(buffer); 42 | pixeldata_counter.push_back(length); 43 | } 44 | 45 | void PixelFunction::load(char * buffer,int & pos){ 46 | float * buffer_float = (float *)buffer; 47 | r_mul = buffer_float[0]; 48 | g_mul = buffer_float[1]; 49 | b_mul = buffer_float[2]; 50 | d_mul = buffer_float[3]; 51 | pos += 4*sizeof(float); 52 | } 53 | -------------------------------------------------------------------------------- /world_state/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /world_state/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | world_state 4 | 0.0.0 5 | The world_state package 6 | 7 | Chris Burbridge 8 | 9 | GPL 10 | 11 | catkin 12 | geometry_msgs 13 | rospy 14 | geometry_msgs 15 | rospy 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /world_state/scripts/observation_trial.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | from world_state.state import World, Object 4 | from world_state.identification import ObjectIdentification 5 | from world_state.observation import TransformationStore, MessageStoreObject, Observation, DEFAULT_TOPICS 6 | from world_state.geometry import Pose 7 | from sensor_msgs.msg import Image, PointCloud2 8 | 9 | import world_state.objectmaster as objectmaster 10 | 11 | from mongodb_store.message_store import MessageStoreProxy 12 | 13 | from strands_perception_msgs.msg import Table 14 | 15 | if __name__ == '__main__': 16 | ''' Main Program ''' 17 | rospy.init_node("observation_tester") 18 | pub = rospy.Publisher("git_image", Image, latch=1) 19 | 20 | w = World() 21 | om = objectmaster.ObjectMaster() 22 | 23 | rospy.loginfo("Creating object.") 24 | ob = w.create_object() 25 | observation = Observation.make_observation(DEFAULT_TOPICS) 26 | ob.add_observation(observation) 27 | print ob.name 28 | 29 | tf = TransformationStore.msg_to_transformer(observation.get_message("/tf")) 30 | 31 | 32 | print tf.lookupTransform("/map", "/head_xtion_rgb_optical_frame", rospy.Time(0)) 33 | print tf.lookupTransform("/map", "/head_xtion_rgb_optical_frame", 34 | observation.get_message("/head_xtion/rgb/camera_info").header.stamp) 35 | 36 | 37 | -------------------------------------------------------------------------------- /world_state/scripts/test_data.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_3d_mapping/28ee7a6b6ab6f591de201bd0a6e8a37c5a57a9ea/world_state/scripts/test_data.py -------------------------------------------------------------------------------- /world_state/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['world_state'], 9 | package_dir={'': 'src'}, 10 | requires=['geometry_msgs', 'rospy', 'strands_perception_msgs'] 11 | ) 12 | 13 | setup(**setup_args) 14 | -------------------------------------------------------------------------------- /world_state/src/world_state/__init__.py: -------------------------------------------------------------------------------- 1 | import state 2 | import observation 3 | import objectmaster 4 | import mongo 5 | import identification 6 | import geometry 7 | import exceptions -------------------------------------------------------------------------------- /world_state/src/world_state/exceptions.py: -------------------------------------------------------------------------------- 1 | class ObjectMasterException(Exception): 2 | """Exceptions realted to the object master. """ 3 | def __init__(self, tp, additional_info=None): 4 | self.types={} 5 | self.types["UNKNOWN_CAT"]="Object class not known." 6 | self.types["UNKNOWN_INSTANCE"]="Object instance not known." 7 | 8 | self.tp = tp 9 | self.additional_info=additional_info 10 | 11 | def __str__(self): 12 | if self.additional_info: 13 | return '\n\n' + str(self.types[self.tp] + '\n'+str(self.additional_info)) 14 | else: 15 | return '\n\n' +repr(self.types[self.tp] ) 16 | 17 | 18 | 19 | class StateException(Exception): 20 | """Exceptions realted to the world state. """ 21 | def __init__(self, tp, additional_info=None): 22 | self.types={} 23 | self.types["NO_POSE"]="Object has no pose." 24 | self.types["NO_OBSERVATION"]="Observation has not got this topic." 25 | 26 | self.tp = tp 27 | self.additional_info=additional_info 28 | 29 | def __str__(self): 30 | if self.additional_info: 31 | return '\n\n' + str(self.types[self.tp] + '\n'+str(self.additional_info)) 32 | else: 33 | return '\n\n' +repr(self.types[self.tp] ) 34 | 35 | 36 | -------------------------------------------------------------------------------- /world_state/src/world_state/identification.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from mongo import MongoTransformable 3 | 4 | class ObjectIdentification(MongoTransformable): 5 | def __init__(self, class_conf={}, instance_conf={}): 6 | # TODO: check that objects specified actually exist in object master. 7 | # TODO: check the conf sum == 1.0 8 | self.class_distribution = class_conf 9 | self.instance_distribution = instance_conf 10 | self._time_stamp = rospy.Time.now().to_time() 11 | 12 | if len(class_conf.keys()) == 0: 13 | self.class_type = ("unknown", 1.0) 14 | else: 15 | self.class_type = max(self.class_distribution.items(), 16 | key=lambda f: f[1]) 17 | 18 | if len(instance_conf.keys()) == 0: 19 | self.instance_type = ("unknown", 1.0) 20 | else: 21 | self.class_type = max(self.instance_distribution.items(), 22 | key=lambda f: f[1]) 23 | 24 | def set_time_stamp(self, time): 25 | self._time_stamp = time 26 | -------------------------------------------------------------------------------- /world_state/unittests/test_world.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import unittest 5 | from world_state.state import World, Object 6 | from world_state.identification import ObjectIdentification 7 | import rospy 8 | 9 | 10 | class TestWorld(unittest.TestCase): 11 | def setUp(self): 12 | try: 13 | rospy.init_node("testing_world", anonymous=True) 14 | except rospy.ROSException, e: 15 | pass # Already a node? 16 | 17 | def test_world_add_object(self): 18 | w = World("world_state") 19 | obj = w.create_object() 20 | 21 | name = obj.name 22 | obj = w.get_object(obj.name) 23 | obj.add_identification("TableDetection", 24 | ObjectIdentification({'Table': 0.2, 25 | 'Football': 0.3})) 26 | 27 | 28 | obj = w.get_object(obj.name) 29 | 30 | obj.alpha = 45 31 | 32 | self.assertEqual(name, obj.name) 33 | 34 | self.assertEqual(obj.get_identification("TableDetection").class_type, 35 | ["Football", 0.3]) 36 | 37 | w.remove_object(obj) 38 | 39 | with self.assertRaises(Exception) as ex: 40 | w.remove_object(obj.name) 41 | 42 | the_exception = ex.exception 43 | self.assertEqual(str(the_exception), 44 | "get_object failed to find object '%s' in database."%obj.name) 45 | 46 | if __name__ == '__main__': 47 | import rosunit 48 | PKG='world_state' 49 | rosunit.unitrun(PKG, 'test_objects', TestObjects) 50 | --------------------------------------------------------------------------------