├── chisel_ros ├── Makefile ├── srv │ ├── PauseService.srv │ ├── ResetService.srv │ ├── SaveMeshService.srv │ └── GetAllChunksService.srv ├── msg │ ├── ChunkListMessage.msg │ └── ChunkMessage.msg ├── mainpage.dox ├── CMakeLists.txt ├── manifest.xml ├── catkin.cmake ├── package.xml ├── launch │ ├── launch_sim.launch │ ├── launch_kinect_local.launch │ ├── launch_ada_softkinetic.launch │ ├── launch_lsd_slam_kinect.launch │ └── launch_freiburg_dataset.launch ├── rosbuild.cmake ├── include │ └── chisel_ros │ │ ├── Serialization.h │ │ ├── ChiselServer.h │ │ └── Conversions.h └── src │ └── ChiselNode.cpp ├── open_chisel ├── Makefile ├── CMakeLists.txt ├── manifest.xml ├── include │ └── open_chisel │ │ ├── geometry │ │ ├── Raycast.h │ │ ├── Interpolate.h │ │ ├── Geometry.h │ │ ├── Plane.h │ │ ├── AABB.h │ │ └── Frustum.h │ │ ├── io │ │ └── PLY.h │ │ ├── weighting │ │ ├── Weighter.h │ │ └── ConstantWeighter.h │ │ ├── truncation │ │ ├── Truncator.h │ │ ├── ConstantTruncator.h │ │ └── QuadraticTruncator.h │ │ ├── threading │ │ └── Threading.h │ │ ├── mesh │ │ └── Mesh.h │ │ ├── camera │ │ ├── Intrinsics.h │ │ ├── PinholeCamera.h │ │ ├── DepthImage.h │ │ └── ColorImage.h │ │ ├── DistVoxel.h │ │ ├── FixedPointFloat.h │ │ ├── pointcloud │ │ └── PointCloud.h │ │ ├── ColorVoxel.h │ │ ├── Chunk.h │ │ ├── marching_cubes │ │ └── MarchingCubes.h │ │ ├── Chisel.h │ │ ├── ChunkManager.h │ │ └── ProjectionIntegrator.h ├── package.xml ├── catkin.cmake ├── src │ ├── mesh │ │ └── Mesh.cpp │ ├── pointcloud │ │ └── PointCloud.cpp │ ├── DistVoxel.cpp │ ├── ColorVoxel.cpp │ ├── camera │ │ ├── Intrinsics.cpp │ │ └── PinholeCamera.cpp │ ├── geometry │ │ ├── Plane.cpp │ │ ├── AABB.cpp │ │ ├── Raycast.cpp │ │ └── Frustum.cpp │ ├── io │ │ └── PLY.cpp │ ├── Chunk.cpp │ ├── ProjectionIntegrator.cpp │ └── Chisel.cpp └── rosbuild.cmake └── README.md /chisel_ros/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /open_chisel/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /chisel_ros/srv/PauseService.srv: -------------------------------------------------------------------------------- 1 | #Request 2 | --- 3 | #Respose 4 | -------------------------------------------------------------------------------- /chisel_ros/srv/ResetService.srv: -------------------------------------------------------------------------------- 1 | #Request 2 | --- 3 | #Respose 4 | -------------------------------------------------------------------------------- /chisel_ros/msg/ChunkListMessage.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | ChunkMessage[] chunks 3 | -------------------------------------------------------------------------------- /chisel_ros/srv/SaveMeshService.srv: -------------------------------------------------------------------------------- 1 | #Request 2 | string file_name 3 | --- 4 | #Respose 5 | -------------------------------------------------------------------------------- /chisel_ros/srv/GetAllChunksService.srv: -------------------------------------------------------------------------------- 1 | #Request 2 | --- 3 | ChunkListMessage chunks 4 | #Respose 5 | -------------------------------------------------------------------------------- /chisel_ros/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b chisel_ros 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /chisel_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chisel_ros) 3 | 4 | if (DEFINED CATKIN_TOPLEVEL OR (NOT ("$ENV{ROS_DISTRO}" STREQUAL "fuerte"))) 5 | include(${PROJECT_SOURCE_DIR}/catkin.cmake) 6 | else () 7 | include(${PROJECT_SOURCE_DIR}/rosbuild.cmake) 8 | endif () 9 | -------------------------------------------------------------------------------- /open_chisel/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(open_chisel) 3 | 4 | if (DEFINED CATKIN_TOPLEVEL OR (NOT ("$ENV{ROS_DISTRO}" STREQUAL "fuerte"))) 5 | include(${PROJECT_SOURCE_DIR}/catkin.cmake) 6 | else () 7 | include(${PROJECT_SOURCE_DIR}/rosbuild.cmake) 8 | endif () 9 | -------------------------------------------------------------------------------- /open_chisel/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | open_chisel 5 | 6 | 7 | Matt Klingensmith 8 | BSD 9 | 10 | http://ros.org/wiki/open_chisel 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /chisel_ros/msg/ChunkMessage.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | #Chunk ID spatial position 4 | int32 ID_x 5 | int32 ID_y 6 | int32 ID_z 7 | 8 | #Chunk spatial hash value 9 | int64 spatial_hash 10 | 11 | #Size of a voxel in meters 12 | float32 resolution_meters 13 | 14 | #number of voxels in each dimension 15 | int32 num_voxels_x 16 | int32 num_voxels_y 17 | int32 num_voxels_z 18 | 19 | #Binary serialization of distances/weights 20 | uint32[] distance_data 21 | 22 | #Binary serialization of colors/weights 23 | uint32[] color_data 24 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/geometry/Raycast.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST_H_ 2 | #define RAYCAST_H_ 3 | 4 | #include "Geometry.h" 5 | 6 | float signum(float x); 7 | float mod(float value, float modulus); 8 | float intbound(float s, float ds); 9 | bool RayIntersectsAABB(const chisel::Vec3& start, const chisel::Vec3& end, const chisel::Point3& lb, const chisel::Point3& rt); 10 | void Raycast(const chisel::Vec3& start, const chisel::Vec3& end, const chisel::Point3& min, const chisel::Point3& max, chisel::Point3List* output); 11 | 12 | #endif // RAYCAST_H_ 13 | -------------------------------------------------------------------------------- /open_chisel/package.xml: -------------------------------------------------------------------------------- 1 | 2 | open_chisel 3 | 0.0.0 4 | 5 | 6 | open_chisel implements the Chisel chunked TSDF library. 7 | 8 | 9 | 10 | Matt Klingensmith 11 | MIT 12 | catkin 13 | eigen 14 | cmake_modules 15 | cmake_modules 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /chisel_ros/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | chisel_ros 5 | 6 | 7 | Matt Klingensmith 8 | BSD 9 | 10 | http://ros.org/wiki/chisel_ros 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /open_chisel/catkin.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | find_package(catkin REQUIRED) 3 | catkin_package(INCLUDE_DIRS include 4 | LIBRARIES ${PROJECT_NAME}) 5 | 6 | find_package(cmake_modules REQUIRED) 7 | find_package(Eigen REQUIRED) 8 | include_directories(${Eigen_INCLUDE_DIRS}) 9 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --std=c++0x") 10 | 11 | include_directories(include) 12 | 13 | add_library(${PROJECT_NAME} 14 | src/Chunk.cpp 15 | src/ChunkManager.cpp 16 | src/DistVoxel.cpp 17 | src/ColorVoxel.cpp 18 | src/geometry/AABB.cpp 19 | src/geometry/Plane.cpp 20 | src/geometry/Frustum.cpp 21 | src/camera/Intrinsics.cpp 22 | src/camera/PinholeCamera.cpp 23 | src/pointcloud/PointCloud.cpp 24 | src/ProjectionIntegrator.cpp 25 | src/Chisel.cpp 26 | src/mesh/Mesh.cpp 27 | src/marching_cubes/MarchingCubes.cpp 28 | src/io/PLY.cpp 29 | src/geometry/Raycast.cpp) 30 | 31 | target_link_libraries(${PROJECT_NAME} ${CATKIN_LIBRARIES}) 32 | 33 | install(DIRECTORY include/${PROJECT_NAME}/ 34 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 35 | ) 36 | -------------------------------------------------------------------------------- /chisel_ros/catkin.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs geometry_msgs tf open_chisel pcl_ros chisel_msgs) 4 | 5 | find_package(cmake_modules REQUIRED) 6 | find_package(Eigen REQUIRED) 7 | find_package(PCL 1.8 REQUIRED) 8 | include_directories(${Eigen_INCLUDE_DIRS}) 9 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --std=c++0x") 10 | 11 | 12 | generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs) 13 | 14 | catkin_package(CATKIN_DEPENDS roscpp tf std_msgs sensor_msgs open_chisel pcl_ros chisel_msgs 15 | INCLUDE_DIRS include 16 | LIBRARIES ${PROJECT_NAME}) 17 | 18 | 19 | include_directories(include ${catkin_INCLUDE_DIRS}) 20 | 21 | add_library(${PROJECT_NAME} src/ChiselServer.cpp) 22 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 23 | add_executable(ChiselNode src/ChiselNode.cpp) 24 | target_link_libraries(ChiselNode ${PROJECT_NAME} ${catkin_LIBRARIES}) 25 | 26 | install(DIRECTORY include/${PROJECT_NAME}/ 27 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 28 | PATTERN ".git" EXCLUDE 29 | ) 30 | -------------------------------------------------------------------------------- /chisel_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | chisel_ros 3 | 0.0.0 4 | 5 | 6 | chisel_ros connects open_chisel with ROS through the TF library, and image_transport 7 | 8 | 9 | 10 | Matt Klingensmith 11 | MIT 12 | catkin 13 | roscpp 14 | pcl 15 | pcl_ros 16 | std_msgs 17 | sensor_msgs 18 | open_chisel 19 | chisel_msgs 20 | tf 21 | open_chisel 22 | visualization_msgs 23 | pcl_ros 24 | std_msgs 25 | chisel_msgs 26 | sensor_msgs 27 | open_chisel 28 | roscpp 29 | tf 30 | pcl_ros 31 | cmake_modules 32 | cmake_modules 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /chisel_ros/launch/launch_sim.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 | -------------------------------------------------------------------------------- /open_chisel/src/mesh/Mesh.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | Mesh::Mesh() 28 | { 29 | 30 | } 31 | 32 | Mesh::~Mesh() 33 | { 34 | 35 | } 36 | 37 | } // namespace chisel 38 | -------------------------------------------------------------------------------- /chisel_ros/launch/launch_kinect_local.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 | -------------------------------------------------------------------------------- /chisel_ros/rosbuild.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | find_package(Eigen REQUIRED) 15 | include_directories(${Eigen_INCLUDE_DIRS}) 16 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --std=c++0x") 17 | 18 | #set the default path for built executables to the "bin" directory 19 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 20 | #set the default path for built libraries to the "lib" directory 21 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 22 | 23 | #uncomment if you have defined messages 24 | rosbuild_genmsg() 25 | #uncomment if you have defined services 26 | rosbuild_gensrv() 27 | 28 | #common commands for building c++ executables and libraries 29 | rosbuild_add_library(${PROJECT_NAME} src/ChiselServer.cpp src/ChiselNode.cpp) 30 | 31 | #target_link_libraries(${PROJECT_NAME} another_library) 32 | #rosbuild_add_boost_directories() 33 | #rosbuild_link_boost(${PROJECT_NAME} thread) 34 | rosbuild_add_executable(ChiselNode src/ChiselNode.cpp) 35 | target_link_libraries(ChiselNode ${PROJECT_NAME} open_chisel) 36 | -------------------------------------------------------------------------------- /open_chisel/src/pointcloud/PointCloud.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | PointCloud::PointCloud() 28 | { 29 | 30 | } 31 | 32 | PointCloud::~PointCloud() 33 | { 34 | 35 | } 36 | 37 | } // namespace chisel 38 | -------------------------------------------------------------------------------- /chisel_ros/launch/launch_ada_softkinetic.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 | -------------------------------------------------------------------------------- /open_chisel/src/DistVoxel.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | DistVoxel::DistVoxel() : 28 | sdf(99999), weight(0) 29 | { 30 | 31 | } 32 | 33 | DistVoxel::~DistVoxel() 34 | { 35 | 36 | } 37 | 38 | } // namespace chisel 39 | -------------------------------------------------------------------------------- /open_chisel/src/ColorVoxel.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | ColorVoxel::ColorVoxel() : 28 | red(0), green(0), blue(0), weight(0) 29 | { 30 | 31 | } 32 | 33 | ColorVoxel::~ColorVoxel() 34 | { 35 | 36 | } 37 | 38 | } // namespace chisel 39 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/io/PLY.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef PLY_H_ 24 | #define PLY_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace chisel 31 | { 32 | bool SaveMeshPLYASCII(const std::string& fileName, const chisel::MeshConstPtr& mesh); 33 | } 34 | 35 | #endif // PLY_H_ 36 | -------------------------------------------------------------------------------- /open_chisel/src/camera/Intrinsics.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | Intrinsics::Intrinsics() 28 | { 29 | // TODO Auto-generated constructor stub 30 | 31 | } 32 | 33 | Intrinsics::~Intrinsics() 34 | { 35 | // TODO Auto-generated destructor stub 36 | } 37 | 38 | } // namespace chisel 39 | -------------------------------------------------------------------------------- /chisel_ros/launch/launch_lsd_slam_kinect.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 | -------------------------------------------------------------------------------- /chisel_ros/launch/launch_freiburg_dataset.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 | 35 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/geometry/Interpolate.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef INTERPOLATE_H_ 24 | #define INTERPOLATE_H_ 25 | 26 | namespace chisel 27 | { 28 | inline float LinearInterpolate(float s, float e, float t) 29 | { 30 | return s+(e-s)*t; 31 | } 32 | 33 | inline float BilinearInterpolate(float c00, float c10, float c01, float c11, float tx, float ty) 34 | { 35 | return LinearInterpolate(LinearInterpolate(c00, c10, tx), LinearInterpolate(c01, c11, tx), ty); 36 | } 37 | } 38 | 39 | #endif // INTERPOLATE_H_ 40 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/weighting/Weighter.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef WEIGHTER_H_ 23 | #define WEIGHTER_H_ 24 | 25 | namespace chisel 26 | { 27 | 28 | class Weighter 29 | { 30 | public: 31 | Weighter() = default; 32 | virtual ~Weighter() 33 | { 34 | 35 | } 36 | 37 | virtual float GetWeight(float surfaceDist, float truncationDist) const = 0; 38 | }; 39 | typedef std::shared_ptr WeighterPtr; 40 | typedef std::shared_ptr WeighterConstPtr; 41 | 42 | } // namespace chisel 43 | 44 | #endif // WEIGHTER_H_ 45 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/truncation/Truncator.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef TRUNCATOR_H_ 23 | #define TRUNCATOR_H_ 24 | 25 | namespace chisel 26 | { 27 | 28 | class Truncator 29 | { 30 | public: 31 | Truncator() = default; 32 | virtual ~Truncator() 33 | { 34 | 35 | } 36 | 37 | virtual float GetTruncationDistance(float depthReading) const = 0; 38 | }; 39 | 40 | typedef std::shared_ptr TruncatorPtr; 41 | typedef std::shared_ptr TruncatorConstPtr; 42 | } // namespace chisel 43 | 44 | #endif // TRUNCATOR_H_ 45 | -------------------------------------------------------------------------------- /open_chisel/rosbuild.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE Release) 11 | 12 | rosbuild_init() 13 | 14 | find_package(Eigen REQUIRED) 15 | include_directories(${Eigen_INCLUDE_DIRS}) 16 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --std=c++0x") 17 | 18 | #set the default path for built executables to the "bin" directory 19 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 20 | #set the default path for built libraries to the "lib" directory 21 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 22 | 23 | #uncomment if you have defined messages 24 | #rosbuild_genmsg() 25 | #uncomment if you have defined services 26 | #rosbuild_gensrv() 27 | 28 | #common commands for building c++ executables and libraries 29 | rosbuild_add_library(${PROJECT_NAME} 30 | src/Chunk.cpp 31 | src/ChunkManager.cpp 32 | src/DistVoxel.cpp 33 | src/ColorVoxel.cpp 34 | src/geometry/AABB.cpp 35 | src/geometry/Plane.cpp 36 | src/geometry/Frustum.cpp 37 | src/geometry/Raycast.cpp 38 | src/camera/Intrinsics.cpp 39 | src/camera/PinholeCamera.cpp 40 | src/pointcloud/PointCloud.cpp 41 | src/ProjectionIntegrator.cpp 42 | src/Chisel.cpp 43 | src/mesh/Mesh.cpp 44 | src/marching_cubes/MarchingCubes.cpp 45 | src/io/PLY.cpp) 46 | 47 | #target_link_libraries(${PROJECT_NAME} another_library) 48 | #rosbuild_add_boost_directories() 49 | #rosbuild_link_boost(${PROJECT_NAME} thread) 50 | #rosbuild_add_executable(example examples/example.cpp) 51 | #target_link_libraries(example ${PROJECT_NAME}) 52 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/weighting/ConstantWeighter.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef CONSTANTWEIGHTER_H_ 23 | #define CONSTANTWEIGHTER_H_ 24 | 25 | #include "Weighter.h" 26 | 27 | namespace chisel 28 | { 29 | 30 | class ConstantWeighter : public Weighter 31 | { 32 | public: 33 | ConstantWeighter() = default; 34 | ConstantWeighter(float w) 35 | { 36 | weight = w; 37 | } 38 | virtual ~ConstantWeighter() 39 | { 40 | 41 | } 42 | 43 | virtual float GetWeight(float surfaceDist, float truncationDist) const 44 | { 45 | return weight / (2 * truncationDist); 46 | } 47 | 48 | protected: 49 | float weight; 50 | 51 | }; 52 | typedef std::shared_ptr ConstantWeighterPtr; 53 | typedef std::shared_ptr ConstantWeighterConstPtr; 54 | 55 | } // namespace chisel 56 | 57 | #endif // CONSTANTWEIGHTER_H_ 58 | -------------------------------------------------------------------------------- /open_chisel/src/geometry/Plane.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | Plane::Plane() 28 | { 29 | 30 | } 31 | 32 | Plane::Plane(const Vec4& params) : 33 | normal(Vec3(params(0), params(1), params(2))), distance(params(3)) 34 | { 35 | 36 | } 37 | 38 | Plane::Plane(const Vec3& _normal, float _distance) : 39 | normal(_normal), distance() 40 | { 41 | 42 | } 43 | 44 | Plane::Plane(const Vec3& a, const Vec3& b, const Vec3& c) 45 | { 46 | Vec3 ab = b - a; 47 | Vec3 ac = c - a; 48 | 49 | Vec3 cross = ab.cross(ac); 50 | normal = cross.normalized(); 51 | distance = -(cross.dot(a)); 52 | } 53 | 54 | Plane::Plane(float a, float b, float c, float d) : 55 | normal(a, b, c), distance(d) 56 | { 57 | 58 | } 59 | 60 | float Plane::GetSignedDistance(const Vec3& point) const 61 | { 62 | return point.dot(normal) + distance; 63 | } 64 | 65 | Plane::~Plane() 66 | { 67 | 68 | } 69 | 70 | } // namespace chisel 71 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/threading/Threading.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef THREADING_H_ 24 | #define THREADING_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace chisel 32 | { 33 | template 34 | void parallel_for(const Iterator& first, const Iterator& last, Function&& f, const int nthreads = 16, const int threshold = 1000) 35 | { 36 | const auto group = std::max(std::max(ptrdiff_t(1), ptrdiff_t(std::abs(threshold))), ((last-first))/std::abs(nthreads)); 37 | std::vector threads; 38 | threads.reserve(nthreads); 39 | Iterator it = first; 40 | for (; it < last - group; it = std::min(it + group, last)) 41 | { 42 | threads.push_back(std::thread([=,&f](){std::for_each(it, std::min(it+group, last), f);})); 43 | } 44 | // last steps while we wait for other threads 45 | std::for_each(it, last, f); 46 | 47 | std::for_each(threads.begin(), threads.end(), [](std::thread& x){x.join();}); 48 | } 49 | } 50 | 51 | #endif // THREADING_H_ 52 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/truncation/ConstantTruncator.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef CONSTANTTRUNCATOR_H_ 23 | #define CONSTANTTRUNCATOR_H_ 24 | 25 | #include "Truncator.h" 26 | 27 | namespace chisel 28 | { 29 | 30 | class ConstantTruncator : public Truncator 31 | { 32 | public: 33 | ConstantTruncator() = default; 34 | 35 | ConstantTruncator(float value) : 36 | truncationDistance(value) 37 | { 38 | 39 | } 40 | 41 | virtual ~ConstantTruncator() 42 | { 43 | 44 | } 45 | 46 | inline void SetTruncationDistance(float value) { truncationDistance = value; } 47 | 48 | float GetTruncationDistance(float reading) const 49 | { 50 | return truncationDistance; 51 | } 52 | 53 | protected: 54 | float truncationDistance; 55 | 56 | }; 57 | typedef std::shared_ptr ConstantTruncatorPtr; 58 | typedef std::shared_ptr ConstantTruncatorConstPtr; 59 | 60 | } // namespace chisel 61 | 62 | #endif // CONSTANTTRUNCATOR_H_ 63 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/mesh/Mesh.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef MESH_H_ 23 | #define MESH_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace chisel 30 | { 31 | typedef size_t VertIndex; 32 | typedef std::vector VertIndexList; 33 | class Mesh 34 | { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 37 | Mesh(); 38 | virtual ~Mesh(); 39 | 40 | inline bool HasVertices() const { return !vertices.empty(); } 41 | inline bool HasNormals() const { return !normals.empty(); } 42 | inline bool HasColors() const { return !colors.empty(); } 43 | inline bool HasIndices() const { return !indices.empty(); } 44 | 45 | inline void Clear() 46 | { 47 | vertices.clear(); 48 | normals.clear(); 49 | colors.clear(); 50 | indices.clear(); 51 | } 52 | 53 | Vec3List vertices; 54 | VertIndexList indices; 55 | Vec3List normals; 56 | Vec3List colors; 57 | 58 | }; 59 | typedef std::shared_ptr MeshPtr; 60 | typedef std::shared_ptr MeshConstPtr; 61 | 62 | } // namespace chisel 63 | 64 | #endif // MESH_H_ 65 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/geometry/Geometry.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef CHISEL_GEOMETRY_H_ 24 | #define CHISEL_GEOMETRY_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace chisel 32 | { 33 | typedef Eigen::Vector2i Point2; 34 | typedef Eigen::Vector3i Point3; 35 | typedef Eigen::Vector2f Vec2; 36 | typedef Eigen::Vector3f Vec3; 37 | typedef Eigen::Vector4f Vec4; 38 | typedef Eigen::Matrix3f Mat3x3; 39 | typedef Eigen::Matrix4f Mat4x4; 40 | typedef Eigen::Affine3f Transform; 41 | typedef Eigen::Quaternionf Quaternion; 42 | 43 | typedef std::vector > Point2List; 44 | typedef std::vector > Point3List; 45 | typedef std::vector > Vec2List; 46 | typedef std::vector > Vec3List; 47 | typedef std::vector > Vec4List; 48 | typedef std::vector > Mat3x3List; 49 | typedef std::vector > Mat4List; 50 | typedef std::vector > TransformList; 51 | typedef std::vector > QuaternionList; 52 | } 53 | #endif // GEOMETRY_H_ 54 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/camera/Intrinsics.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef INTRINSICS_H_ 23 | #define INTRINSICS_H_ 24 | 25 | #include 26 | #include 27 | 28 | namespace chisel 29 | { 30 | 31 | class Intrinsics 32 | { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | Intrinsics(); 36 | virtual ~Intrinsics(); 37 | 38 | inline float GetFx() const { return matrix(0, 0); } 39 | inline void SetFx(float fx) { matrix(0, 0) = fx; } 40 | inline float GetFy() const { return matrix(1, 1); } 41 | inline void SetFy(float fy) { matrix(1, 1) = fy; } 42 | inline float GetCx() const { return matrix(0, 2); } 43 | inline void SetCx(float cx) { matrix(0, 2) = cx; } 44 | inline float GetCy() const { return matrix(1, 2); } 45 | inline void SetCy(float cy) { matrix(1, 2) = cy; } 46 | 47 | inline const Mat3x3& GetMatrix() const { return matrix; } 48 | inline Mat3x3& GetMutableMatrix() { return matrix; } 49 | inline void SetMatrix(const Mat3x3& m) { matrix = m; } 50 | 51 | protected: 52 | Mat3x3 matrix; 53 | }; 54 | typedef std::shared_ptr IntrinsicsPtr; 55 | typedef std::shared_ptr IntrinsicsConstPtr; 56 | 57 | } // namespace chisel 58 | 59 | #endif // INTRINSICS_H_ 60 | -------------------------------------------------------------------------------- /open_chisel/src/camera/PinholeCamera.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | PinholeCamera::PinholeCamera() 28 | { 29 | 30 | 31 | } 32 | 33 | PinholeCamera::~PinholeCamera() 34 | { 35 | 36 | } 37 | 38 | Vec3 PinholeCamera::ProjectPoint(const Vec3& point) const 39 | { 40 | const float& x = point(0); 41 | const float& y = point(1); 42 | const float& z = point(2); 43 | const float invZ = 1.0f / z; 44 | return Vec3(intrinsics.GetFx() * x * invZ + intrinsics.GetCx(), intrinsics.GetFy() * y * invZ + intrinsics.GetCy(), z); 45 | } 46 | 47 | Vec3 PinholeCamera::UnprojectPoint(const Vec3& point) const 48 | { 49 | const float& u = point(0); 50 | const float& v = point(1); 51 | const float& z = point(2); 52 | return Vec3(z * ((u - intrinsics.GetCx()) / intrinsics.GetFx()), z * ((v - intrinsics.GetCy()) / intrinsics.GetFy()), z); 53 | } 54 | 55 | void PinholeCamera::SetupFrustum(const Transform& view, Frustum* frustum) const 56 | { 57 | assert(frustum != nullptr); 58 | frustum->SetFromParams(view, nearPlane, farPlane, intrinsics.GetFy(), intrinsics.GetFy(), intrinsics.GetCx(), intrinsics.GetCy(), width, height); 59 | } 60 | 61 | bool PinholeCamera::IsPointOnImage(const Vec3& point) const 62 | { 63 | return point(0) >= 0 && point(1) >= 0 && point(0) < width && point(1) < height; 64 | } 65 | 66 | } // namespace chisel 67 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/geometry/Plane.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef PLANE_H_ 23 | #define PLANE_H_ 24 | 25 | #include 26 | #include "Geometry.h" 27 | 28 | namespace chisel 29 | { 30 | 31 | class Plane 32 | { 33 | public: 34 | 35 | enum class IntersectionType 36 | { 37 | Inside, 38 | Outside, 39 | Intersects 40 | }; 41 | 42 | Plane(); 43 | Plane(const Vec4& params); 44 | Plane(const Vec3& normal, float distance); 45 | Plane(const Vec3& p1, const Vec3& p2, const Vec3& p3); 46 | Plane(float a, float b, float c, float d); 47 | virtual ~Plane(); 48 | 49 | float GetSignedDistance(const Vec3& point) const; 50 | 51 | inline IntersectionType ClassifyPoint(const Vec3& point) const 52 | { 53 | float d = GetSignedDistance(point); 54 | 55 | if(d < 0) 56 | { 57 | return IntersectionType::Inside; 58 | } 59 | else if(d > 0) 60 | { 61 | return IntersectionType::Outside; 62 | } 63 | return IntersectionType::Intersects; 64 | } 65 | 66 | Vec3 normal; 67 | float distance; 68 | 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | }; 71 | typedef std::shared_ptr PlanePtr; 72 | typedef std::shared_ptr PlaneConstPtr; 73 | 74 | } // namespace chisel 75 | 76 | #endif // PLANE_H_ 77 | -------------------------------------------------------------------------------- /open_chisel/src/geometry/AABB.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | AABB::AABB() 28 | { 29 | 30 | } 31 | 32 | AABB::AABB(const Vec3& _min, const Vec3& _max) : 33 | min(_min), max(_max) 34 | { 35 | 36 | } 37 | 38 | Plane::IntersectionType AABB::Intersects(const Plane& plane) const 39 | { 40 | //check all corner side of plane 41 | Vec3 ext = GetExtents(); 42 | 43 | Vec3List corners; 44 | corners.resize(8); 45 | corners[0] = max; 46 | corners[1] = min; 47 | corners[2] = min + Vec3(ext(0), 0, 0); 48 | corners[3] = min + Vec3(0, ext(1), 0); 49 | corners[4] = min + Vec3(0, 0, ext(2)); 50 | corners[5] = min + Vec3(ext(0), 0, ext(2)); 51 | corners[6] = min + Vec3(ext(0), ext(1), 0); 52 | corners[7] = min + Vec3(0, ext(1), ext(2)); 53 | 54 | float lastdistance = plane.normal.dot(corners[0]) + plane.distance; 55 | 56 | for (int i = 1; i < 8; i++) 57 | { 58 | float distance = plane.normal.dot(corners[i]) + plane.distance; 59 | 60 | if ((distance <= 0.0f && lastdistance > 0.0f) || (distance >= 0.0f && lastdistance < 0.0f)) 61 | return Plane::IntersectionType::Intersects; 62 | 63 | lastdistance = distance; 64 | } 65 | 66 | if (lastdistance > 0.0f) 67 | return Plane::IntersectionType::Outside; 68 | 69 | return Plane::IntersectionType::Inside; 70 | 71 | } 72 | 73 | AABB::~AABB() 74 | { 75 | 76 | } 77 | 78 | } // namespace chisel 79 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/DistVoxel.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef DISTVOXEL_H_ 23 | #define DISTVOXEL_H_ 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace chisel 31 | { 32 | 33 | class DistVoxel 34 | { 35 | public: 36 | DistVoxel(); 37 | virtual ~DistVoxel(); 38 | 39 | inline float GetSDF() const 40 | { 41 | return sdf; 42 | } 43 | 44 | inline void SetSDF(const float& distance) 45 | { 46 | sdf = distance; 47 | } 48 | 49 | inline float GetWeight() const { return weight; } 50 | inline void SetWeight(const float& w) { weight = w; } 51 | 52 | inline void Integrate(const float& distUpdate, const float& weightUpdate) 53 | { 54 | float oldSDF = GetSDF(); 55 | float oldWeight = GetWeight(); 56 | float newDist = (oldWeight * oldSDF + weightUpdate * distUpdate) / (weightUpdate + oldWeight); 57 | SetSDF(newDist); 58 | SetWeight(oldWeight + weightUpdate); 59 | 60 | } 61 | 62 | inline void Carve() 63 | { 64 | //Reset(); 65 | Integrate(0.0, 1.5); 66 | } 67 | 68 | inline void Reset() 69 | { 70 | sdf = 99999; 71 | weight = 0; 72 | } 73 | 74 | protected: 75 | float sdf; 76 | float weight; 77 | }; 78 | 79 | } // namespace chisel 80 | 81 | #endif // DISTVOXEL_H_ 82 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/FixedPointFloat.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef FIXEDPOINTFLOAT_H_ 24 | #define FIXEDPOINTFLOAT_H_ 25 | 26 | #include 27 | 28 | namespace chisel 29 | { 30 | const float MinFloatValue = -1000; 31 | const float MaxFloatValue = 1000; 32 | const float MaxUFloat = 1000; 33 | typedef uint16_t FixedFloat16; 34 | typedef uint16_t UFixedFloat16; 35 | 36 | inline float ClampFloat(const float& input) 37 | { 38 | return std::max(std::min(input, MaxFloatValue), MinFloatValue); 39 | } 40 | 41 | inline float ClampUnsignedFloat(const float input) 42 | { 43 | return std::max(std::min(input, MaxUFloat), 0.0f); 44 | } 45 | 46 | inline FixedFloat16 FloatToFixedFloat16(const float& input) 47 | { 48 | return static_cast(((ClampFloat(input) - MinFloatValue) / (MaxFloatValue - MinFloatValue)) * std::numeric_limits::max()); 49 | } 50 | 51 | inline float FixedFloat16ToFloat(const FixedFloat16& input) 52 | { 53 | const float t = static_cast(input) / std::numeric_limits::max(); 54 | return MinFloatValue + t * (MaxFloatValue - MinFloatValue); 55 | } 56 | 57 | inline UFixedFloat16 FloatToUFixedFloat16(const float& input) 58 | { 59 | return static_cast((ClampUnsignedFloat(input) / MaxUFloat) * std::numeric_limits::max()); 60 | } 61 | 62 | inline float UFixedFloat16ToFloat(const UFixedFloat16& input) 63 | { 64 | const float t = static_cast(input) / std::numeric_limits::max(); 65 | return t * (MaxUFloat); 66 | } 67 | } 68 | 69 | 70 | 71 | #endif // FIXEDPOINTFLOAT_H_ 72 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/pointcloud/PointCloud.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef POINTCLOUD_H_ 23 | #define POINTCLOUD_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace chisel 30 | { 31 | 32 | class PointCloud 33 | { 34 | public: 35 | PointCloud(); 36 | virtual ~PointCloud(); 37 | 38 | inline bool HasColor() const 39 | { 40 | return !colors.empty(); 41 | } 42 | 43 | inline const Vec3List& GetPoints() const { return points; } 44 | inline Vec3List& GetMutablePoints() { return points; } 45 | inline const Vec3List& GetColors() const { return colors; } 46 | inline Vec3List& GetMutableColors() { return colors; } 47 | 48 | 49 | inline void AddPoint(const Vec3& point) 50 | { 51 | points.push_back(point); 52 | } 53 | 54 | inline void AddColor(const Vec3& color) 55 | { 56 | colors.push_back(color); 57 | } 58 | 59 | inline void AddPointAndColor(const Vec3& point, const Vec3& color) 60 | { 61 | AddPoint(point); 62 | AddColor(color); 63 | } 64 | 65 | inline void Clear() 66 | { 67 | points.clear(); 68 | colors.clear(); 69 | } 70 | 71 | 72 | protected: 73 | Vec3List points; 74 | Vec3List colors; 75 | }; 76 | typedef std::shared_ptr PointCloudPtr; 77 | typedef std::shared_ptr PointCloudConstPtr; 78 | 79 | } // namespace chisel 80 | 81 | #endif // POINTCLOUD_H_ 82 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/geometry/AABB.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef AABB_H_ 23 | #define AABB_H_ 24 | 25 | #include 26 | #include 27 | 28 | #include "Geometry.h" 29 | #include "Plane.h" 30 | 31 | namespace chisel 32 | { 33 | 34 | class AABB 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | AABB(); 39 | AABB(const Vec3& min, const Vec3& max); 40 | virtual ~AABB(); 41 | 42 | inline bool Contains(const Vec3& pos) const 43 | { 44 | return pos(0) >= min(0) && pos(1) >= min(1) && pos(2) >= min(2) && 45 | pos(0) <= max(0) && pos(1) <= max(1) && pos(2) <= max(2); 46 | } 47 | 48 | inline bool Intersects(const AABB& other) const 49 | { 50 | if(min.x() > other.max.x()) return false; 51 | if(min.y() > other.max.y()) return false; 52 | if(min.z() > other.max.z()) return false; 53 | if(max.x() < other.min.x()) return false; 54 | if(max.y() < other.min.y()) return false; 55 | if(max.z() < other.min.z()) return false; 56 | return true; 57 | } 58 | 59 | inline Vec3 GetCenter() const 60 | { 61 | return (max + min) * 0.5f; 62 | } 63 | 64 | inline Vec3 GetExtents() const 65 | { 66 | return (max - min) * 0.5f; 67 | } 68 | 69 | Plane::IntersectionType Intersects(const Plane& other) const; 70 | 71 | Vec3 min; 72 | Vec3 max; 73 | }; 74 | typedef std::shared_ptr AABBPtr; 75 | typedef std::shared_ptr AABBConstPtr; 76 | 77 | } // namespace chisel 78 | 79 | #endif // AABB_H_ 80 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/geometry/Frustum.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef FRUSTUM_H_ 23 | #define FRUSTUM_H_ 24 | 25 | #include 26 | #include "Geometry.h" 27 | #include "AABB.h" 28 | 29 | namespace chisel 30 | { 31 | 32 | class Frustum 33 | { 34 | public: 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | 37 | Frustum(); 38 | virtual ~Frustum(); 39 | 40 | bool Intersects(const AABB& box) const; 41 | bool Contains(const Vec3& point) const; 42 | void ComputeBoundingBox(AABB* box) const; 43 | void SetFromParams(const Transform& view, float near, float far, float fx, float fy, float cx, float cy, float imgWidth, float imgHeight); 44 | void SetFromVectors(const Vec3& forward, const Vec3& pos, const Vec3& right, const Vec3& up, float near, float far, float fov, float aspect); 45 | void SetFromOpenGLViewProjection(const Mat4x4& view, const Mat4x4& proj); 46 | 47 | const Plane& GetBottomPlane() const { return bottom; } 48 | const Plane& GetTopPlane() const { return top; } 49 | const Plane& GetLeftPlane() const { return left; } 50 | const Plane& GetRightPlane() const { return right; } 51 | const Plane& GetNearPlane() const { return near; } 52 | const Plane& GetFarPlane() const { return far; } 53 | 54 | const Vec3* GetLines() const { return lines; } 55 | const Vec3* GetCorners() const { return corners; } 56 | 57 | protected: 58 | Vec3 corners[8]; 59 | Vec3 lines[24]; 60 | Plane top; 61 | Plane left; 62 | Plane right; 63 | Plane bottom; 64 | Plane near; 65 | Plane far; 66 | }; 67 | typedef std::shared_ptr FrustumPtr; 68 | typedef std::shared_ptr FrustumConstPtr; 69 | 70 | } // namespace chisel 71 | 72 | #endif // FRUSTUM_H_ 73 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/truncation/QuadraticTruncator.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef QUADRATICTRUNCATOR_H_ 23 | #define QUADRATICTRUNCATOR_H_ 24 | 25 | namespace chisel 26 | { 27 | 28 | class QuadraticTruncator : public Truncator 29 | { 30 | public: 31 | QuadraticTruncator() = default; 32 | 33 | QuadraticTruncator(float quadratic, float linear, float constant, float scale) : 34 | quadraticTerm(quadratic), linearTerm(linear), constantTerm(constant), scalingFactor(scale) 35 | { 36 | 37 | } 38 | 39 | virtual ~QuadraticTruncator() 40 | { 41 | 42 | } 43 | 44 | 45 | float GetTruncationDistance(float reading) const 46 | { 47 | return std::abs(GetQuadraticTerm() * pow(reading, 2) + GetLinearTerm() * reading + GetConstantTerm()) * scalingFactor; 48 | } 49 | 50 | inline float GetQuadraticTerm() const { return quadraticTerm; } 51 | inline float GetLinearTerm() const { return linearTerm; } 52 | inline float GetConstantTerm() const { return constantTerm; } 53 | inline float GetScalingFactor() const { return scalingFactor; } 54 | inline void SetQuadraticTerm(float value) { quadraticTerm = value; } 55 | inline void SetLinearTerm(float value) { linearTerm = value; } 56 | inline void SetConstantTerm(float value) { constantTerm = value; } 57 | inline void SetScalingFactor(float value) { scalingFactor = value; } 58 | 59 | protected: 60 | float quadraticTerm; 61 | float linearTerm; 62 | float constantTerm; 63 | float scalingFactor; 64 | 65 | }; 66 | typedef std::shared_ptr QuadraticTruncatorPtr; 67 | typedef std::shared_ptr QuadraticTruncatorConstPtr; 68 | 69 | } // namespace chisel 70 | 71 | #endif // QUADRATICTRUNCATOR_H_ 72 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/camera/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef PINHOLECAMERA_H_ 23 | #define PINHOLECAMERA_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace chisel 31 | { 32 | 33 | class PinholeCamera 34 | { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | PinholeCamera(); 39 | virtual ~PinholeCamera(); 40 | 41 | inline const Intrinsics& GetIntrinsics() const { return intrinsics; } 42 | inline Intrinsics& GetMutableIntrinsics() { return intrinsics; } 43 | inline void SetIntrinsics(const Intrinsics& value) { intrinsics = value; } 44 | 45 | inline int GetWidth() const { return width; } 46 | inline int GetHeight() const { return height; } 47 | inline void SetWidth(int value) { width = value; } 48 | inline void SetHeight(int value) { height = value; } 49 | inline float GetNearPlane() const { return nearPlane; } 50 | inline float GetFarPlane() const { return farPlane; } 51 | inline void SetNearPlane(float value) { nearPlane = value; } 52 | inline void SetFarPlane(float value) { farPlane = value; } 53 | 54 | void SetupFrustum(const Transform& view, Frustum* frustum) const; 55 | 56 | Vec3 ProjectPoint(const Vec3& point) const; 57 | Vec3 UnprojectPoint(const Vec3& point) const; 58 | 59 | bool IsPointOnImage(const Vec3& point) const; 60 | 61 | protected: 62 | Intrinsics intrinsics; 63 | int width; 64 | int height; 65 | float nearPlane; 66 | float farPlane; 67 | 68 | }; 69 | typedef std::shared_ptr PinholeCameraPtr; 70 | typedef std::shared_ptr PinholeCameraConstPtr; 71 | 72 | } // namespace chisel 73 | 74 | #endif // PINHOLECAMERA_H_ 75 | -------------------------------------------------------------------------------- /open_chisel/src/io/PLY.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace chisel 28 | { 29 | bool SaveMeshPLYASCII(const std::string& fileName, const chisel::MeshConstPtr& mesh) 30 | { 31 | std::ofstream stream(fileName.c_str()); 32 | 33 | if (!stream) 34 | { 35 | return false; 36 | } 37 | 38 | size_t numPoints = mesh->vertices.size(); 39 | stream << "ply" << std::endl; 40 | stream << "format ascii 1.0" << std::endl; 41 | stream << "element vertex " << numPoints << std::endl; 42 | stream << "property float x" << std::endl; 43 | stream << "property float y" << std::endl; 44 | stream << "property float z" << std::endl; 45 | if (mesh->HasColors()) 46 | { 47 | stream << "property uchar red" << std::endl; 48 | stream << "property uchar green" << std::endl; 49 | stream << "property uchar blue" << std::endl; 50 | } 51 | stream << "element face " << numPoints / 3 << std::endl; 52 | stream << "property list uchar int vertex_index" << std::endl; 53 | stream << "end_header" << std::endl; 54 | 55 | size_t vert_idx = 0; 56 | for (const Vec3& vert : mesh->vertices) 57 | { 58 | stream << vert(0) << " " << vert(1) << " " << vert(2); 59 | 60 | if (mesh->HasColors()) 61 | { 62 | const Vec3& color = mesh->colors[vert_idx]; 63 | int r = static_cast(color(0) * 255.0f); 64 | int g = static_cast(color(1) * 255.0f); 65 | int b = static_cast(color(2) * 255.0f); 66 | 67 | stream << " " << r << " " << g << " " << b; 68 | } 69 | 70 | stream << std::endl; 71 | vert_idx++; 72 | } 73 | 74 | for (size_t i = 0; i < mesh->indices.size(); i+=3) 75 | { 76 | stream << "3 "; 77 | 78 | for(int j = 0; j < 3; j++) 79 | { 80 | stream << mesh->indices.at(i + j) << " "; 81 | } 82 | 83 | stream << std::endl; 84 | } 85 | 86 | 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /chisel_ros/include/chisel_ros/Serialization.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef SERIALIZATION_H_ 24 | #define SERIALIZATION_H_ 25 | 26 | #include 27 | #include 28 | 29 | namespace chisel_ros 30 | { 31 | void FillChunkMessage(chisel::ChunkConstPtr chunk, chisel_msgs::ChunkMessage* message) 32 | { 33 | chisel::ChunkHasher hasher; 34 | assert(message != nullptr); 35 | message->header.stamp = ros::Time::now(); 36 | 37 | chisel::ChunkID id = chunk->GetID(); 38 | message->ID_x = id.x(); 39 | message->ID_y = id.y(); 40 | message->ID_z = id.z(); 41 | message->spatial_hash = hasher(id); 42 | 43 | message->resolution_meters = chunk->GetVoxelResolutionMeters(); 44 | 45 | Eigen::Vector3i size = chunk->GetNumVoxels(); 46 | message->num_voxels_x = size.x(); 47 | message->num_voxels_y = size.y(); 48 | message->num_voxels_z = size.z(); 49 | 50 | message->distance_data.reserve(chunk->GetTotalNumVoxels()); 51 | 52 | if(chunk->HasColors()) 53 | { 54 | message->color_data.reserve(chunk->GetTotalNumVoxels()); 55 | } 56 | 57 | const std::vector& voxels = chunk->GetVoxels(); 58 | 59 | for (const chisel::DistVoxel& voxel : voxels) 60 | { 61 | float sdf = voxel.GetSDF(); 62 | float weight = voxel.GetWeight(); 63 | message->distance_data.push_back 64 | ( 65 | *(reinterpret_cast(&sdf)) 66 | | *(reinterpret_cast(&weight)) << sizeof(uint32_t) 67 | ); 68 | } 69 | 70 | const std::vector& colors = chunk->GetColorVoxels(); 71 | 72 | for (const chisel::ColorVoxel& voxel : colors) 73 | { 74 | message->color_data.push_back 75 | ( 76 | static_cast(voxel.GetRed()) 77 | | static_cast(voxel.GetBlue()) << sizeof(uint8_t) 78 | | static_cast(voxel.GetGreen()) << 2 * sizeof(uint8_t) 79 | | static_cast(voxel.GetBlue()) << 3 * sizeof(uint8_t) 80 | | static_cast(voxel.GetWeight()) << 4 * sizeof(uint8_t) 81 | ); 82 | } 83 | 84 | } 85 | } 86 | 87 | 88 | #endif // SERIALIZATION_H_ 89 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/ColorVoxel.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef COLORVOXEL_H_ 23 | #define COLORVOXEL_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | 30 | namespace chisel 31 | { 32 | 33 | class ColorVoxel 34 | { 35 | public: 36 | ColorVoxel(); 37 | virtual ~ColorVoxel(); 38 | 39 | static inline float Saturate(float value) 40 | { 41 | return std::min(std::max(value, 0.0f), 255.0f); 42 | } 43 | 44 | inline uint8_t GetRed() const { return red; } 45 | inline uint8_t GetGreen() const { return green; } 46 | inline uint8_t GetBlue() const { return blue; } 47 | inline uint8_t GetWeight() const { return weight; } 48 | inline void SetRed(uint8_t value) 49 | { 50 | red = value; 51 | } 52 | inline void SetGreen(uint8_t value) 53 | { 54 | green = value; 55 | } 56 | inline void SetBlue(uint8_t value) 57 | { 58 | blue = value; 59 | } 60 | inline void SetWeight(uint8_t value) 61 | { 62 | weight = value; 63 | } 64 | 65 | inline void Integrate(const uint8_t& newRed, const uint8_t& newGreen, const uint8_t& newBlue, const uint8_t& weightUpdate) 66 | { 67 | if(weight >= std::numeric_limits::max() - weightUpdate) 68 | { 69 | return; 70 | } 71 | 72 | float oldRed = static_cast(GetRed()); 73 | float updatedRed = Saturate(static_cast(weight * oldRed + weightUpdate * newRed) / (weightUpdate + weight)); 74 | red = static_cast(updatedRed); 75 | 76 | float oldGreen = static_cast(GetGreen()); 77 | float updatedGreen = Saturate(static_cast(weight * oldGreen + weightUpdate * newGreen) / (weightUpdate + weight)); 78 | green = static_cast(updatedGreen); 79 | 80 | float oldBlue = static_cast(GetBlue()); 81 | float updatedBlue = Saturate(static_cast(weight * oldBlue + weightUpdate * newBlue) / (weightUpdate + weight)); 82 | blue = static_cast(updatedBlue); 83 | 84 | SetWeight(weight + weightUpdate); 85 | } 86 | 87 | inline void Reset() 88 | { 89 | red = 0; 90 | green = 0; 91 | blue = 0; 92 | weight = 0; 93 | } 94 | 95 | protected: 96 | uint8_t red; 97 | uint8_t green; 98 | uint8_t blue; 99 | uint8_t weight; 100 | }; 101 | 102 | } // namespace chisel 103 | 104 | #endif // COLORVOXEL_H_ 105 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/camera/DepthImage.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef DEPTHIMAGE_H_ 23 | #define DEPTHIMAGE_H_ 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace chisel 31 | { 32 | 33 | template class DepthImage 34 | { 35 | public: 36 | DepthImage() : 37 | data(nullptr), width(-1), height(-1) 38 | { 39 | 40 | } 41 | 42 | DepthImage(int w, int h) : 43 | data(nullptr), width(w), height(h) 44 | { 45 | data = new DataType[width * height]; 46 | } 47 | 48 | virtual ~DepthImage() 49 | { 50 | delete [] data; 51 | data = nullptr; 52 | } 53 | 54 | inline int Index(int row, int col) const 55 | { 56 | return col + row * width; 57 | } 58 | 59 | inline float BilinearInterpolateDepth(float x, float y) const 60 | { 61 | int gxi = static_cast(x); 62 | int gyi = static_cast(y); 63 | float c00 = DepthAt(gyi, gxi); 64 | float c10 = DepthAt(gyi, gxi + 1); 65 | float c01 = DepthAt(gyi + 1, gxi); 66 | float c11 = DepthAt(gyi + 1, gxi + 1); 67 | return BilinearInterpolate(c00, c10, c01, c11, x - gxi, y - gyi); 68 | } 69 | 70 | inline float DepthAt(int row, int col) const 71 | { 72 | const DataType& d = At(row, col); 73 | return static_cast(d); 74 | } 75 | 76 | inline const DataType& At(int row, int col) const 77 | { 78 | return data[Index(row, col)]; 79 | } 80 | 81 | inline DataType& AtMutable(int row, int col) 82 | { 83 | return data[Index(row, col)]; 84 | } 85 | 86 | inline bool IsInside(int row, int col) const 87 | { 88 | return row >= 0 && row < width && col >= 0 && col < height; 89 | } 90 | 91 | void GetStats(DataType& minimum, DataType& maximum, DataType& mean, DataType invalidValid=0) const 92 | { 93 | int numPixels = width * height; 94 | minimum = std::numeric_limits::max(); 95 | maximum = -std::numeric_limits::max(); 96 | mean = static_cast(0); 97 | 98 | for (int i = 0; i < numPixels; i++) 99 | { 100 | const DataType& pixel = data[i]; 101 | 102 | if (pixel == invalidValid) continue; 103 | if (std::isnan(pixel)) continue; 104 | 105 | minimum = std::min(pixel, minimum); 106 | maximum = std::max(pixel, maximum); 107 | mean += pixel; 108 | } 109 | mean /= numPixels; 110 | } 111 | 112 | inline const DataType* GetData() const { return data; } 113 | inline DataType* GetMutableData() { return data; } 114 | inline void SetData(DataType* d) { data = d; } 115 | inline int GetWidth() const { return width; } 116 | inline int GetHeight() const { return height; } 117 | inline void SetWidth(int w) { width = w; } 118 | inline void SetHeight(int h) { height = h; } 119 | 120 | protected: 121 | DataType* data; 122 | int width; 123 | int height; 124 | }; 125 | 126 | 127 | 128 | } // namespace chisel 129 | 130 | #endif // DEPTHIMAGE_H_ 131 | -------------------------------------------------------------------------------- /open_chisel/src/Chunk.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | namespace chisel 25 | { 26 | 27 | Chunk::Chunk() : 28 | voxelResolutionMeters(0) 29 | { 30 | // TODO Auto-generated constructor stub 31 | 32 | } 33 | 34 | Chunk::Chunk(const ChunkID id, const Eigen::Vector3i& nv, float r, bool useColor) : 35 | ID(id), numVoxels(nv), voxelResolutionMeters(r) 36 | { 37 | AllocateDistVoxels(); 38 | 39 | if(useColor) 40 | { 41 | AllocateColorVoxels(); 42 | } 43 | 44 | origin = Vec3(numVoxels(0) * ID(0) * voxelResolutionMeters, numVoxels(1) * ID(1) * voxelResolutionMeters, numVoxels(2) * ID(2) * voxelResolutionMeters); 45 | } 46 | 47 | Chunk::~Chunk() 48 | { 49 | 50 | } 51 | 52 | void Chunk::AllocateDistVoxels() 53 | { 54 | int totalNum = GetTotalNumVoxels(); 55 | voxels.clear(); 56 | voxels.resize(totalNum, DistVoxel()); 57 | } 58 | 59 | void Chunk::AllocateColorVoxels() 60 | { 61 | int totalNum = GetTotalNumVoxels(); 62 | colors.clear(); 63 | colors.resize(totalNum, ColorVoxel()); 64 | } 65 | 66 | AABB Chunk::ComputeBoundingBox() 67 | { 68 | Vec3 pos = origin; 69 | Vec3 size = numVoxels.cast() * voxelResolutionMeters; 70 | return AABB(pos, pos + size); 71 | } 72 | 73 | Point3 Chunk::GetVoxelCoords(const Vec3& worldCoords) const 74 | { 75 | const float roundingFactorX = 1.0f / (voxelResolutionMeters); 76 | const float roundingFactorY = 1.0f / (voxelResolutionMeters); 77 | const float roundingFactorZ = 1.0f / (voxelResolutionMeters); 78 | 79 | return Point3( static_cast(std::floor(worldCoords(0) * roundingFactorX)), 80 | static_cast(std::floor(worldCoords(1) * roundingFactorY)), 81 | static_cast(std::floor(worldCoords(2) * roundingFactorZ))); 82 | } 83 | 84 | VoxelID Chunk::GetVoxelID(const Vec3& relativePos) const 85 | { 86 | return GetVoxelID(GetVoxelCoords(relativePos)); 87 | } 88 | 89 | VoxelID Chunk::GetLocalVoxelIDFromGlobal(const Point3& worldPoint) const 90 | { 91 | return GetVoxelID(GetLocalCoordsFromGlobal(worldPoint)); 92 | } 93 | 94 | Point3 Chunk::GetLocalCoordsFromGlobal(const Point3& worldPoint) const 95 | { 96 | return (worldPoint - Point3(ID.x() * numVoxels.x(), ID.y() * numVoxels.y(), ID.z() * numVoxels.z())); 97 | } 98 | 99 | 100 | void Chunk::ComputeStatistics(ChunkStatistics* stats) 101 | { 102 | assert(stats != nullptr); 103 | 104 | for (const DistVoxel& vox : voxels) 105 | { 106 | float weight = vox.GetWeight(); 107 | if (weight > 0) 108 | { 109 | float sdf = vox.GetSDF(); 110 | if (sdf < 0) 111 | { 112 | stats->numKnownInside++; 113 | } 114 | else 115 | { 116 | stats->numKnownOutside++; 117 | } 118 | } 119 | else 120 | { 121 | stats->numUnknown++; 122 | } 123 | 124 | stats->totalWeight += weight; 125 | 126 | } 127 | } 128 | 129 | Vec3 Chunk::GetColorAt(const Vec3& pos) 130 | { 131 | if(ComputeBoundingBox().Contains(pos)) 132 | { 133 | Vec3 chunkPos = (pos - origin) / voxelResolutionMeters; 134 | int chunkX = static_cast(chunkPos(0)); 135 | int chunkY = static_cast(chunkPos(1)); 136 | int chunkZ = static_cast(chunkPos(2)); 137 | 138 | if(IsCoordValid(chunkX, chunkY, chunkZ)) 139 | { 140 | const ColorVoxel& color = GetColorVoxel(chunkX, chunkY, chunkZ); 141 | float maxVal = static_cast(std::numeric_limits::max()); 142 | return Vec3(static_cast(color.GetRed()) / maxVal, static_cast(color.GetGreen()) / maxVal, static_cast(color.GetBlue()) / maxVal); 143 | } 144 | } 145 | 146 | return Vec3::Zero(); 147 | } 148 | 149 | 150 | } // namespace chisel 151 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/camera/ColorImage.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef COLORIMAGE_H_ 24 | #define COLORIMAGE_H_ 25 | 26 | 27 | namespace chisel 28 | { 29 | 30 | template struct Color 31 | { 32 | DataType red; 33 | DataType green; 34 | DataType blue; 35 | DataType alpha; 36 | }; 37 | 38 | template class ColorImage 39 | { 40 | public: 41 | ColorImage() : 42 | data(nullptr), width(-1), height(-1), numChannels(0) 43 | { 44 | 45 | } 46 | ColorImage(int w, int h, size_t channels) : 47 | data(nullptr), width(w), height(h), numChannels(channels) 48 | { 49 | data = new DataType[width * height * numChannels]; 50 | } 51 | 52 | virtual ~ColorImage() 53 | { 54 | if(data != nullptr) 55 | { 56 | delete [] data; 57 | data = nullptr; 58 | } 59 | } 60 | 61 | inline int Index(int row, int col, int channel) const 62 | { 63 | return (col + row * width) * numChannels + channel; 64 | } 65 | 66 | inline void At(int row, int col, Color* colorOut) const 67 | { 68 | const int index = Index(row, col, 0); 69 | 70 | switch (numChannels) 71 | { 72 | // Mono 73 | case 1: 74 | colorOut->red = data[index]; 75 | colorOut->green = colorOut->red; 76 | colorOut->blue = colorOut->red; 77 | colorOut->alpha = colorOut->red; 78 | break; 79 | // Two Channel 80 | case 2: 81 | colorOut->red = data[index]; 82 | colorOut->green = data[index + 1]; 83 | colorOut->blue = colorOut->green; 84 | colorOut->alpha = colorOut->green; 85 | break; 86 | // BGR 87 | case 3: 88 | colorOut->red = data[index + 2]; 89 | colorOut->green = data[index + 1]; 90 | colorOut->blue = data[index]; 91 | colorOut->alpha = colorOut->red; 92 | break; 93 | // BGRA 94 | case 4: 95 | colorOut->red = data[index + 2]; 96 | colorOut->green = data[index + 1]; 97 | colorOut->blue = data[index]; 98 | colorOut->alpha = data[index + 3]; 99 | break; 100 | } 101 | } 102 | 103 | inline const DataType& At(int row, int col, int channel) const 104 | { 105 | const int index = Index(row, col, channel); 106 | return data[index]; 107 | } 108 | 109 | inline DataType& AtMutable(int row, int col, int channel) const 110 | { 111 | return data[Index(row, col, channel)]; 112 | } 113 | 114 | inline bool IsInside(int row, int col) const 115 | { 116 | return row >= 0 && row < width && col >= 0 && col < height; 117 | } 118 | 119 | inline const DataType* GetData() const { return data; } 120 | inline DataType* GetMutableData() { return data; } 121 | inline void SetData(DataType* d) { data = d; } 122 | inline int GetWidth() const { return width; } 123 | inline int GetHeight() const { return height; } 124 | inline void SetWidth(int w) { width = w; } 125 | inline void SetHeight(int h) { height = h; } 126 | 127 | inline size_t GetNumChannels() const { return numChannels; } 128 | 129 | protected: 130 | DataType* data; 131 | int width; 132 | int height; 133 | size_t numChannels; 134 | }; 135 | 136 | } // namespace chisel 137 | 138 | 139 | 140 | #endif // COLORIMAGE_H_ 141 | -------------------------------------------------------------------------------- /open_chisel/src/geometry/Raycast.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace chisel; 5 | 6 | int signum(int x) 7 | { 8 | return x == 0 ? 0 : x < 0 ? -1 : 1; 9 | } 10 | 11 | float mod(float value, float modulus) 12 | { 13 | return fmod(fmod(value, modulus) + modulus, modulus); 14 | } 15 | 16 | float intbound(float s, float ds) 17 | { 18 | // Find the smallest positive t such that s+t*ds is an integer. 19 | if (ds < 0) 20 | { 21 | return intbound(-s, -ds); 22 | } 23 | else 24 | { 25 | s = mod(s, 1); 26 | // problem is now s+t*ds = 1 27 | return (1-s)/ds; 28 | } 29 | } 30 | 31 | bool RayIntersectsAABB(const Vec3& start, const Vec3& end, const Point3& lb, const Point3& rt) 32 | { 33 | Vec3 dir = (end - start).normalized(); 34 | Vec3 dirfrac(1.0f / dir.x(), 1.0f / dir.y(), 1.0f / dir.z()); 35 | 36 | // r.dir is unit direction vector of ray 37 | // lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner 38 | // start is origin of ray 39 | float t1 = (lb.x() - start.x()) * dirfrac.x(); 40 | float t2 = (rt.x() - start.x()) * dirfrac.x(); 41 | float t3 = (lb.y() - start.y()) * dirfrac.y(); 42 | float t4 = (rt.y() - start.y()) * dirfrac.y(); 43 | float t5 = (lb.z() - start.z()) * dirfrac.z(); 44 | float t6 = (rt.z() - start.z()) * dirfrac.z(); 45 | 46 | float tmin = fmax(fmax(fmin(t1, t2), fmin(t3, t4)), fmin(t5, t6)); 47 | float tmax = fmin(fmin(fmax(t1, t2), fmax(t3, t4)), fmax(t5, t6)); 48 | 49 | // if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us 50 | if (tmax < 0) 51 | { 52 | return false; 53 | } 54 | 55 | // if tmin > tmax, ray doesn't intersect AABB 56 | if (tmin > tmax) 57 | { 58 | return false; 59 | } 60 | 61 | return true; 62 | } 63 | 64 | void Raycast(const Vec3& start, const Vec3& end, const Point3& min, const Point3& max, Point3List* output) 65 | { 66 | assert(!!output); 67 | // From "A Fast Voxel Traversal Algorithm for Ray Tracing" 68 | // by John Amanatides and Andrew Woo, 1987 69 | // 70 | // 71 | // Extensions to the described algorithm: 72 | // • Imposed a distance limit. 73 | // • The face passed through to reach the current cube is provided to 74 | // the callback. 75 | 76 | // The foundation of this algorithm is a parameterized representation of 77 | // the provided ray, 78 | // origin + t * direction, 79 | // except that t is not actually stored; rather, at any given point in the 80 | // traversal, we keep track of the *greater* t values which we would have 81 | // if we took a step sufficient to cross a cube boundary along that axis 82 | // (i.e. change the integer part of the coordinate) in the variables 83 | // tMaxX, tMaxY, and tMaxZ. 84 | 85 | // Cube containing origin point. 86 | int x = (int)std::floor(start.x()); 87 | int y = (int)std::floor(start.y()); 88 | int z = (int)std::floor(start.z()); 89 | int endX = (int)std::floor(end.x()); 90 | int endY = (int)std::floor(end.y()); 91 | int endZ = (int)std::floor(end.z()); 92 | Vec3 direction = (end - start); 93 | float maxDist = direction.squaredNorm(); 94 | 95 | // Break out direction vector. 96 | float dx = endX - x; 97 | float dy = endY - y; 98 | float dz = endZ - z; 99 | 100 | // Direction to increment x,y,z when stepping. 101 | int stepX = (int)signum((int)dx); 102 | int stepY = (int)signum((int)dy); 103 | int stepZ = (int)signum((int)dz); 104 | 105 | // See description above. The initial values depend on the fractional 106 | // part of the origin. 107 | float tMaxX = intbound(start.x(), dx); 108 | float tMaxY = intbound(start.y(), dy); 109 | float tMaxZ = intbound(start.z(), dz); 110 | 111 | // The change in t when taking a step (always positive). 112 | float tDeltaX = ((float)stepX) / dx; 113 | float tDeltaY = ((float)stepY) / dy; 114 | float tDeltaZ = ((float)stepZ) / dz; 115 | 116 | // Avoids an infinite loop. 117 | if (stepX == 0 && stepY == 0 && stepZ == 0) 118 | return; 119 | 120 | float dist = 0; 121 | 122 | while (true) 123 | { 124 | 125 | if(x >= min.x() && x < max.x() && 126 | y >= min.y() && y < max.y() && 127 | z >= min.z() && z < max.z()) 128 | { 129 | output->push_back(Point3(x, y, z)); 130 | 131 | dist = (Vec3(x, y, z) - start).squaredNorm(); 132 | 133 | if (dist > maxDist) return; 134 | 135 | if (output->size() > 1500) 136 | { 137 | std::cerr << "Error, too many racyast voxels." << std::endl; 138 | throw std::out_of_range("Too many raycast voxels"); 139 | } 140 | } 141 | 142 | if(x == endX && y == endY && z == endZ) break; 143 | 144 | // tMaxX stores the t-value at which we cross a cube boundary along the 145 | // X axis, and similarly for Y and Z. Therefore, choosing the least tMax 146 | // chooses the closest cube boundary. Only the first case of the four 147 | // has been commented in detail. 148 | if (tMaxX < tMaxY) 149 | { 150 | if (tMaxX < tMaxZ) 151 | { 152 | // Update which cube we are now in. 153 | x += stepX; 154 | // Adjust tMaxX to the next X-oriented boundary crossing. 155 | tMaxX += tDeltaX; 156 | } 157 | else 158 | { 159 | z += stepZ; 160 | tMaxZ += tDeltaZ; 161 | } 162 | } 163 | else 164 | { 165 | if (tMaxY < tMaxZ) 166 | { 167 | y += stepY; 168 | tMaxY += tDeltaY; 169 | } 170 | else 171 | { 172 | z += stepZ; 173 | tMaxZ += tDeltaZ; 174 | } 175 | } 176 | } 177 | } 178 | 179 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/Chunk.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef CHUNK_H_ 23 | #define CHUNK_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include "DistVoxel.h" 31 | #include "ColorVoxel.h" 32 | 33 | namespace chisel 34 | { 35 | 36 | typedef Eigen::Vector3i ChunkID; 37 | typedef int VoxelID; 38 | 39 | struct ChunkStatistics 40 | { 41 | size_t numKnownInside; 42 | size_t numKnownOutside; 43 | size_t numUnknown; 44 | float totalWeight; 45 | }; 46 | 47 | class Chunk 48 | { 49 | public: 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | Chunk(); 52 | Chunk(const ChunkID id, const Eigen::Vector3i& numVoxels, float resolution, bool useColor); 53 | virtual ~Chunk(); 54 | 55 | void AllocateDistVoxels(); 56 | void AllocateColorVoxels(); 57 | 58 | inline const ChunkID& GetID() const { return ID; } 59 | inline ChunkID& GetIDMutable() { return ID; } 60 | inline void SetID(const ChunkID& id) { ID = id; } 61 | 62 | inline bool HasColors() const { return !colors.empty(); } 63 | inline bool HasVoxels() const { return !voxels.empty(); } 64 | inline const std::vector& GetVoxels() const { return voxels; } 65 | 66 | inline const Eigen::Vector3i& GetNumVoxels() const { return numVoxels; } 67 | inline float GetVoxelResolutionMeters() const { return voxelResolutionMeters; } 68 | 69 | inline const DistVoxel& GetDistVoxel(const VoxelID& voxelID) const { return voxels.at(voxelID); } 70 | inline DistVoxel& GetDistVoxelMutable(const VoxelID& voxelID) { return voxels.at(voxelID); } 71 | inline const ColorVoxel& GetColorVoxel(const VoxelID& voxelID) const { return colors.at(voxelID); } 72 | inline ColorVoxel& GetColorVoxelMutable(const VoxelID& voxelID) { return colors.at(voxelID); } 73 | 74 | Point3 GetVoxelCoords(const Vec3& worldCoords) const; 75 | 76 | inline VoxelID GetVoxelID(const Point3& coords) const 77 | { 78 | return GetVoxelID(coords.x(), coords.y(), coords.z()); 79 | } 80 | 81 | inline VoxelID GetVoxelID(int x, int y, int z) const 82 | { 83 | return (z * numVoxels(1) + y) * numVoxels(0) + x; 84 | } 85 | 86 | inline const DistVoxel& GetDistVoxel(int x, int y, int z) const 87 | { 88 | return GetDistVoxel(GetVoxelID(x, y, z)); 89 | } 90 | 91 | inline DistVoxel& GetDistVoxelMutable(int x, int y, int z) 92 | { 93 | return GetDistVoxelMutable(GetVoxelID(x, y, z)); 94 | } 95 | 96 | inline const ColorVoxel& GetColorVoxel(int x, int y, int z) const 97 | { 98 | return GetColorVoxel(GetVoxelID(x, y, z)); 99 | } 100 | 101 | inline ColorVoxel& GetColorVoxelMutable(int x, int y, int z) 102 | { 103 | return GetColorVoxelMutable(GetVoxelID(x, y, z)); 104 | } 105 | 106 | inline bool IsCoordValid(VoxelID idx) const 107 | { 108 | return idx >= 0 && idx < voxels.size(); 109 | } 110 | 111 | inline bool IsCoordValid(int x, int y, int z) const 112 | { 113 | return (x >= 0 && x < numVoxels(0) && y >= 0 && y < numVoxels(1) && z >= 0 && z < numVoxels(2)); 114 | } 115 | 116 | 117 | inline size_t GetTotalNumVoxels() const 118 | { 119 | return numVoxels(0) * numVoxels(1) * numVoxels(2); 120 | } 121 | 122 | inline const std::vector& GetColorVoxels() const 123 | { 124 | return colors; 125 | } 126 | 127 | void ComputeStatistics(ChunkStatistics* stats); 128 | 129 | AABB ComputeBoundingBox(); 130 | 131 | inline const Vec3& GetOrigin() { return origin; } 132 | 133 | Vec3 GetColorAt(const Vec3& pos); 134 | 135 | VoxelID GetVoxelID(const Vec3& worldPos) const; 136 | VoxelID GetLocalVoxelIDFromGlobal(const Point3& worldPoint) const; 137 | Point3 GetLocalCoordsFromGlobal(const Point3& worldPoint) const; 138 | 139 | protected: 140 | ChunkID ID; 141 | Eigen::Vector3i numVoxels; 142 | float voxelResolutionMeters; 143 | std::vector voxels; 144 | std::vector colors; 145 | Vec3 origin; 146 | 147 | }; 148 | 149 | typedef std::shared_ptr ChunkPtr; 150 | typedef std::shared_ptr ChunkConstPtr; 151 | typedef std::vector > ChunkIDList; 152 | 153 | } // namespace chisel 154 | 155 | #endif // CHUNK_H_ 156 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/marching_cubes/MarchingCubes.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef MARCHINGCUBES_H_ 23 | #define MARCHINGCUBES_H_ 24 | 25 | #include 26 | #include 27 | 28 | namespace chisel 29 | { 30 | 31 | typedef std::vector > TriangleVector; 32 | class MarchingCubes 33 | { 34 | public: 35 | static int triangleTable[256][16]; 36 | static int edgeIndexPairs[12][2]; 37 | 38 | MarchingCubes(); 39 | virtual ~MarchingCubes(); 40 | 41 | 42 | static void MeshCube(const Eigen::Matrix& vertex_coordinates, const Eigen::Matrix& vertexSDF, TriangleVector* triangles) 43 | { 44 | assert(triangles != nullptr); 45 | 46 | const int index = CalculateVertexConfiguration(vertexSDF); 47 | 48 | Eigen::Matrix edgeCoords; 49 | InterpolateEdgeVertices(vertex_coordinates, vertexSDF, &edgeCoords); 50 | 51 | const int* table_row = triangleTable[index]; 52 | 53 | int edgeIDX = 0; 54 | int tableCol = 0; 55 | while ((edgeIDX = table_row[tableCol]) != -1) 56 | { 57 | Eigen::Matrix3f triangle; 58 | triangle.col(0) = edgeCoords.col(edgeIDX); 59 | edgeIDX = table_row[tableCol + 1]; 60 | triangle.col(1) = edgeCoords.col(edgeIDX); 61 | edgeIDX = table_row[tableCol + 2]; 62 | triangle.col(2) = edgeCoords.col(edgeIDX); 63 | triangles->push_back(triangle); 64 | tableCol += 3; 65 | } 66 | } 67 | 68 | static void MeshCube(const Eigen::Matrix& vertexCoords, const Eigen::Matrix& vertexSDF, VertIndex* nextIDX, Mesh* mesh) 69 | { 70 | assert(nextIDX != nullptr); 71 | assert(mesh != nullptr); 72 | const int index = CalculateVertexConfiguration(vertexSDF); 73 | 74 | Eigen::Matrix edge_vertex_coordinates; 75 | InterpolateEdgeVertices(vertexCoords, vertexSDF, &edge_vertex_coordinates); 76 | 77 | const int* table_row = triangleTable[index]; 78 | 79 | int table_col = 0; 80 | while (table_row[table_col] != -1) 81 | { 82 | mesh->vertices.emplace_back(edge_vertex_coordinates.col(table_row[table_col + 2])); 83 | mesh->vertices.emplace_back(edge_vertex_coordinates.col(table_row[table_col + 1])); 84 | mesh->vertices.emplace_back(edge_vertex_coordinates.col(table_row[table_col])); 85 | mesh->indices.push_back(*nextIDX); 86 | mesh->indices.push_back((*nextIDX) + 1); 87 | mesh->indices.push_back((*nextIDX) + 2); 88 | const Eigen::Vector3f& p0 = mesh->vertices[*nextIDX]; 89 | const Eigen::Vector3f& p1 = mesh->vertices[*nextIDX + 1]; 90 | const Eigen::Vector3f& p2 = mesh->vertices[*nextIDX + 2]; 91 | Eigen::Vector3f px = (p1 - p0); 92 | Eigen::Vector3f py = (p2 - p0); 93 | Eigen::Vector3f n = px.cross(py).normalized(); 94 | mesh->normals.push_back(n); 95 | mesh->normals.push_back(n); 96 | mesh->normals.push_back(n); 97 | *nextIDX += 3; 98 | table_col += 3; 99 | } 100 | } 101 | 102 | static int CalculateVertexConfiguration(const Eigen::Matrix& vertexSDF) 103 | { 104 | return (vertexSDF(0) < 0 ? (1<<0) : 0) | 105 | (vertexSDF(1) < 0 ? (1<<1) : 0) | 106 | (vertexSDF(2) < 0 ? (1<<2) : 0) | 107 | (vertexSDF(3) < 0 ? (1<<3) : 0) | 108 | (vertexSDF(4) < 0 ? (1<<4) : 0) | 109 | (vertexSDF(5) < 0 ? (1<<5) : 0) | 110 | (vertexSDF(6) < 0 ? (1<<6) : 0) | 111 | (vertexSDF(7) < 0 ? (1<<7) : 0); 112 | } 113 | 114 | static void InterpolateEdgeVertices(const Eigen::Matrix& vertexCoords, const Eigen::Matrix& vertSDF, Eigen::Matrix* edgeCoords) 115 | { 116 | assert(edgeCoords != nullptr); 117 | for (std::size_t i = 0; i < 12; ++i) 118 | { 119 | const int* pairs = edgeIndexPairs[i]; 120 | const int edge0 = pairs[0]; 121 | const int edge1 = pairs[1]; 122 | // Only interpolate along edges where there is a zero crossing. 123 | if ((vertSDF(edge0) < 0 && vertSDF(edge1) >= 0) || (vertSDF(edge0) >= 0 && vertSDF(edge1) < 0)) 124 | edgeCoords->col(i) = InterpolateVertex(vertexCoords.col(edge0), vertexCoords.col(edge1), vertSDF(edge0), vertSDF(edge1)); 125 | } 126 | } 127 | 128 | // Performs linear interpolation on two cube corners to find the approximate 129 | // zero crossing (surface) value. 130 | static inline Vec3 InterpolateVertex(const Vec3& vertex1, const Vec3& vertex2, const float& sdf1, const float& sdf2) 131 | { 132 | const float minDiff = 1e-6; 133 | const float sdfDiff = sdf1 - sdf2; 134 | if (fabs(sdfDiff) < minDiff) 135 | { 136 | return Vec3(vertex1 + 0.5 * vertex2); 137 | } 138 | const float t = sdf1 / sdfDiff; 139 | return Vec3(vertex1 + t * (vertex2 - vertex1)); 140 | } 141 | }; 142 | 143 | } // namespace chisel 144 | 145 | #endif // MARCHINGCUBES_H_ 146 | -------------------------------------------------------------------------------- /chisel_ros/src/ChiselNode.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | #include 22 | #include 23 | #include 24 | 25 | int main(int argc, char** argv) 26 | { 27 | ROS_INFO("Starting up chisel node."); 28 | ros::init(argc, argv, "Chisel"); 29 | ros::NodeHandle nh("~"); 30 | int chunkSizeX, chunkSizeY, chunkSizeZ; 31 | double voxelResolution; 32 | double truncationDistQuad; 33 | double truncationDistLinear; 34 | double truncationDistConst; 35 | double truncationDistScale; 36 | int weight; 37 | bool useCarving; 38 | bool useColor; 39 | bool saveFile; 40 | double carvingDist; 41 | std::string depthImageTopic; 42 | std::string depthImageInfoTopic; 43 | std::string depthImageTransform; 44 | std::string colorImageTopic; 45 | std::string colorImageInfoTopic; 46 | std::string colorImageTransform; 47 | std::string baseTransform; 48 | std::string meshTopic; 49 | std::string chunkBoxTopic; 50 | std::string fileToSave; 51 | double nearPlaneDist; 52 | double farPlaneDist; 53 | chisel_ros::ChiselServer::FusionMode mode; 54 | std::string modeString; 55 | std::string pointCloudTopic; 56 | 57 | nh.param("chunk_size_x", chunkSizeX, 32); 58 | nh.param("chunk_size_y", chunkSizeY, 32); 59 | nh.param("chunk_size_z", chunkSizeZ, 32); 60 | nh.param("truncation_constant", truncationDistConst, 0.001504); 61 | nh.param("truncation_linear", truncationDistLinear, 0.00152); 62 | nh.param("truncation_quadratic", truncationDistQuad, 0.0019); 63 | nh.param("truncation_scale", truncationDistScale, 8.0); 64 | nh.param("integration_weight", weight, 1); 65 | nh.param("use_voxel_carving", useCarving, true); 66 | nh.param("use_color", useColor, true); 67 | nh.param("save_file_on_exit", saveFile, true); 68 | nh.param("carving_dist_m", carvingDist, 0.05); 69 | nh.param("voxel_resolution_m", voxelResolution, 0.03); 70 | nh.param("near_plane_dist", nearPlaneDist, 0.05); 71 | nh.param("far_plane_dist", farPlaneDist, 5.0); 72 | nh.param("depth_image_topic", depthImageTopic, std::string("/depth_image")); 73 | nh.param("point_cloud_topic", pointCloudTopic, std::string("/camera/depth_registered/points")); 74 | nh.param("depth_image_info_topic", depthImageInfoTopic, std::string("/depth_camera_info")); 75 | nh.param("depth_image_transform", depthImageTransform, std::string("/camera_depth_optical_frame")); 76 | nh.param("color_image_topic", colorImageTopic, std::string("/color_image")); 77 | nh.param("color_image_info_topic", colorImageInfoTopic, std::string("/color_camera_info")); 78 | nh.param("color_image_transform", colorImageTransform, std::string("/camera_rgb_optical_frame")); 79 | nh.param("base_transform", baseTransform, std::string("/camera_link")); 80 | nh.param("mesh_topic", meshTopic, std::string("full_mesh")); 81 | nh.param("chunk_box_topic", chunkBoxTopic, std::string("chunk_boxes")); 82 | nh.param("fusion_mode", modeString, std::string("DepthImage")); 83 | nh.param("file_path", fileToSave, std::string("/home/mklingen/.ros/chisel.ply")); 84 | 85 | if(modeString == "DepthImage") 86 | { 87 | ROS_INFO("Mode depth image"); 88 | mode = chisel_ros::ChiselServer::FusionMode::DepthImage; 89 | } 90 | else if(modeString == "PointCloud") 91 | { 92 | ROS_INFO("Mode point cloud"); 93 | mode = chisel_ros::ChiselServer::FusionMode::PointCloud; 94 | } 95 | else 96 | { 97 | ROS_ERROR("Unrecognized fusion mode %s. Recognized modes: \"DepthImage\", \"PointCloud\"\n", modeString.c_str()); 98 | return -1; 99 | } 100 | 101 | ROS_INFO("Subscribing."); 102 | chisel::Vec4 truncation(truncationDistQuad, truncationDistLinear, truncationDistConst, truncationDistScale); 103 | 104 | chisel_ros::ChiselServerPtr server(new chisel_ros::ChiselServer(nh, chunkSizeX, chunkSizeY, chunkSizeZ, voxelResolution, useColor, mode)); 105 | server->SetupProjectionIntegrator(truncation, static_cast(weight), useCarving, carvingDist); 106 | 107 | if (mode == chisel_ros::ChiselServer::FusionMode::DepthImage) 108 | { 109 | server->SubscribeDepthImage(depthImageTopic, depthImageInfoTopic, depthImageTransform); 110 | } 111 | else 112 | { 113 | server->SubscribePointCloud(pointCloudTopic); 114 | } 115 | 116 | server->SetupDepthPosePublisher("last_depth_pose"); 117 | server->SetupDepthFrustumPublisher("last_depth_frustum"); 118 | 119 | server->SetNearPlaneDist(nearPlaneDist); 120 | server->SetFarPlaneDist(farPlaneDist); 121 | server->AdvertiseServices(); 122 | 123 | if (useColor && mode == chisel_ros::ChiselServer::FusionMode::DepthImage) 124 | { 125 | server->SubscribeColorImage(colorImageTopic, colorImageInfoTopic, colorImageTransform); 126 | server->SetupColorPosePublisher("last_color_pose"); 127 | server->SetupColorFrustumPublisher("last_color_frustum"); 128 | } 129 | 130 | server->SetBaseTransform(baseTransform); 131 | server->SetupMeshPublisher(meshTopic); 132 | server->SetupChunkBoxPublisher(chunkBoxTopic); 133 | ROS_INFO("Beginning to loop."); 134 | 135 | ros::Rate loop_rate(100); 136 | 137 | while (ros::ok()) 138 | { 139 | loop_rate.sleep(); 140 | ros::spinOnce(); 141 | 142 | if(!server->IsPaused() && server->HasNewData()) 143 | { 144 | switch (server->GetMode()) 145 | { 146 | case chisel_ros::ChiselServer::FusionMode::DepthImage: 147 | server->IntegrateLastDepthImage(); 148 | break; 149 | case chisel_ros::ChiselServer::FusionMode::PointCloud: 150 | server->IntegrateLastPointCloud(); 151 | break; 152 | } 153 | 154 | server->PublishMeshes(); 155 | server->PublishChunkBoxes(); 156 | 157 | if(mode == chisel_ros::ChiselServer::FusionMode::DepthImage) 158 | { 159 | server->PublishDepthPose(); 160 | server->PublishDepthFrustum(); 161 | 162 | if(useColor) 163 | { 164 | server->PublishColorPose(); 165 | server->PublishColorFrustum(); 166 | } 167 | } 168 | } 169 | } 170 | 171 | if (saveFile) 172 | { 173 | ROS_INFO("Saving to %s\n", fileToSave.c_str()); 174 | server->GetChiselMap()->SaveAllMeshesToPLY(fileToSave); 175 | } 176 | 177 | } 178 | 179 | 180 | 181 | 182 | 183 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | OpenChisel 2 | ========== 3 | 4 | An open-source version of the Chisel chunked TSDF library. It contains two packages: 5 | 6 | ## open_chisel 7 | `open_chisel` is an implementation of a generic truncated signed distance field ([TSDF](https://graphics.stanford.edu/papers/volrange/volrange.pdf)) 3D mapping library; based on the Chisel mapping framework developed originally for Google's [Project Tango](https://www.google.com/atap/project-tango/). It is a complete re-write of the original mapping system (which is proprietary). `open_chisel` is chunked and spatially hashed [inspired by this work from Neissner et. al](http://www.graphics.stanford.edu/~niessner/niessner2013hashing.html), making it more memory-efficient than fixed-grid mapping approaches, and more performant than octree-based approaches. A technical description of how it works can be found in our [RSS 2015 paper](http://www.roboticsproceedings.org/rss11/p40.pdf). 8 | 9 | This reference implementation does not include any pose estimation. Therefore **the pose of the sensor must be provided from an external source**. This implementation also *avoids the use of any GPU computing*, which makes it suitable for limited hardware platforms. It does not contain any system for rendering/displaying the resulting 3D reconstruction. It has been tested on Ubuntu 14.04 in Linux with ROS hydro/indigo. 10 | 11 | ### API Usage 12 | Check the `chisel_ros` package source for an example of how to use the API. The `ChiselServer` class makes use of the `chisel_ros` API. 13 | 14 | ### Dependencies 15 | * [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) 16 | * C++11 17 | * [catkin](http://wiki.ros.org/catkin) build system 18 | 19 | Compilation note: 20 | For speed, it is essential to compile `open_chisel` with optimization. You will need to add the flag `-DCMAKE_BUILD_TYPE=Release` to your `catkin_make` command when building. 21 | 22 | ## chisel_ros 23 | `chisel_ros` is a wrapper around `open_chisel` that interfaces with ROS-based depth and color sensors. The main class `chisel_ros` provides is `ChiselServer`, which subscribes to depth images, color images, TF frames, and camera intrinsics. 24 | 25 | Note: you will also need to get the messages package, [chisel_msgs](https://github.com/personalrobotics/chisel_msgs) to build this. 26 | 27 | ### Supported ROS image types: 28 | **Depth Images** 29 | * 32 bit floating point mono in meters (`32FC1`) 30 | * 16 bit unsigned characters in millimeters (`16UC1`) 31 | 32 | **Color Images** 33 | * `BRG8` 34 | * `BGRA8` 35 | * `Mono8` 36 | 37 | ### Dependencies 38 | * Eigen 39 | * C++11 40 | * catkin (`ros-hydro` or `ros-indigo` or higher) 41 | * [PCL 1.8](http://pointclouds.org/) compiled with stdC++11 enabled. 42 | * ROS OpenCV [cv_bridge](http://wiki.ros.org/cv_bridge) 43 | 44 | ### A note on PCL 45 | Unfortunately, PCL 1.7x (the standard PCL included in current versions of ROS) doesn't work with C++11. This project makes use of C++11, so in order to use Chisel, you will have to download and install PCL 1.8 from source, and compile it with C++11 enabled. 46 | 47 | 1. Download PCL 1.8 from here: https://github.com/PointCloudLibrary/pcl 48 | 2. Modify line 112 of `CMakeLists.txt` in PCL to say `SET(CMAKE_CXX_FLAGS "-Wall -std=c++11 ...` 49 | 3. Build and install PCL 1.8 50 | 4. Download `pcl_ros` from here: https://github.com/ros-perception/perception_pcl 51 | 5. Change the dependency from `PCL` to `PCL 1.8` in `find_package` of the `CMakeLists.txt` 52 | 6. Compile `pcl_ros`. 53 | 4. Rebuild Chisel 54 | 55 | If PCL does not gain `c++11` support by default soon, we may just get rid of `c++11` in `OpenChisel` and use `boost` instead. 56 | 57 | ### Launching chisel_ros Server 58 | 59 | Once built, the `chisel_ros` server can be launched by using a launch file. There's an example launch file located at `chisel_ros/launch/launch_kinect_local.launch`. Modify the parameters as necessary to connect to your camera and TF frame. 60 | ```XML 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 74 | 75 | 76 | 77 | 79 | 80 | 81 | 82 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | ``` 109 | Then, launch the server using `roslaunch chisel_ros .launch`. You should see an output saying that `open_chisel` received depth images. Now, you can visualize the results in `rviz`. 110 | 111 | Type `rosrun rviz rviz` to open up the RVIZ visualizer. Then, add a `Marker` topic with the name `/Chisel/full_mesh`. This topic displays the mesh reconstructed by Chisel. 112 | 113 | ### Services 114 | `chisel_ros` provides several ROS services you can use to interface with the reconstruction in real-time. These are: 115 | 116 | * `Reset` -- Deletes all the TSDF data and starts the reconstruction from scratch. 117 | * `TogglePaused` -- Pauses/Unpauses reconstruction 118 | * `SaveMesh` -- Saves a `PLY` mesh file to the desired location of the entire scene 119 | * `GetAllChunks` -- Returns a list of all of the voxel data in the scene. Each chunk is stored as a seperate entity with its data stored in a byte array. 120 | -------------------------------------------------------------------------------- /open_chisel/src/ProjectionIntegrator.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace chisel 27 | { 28 | 29 | ProjectionIntegrator::ProjectionIntegrator() : 30 | carvingDist(0), enableVoxelCarving(false) 31 | { 32 | // TODO Auto-generated constructor stub 33 | 34 | } 35 | 36 | ProjectionIntegrator::ProjectionIntegrator(const TruncatorPtr& t, const WeighterPtr& w, float crvDist, bool enableCrv, const Vec3List& centers) : 37 | truncator(t), weighter(w), carvingDist(crvDist), enableVoxelCarving(enableCrv), centroids(centers) 38 | { 39 | 40 | } 41 | 42 | bool ProjectionIntegrator::Integrate(const PointCloud& cloud, const Transform& cameraPose, Chunk* chunk, const std::vector& idx) const 43 | { 44 | assert(!!chunk); 45 | 46 | if(cloud.HasColor() && chunk->HasColors()) 47 | { 48 | return IntegrateColorPointCloud(cloud, cameraPose, chunk, idx); 49 | } 50 | else 51 | { 52 | return IntegratePointCloud(cloud, cameraPose, chunk, idx); 53 | } 54 | } 55 | 56 | 57 | bool ProjectionIntegrator::IntegratePointCloud(const PointCloud& cloud, const Transform& cameraPose, Chunk* chunk, const std::vector& idx) const 58 | { 59 | const float round = 1.0f / chunk->GetVoxelResolutionMeters(); 60 | 61 | Point3List raycastVoxels; 62 | const Point3& chunkMin = Point3::Zero(); 63 | const Point3& chunkMax = chunk->GetNumVoxels(); 64 | bool updated = false; 65 | Vec3 startCamera = cameraPose.translation(); 66 | Transform inversePose = cameraPose.inverse(); 67 | //std::cout << "Start camera: " << startCamera.transpose() << std::endl; 68 | //std::cout << "Inverse pose: " << std::endl << inversePose.matrix() << std::endl; 69 | //std::cout << idx.size() << " points intersect " << chunk->GetID().transpose() << std::endl; 70 | 71 | for (size_t i : idx) 72 | { 73 | const Vec3& point = cloud.GetPoints().at(i); 74 | Vec3 worldPoint = cameraPose * point; 75 | float depth = point.z(); 76 | Vec3 dir = (worldPoint - startCamera).normalized(); 77 | float truncation = truncator->GetTruncationDistance(depth); 78 | Vec3 start = (worldPoint - dir * truncation - chunk->GetOrigin()) * round; 79 | Vec3 end = (worldPoint + dir * truncation - chunk->GetOrigin()) * round; 80 | 81 | raycastVoxels.clear(); 82 | Raycast(start, end, chunkMin, chunkMax, &raycastVoxels); 83 | 84 | 85 | //std::cout << "chunk: " << chunk->GetID().transpose() << " from " 86 | // << start.transpose() << " to " << end.transpose() << " : " << raycastVoxels.size() << std::endl; 87 | for (const Point3& voxelCoords : raycastVoxels) 88 | { 89 | VoxelID id = chunk->GetVoxelID(voxelCoords); 90 | DistVoxel& distVoxel = chunk->GetDistVoxelMutable(id); 91 | const Vec3& centroid = centroids[id] + chunk->GetOrigin(); 92 | float u = depth - (inversePose * (centroid)).z(); 93 | float weight = weighter->GetWeight(u, truncation); 94 | 95 | if (fabs(u) < truncation) 96 | { 97 | distVoxel.Integrate(u, weight); 98 | updated = true; 99 | } 100 | else if (enableVoxelCarving && u > truncation + carvingDist) 101 | { 102 | if (distVoxel.GetWeight() > 0 && distVoxel.GetSDF() < 0.0f) 103 | { 104 | distVoxel.Carve(); 105 | updated = true; 106 | } 107 | } 108 | } 109 | } 110 | 111 | return updated; 112 | } 113 | 114 | bool ProjectionIntegrator::IntegrateColorPointCloud(const PointCloud& cloud, const Transform& cameraPose, Chunk* chunk, const std::vector& idx) const 115 | { 116 | const float roundX = 1.0f / chunk->GetVoxelResolutionMeters(); 117 | const float roundY = 1.0f / chunk->GetVoxelResolutionMeters(); 118 | const float roundZ = 1.0f / chunk->GetVoxelResolutionMeters(); 119 | 120 | Point3List raycastVoxels; 121 | Point3 chunkMin = Point3::Zero(); 122 | Point3 chunkMax = chunk->GetNumVoxels(); 123 | bool updated = false; 124 | size_t i = 0; 125 | Vec3 startCamera = cameraPose.translation(); 126 | Transform inversePose = cameraPose.inverse(); 127 | for (const Vec3& point : cloud.GetPoints()) 128 | { 129 | const Vec3& color = cloud.GetColors()[i]; 130 | Vec3 worldPoint = cameraPose * point; 131 | float depth = point.z(); 132 | Vec3 dir = (worldPoint - startCamera).normalized(); 133 | float truncation = truncator->GetTruncationDistance(depth); 134 | Vec3 start = worldPoint - dir * truncation - chunk->GetOrigin(); 135 | Vec3 end = worldPoint + dir * truncation - chunk->GetOrigin(); 136 | start.x() *= roundX; 137 | start.y() *= roundY; 138 | start.z() *= roundZ; 139 | end.x() *= roundX; 140 | end.y() *= roundY; 141 | end.z() *= roundZ; 142 | raycastVoxels.clear(); 143 | Raycast(start, end, chunkMin, chunkMax, &raycastVoxels); 144 | 145 | for (const Point3& voxelCoords : raycastVoxels) 146 | { 147 | VoxelID id = chunk->GetVoxelID(voxelCoords); 148 | ColorVoxel& voxel = chunk->GetColorVoxelMutable(id); 149 | DistVoxel& distVoxel = chunk->GetDistVoxelMutable(id); 150 | const Vec3& centroid = centroids[id] + chunk->GetOrigin(); 151 | float u = depth - (inversePose * centroid - startCamera).z(); 152 | float weight = weighter->GetWeight(u, truncation); 153 | if (fabs(u) < truncation) 154 | { 155 | distVoxel.Integrate(u, weight); 156 | voxel.Integrate((uint8_t)(color.x() * 255.0f), (uint8_t)(color.y() * 255.0f), (uint8_t)(color.z() * 255.0f), 1); 157 | updated = true; 158 | } 159 | else if (enableVoxelCarving && u > truncation + carvingDist) 160 | { 161 | if (distVoxel.GetWeight() > 0) 162 | { 163 | distVoxel.Integrate(1.0e-5, 5.0f); 164 | updated = true; 165 | } 166 | } 167 | } 168 | i++; 169 | 170 | } 171 | return updated; 172 | } 173 | 174 | ProjectionIntegrator::~ProjectionIntegrator() 175 | { 176 | // TODO Auto-generated destructor stub 177 | } 178 | 179 | } // namespace chisel 180 | -------------------------------------------------------------------------------- /open_chisel/src/Chisel.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include 29 | namespace chisel 30 | { 31 | 32 | Chisel::Chisel() 33 | { 34 | // TODO Auto-generated constructor stub 35 | } 36 | 37 | Chisel::Chisel(const Eigen::Vector3i& chunkSize, float voxelResolution, bool useColor) : 38 | chunkManager(chunkSize, voxelResolution, useColor) 39 | { 40 | 41 | } 42 | 43 | Chisel::~Chisel() 44 | { 45 | // TODO Auto-generated destructor stub 46 | } 47 | 48 | void Chisel::Reset() 49 | { 50 | chunkManager.Reset(); 51 | meshesToUpdate.clear(); 52 | } 53 | 54 | void Chisel::UpdateMeshes() 55 | { 56 | chunkManager.RecomputeMeshes(meshesToUpdate); 57 | meshesToUpdate.clear(); 58 | } 59 | 60 | void Chisel::GarbageCollect(const ChunkIDList& chunks) 61 | { 62 | std::cout << chunkManager.GetChunks().size() << " chunks " << chunkManager.GetAllMeshes().size() << "meshes before collect."; 63 | for (const ChunkID& chunkID : chunks) 64 | { 65 | chunkManager.RemoveChunk(chunkID); 66 | meshesToUpdate.erase(chunkID); 67 | } 68 | std::cout << chunkManager.GetChunks().size() << " chunks " << chunkManager.GetAllMeshes().size() << "meshes after collect."; 69 | } 70 | 71 | bool Chisel::SaveAllMeshesToPLY(const std::string& filename) 72 | { 73 | printf("Saving all meshes to PLY file...\n"); 74 | 75 | chisel::MeshPtr fullMesh(new chisel::Mesh()); 76 | 77 | size_t v = 0; 78 | for (const std::pair& it : chunkManager.GetAllMeshes()) 79 | { 80 | for (const Vec3& vert : it.second->vertices) 81 | { 82 | fullMesh->vertices.push_back(vert); 83 | fullMesh->indices.push_back(v); 84 | v++; 85 | } 86 | 87 | for (const Vec3& color : it.second->colors) 88 | { 89 | fullMesh->colors.push_back(color); 90 | } 91 | 92 | for (const Vec3& normal : it.second->normals) 93 | { 94 | fullMesh->normals.push_back(normal); 95 | } 96 | } 97 | 98 | printf("Full mesh has %lu verts\n", v); 99 | bool success = SaveMeshPLYASCII(filename, fullMesh); 100 | 101 | if (!success) 102 | { 103 | printf("Saving failed!\n"); 104 | } 105 | 106 | return success; 107 | } 108 | 109 | void Chisel::IntegratePointCloud(const ProjectionIntegrator& integrator, 110 | const PointCloud& cloud, 111 | const Transform& cameraPose, 112 | float maxDist) 113 | { 114 | 115 | const float roundToVoxel = 1.0f / chunkManager.GetResolution(); 116 | const Vec3 halfVoxel = Vec3(0.5, 0.5, 0.5) * chunkManager.GetResolution(); 117 | std::unordered_map updated; 118 | std::unordered_map newChunks; 119 | const Vec3 startCamera = cameraPose.translation(); 120 | const Transform inversePose = cameraPose.inverse(); 121 | const Point3 minVal(-std::numeric_limits::max(), -std::numeric_limits::max(), -std::numeric_limits::max()); 122 | const Point3 maxVal(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); 123 | const size_t numPoints = cloud.GetPoints().size(); 124 | const bool carving = integrator.IsCarvingEnabled(); 125 | const float carvingDist = integrator.GetCarvingDist(); 126 | 127 | for (size_t i = 0; i < numPoints; i++) 128 | { 129 | Point3List raycastVoxels; 130 | const Vec3& point = cloud.GetPoints().at(i); 131 | Vec3 worldPoint = cameraPose * point; 132 | float depth = point.z(); 133 | 134 | if (depth < 0.01f) continue; 135 | 136 | Vec3 dir = (worldPoint - startCamera).normalized(); 137 | float truncation = integrator.GetTruncator()->GetTruncationDistance(depth); 138 | Vec3 start = carving ? static_cast(startCamera * roundToVoxel) : static_cast((worldPoint - dir * truncation) * roundToVoxel); 139 | Vec3 end = (worldPoint + dir * truncation) * roundToVoxel; 140 | 141 | raycastVoxels.clear(); 142 | Raycast(start, end, minVal, maxVal, &raycastVoxels); 143 | if (raycastVoxels.size() > 500) 144 | { 145 | std::cerr << "Error, too many racyast voxels." << std::endl; 146 | throw std::out_of_range("Too many raycast voxels"); 147 | } 148 | 149 | //std::cout << "chunk: " << chunk->GetID().transpose() << " from " 150 | // << start.transpose() << " to " << end.transpose() << " : " << raycastVoxels.size() << std::endl; 151 | for (const Point3& voxelCoords : raycastVoxels) 152 | { 153 | Vec3 center = chunkManager.GetCentroid(voxelCoords); 154 | bool wasNew = false; 155 | ChunkPtr chunk = chunkManager.GetOrCreateChunkAt(center, &wasNew); 156 | if (wasNew) 157 | { 158 | newChunks[chunk->GetID()] = true; 159 | updated[chunk->GetID()] = false; 160 | } 161 | 162 | VoxelID id = chunk->GetLocalVoxelIDFromGlobal(voxelCoords); 163 | if (!chunk->IsCoordValid(id)) 164 | { 165 | continue; 166 | } 167 | 168 | DistVoxel& distVoxel = chunk->GetDistVoxelMutable(id); 169 | float u = depth - (inversePose * (center)).z(); 170 | float weight = integrator.GetWeighter()->GetWeight(u, truncation); 171 | 172 | if (fabs(u) < truncation) 173 | { 174 | distVoxel.Integrate(u, weight); 175 | updated[chunk->GetID()] = true; 176 | } 177 | else if (!wasNew && carving && u > truncation + carvingDist) 178 | { 179 | if (distVoxel.GetWeight() > 0 && distVoxel.GetSDF() < 0.0f) 180 | { 181 | distVoxel.Carve(); 182 | updated[chunk->GetID()] = true; 183 | } 184 | } 185 | } 186 | } 187 | 188 | for (const std::pair& updatedChunk : updated) 189 | { 190 | if (updatedChunk.second) 191 | { 192 | for (int dx = -1; dx <= 1; dx++) 193 | { 194 | for (int dy = -1; dy <= 1; dy++) 195 | { 196 | for (int dz = -1; dz <= 1; dz++) 197 | { 198 | meshesToUpdate[updatedChunk.first + ChunkID(dx, dy, dz)] = true; 199 | } 200 | } 201 | } 202 | } 203 | } 204 | ChunkIDList garbageChunks; 205 | for (const std::pair& newChunk : newChunks) 206 | { 207 | if (!updated[newChunk.first]) 208 | { 209 | garbageChunks.push_back(newChunk.first); 210 | } 211 | } 212 | 213 | GarbageCollect(garbageChunks); 214 | } 215 | 216 | 217 | } // namespace chisel 218 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/Chisel.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef CHISEL_H_ 23 | #define CHISEL_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace chisel 35 | { 36 | 37 | class Chisel 38 | { 39 | public: 40 | Chisel(); 41 | Chisel(const Eigen::Vector3i& chunkSize, float voxelResolution, bool useColor); 42 | virtual ~Chisel(); 43 | 44 | inline const ChunkManager& GetChunkManager() const { return chunkManager; } 45 | inline ChunkManager& GetMutableChunkManager() { return chunkManager; } 46 | inline void SetChunkManager(const ChunkManager& manager) { chunkManager = manager; } 47 | 48 | void IntegratePointCloud(const ProjectionIntegrator& integrator, 49 | const PointCloud& cloud, 50 | const Transform& extrinsic, 51 | float maxDist); 52 | 53 | template void IntegrateDepthScan(const ProjectionIntegrator& integrator, 54 | const std::shared_ptr >& depthImage, 55 | const Transform& extrinsic, 56 | const PinholeCamera& camera) 57 | { 58 | printf("CHISEL: Integrating a scan\n"); 59 | 60 | DataType minimum, maximum, mean; 61 | depthImage->GetStats(minimum, maximum, mean); 62 | 63 | Frustum frustum; 64 | PinholeCamera cameraCopy = camera; 65 | cameraCopy.SetNearPlane(static_cast(minimum)); 66 | cameraCopy.SetFarPlane(static_cast(maximum)); 67 | 68 | cameraCopy.SetupFrustum(extrinsic, &frustum); 69 | 70 | ChunkIDList chunksIntersecting; 71 | chunkManager.GetChunkIDsIntersecting(frustum, &chunksIntersecting); 72 | 73 | std::mutex mutex; 74 | ChunkIDList garbageChunks; 75 | for(const ChunkID& chunkID : chunksIntersecting) 76 | //parallel_for(chunksIntersecting.begin(), chunksIntersecting.end(), [&](const ChunkID& chunkID) 77 | { 78 | bool chunkNew = false; 79 | 80 | mutex.lock(); 81 | if (!chunkManager.HasChunk(chunkID)) 82 | { 83 | chunkNew = true; 84 | chunkManager.CreateChunk(chunkID); 85 | } 86 | 87 | ChunkPtr chunk = chunkManager.GetChunk(chunkID); 88 | mutex.unlock(); 89 | 90 | bool needsUpdate = integrator.Integrate(depthImage, camera, extrinsic, chunk.get()); 91 | 92 | mutex.lock(); 93 | if (needsUpdate) 94 | { 95 | for (int dx = -1; dx <= 1; dx++) 96 | { 97 | for (int dy = -1; dy <= 1; dy++) 98 | { 99 | for (int dz = -1; dz <= 1; dz++) 100 | { 101 | meshesToUpdate[chunkID + ChunkID(dx, dy, dz)] = true; 102 | } 103 | } 104 | } 105 | } 106 | else if(chunkNew) 107 | { 108 | garbageChunks.push_back(chunkID); 109 | } 110 | mutex.unlock(); 111 | } 112 | //); 113 | printf("CHISEL: Done with scan\n"); 114 | GarbageCollect(garbageChunks); 115 | //chunkManager.PrintMemoryStatistics(); 116 | } 117 | 118 | template void IntegrateDepthScanColor(const ProjectionIntegrator& integrator, const std::shared_ptr >& depthImage, const Transform& depthExtrinsic, const PinholeCamera& depthCamera, const std::shared_ptr >& colorImage, const Transform& colorExtrinsic, const PinholeCamera& colorCamera) 119 | { 120 | Frustum frustum; 121 | depthCamera.SetupFrustum(depthExtrinsic, &frustum); 122 | 123 | ChunkIDList chunksIntersecting; 124 | chunkManager.GetChunkIDsIntersecting(frustum, &chunksIntersecting); 125 | 126 | std::mutex mutex; 127 | ChunkIDList garbageChunks; 128 | //for ( const ChunkID& chunkID : chunksIntersecting) 129 | parallel_for(chunksIntersecting.begin(), chunksIntersecting.end(), [&](const ChunkID& chunkID) 130 | { 131 | 132 | mutex.lock(); 133 | bool chunkNew = false; 134 | if (!chunkManager.HasChunk(chunkID)) 135 | { 136 | chunkNew = true; 137 | chunkManager.CreateChunk(chunkID); 138 | } 139 | 140 | ChunkPtr chunk = chunkManager.GetChunk(chunkID); 141 | mutex.unlock(); 142 | 143 | 144 | bool needsUpdate = integrator.IntegrateColor(depthImage, depthCamera, depthExtrinsic, colorImage, colorCamera, colorExtrinsic, chunk.get()); 145 | 146 | mutex.lock(); 147 | if (needsUpdate) 148 | { 149 | for (int dx = -1; dx <= 1; dx++) 150 | { 151 | for (int dy = -1; dy <= 1; dy++) 152 | { 153 | for (int dz = -1; dz <= 1; dz++) 154 | { 155 | meshesToUpdate[chunkID + ChunkID(dx, dy, dz)] = true; 156 | } 157 | } 158 | } 159 | } 160 | else if(chunkNew) 161 | { 162 | garbageChunks.push_back(chunkID); 163 | } 164 | mutex.unlock(); 165 | } 166 | ); 167 | 168 | GarbageCollect(garbageChunks); 169 | //chunkManager.PrintMemoryStatistics(); 170 | } 171 | 172 | void GarbageCollect(const ChunkIDList& chunks); 173 | void UpdateMeshes(); 174 | 175 | bool SaveAllMeshesToPLY(const std::string& filename); 176 | void Reset(); 177 | 178 | const ChunkSet& GetMeshesToUpdate() const { return meshesToUpdate; } 179 | 180 | protected: 181 | ChunkManager chunkManager; 182 | ChunkSet meshesToUpdate; 183 | 184 | }; 185 | typedef std::shared_ptr ChiselPtr; 186 | typedef std::shared_ptr ChiselConstPtr; 187 | 188 | } // namespace chisel 189 | 190 | #endif // CHISEL_H_ 191 | -------------------------------------------------------------------------------- /chisel_ros/include/chisel_ros/ChiselServer.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef CHISELSERVER_H_ 23 | #define CHISELSERVER_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace chisel_ros 45 | { 46 | 47 | typedef float DepthData; 48 | typedef uint8_t ColorData; 49 | 50 | class ChiselServer 51 | { 52 | public: 53 | enum class FusionMode 54 | { 55 | DepthImage, 56 | PointCloud 57 | }; 58 | 59 | struct RosCameraTopic 60 | { 61 | std::string imageTopic; 62 | std::string infoTopic; 63 | std::string transform; 64 | chisel::PinholeCamera cameraModel; 65 | ros::Subscriber imageSubscriber; 66 | ros::Subscriber infoSubscriber; 67 | ros::Publisher lastPosePublisher; 68 | ros::Publisher frustumPublisher; 69 | chisel::Transform lastPose; 70 | ros::Time lastImageTimestamp; 71 | bool gotPose; 72 | bool gotInfo; 73 | bool gotImage; 74 | }; 75 | 76 | struct RosPointCloudTopic 77 | { 78 | std::string cloudTopic; 79 | std::string transform; 80 | ros::Subscriber cloudSubscriber; 81 | chisel::Transform lastPose; 82 | ros::Time lastTimestamp; 83 | bool gotPose; 84 | bool gotCloud; 85 | }; 86 | 87 | ChiselServer(); 88 | ChiselServer(const ros::NodeHandle& nodeHanlde, int chunkSizeX, int chunkSizeY, int chunkSizeZ, float resolution, bool color, FusionMode fusionMode); 89 | virtual ~ChiselServer(); 90 | 91 | inline chisel::ChiselPtr GetChiselMap() { return chiselMap; } 92 | inline void SetChiselMap(const chisel::ChiselPtr value) { chiselMap = value; } 93 | 94 | inline const std::string& GetBaseTransform() const { return baseTransform; } 95 | inline const std::string& GetMeshTopic() const { return meshTopic; } 96 | 97 | void SetupProjectionIntegrator(const chisel::Vec4& truncation, uint16_t weight, bool useCarving, float carvingDist); 98 | void SetupMeshPublisher(const std::string& meshTopic); 99 | void SetupChunkBoxPublisher(const std::string& boxTopic); 100 | void SetupDepthPosePublisher(const std::string& depthPoseTopic); 101 | void SetupColorPosePublisher(const std::string& colorPoseTopic); 102 | void SetupDepthFrustumPublisher(const std::string& frustumTopic); 103 | void SetupColorFrustumPublisher(const std::string& frustumTopic); 104 | 105 | void PublishMeshes(); 106 | void PublishChunkBoxes(); 107 | void PublishLatestChunkBoxes(); 108 | void PublishDepthPose(); 109 | void PublishColorPose(); 110 | void PublishDepthFrustum(); 111 | void PublishColorFrustum(); 112 | 113 | void SubscribeDepthImage(const std::string& depthImageTopic, const std::string& cameraInfoTopic, const std::string& transform); 114 | void DepthCameraInfoCallback(sensor_msgs::CameraInfoConstPtr cameraInfo); 115 | void DepthImageCallback(sensor_msgs::ImageConstPtr depthImage); 116 | 117 | void SubscribeColorImage(const std::string& colorImageTopic, const std::string& cameraInfoTopic, const std::string& transform); 118 | void ColorCameraInfoCallback(sensor_msgs::CameraInfoConstPtr cameraInfo); 119 | void ColorImageCallback(sensor_msgs::ImageConstPtr colorImage); 120 | 121 | void SubscribePointCloud(const std::string& topic); 122 | void PointCloudCallback(sensor_msgs::PointCloud2ConstPtr pointcloud); 123 | 124 | void IntegrateLastDepthImage(); 125 | void IntegrateLastPointCloud(); 126 | void FillMarkerTopicWithMeshes(visualization_msgs::Marker* marker); 127 | inline void SetBaseTransform(const std::string& frameName) { baseTransform = frameName; } 128 | 129 | inline bool HasNewData() { return hasNewData; } 130 | 131 | inline float GetNearPlaneDist() const { return nearPlaneDist; } 132 | inline float GetFarPlaneDist() const { return farPlaneDist; } 133 | inline void SetNearPlaneDist(float dist) { nearPlaneDist = dist; } 134 | inline void SetFarPlaneDist(float dist) { farPlaneDist = dist; } 135 | 136 | bool Reset(chisel_msgs::ResetService::Request& request, chisel_msgs::ResetService::Response& response); 137 | bool TogglePaused(chisel_msgs::PauseService::Request& request, chisel_msgs::PauseService::Response& response); 138 | bool SaveMesh(chisel_msgs::SaveMeshService::Request& request, chisel_msgs::SaveMeshService::Response& response); 139 | bool GetAllChunks(chisel_msgs::GetAllChunksService::Request& request, chisel_msgs::GetAllChunksService::Response& response); 140 | 141 | inline bool IsPaused() { return isPaused; } 142 | inline void SetPaused(bool paused) { isPaused = paused; } 143 | 144 | void AdvertiseServices(); 145 | 146 | inline FusionMode GetMode() { return mode; } 147 | inline void SetMode(const FusionMode& m) { mode = m; } 148 | 149 | void SetDepthImage(const sensor_msgs::ImageConstPtr& img); 150 | void SetDepthPose(const Eigen::Affine3f& tf); 151 | void SetColorImage(const sensor_msgs::ImageConstPtr& img); 152 | void SetColorPose(const Eigen::Affine3f& tf); 153 | void SetColorCameraInfo(const sensor_msgs::CameraInfoConstPtr& info); 154 | void SetDepthCameraInfo(const sensor_msgs::CameraInfoConstPtr& info); 155 | 156 | protected: 157 | visualization_msgs::Marker CreateFrustumMarker(const chisel::Frustum& frustum); 158 | 159 | ros::NodeHandle nh; 160 | chisel::ChiselPtr chiselMap; 161 | tf::TransformListener transformListener; 162 | std::shared_ptr > lastDepthImage; 163 | std::shared_ptr > lastColorImage; 164 | chisel::PointCloudPtr lastPointCloud; 165 | chisel::ProjectionIntegrator projectionIntegrator; 166 | std::string baseTransform; 167 | std::string meshTopic; 168 | std::string chunkBoxTopic; 169 | ros::Publisher meshPublisher; 170 | ros::Publisher chunkBoxPublisher; 171 | ros::Publisher latestChunkPublisher; 172 | ros::ServiceServer resetServer; 173 | ros::ServiceServer pauseServer; 174 | ros::ServiceServer saveMeshServer; 175 | ros::ServiceServer getAllChunksServer; 176 | RosCameraTopic depthCamera; 177 | RosCameraTopic colorCamera; 178 | RosPointCloudTopic pointcloudTopic; 179 | bool useColor; 180 | bool hasNewData; 181 | float nearPlaneDist; 182 | float farPlaneDist; 183 | bool isPaused; 184 | FusionMode mode; 185 | }; 186 | typedef std::shared_ptr ChiselServerPtr; 187 | typedef std::shared_ptr ChiselServerConstPtr; 188 | 189 | } // namespace chisel 190 | 191 | #endif // CHISELSERVER_H_ 192 | -------------------------------------------------------------------------------- /open_chisel/src/geometry/Frustum.cpp: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace chisel 28 | { 29 | 30 | Frustum::Frustum() 31 | { 32 | // TODO Auto-generated constructor stub 33 | 34 | } 35 | 36 | Frustum::~Frustum() 37 | { 38 | // TODO Auto-generated destructor stub 39 | } 40 | 41 | bool Frustum::Intersects(const AABB& box) const 42 | { 43 | const Plane* planes[] = { &far, &near, &top, &bottom, &left, &right }; 44 | 45 | for (const Plane* plane : planes) 46 | { 47 | Vec3 axisVert; 48 | const Vec3& normal = plane->normal; 49 | 50 | // x-axis 51 | // Which AABB vertex is furthest down (plane normals direction) the x axis. 52 | if (normal(0) < 0.0f) 53 | axisVert(0) = box.min(0); 54 | else 55 | axisVert(0) = box.max(0); 56 | 57 | // y-axis 58 | // Which AABB vertex is furthest down (plane normals direction) the y axis. 59 | if (normal(1) < 0.0f) 60 | axisVert(1) = box.min(1); 61 | else 62 | axisVert(1) = box.max(1); 63 | 64 | // z-axis 65 | // Which AABB vertex is furthest down (plane normals direction) the z axis. 66 | if (normal(2) < 0.0f) 67 | axisVert(2) = box.min(2); 68 | else 69 | axisVert(2) = box.max(2); 70 | 71 | // Now we get the signed distance from the AABB vertex that's furthest down 72 | // the frustum planes normal, and if the signed distance is negative, then 73 | // the entire bounding box is behind the frustum plane. 74 | if (axisVert.dot(normal) + plane->distance > 0.0f) 75 | return true; 76 | } 77 | 78 | return false; 79 | } 80 | 81 | bool Frustum::Contains(const Vec3& point) const 82 | { 83 | // Compute distance to all the planes of the frustum. 84 | const Plane* planes[] = { &far, &near, &top, &bottom, &left, &right }; 85 | 86 | int i = 0; 87 | for (const Plane* plane : planes) 88 | { 89 | if (plane->ClassifyPoint(point) == Plane::IntersectionType::Outside) 90 | { 91 | return false; 92 | } 93 | i++; 94 | } 95 | 96 | // Frustrum is convex intersection of positive parts of 6 planes. 97 | // If it is inside all the planes, it is inside the frustum. 98 | return true; 99 | } 100 | 101 | void Frustum::ComputeBoundingBox(AABB* box) const 102 | { 103 | assert(box != nullptr); 104 | float bigNum = std::numeric_limits::max(); 105 | 106 | Vec3 tempMin(bigNum, bigNum, bigNum); 107 | Vec3 tempMax(-bigNum, -bigNum, -bigNum); 108 | for (int i = 0; i < 8; i++) 109 | { 110 | const Vec3& corner = corners[i]; 111 | tempMin(0) = std::min(tempMin(0), corner(0)); 112 | tempMin(1) = std::min(tempMin(1), corner(1)); 113 | tempMin(2) = std::min(tempMin(2), corner(2)); 114 | 115 | tempMax(0) = std::max(tempMax(0), corner(0)); 116 | tempMax(1) = std::max(tempMax(1), corner(1)); 117 | tempMax(2) = std::max(tempMax(2), corner(2)); 118 | } 119 | 120 | box->min = tempMin; 121 | box->max = tempMax; 122 | } 123 | 124 | void Frustum::SetFromOpenGLViewProjection(const Mat4x4& view, const Mat4x4& proj) 125 | { 126 | Vec3 right = view.transpose().block(0, 0, 3, 1).cast(); 127 | Vec3 up = view.transpose().block(0, 1, 3, 1).cast(); 128 | Vec3 d = -view.transpose().block(0, 2, 3, 1).cast(); 129 | Vec3 p = view.block(0, 3, 3, 1).cast(); 130 | const float& aa = proj.data()[0]; 131 | const float& bb = proj.data()[5]; 132 | const float& cc = proj.data()[10]; 133 | const float& dd = proj.data()[14]; 134 | 135 | float aspect = bb / aa; 136 | float fov = 2.0f * atan(1.0f / bb); 137 | float kk = (cc - 1.0f) / (cc + 1.0f); 138 | float n = (dd * (1.0f - kk)) / (2.0f * kk); 139 | float f = kk * n; 140 | SetFromVectors(d, p, right, up, n, f, fov, aspect); 141 | } 142 | 143 | void Frustum::SetFromParams(const Transform& view, float nearDist, float farDist, float fx, float fy, float /*cx*/, float cy, float imgWidth, float imgHeight) 144 | { 145 | Mat3x3 r = view.linear(); 146 | Vec3 right = r.col(0); 147 | Vec3 up = -r.col(1); 148 | Vec3 d = r.col(2); 149 | Vec3 p = view.translation(); 150 | float aspect = (fx * imgWidth) / (fy * imgHeight); 151 | float fov = atan2(cy, fy) + atan2(imgHeight - cy, fy); 152 | SetFromVectors(d, p, right, up, nearDist, farDist, fov, aspect); 153 | } 154 | 155 | void Frustum::SetFromVectors(const Vec3& forward, const Vec3& pos, const Vec3& rightVec, const Vec3& up, float nearDist, float farDist, float fov, float aspect) 156 | { 157 | float angleTangent = tan(fov / 2); 158 | float heightFar = angleTangent * farDist; 159 | float widthFar = heightFar * aspect; 160 | float heightNear = angleTangent * nearDist; 161 | float widthNear = heightNear * aspect; 162 | Vec3 farCenter = pos + forward * farDist; 163 | Vec3 farTopLeft = farCenter + (up * heightFar) - (rightVec * widthFar); 164 | Vec3 farTopRight = farCenter + (up * heightFar) + (rightVec * widthFar); 165 | Vec3 farBotLeft = farCenter - (up * heightFar) - (rightVec * widthFar); 166 | Vec3 farBotRight = farCenter - (up * heightFar) + (rightVec * widthFar); 167 | 168 | Vec3 nearCenter = pos + forward * nearDist; 169 | Vec3 nearTopLeft = nearCenter + (up * heightNear) - (rightVec * widthNear); 170 | Vec3 nearTopRight = nearCenter + (up * heightNear) + (rightVec * widthNear); 171 | Vec3 nearBotLeft = nearCenter - (up * heightNear) - (rightVec * widthNear); 172 | Vec3 nearBotRight = nearCenter - (up * heightNear) + (rightVec * widthNear); 173 | 174 | near = Plane(nearBotLeft, nearTopLeft, nearBotRight); 175 | far = Plane(farTopRight, farTopLeft, farBotRight); 176 | left = Plane(farTopLeft, nearTopLeft, farBotLeft); 177 | right = Plane(nearTopRight, farTopRight, nearBotRight); 178 | top = Plane(nearTopLeft, farTopLeft, nearTopRight); 179 | bottom = Plane(nearBotRight, farBotLeft, nearBotLeft); 180 | 181 | corners[0] = farTopLeft.cast(); 182 | corners[1] = farTopRight.cast(); 183 | corners[2] = farBotLeft.cast(); 184 | corners[3] = farBotRight.cast(); 185 | corners[4] = nearBotRight.cast(); 186 | corners[5] = nearTopLeft.cast(); 187 | corners[6] = nearTopRight.cast(); 188 | corners[7] = nearBotLeft.cast(); 189 | 190 | // Far face lines. 191 | lines[0] = corners[0]; 192 | lines[1] = corners[1]; 193 | lines[2] = corners[3]; 194 | lines[3] = corners[2]; 195 | lines[4] = corners[1]; 196 | lines[5] = corners[3]; 197 | lines[6] = corners[2]; 198 | lines[7] = corners[0]; 199 | 200 | // Near face lines. 201 | lines[8] = corners[4]; 202 | lines[9] = corners[7]; 203 | lines[10] = corners[6]; 204 | lines[11] = corners[5]; 205 | lines[12] = corners[5]; 206 | lines[13] = corners[7]; 207 | lines[14] = corners[6]; 208 | lines[15] = corners[4]; 209 | 210 | // Connecting lines. 211 | lines[16] = corners[0]; 212 | lines[17] = corners[5]; 213 | lines[18] = corners[1]; 214 | lines[19] = corners[6]; 215 | lines[20] = corners[2]; 216 | lines[21] = corners[7]; 217 | lines[22] = corners[3]; 218 | lines[23] = corners[4]; 219 | } 220 | 221 | 222 | } // namespace chisel 223 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/ChunkManager.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef CHUNKMANAGER_H_ 23 | #define CHUNKMANAGER_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Chunk.h" 36 | 37 | namespace chisel 38 | { 39 | // Spatial hashing function from Matthias Teschner 40 | // Optimized Spatial Hashing for Collision Detection of Deformable Objects 41 | struct ChunkHasher 42 | { 43 | // Three large primes are used for spatial hashing. 44 | static constexpr size_t p1 = 73856093; 45 | static constexpr size_t p2 = 19349663; 46 | static constexpr size_t p3 = 83492791; 47 | 48 | std::size_t operator()(const ChunkID& key) const 49 | { 50 | return ( key(0) * p1 ^ key(1) * p2 ^ key(2) * p3); 51 | } 52 | }; 53 | 54 | 55 | typedef std::unordered_map ChunkMap; 56 | typedef std::unordered_map ChunkSet; 57 | typedef std::unordered_map MeshMap; 58 | typedef std::unordered_map, ChunkHasher> ChunkPointMap; 59 | class Frustum; 60 | class AABB; 61 | class ProjectionIntegrator; 62 | class ChunkManager 63 | { 64 | public: 65 | ChunkManager(); 66 | ChunkManager(const Eigen::Vector3i& chunkSize, float voxelResolution, bool color); 67 | virtual ~ChunkManager(); 68 | 69 | inline const ChunkMap& GetChunks() const { return chunks; } 70 | inline ChunkMap& GetMutableChunks() { return chunks; } 71 | 72 | inline bool HasChunk(const ChunkID& chunk) const 73 | { 74 | return chunks.find(chunk) != chunks.end(); 75 | } 76 | 77 | inline ChunkPtr GetChunk(const ChunkID& chunk) const 78 | { 79 | return chunks.at(chunk); 80 | } 81 | 82 | inline void AddChunk(const ChunkPtr& chunk) 83 | { 84 | chunks.insert(std::make_pair(chunk->GetID(), chunk)); 85 | } 86 | 87 | inline bool RemoveChunk(const ChunkID& chunk) 88 | { 89 | if(HasChunk(chunk)) 90 | { 91 | chunks.erase(chunk); 92 | 93 | if (HasMesh(chunk)) 94 | { 95 | allMeshes.erase(chunk); 96 | } 97 | return true; 98 | } 99 | return false; 100 | } 101 | 102 | inline bool RemoveChunk(const ChunkPtr& chunk) 103 | { 104 | return RemoveChunk(chunk->GetID()); 105 | } 106 | 107 | inline bool HasChunk(int x, int y, int z) const { return HasChunk(ChunkID(x, y, z)); } 108 | inline ChunkPtr GetChunk(int x, int y, int z) const { return GetChunk(ChunkID(x, y, z)); } 109 | 110 | inline ChunkPtr GetChunkAt(const Vec3& pos) 111 | { 112 | ChunkID id = GetIDAt(pos); 113 | 114 | if (HasChunk(id)) 115 | { 116 | return GetChunk(id); 117 | } 118 | 119 | return ChunkPtr(); 120 | } 121 | 122 | inline ChunkPtr GetOrCreateChunkAt(const Vec3& pos, bool* wasNew) 123 | { 124 | ChunkID id = GetIDAt(pos); 125 | 126 | if (HasChunk(id)) 127 | { 128 | *wasNew = false; 129 | return GetChunk(id); 130 | } 131 | else 132 | { 133 | *wasNew = true; 134 | CreateChunk(id); 135 | return GetChunk(id); 136 | } 137 | } 138 | 139 | inline ChunkID GetIDAt(const Vec3& pos) const 140 | { 141 | static const float roundingFactorX = 1.0f / (chunkSize(0) * voxelResolutionMeters); 142 | static const float roundingFactorY = 1.0f / (chunkSize(1) * voxelResolutionMeters); 143 | static const float roundingFactorZ = 1.0f / (chunkSize(2) * voxelResolutionMeters); 144 | 145 | return ChunkID(static_cast(std::floor(pos(0) * roundingFactorX)), 146 | static_cast(std::floor(pos(1) * roundingFactorY)), 147 | static_cast(std::floor(pos(2) * roundingFactorZ))); 148 | } 149 | 150 | inline Vec3 GetCentroid(const Point3& globalVoxelID) 151 | { 152 | return globalVoxelID.cast() * voxelResolutionMeters + halfVoxel; 153 | } 154 | 155 | const DistVoxel* GetDistanceVoxel(const Vec3& pos); 156 | const ColorVoxel* GetColorVoxel(const Vec3& pos); 157 | 158 | void GetChunkIDsIntersecting(const AABB& box, ChunkIDList* chunkList); 159 | void GetChunkIDsIntersecting(const Frustum& frustum, ChunkIDList* chunkList); 160 | void GetChunkIDsIntersecting(const PointCloud& cloud, 161 | const Transform& cameraTransform, 162 | const ProjectionIntegrator& integrator, 163 | float maxDist, 164 | ChunkPointMap* chunkList); 165 | void CreateChunk(const ChunkID& id); 166 | 167 | void GenerateMesh(const ChunkPtr& chunk, Mesh* mesh); 168 | void ColorizeMesh(Mesh* mesh); 169 | Vec3 InterpolateColor(const Vec3& colorPos); 170 | 171 | void CacheCentroids(); 172 | void ExtractBorderVoxelMesh(const ChunkPtr& chunk, const Eigen::Vector3i& index, const Eigen::Vector3f& coordinates, VertIndex* nextMeshIndex, Mesh* mesh); 173 | void ExtractInsideVoxelMesh(const ChunkPtr& chunk, const Eigen::Vector3i& index, const Vec3& coords, VertIndex* nextMeshIndex, Mesh* mesh); 174 | 175 | inline const MeshMap& GetAllMeshes() const { return allMeshes; } 176 | inline MeshMap& GetAllMutableMeshes() { return allMeshes; } 177 | inline const MeshPtr& GetMesh(const ChunkID& chunkID) const { return allMeshes.at(chunkID); } 178 | inline MeshPtr& GetMutableMesh(const ChunkID& chunkID) { return allMeshes.at(chunkID); } 179 | inline bool HasMesh(const ChunkID& chunkID) const { return allMeshes.find(chunkID) != allMeshes.end(); } 180 | 181 | inline bool GetUseColor() { return useColor; } 182 | 183 | void RecomputeMesh(const ChunkID& chunkID, std::mutex& mutex); 184 | void RecomputeMeshes(const ChunkSet& chunks); 185 | void ComputeNormalsFromGradients(Mesh* mesh); 186 | 187 | inline const Eigen::Vector3i& GetChunkSize() const { return chunkSize; } 188 | inline float GetResolution() const { return voxelResolutionMeters; } 189 | 190 | inline const Vec3List& GetCentroids() const { return centroids; } 191 | 192 | void PrintMemoryStatistics(); 193 | 194 | void Reset(); 195 | 196 | bool GetSDFAndGradient(const Eigen::Vector3f& pos, double* dist, Eigen::Vector3f* grad); 197 | bool GetSDF(const Eigen::Vector3f& pos, double* dist); 198 | 199 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 200 | protected: 201 | ChunkMap chunks; 202 | Eigen::Vector3i chunkSize; 203 | float voxelResolutionMeters; 204 | Vec3 halfVoxel; 205 | Vec3List centroids; 206 | Eigen::Matrix cubeIndexOffsets; 207 | MeshMap allMeshes; 208 | bool useColor; 209 | }; 210 | 211 | typedef std::shared_ptr ChunkManagerPtr; 212 | typedef std::shared_ptr ChunkManagerConstPtr; 213 | 214 | 215 | } // namespace chisel 216 | 217 | #endif // CHUNKMANAGER_H_ 218 | -------------------------------------------------------------------------------- /chisel_ros/include/chisel_ros/Conversions.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | 23 | #ifndef CONVERSIONS_H_ 24 | #define CONVERSIONS_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace chisel_ros 42 | { 43 | void PclPointCloudToChisel(const pcl::PointCloud& cloudIn, chisel::PointCloud* cloudOut) 44 | { 45 | assert(!!cloudOut); 46 | cloudOut->GetMutablePoints().resize(cloudIn.points.size()); 47 | 48 | size_t i = 0; 49 | for (const pcl::PointXYZ& pt : cloudIn.points) 50 | { 51 | chisel::Vec3& xyz = cloudOut->GetMutablePoints().at(i); 52 | xyz(0) = pt.x; 53 | xyz(1) = pt.y; 54 | xyz(2) = pt.z; 55 | i++; 56 | } 57 | } 58 | 59 | void SetColors(const pcl::PointCloud& cloudIn, chisel::PointCloud* cloudOut) 60 | { 61 | assert(!!cloudOut); 62 | cloudOut->GetMutableColors().resize(cloudIn.points.size()); 63 | 64 | size_t i = 0; 65 | float byteToFloat = 1.0f / 255.0f; 66 | for (const pcl::PointXYZRGB& pt : cloudIn.points) 67 | { 68 | chisel::Vec3& rgb = cloudOut->GetMutableColors().at(i); 69 | rgb(0) = pt.r * byteToFloat; 70 | rgb(1) = pt.g * byteToFloat; 71 | rgb(2) = pt.b * byteToFloat; 72 | i++; 73 | } 74 | } 75 | 76 | void SetColors(const pcl::PointCloud& cloudIn, chisel::PointCloud* cloudOut) 77 | { 78 | // Empty because of template shenanigans 79 | } 80 | 81 | 82 | template void PclPointCloudToChisel(const pcl::PointCloud& cloudIn, chisel::PointCloud* cloudOut, bool useColor=true) 83 | { 84 | assert(!!cloudOut); 85 | cloudOut->GetMutablePoints().resize(cloudIn.points.size()); 86 | size_t i = 0; 87 | float byteToFloat = 1.0f / 255.0f; 88 | for (const PointType& pt : cloudIn.points) 89 | { 90 | chisel::Vec3& xyz = cloudOut->GetMutablePoints().at(i); 91 | xyz(0) = pt.x; 92 | xyz(1) = pt.y; 93 | xyz(2) = pt.z; 94 | i++; 95 | } 96 | 97 | if (useColor) 98 | { 99 | SetColors(cloudIn, cloudOut); 100 | } 101 | } 102 | 103 | template void ROSPointCloudToChisel(sensor_msgs::PointCloud2ConstPtr cloudIn, chisel::PointCloud* cloudOut, bool useColor=true) 104 | { 105 | assert(!!cloudOut); 106 | pcl::PointCloud pclCloud; 107 | pcl::fromROSMsg(*cloudIn, pclCloud); 108 | 109 | //remove NAN points from the cloud 110 | std::vector indices; 111 | pcl::removeNaNFromPointCloud(pclCloud, pclCloud, indices); 112 | PclPointCloudToChisel(pclCloud, cloudOut, useColor); 113 | } 114 | 115 | 116 | template void ROSImgToDepthImg(sensor_msgs::ImageConstPtr image, chisel::DepthImage* depthImage) 117 | { 118 | ROS_INFO("Got depth image of format %s", image->encoding.c_str()); 119 | bool mmImage = false; 120 | 121 | if (image->encoding == "16UC1") 122 | { 123 | mmImage = true; 124 | } 125 | else if (image->encoding == "32FC1") 126 | { 127 | mmImage = false; 128 | } 129 | else 130 | { 131 | ROS_ERROR("Unrecognized depth image format."); 132 | return; 133 | } 134 | 135 | if (!mmImage) 136 | { 137 | size_t dataSize =image->step / image->width; 138 | assert(depthImage->GetHeight() == static_cast(image->height) && depthImage->GetWidth() == static_cast(image->width)); 139 | assert(dataSize == sizeof(DataType)); 140 | const DataType* imageData = reinterpret_cast(image->data.data()); 141 | DataType* depthImageData = depthImage->GetMutableData(); 142 | int totalPixels = image->width * image->height; 143 | for (int i = 0; i < totalPixels; i++) 144 | { 145 | depthImageData[i] = imageData[i]; 146 | } 147 | } 148 | else 149 | { 150 | assert(depthImage->GetHeight() == static_cast(image->height) && depthImage->GetWidth() == static_cast(image->width)); 151 | const uint16_t* imageData = reinterpret_cast(image->data.data()); 152 | DataType* depthImageData = depthImage->GetMutableData(); 153 | int totalPixels = image->width * image->height; 154 | for (int i = 0; i < totalPixels; i++) 155 | { 156 | depthImageData[i] = (1.0f / 1000.0f) * imageData[i]; 157 | } 158 | } 159 | } 160 | 161 | 162 | template void ROSImgToColorImg(sensor_msgs::ImageConstPtr image, chisel::ColorImage* colorImage) 163 | { 164 | size_t numChannels = colorImage->GetNumChannels(); 165 | size_t dataSize =image->step / image->width; 166 | assert(colorImage->GetHeight() == static_cast(image->height) && colorImage->GetWidth() == static_cast(image->width)); 167 | 168 | if (dataSize != numChannels * sizeof(DataType)) 169 | { 170 | ROS_ERROR("Inconsistent channel width: %lu. Expected %lu\n", dataSize, numChannels * sizeof(DataType)); 171 | return; 172 | } 173 | 174 | const DataType* imageData = reinterpret_cast(image->data.data()); 175 | DataType* colorImageData = colorImage->GetMutableData(); 176 | int totalPixels = image->width * image->height * numChannels; 177 | for (int i = 0; i < totalPixels; i++) 178 | { 179 | colorImageData[i] = imageData[i]; 180 | } 181 | } 182 | 183 | template chisel::ColorImage* ROSImgToColorImg(sensor_msgs::ImageConstPtr image) 184 | { 185 | size_t numChannels = 0; 186 | 187 | if (image->encoding == "mono8") 188 | { 189 | numChannels = 1; 190 | } 191 | else if(image->encoding == "bgr8" || image->encoding == "rgb8") 192 | { 193 | numChannels = 3; 194 | } 195 | else if(image->encoding == "bgra8") 196 | { 197 | numChannels = 4; 198 | } 199 | else 200 | { 201 | ROS_ERROR("Unsupported color image format %s. Supported formats are mono8, rgb8, bgr8, and bgra8\n", image->encoding.c_str()); 202 | } 203 | 204 | chisel::ColorImage* toReturn = new chisel::ColorImage(image->width, image->height, numChannels); 205 | ROSImgToColorImg(image, toReturn); 206 | return toReturn; 207 | } 208 | 209 | 210 | inline chisel::Transform RosTfToChiselTf(const tf::StampedTransform& tf) 211 | { 212 | chisel::Transform transform(chisel::Transform::Identity()); 213 | transform.translation()(0) = tf.getOrigin().x(); 214 | transform.translation()(1) = tf.getOrigin().y(); 215 | transform.translation()(2) = tf.getOrigin().z(); 216 | 217 | 218 | chisel::Quaternion quat(chisel::Quaternion::Identity()); 219 | quat.x() = tf.getRotation().x(); 220 | quat.y() = tf.getRotation().y(); 221 | quat.z() = tf.getRotation().z(); 222 | quat.w() = tf.getRotation().w(); 223 | transform.linear() = quat.toRotationMatrix(); 224 | 225 | return transform.inverse(); 226 | } 227 | 228 | inline chisel::PinholeCamera RosCameraToChiselCamera(const sensor_msgs::CameraInfoConstPtr& camera) 229 | { 230 | chisel::PinholeCamera cameraToReturn; 231 | chisel::Intrinsics intrinsics; 232 | intrinsics.SetFx(camera->P[0]); 233 | intrinsics.SetFy(camera->P[5]); 234 | intrinsics.SetCx(camera->P[2]); 235 | intrinsics.SetCy(camera->P[6]); 236 | cameraToReturn.SetIntrinsics(intrinsics); 237 | cameraToReturn.SetWidth(camera->width); 238 | cameraToReturn.SetHeight(camera->height); 239 | return cameraToReturn; 240 | } 241 | } 242 | 243 | 244 | #endif // CONVERSIONS_H_ 245 | -------------------------------------------------------------------------------- /open_chisel/include/open_chisel/ProjectionIntegrator.h: -------------------------------------------------------------------------------- 1 | // The MIT License (MIT) 2 | // Copyright (c) 2014 Matthew Klingensmith and Ivan Dryanovski 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in all 12 | // copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef PROJECTIONINTEGRATOR_H_ 23 | #define PROJECTIONINTEGRATOR_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | namespace chisel 38 | { 39 | 40 | class ProjectionIntegrator 41 | { 42 | public: 43 | ProjectionIntegrator(); 44 | ProjectionIntegrator(const TruncatorPtr& t, 45 | const WeighterPtr& w, 46 | float carvingDist, 47 | bool enableCarving, 48 | const Vec3List& centroids); 49 | 50 | virtual ~ProjectionIntegrator(); 51 | 52 | bool Integrate(const PointCloud& cloud, const Transform& cameraPose, Chunk* chunk, const std::vector& idx) const; 53 | bool IntegratePointCloud(const PointCloud& cloud, const Transform& cameraPose, Chunk* chunk, const std::vector& idx) const; 54 | bool IntegrateColorPointCloud(const PointCloud& cloud, const Transform& cameraPose, Chunk* chunk, const std::vector& idx) const; 55 | 56 | template bool Integrate(const std::shared_ptr >& depthImage, 57 | const PinholeCamera& camera, 58 | const Transform& cameraPose, Chunk* chunk) const 59 | { 60 | assert(chunk != nullptr); 61 | 62 | Eigen::Vector3i numVoxels = chunk->GetNumVoxels(); 63 | float resolution = chunk->GetVoxelResolutionMeters(); 64 | Vec3 origin = chunk->GetOrigin(); 65 | float diag = 2.0 * sqrt(3.0f) * resolution; 66 | Vec3 voxelCenter; 67 | bool updated = false; 68 | for (size_t i = 0; i < centroids.size(); i++) 69 | { 70 | voxelCenter = centroids[i] + origin; 71 | Vec3 voxelCenterInCamera = cameraPose.linear().transpose() * (voxelCenter - cameraPose.translation()); 72 | Vec3 cameraPos = camera.ProjectPoint(voxelCenterInCamera); 73 | 74 | if (!camera.IsPointOnImage(cameraPos) || voxelCenterInCamera.z() < 0) 75 | continue; 76 | 77 | float voxelDist = voxelCenterInCamera.z(); 78 | float depth = depthImage->DepthAt((int)cameraPos(1), (int)cameraPos(0)); //depthImage->BilinearInterpolateDepth(cameraPos(0), cameraPos(1)); 79 | 80 | if(std::isnan(depth)) 81 | { 82 | continue; 83 | } 84 | 85 | float truncation = truncator->GetTruncationDistance(depth); 86 | float surfaceDist = depth - voxelDist; 87 | 88 | if (fabs(surfaceDist) < truncation + diag) 89 | { 90 | DistVoxel& voxel = chunk->GetDistVoxelMutable(i); 91 | voxel.Integrate(surfaceDist, 1.0f); 92 | updated = true; 93 | } 94 | else if (enableVoxelCarving && surfaceDist > truncation + carvingDist) 95 | { 96 | DistVoxel& voxel = chunk->GetDistVoxelMutable(i); 97 | if (voxel.GetWeight() > 0 && voxel.GetSDF() < 1e-5) 98 | { 99 | voxel.Carve(); 100 | updated = true; 101 | } 102 | } 103 | 104 | 105 | } 106 | return updated; 107 | } 108 | template bool IntegrateColor(const std::shared_ptr >& depthImage, const PinholeCamera& depthCamera, const Transform& depthCameraPose, const std::shared_ptr >& colorImage, const PinholeCamera& colorCamera, const Transform& colorCameraPose, Chunk* chunk) const 109 | { 110 | assert(chunk != nullptr); 111 | 112 | float resolution = chunk->GetVoxelResolutionMeters(); 113 | Vec3 origin = chunk->GetOrigin(); 114 | float resolutionDiagonal = 2.0 * sqrt(3.0f) * resolution; 115 | bool updated = false; 116 | //std::vector indexes; 117 | //indexes.resize(centroids.size()); 118 | //for (size_t i = 0; i < centroids.size(); i++) 119 | //{ 120 | // indexes[i] = i; 121 | //} 122 | 123 | for (size_t i = 0; i < centroids.size(); i++) 124 | //parallel_for(indexes.begin(), indexes.end(), [&](const size_t& i) 125 | { 126 | Color color; 127 | Vec3 voxelCenter = centroids[i] + origin; 128 | Vec3 voxelCenterInCamera = depthCameraPose.linear().transpose() * (voxelCenter - depthCameraPose.translation()); 129 | Vec3 cameraPos = depthCamera.ProjectPoint(voxelCenterInCamera); 130 | 131 | if (!depthCamera.IsPointOnImage(cameraPos) || voxelCenterInCamera.z() < 0) 132 | { 133 | continue; 134 | } 135 | 136 | float voxelDist = voxelCenterInCamera.z(); 137 | float depth = depthImage->DepthAt((int)cameraPos(1), (int)cameraPos(0)); //depthImage->BilinearInterpolateDepth(cameraPos(0), cameraPos(1)); 138 | 139 | if(std::isnan(depth)) 140 | { 141 | continue; 142 | } 143 | 144 | float truncation = truncator->GetTruncationDistance(depth); 145 | float surfaceDist = depth - voxelDist; 146 | 147 | if (std::abs(surfaceDist) < truncation + resolutionDiagonal) 148 | { 149 | Vec3 voxelCenterInColorCamera = colorCameraPose.linear().transpose() * (voxelCenter - colorCameraPose.translation()); 150 | Vec3 colorCameraPos = colorCamera.ProjectPoint(voxelCenterInColorCamera); 151 | if(colorCamera.IsPointOnImage(colorCameraPos)) 152 | { 153 | ColorVoxel& colorVoxel = chunk->GetColorVoxelMutable(i); 154 | 155 | if (colorVoxel.GetWeight() < 5) 156 | { 157 | int r = static_cast(colorCameraPos(1)); 158 | int c = static_cast(colorCameraPos(0)); 159 | colorImage->At(r, c, &color); 160 | colorVoxel.Integrate(color.red, color.green, color.blue, 1); 161 | } 162 | } 163 | 164 | DistVoxel& voxel = chunk->GetDistVoxelMutable(i); 165 | voxel.Integrate(surfaceDist, weighter->GetWeight(surfaceDist, truncation)); 166 | 167 | updated = true; 168 | } 169 | else if (enableVoxelCarving && surfaceDist > truncation + carvingDist) 170 | { 171 | DistVoxel& voxel = chunk->GetDistVoxelMutable(i); 172 | if (voxel.GetWeight() > 0 && voxel.GetSDF() < 1e-5) 173 | { 174 | voxel.Carve(); 175 | updated = true; 176 | } 177 | } 178 | 179 | 180 | } 181 | //); 182 | 183 | return updated; 184 | } 185 | 186 | inline const TruncatorPtr& GetTruncator() const { return truncator; } 187 | inline void SetTruncator(const TruncatorPtr& value) { truncator = value; } 188 | inline const WeighterPtr& GetWeighter() const { return weighter; } 189 | inline void SetWeighter(const WeighterPtr& value) { weighter = value; } 190 | 191 | inline float GetCarvingDist() const { return carvingDist; } 192 | inline bool IsCarvingEnabled() const { return enableVoxelCarving; } 193 | inline void SetCarvingDist(float dist) { carvingDist = dist; } 194 | inline void SetCarvingEnabled(bool enabled) { enableVoxelCarving = enabled; } 195 | 196 | inline void SetCentroids(const Vec3List& c) { centroids = c; } 197 | 198 | protected: 199 | TruncatorPtr truncator; 200 | WeighterPtr weighter; 201 | float carvingDist; 202 | bool enableVoxelCarving; 203 | Vec3List centroids; 204 | }; 205 | 206 | } // namespace chisel 207 | 208 | #endif // PROJECTIONINTEGRATOR_H_ 209 | --------------------------------------------------------------------------------