├── CMakeLists.txt ├── CMakeModules ├── FindDEPTHSENSE.cmake └── FindOpenNI2.cmake ├── LICENSE ├── README.md ├── doxygen.config ├── models └── leftHand │ ├── leftHand.xml │ └── palm.obj ├── src ├── depth_sources │ ├── depth_source.h │ ├── depthsense_depth_source.cpp │ ├── depthsense_depth_source.h │ ├── image_depth_source.h │ ├── openni_depth_source.cpp │ ├── openni_depth_source.h │ └── pango_depth_source.h ├── geometry │ ├── SE3.h │ ├── distance_transforms.cpp │ ├── distance_transforms.cu │ ├── distance_transforms.h │ ├── geometry.cpp │ ├── geometry.h │ ├── grid_2d.h │ ├── grid_3d.h │ ├── plane_fitting.cpp │ ├── plane_fitting.cu │ ├── plane_fitting.h │ ├── sdf.cpp │ └── sdf.h ├── img_proc │ ├── bilateral_filter.cu │ ├── bilateral_filter.h │ ├── img_ops.cpp │ ├── img_ops.cu │ ├── img_ops.h │ ├── organized_point_cloud.cpp │ ├── organized_point_cloud.cu │ ├── organized_point_cloud.h │ ├── resampling.cpp │ ├── resampling.cu │ └── resampling.h ├── mesh │ ├── assimp_mesh_reader.cpp │ ├── assimp_mesh_reader.h │ ├── mesh.cpp │ ├── mesh.h │ ├── mesh_proc.cpp │ ├── mesh_proc.h │ ├── mesh_sample.cpp │ ├── mesh_sample.h │ ├── mesh_splat.cpp │ ├── mesh_splat.h │ ├── primitive_meshing.cpp │ └── primitive_meshing.h ├── model │ ├── host_only_model.cpp │ ├── host_only_model.h │ ├── mirrored_model.cpp │ ├── mirrored_model.h │ ├── model.cpp │ └── model.h ├── optimization │ ├── contact_prior.cpp │ ├── kernels │ │ ├── intersection.cu │ │ ├── intersection.h │ │ ├── kernel_common.h │ │ ├── modToObs.cu │ │ ├── modToObs.h │ │ ├── obsToMod.cu │ │ ├── obsToMod.h │ │ ├── raycast.cu │ │ └── raycast.h │ ├── optimization.h │ ├── optimizer.cpp │ ├── optimizer.h │ ├── point_2d_3d_prior.cpp │ ├── point_3d_3d_prior.cpp │ ├── prediction_renderer.cpp │ ├── prediction_renderer.h │ └── priors.h ├── point_cloud_src │ └── point_cloud_src.h ├── pose │ ├── pose.cpp │ ├── pose.h │ └── pose_reduction.h ├── tracker.cpp ├── tracker.h ├── util │ ├── cuda_utils.h │ ├── dart_io.cpp │ ├── dart_io.h │ ├── dart_types.h │ ├── gl_dart.cpp │ ├── gl_dart.h │ ├── image_io.cpp │ ├── image_io.h │ ├── mirrored_memory.h │ ├── model_renderer.cpp │ ├── model_renderer.h │ ├── ostream_operators.cpp │ ├── ostream_operators.h │ ├── prefix.h │ ├── string_format.cpp │ ├── string_format.h │ └── vector_type_template.h └── visualization │ ├── color_ramps.cpp │ ├── color_ramps.cu │ ├── color_ramps.h │ ├── data_association_viz.cpp │ ├── data_association_viz.cu │ ├── data_association_viz.h │ ├── gradient_viz.cpp │ ├── gradient_viz.cu │ ├── gradient_viz.h │ ├── matrix_viz.cu │ ├── matrix_viz.h │ ├── sdf_viz.cpp │ ├── sdf_viz.cu │ └── sdf_viz.h └── test ├── test_main.cpp ├── test_mirrored_model.cpp ├── test_model_jacobian.cpp ├── test_model_jacobian.cu ├── test_obs_to_mod_kernels.cpp ├── test_pose_reduction.cpp └── test_voxelize.cpp /CMakeModules/FindDEPTHSENSE.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Find DEPTHSENSE 3 | # 4 | # DEPTHSENSE_FOUND - True if DepthSense was found 5 | # DEPTHSENSE_INCLUDE_DIR - Directories containing the DepthSense include files 6 | # DEPTHSENSE_LIBRARIES - Librarires needed to use DepthSense 7 | # 8 | ################################################################################ 9 | 10 | IF (WIN32) 11 | ELSE () 12 | FIND_PATH(DEPTHSENSE_INCLUDE_DIR DepthSense.hxx 13 | /opt/DepthSenseSDK/include 14 | /opt/softkinetic/DepthSenseSDK/include 15 | ${PROJECT_SOURCE_DIR}/external_lib/include 16 | DOC "The directory where DepthSense.hxx resides" 17 | ) 18 | FIND_LIBRARY(DEPTHSENSE_LIBRARIES 19 | NAMES DepthSense 20 | PATHS 21 | /opt/DepthSenseSDK/lib/libDepthSense.so 22 | /opt/softkinetic/DepthSenseSDK/lib/ 23 | ${PROJECT_SOURCE_DIR}/external_lib/lib 24 | DOC "The DepthSense library" 25 | ) 26 | ENDIF() 27 | 28 | IF(DEPTHSENSE_INCLUDE_DIR AND DEPTHSENSE_LIBRARIES) 29 | SET(DEPTHSENSE_FOUND ON) 30 | message(STATUS "DepthSense found: ${DEPTHSENSE_INCLUDE_DIR}") 31 | ELSE() 32 | SET(DEPTHSENSE_FOUND OFF) 33 | ENDIF() 34 | 35 | MARK_AS_ADVANCED(DEPTHSENSE_FOUND) 36 | -------------------------------------------------------------------------------- /CMakeModules/FindOpenNI2.cmake: -------------------------------------------------------------------------------- 1 | find_package(PkgConfig QUIET) 2 | 3 | # Find LibUSB 4 | if(NOT WIN32) 5 | pkg_check_modules(PC_USB_10 libusb-1.0) 6 | find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h 7 | HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" 8 | PATH_SUFFIXES libusb-1.0) 9 | 10 | find_library(USB_10_LIBRARY 11 | NAMES usb-1.0 12 | HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" 13 | PATH_SUFFIXES lib) 14 | 15 | include(FindPackageHandleStandardArgs) 16 | find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) 17 | 18 | if(NOT USB_10_FOUND) 19 | message(STATUS "OpenNI 2 disabled because libusb-1.0 not found.") 20 | return() 21 | else() 22 | include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) 23 | endif() 24 | endif(NOT WIN32) 25 | 26 | if(${CMAKE_VERSION} VERSION_LESS 2.8.2) 27 | pkg_check_modules(PC_OPENNI2 libopenni2) 28 | else() 29 | pkg_check_modules(PC_OPENNI2 QUIET libopenni2) 30 | endif() 31 | 32 | set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) 33 | 34 | set(OPENNI2_SUFFIX) 35 | if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) 36 | set(OPENNI2_SUFFIX 64) 37 | endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) 38 | 39 | find_path(OPENNI2_INCLUDE_DIRS OpenNI.h 40 | PATHS 41 | "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Win64 needs '64' suffix 42 | /usr/include/openni2 # common path for deb packages 43 | ) 44 | 45 | find_library(OPENNI2_LIBRARY 46 | NAMES OpenNI2 # No suffix needed on Win64 47 | libOpenNI2 # Linux 48 | PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" # Windows default path, Win64 needs '64' suffix 49 | "$ENV{OPENNI2_REDIST}" # Linux install does not use a separate 'lib' directory 50 | ) 51 | 52 | if(CMAKE_SYSTEM_NAME STREQUAL "Darwin") 53 | set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} ${LIBUSB_1_LIBRARIES}) 54 | else() 55 | set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) 56 | endif() 57 | 58 | include(FindPackageHandleStandardArgs) 59 | find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) 60 | 61 | mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) 62 | 63 | if(OPENNI2_FOUND) 64 | # Add the include directories 65 | set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) 66 | set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) 67 | message(STATUS "OpenNI 2 found (include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARY}, redist: ${OPENNI2_REDIST_DIR})") 68 | endif(OPENNI2_FOUND) 69 | #find_path(OpenNI2_INCLUDE_DIR OpenNI.h 70 | # /usr/local/OpenNI-2.2/Include 71 | # /usr/local/OpenNI-2.1/Include 72 | 73 | #) 74 | #find_path(OpenNI2_LIBRARY_PATH libOpenNI2.so 75 | # /usr/local/OpenNI-2.2/Redist 76 | # /usr/local/OpenNI-2.1/Redist 77 | #) 78 | #if(OpenNI2_INCLUDE_DIR) 79 | # set(OpenNI2_FOUND ON) 80 | # set(OpenNI2_LIBRARIES libOpenNI2.so libOniFile.so libPS1080.so) 81 | # set(OpenNI2_LIBRARY_COPY_PATH ${OpenNI2_LIBRARY_PATH}/OpenNI2) 82 | # set(OpenNI2_DRIVER_LIBRARY_PATH ${OpenNI2_LIBRARY_PATH}/OpenNI2/Drivers) 83 | #endif() 84 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2017, Tanner Schmidt 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | dart: Dense Articulated Real-time Tracking 2 | ======= 3 | 4 | dart is a C++ library for tracking arbitrary articulated models with an RGB-D 5 | camera. It achieves real-time performance with the aid of a highly parallel CUDA 6 | implementation and state-of-the-art GPUs. 7 | 8 | Required Dependencies 9 | --------------------- 10 | 11 | **CUDA:** https://developer.nvidia.com/cuda-zone 12 | 13 | **Eigen 3:** sudo apt-get install libeigen3-dev 14 | 15 | **GNU libmatheval:** sudo apt-get install libmatheval-dev 16 | 17 | **libjpeg:** sudo apt-get install libjpeg-dev 18 | 19 | **libpng:** sudo apt-get install libpng-dev 20 | 21 | **tinyxml:** sudo apt-get install libtinyxml-dev 22 | 23 | Optional Dependencies 24 | --------------------- 25 | 26 | **Pangolin [GUI for the examples]:** https://github.com/stevenlovegrove/Pangolin 27 | 28 | **OpenNI [for PrimeSense sensors]:** https://github.com/OpenNI/OpenNI2 or http://structure.io/openni 29 | 30 | **DepthSense SDK [for Intel sensor]:** sudo apt-get install depthsensesdk 31 | 32 | **Open Asset Import Library:** sudo apt-get install libassimp-dev [for mesh models] 33 | 34 | **gtest [for testing]:** sudo apt-get install libgtest-dev; cd /usr/src/gtest; sudo mkdir build; cd build; sudo cmake ..; sudo make; sudo mv libgtest* /usr/lib/; 35 | 36 | Installation 37 | ------------ 38 | 39 | cd [dart directory] 40 | mkdir build 41 | cd build 42 | cmake .. 43 | make 44 | 45 | Example usage 46 | ------------ 47 | 48 | An example demonstrating the use of DART to track robot hands manipulating objects, including the depth and color video, can be downloaded from [here](https://drive.google.com/file/d/1HXvOsUcgS6THn17hME-SKQU3FQVUJr_z/view?usp=sharing). 49 | 50 | Notes on using the library 51 | ------------ 52 | 53 | - OpenGL context: dart is a very visual library and therefore assumes that it 54 | will be used in collaboration with a GUI tool. If an instance of dart::Tracker 55 | is instantiated with no active OpenGL context, it will not work. If you are not 56 | using the library with a GUI, you will have to create an OpenGL context (e.g. by 57 | using glutCreateWindow to create a 1x1 window). 58 | 59 | - Frames vs. Joints vs. SDFs: These are three separate but related ways in which 60 | parts of a DART model can be referenced. A frame is a frame of reference in the 61 | kinematic chain, a joint is a connection between two frames with a single degree 62 | of freedom, and a signed distance function (SDF) implicitly stores all geometry 63 | attached to a single frame. Every model has at least one frame (the root) but 64 | need not have any joints or signed distance functions. Because loops are not 65 | allowed, a model with N joints will have N+1 frames (and N+6 degrees of 66 | freedom). The number of SDFs is at most equal to the number of frames, but may 67 | be less if there are frames with no geometry attached to them. Functions in the 68 | Model class and subclasses that require indexing part of the model will indicate 69 | in the parameter name whether the index is by joint, by frame, or by SDF. 70 | 71 | Model file format 72 | ------------ 73 | 74 | DART models are stored as XML files which define the kinematic and geometric 75 | structure, optionally reference other mesh files to further describe the 76 | geometry. All models open with the "model" tag which has a single attribute, 77 | "version" describing the version of the DART XML format (current 1), like so: 78 | 79 | 80 | [model here] 81 | 82 | 83 | The model can then optionally specify a number of parameters using the "param" 84 | tag, with attributes "name" (string) and "value" (floating point), like so: 85 | 86 | 87 | 88 | These parameters can then be referenced when defining sizes, positions, or 89 | orientations, as described below. Parameters are useful if the same value 90 | appears multiple times in your specification (as is often the case) or if you 91 | would like to set the parameter values programatically. 92 | 93 | After defining parameters, the model may contain a number of hierachically 94 | nested "frame" and "geom" tags, which specify new rigid body frames of reference 95 | or geometric objects, respectively. The "frame" tag requires four attributes, 96 | "jointName" (string), "jointType" (currently accepts either "rotational" or 97 | "prismatic", "jointMin" (floating point), and "jointMax" (floating point), the 98 | last two of which define the joint limits. Additionally, the frame tag requires 99 | three nested tags, "position", "orientation", and "axis", each of which require 100 | three floating point attributes, "x", "y", and "z". An example might look like 101 | this: 102 | 103 | 104 | 105 | 106 | 107 | [frame children here] 108 | 109 | 110 | This snippet defines a new frame of reference relative to its parent (the XML 111 | node directly above it in the hierarchy, or the root if the parent is the 112 | "model" tag). The transform from this frame of reference to the world is given 113 | by: 114 | 115 | T_w,f = T_w,p*Trans*R_z*R_y*R_x*R_axis(theta) 116 | 117 | where T_w,p gives the transform from the parent to the world, Trans is a 118 | translation-only transform given by the "position" tag, R_z, R_y, and R_x are 119 | rotations about the z, y, and x axes (i.e. Euler angles) given by the 120 | corresponding entries in the "orientation" tag, and R_axis is a rotation by 121 | theta around the axis defined by the "axis" tag, with theta being given by the 122 | articulated pose of the model. 123 | 124 | Finally, geometry can be rigidly attached to any frame of reference in the model 125 | by nesting a "geom" tag within a "frame" tag (or within the "model" tag for root 126 | geometry). The geometry tag requires 13 attributes: "type" (currently accepts 127 | "sphere","cylinder","cube", or "mesh"), "sx", "sy", and "sz", which define the 128 | scaling of the geometry, "tx", "ty", and "tz", which define the translation of 129 | the geometry root relative to the rigid body frame of reference, "rx", "ry" and 130 | "rz", which define the orientation relative to the rigid body frame of reference 131 | (also in Euler angles, as with the "frame" tag), and "red", "green" and "blue", 132 | which define the geometry color, which is not used for tracking but will affect 133 | how the model is rendered for debugging purposes. If "type" is set to "mesh", 134 | there is one final attribute, "meshFile", which gives the location of the mesh 135 | file, relative to the location of the XML file. 136 | 137 | -------------------------------------------------------------------------------- /src/depth_sources/depth_source.h: -------------------------------------------------------------------------------- 1 | #ifndef DEPTH_SOURCE_H 2 | #define DEPTH_SOURCE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dart { 9 | 10 | enum ColorLayout { 11 | LAYOUT_RGB, LAYOUT_BGR 12 | }; 13 | 14 | class DepthSourceBase { 15 | 16 | public: 17 | DepthSourceBase() : 18 | _hasColor(false), 19 | _hasTimestamps(false), 20 | _isLive(false), 21 | _depthWidth(0), 22 | _depthHeight(0), 23 | _colorWidth(0), 24 | _colorHeight(0), 25 | _frame(0) { } 26 | 27 | virtual ColorLayout getColorLayout() const { return LAYOUT_RGB; } 28 | 29 | inline bool hasColor() const { return _hasColor; } 30 | 31 | inline bool hasTimestamps() const { return _hasTimestamps; } 32 | 33 | inline bool isLive() const { return _isLive; } 34 | 35 | inline uint getDepthWidth() const { return _depthWidth; } 36 | 37 | inline uint getDepthHeight() const { return _depthHeight; } 38 | 39 | inline uint getColorWidth() const { return _colorWidth; } 40 | 41 | inline uint getColorHeight() const { return _colorHeight; } 42 | 43 | inline float2 getFocalLength() const { return _focalLength; } 44 | inline void setFocalLength(const float2 focalLength) { _focalLength = focalLength; } 45 | 46 | inline float2 getPrincipalPoint() const { return _principalPoint; } 47 | inline void setPrincipalPoint(const float2 principalPoint) { _principalPoint = principalPoint; } 48 | 49 | virtual uint64_t getDepthTime() const { return 0; } 50 | 51 | virtual uint64_t getColorTime() const { return 0; } 52 | 53 | virtual uint getFrame() const { return _frame; } 54 | 55 | virtual void setFrame(const uint frame) = 0; 56 | 57 | virtual void advance() = 0; 58 | 59 | virtual bool hasRadialDistortionParams() const = 0; 60 | 61 | virtual const float * getRadialDistortionParams() const { return 0; } 62 | 63 | virtual float getScaleToMeters() const { return 1.0f; } 64 | 65 | protected: 66 | bool _hasColor; 67 | bool _hasTimestamps; 68 | bool _isLive; 69 | uint _depthWidth; 70 | uint _depthHeight; 71 | uint _colorWidth; 72 | uint _colorHeight; 73 | uint _frame; 74 | float2 _focalLength; 75 | float2 _principalPoint; 76 | 77 | }; 78 | 79 | template 80 | class DepthSource : public DepthSourceBase { 81 | public: 82 | 83 | virtual const DepthType * getDepth() const = 0; 84 | virtual const DepthType * getDeviceDepth() const = 0; 85 | 86 | virtual const ColorType * getColor() const { return 0; } 87 | }; 88 | 89 | } 90 | 91 | #endif // DEPTH_SOURCE_H 92 | -------------------------------------------------------------------------------- /src/depth_sources/depthsense_depth_source.cpp: -------------------------------------------------------------------------------- 1 | #include "depthsense_depth_source.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace dart { 10 | 11 | void * startContext(void * wrapper) { 12 | DepthSense::Context * context = (DepthSense::Context *)wrapper; 13 | context->run(); 14 | pthread_exit(NULL); 15 | } 16 | 17 | DepthSenseDepthSource::DepthSenseDepthSource() : DepthSource() { 18 | 19 | _depthData = 0; 20 | _colorData = 0; 21 | _depthTime = _colorTime = 0; 22 | 23 | _nextDepthData = 0; 24 | _nextColorData = 0; 25 | _nextDepthTime = _nextColorTime = 0; 26 | 27 | } 28 | 29 | DepthSenseDepthSource::~DepthSenseDepthSource() { 30 | 31 | if (_context.isSet()) { 32 | _context.stopNodes(); 33 | _context.releaseControl(_device); 34 | _context.unregisterNode(_depthNode); 35 | if (_colorNode.isSet()) { 36 | _context.unregisterNode(_colorNode); 37 | } 38 | } 39 | 40 | delete [] _depthData; 41 | cudaFree(_deviceDepthData); 42 | delete [] _colorData; 43 | 44 | delete [] _nextDepthData; 45 | delete [] _nextColorData; 46 | 47 | } 48 | 49 | bool DepthSenseDepthSource::initialize(const bool getColor, 50 | const bool enableDenoising, 51 | const uint confidenceThreshold) { 52 | 53 | _context = DepthSense::Context::createStandalone(); 54 | 55 | std::vector devices = _context.getDevices(); 56 | 57 | if (devices.size() == 0) { 58 | std::cerr << "could not find any DepthSense devices" << std::endl; 59 | return false; 60 | } 61 | 62 | _device = devices[0]; 63 | 64 | std::vector nodes = _device.getNodes(); 65 | 66 | for (int i=0; i()) { 68 | _depthNode = (DepthSense::DepthNode)nodes[i]; 69 | } 70 | } 71 | 72 | if (!_depthNode.isSet()) { 73 | std::cerr << "could not find depth node" << std::endl; 74 | return false; 75 | } 76 | 77 | if (getColor) { 78 | for (int i=0; i()) { 80 | _colorNode = (DepthSense::ColorNode)nodes[i]; 81 | } 82 | } 83 | 84 | if (!_colorNode.isSet()) { 85 | std::cerr << "could not find color node" << std::endl; 86 | return false; 87 | } 88 | 89 | _hasColor = true; 90 | } 91 | 92 | _context.requestControl(_device); 93 | 94 | _context.registerNode(_depthNode); 95 | 96 | _depthNode.setEnableDepthMap(true); 97 | _depthNode.setEnableDenoising(enableDenoising); 98 | _depthNode.setConfidenceThreshold(confidenceThreshold); 99 | 100 | if (getColor) { 101 | _context.registerNode(_colorNode); 102 | _colorNode.setEnableColorMap(true); 103 | DepthSense::ColorNode::Configuration config = _colorNode.getConfiguration(); 104 | // config.frameFormat = DepthSense::FRAME_FORMAT_QVGA; 105 | config.compression = DepthSense::COMPRESSION_TYPE_MJPEG; 106 | // config.framerate = 30; 107 | _colorNode.setConfiguration(config); 108 | } 109 | 110 | DepthSense::StereoCameraParameters parameters = _device.getStereoCameraParameters(); 111 | 112 | _depthWidth = parameters.depthIntrinsics.width; 113 | _depthHeight = parameters.depthIntrinsics.height; 114 | 115 | _focalLength = make_float2(228.0,228.0); 116 | _principalPoint = make_float2(_depthWidth/2,_depthHeight/2); 117 | 118 | _depthData = new ushort[_depthWidth*_depthHeight]; 119 | memset(_depthData,0,_depthWidth*_depthHeight*sizeof(ushort)); 120 | cudaMalloc(&_deviceDepthData,_depthWidth*_depthHeight*sizeof(ushort)); 121 | _nextDepthData = new ushort[_depthWidth*_depthHeight]; 122 | 123 | if (getColor) { 124 | _colorWidth = parameters.colorIntrinsics.width; 125 | _colorHeight = parameters.colorIntrinsics.height; 126 | 127 | _colorData = new uchar3[_colorWidth*_colorHeight]; 128 | memset(_colorData,0,_colorWidth*_colorHeight*sizeof(uchar3)); 129 | _nextColorData = new uchar3[_colorWidth*_colorHeight]; 130 | } 131 | 132 | _hasTimestamps = true; 133 | _isLive = true; 134 | 135 | _depthTime = 0; 136 | _colorTime = 0; 137 | 138 | pthread_mutex_init(&_depthMutex,NULL); 139 | pthread_mutex_init(&_colorMutex,NULL); 140 | pthread_t contextThread; 141 | pthread_create(&contextThread, NULL, startContext, (void*)&_context); 142 | 143 | _depthNode.newSampleReceivedEvent().connect(this, &DepthSenseDepthSource::updateDepth); 144 | if (getColor) { 145 | _colorNode.newSampleReceivedEvent().connect(this, &DepthSenseDepthSource::updateColor); 146 | } 147 | 148 | _context.startNodes(); 149 | 150 | advance(); 151 | 152 | return true; 153 | } 154 | 155 | void DepthSenseDepthSource::advance() { 156 | 157 | // swap depth buffers 158 | pthread_mutex_lock(&_depthMutex); 159 | if (_nextDepthTime > _depthTime) { 160 | ushort * tmp = _depthData; 161 | _depthData = _nextDepthData; 162 | _nextDepthData = tmp; 163 | _depthTime = _nextDepthTime; 164 | _frame++; 165 | } 166 | pthread_mutex_unlock(&_depthMutex); 167 | 168 | // swap color buffers 169 | pthread_mutex_lock(&_colorMutex); 170 | if (_nextColorTime > _colorTime) { 171 | uchar3 * tmp = _colorData; 172 | _colorData = _nextColorData; 173 | _nextColorData = tmp; 174 | _colorTime = _nextColorTime; 175 | } 176 | pthread_mutex_unlock(&_colorMutex); 177 | 178 | cudaMemcpy(_deviceDepthData,_depthData,_depthWidth*_depthHeight*sizeof(ushort),cudaMemcpyHostToDevice); 179 | 180 | } 181 | 182 | void DepthSenseDepthSource::updateDepth(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data) { 183 | 184 | pthread_mutex_lock(&_depthMutex); 185 | _nextDepthTime = data.timeOfCapture; 186 | memcpy(_nextDepthData,data.depthMap,_depthWidth*_depthHeight*sizeof(ushort)); 187 | pthread_mutex_unlock(&_depthMutex); 188 | 189 | } 190 | 191 | void DepthSenseDepthSource::updateColor(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data) { 192 | 193 | pthread_mutex_lock(&_colorMutex); 194 | _nextColorTime = data.timeOfCapture; 195 | memcpy(_nextColorData,data.colorMap,_colorWidth*_colorHeight*sizeof(uchar3)); 196 | pthread_mutex_unlock(&_colorMutex); 197 | 198 | } 199 | 200 | } 201 | -------------------------------------------------------------------------------- /src/depth_sources/depthsense_depth_source.h: -------------------------------------------------------------------------------- 1 | #ifndef DEPTHSENSE_DEPTH_SOURCE_H 2 | #define DEPTHSENSE_DEPTH_SOURCE_H 3 | 4 | #include "depth_source.h" 5 | 6 | #include "DepthSense.hxx" 7 | 8 | #include 9 | 10 | namespace dart { 11 | 12 | class DepthSenseDepthSource : public DepthSource { 13 | public: 14 | DepthSenseDepthSource(); 15 | ~DepthSenseDepthSource(); 16 | 17 | bool initialize(const bool getColor=true, 18 | const bool enableDenoising=true, 19 | const uint confidenceThreshold=250); 20 | 21 | const ushort * getDepth() const { return _depthData; } 22 | 23 | const ushort * getDeviceDepth() const { return _deviceDepthData; } 24 | 25 | const uchar3 * getColor() const { return _colorData; } 26 | 27 | ColorLayout getColorLayout() const { return LAYOUT_BGR; } 28 | 29 | uint64_t getDepthTime() const { return _depthTime; } 30 | 31 | uint64_t getColorTime() const { return _colorTime; } 32 | 33 | void setFrame(const uint frame) { } 34 | 35 | void advance(); 36 | 37 | bool hasRadialDistortionParams() const { return false; } 38 | 39 | float getScaleToMeters() const { return 1.e-3; } 40 | 41 | private: 42 | void updateDepth(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data); 43 | void updateColor(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data); 44 | 45 | DepthSense::Context _context; 46 | DepthSense::Device _device; 47 | DepthSense::DepthNode _depthNode; 48 | DepthSense::ColorNode _colorNode; 49 | uint64_t _colorTime; 50 | uint64_t _depthTime; 51 | pthread_mutex_t _depthMutex; 52 | pthread_mutex_t _colorMutex; 53 | ushort * _depthData; 54 | ushort * _deviceDepthData; 55 | uchar3 * _colorData; 56 | 57 | ushort * _nextDepthData; 58 | uint64_t _nextDepthTime; 59 | uchar3 * _nextColorData; 60 | uint64_t _nextColorTime; 61 | }; 62 | 63 | } 64 | 65 | #endif // DEPTHSENSE_DEPTH_SOURCE_H 66 | -------------------------------------------------------------------------------- /src/depth_sources/openni_depth_source.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENNI_DEPTH_SOURCE_H 2 | #define OPENNI_DEPTH_SOURCE_H 3 | 4 | #include "depth_source.h" 5 | #include 6 | #include 7 | 8 | namespace dart { 9 | 10 | class OpenNIDepthSource : public DepthSource { 11 | public: 12 | OpenNIDepthSource(); 13 | 14 | ~OpenNIDepthSource(); 15 | 16 | bool initialize(const char * deviceURI = openni::ANY_DEVICE, 17 | const bool getColor = true, 18 | const uint depthWidth = 640, 19 | const uint depthHeight = 480, 20 | const uint depthFPS = 30, 21 | const uint colorWidth = 640, 22 | const uint colorHeight = 480, 23 | const uint colorFPS = 30, 24 | const bool mirror = false, 25 | const bool frameSync = true, 26 | const bool registerDepth = true); 27 | 28 | const ushort * getDepth() const; 29 | 30 | const ushort * getDeviceDepth() const; 31 | 32 | const uchar3 * getColor() const; 33 | 34 | ColorLayout getColorLayout() const { return LAYOUT_RGB; } 35 | 36 | uint64_t getDepthTime() const; 37 | 38 | uint64_t getColorTime() const; 39 | 40 | void setFrame(const uint frame); 41 | 42 | void advance(); 43 | 44 | bool hasRadialDistortionParams() const { return false; } 45 | 46 | inline float getScaleToMeters() const { return 1/(1000.0f); } 47 | 48 | private: 49 | openni::Device _device; 50 | openni::VideoStream _depthStream; 51 | openni::VideoStream _colorStream; 52 | openni::VideoFrameRef _depthFrame; 53 | openni::VideoFrameRef _colorFrame; 54 | int _frameIndexOffset; 55 | ushort * _deviceDepth; 56 | }; 57 | 58 | } 59 | 60 | #endif // OPENNI_DEPTH_SOURCE_H 61 | -------------------------------------------------------------------------------- /src/depth_sources/pango_depth_source.h: -------------------------------------------------------------------------------- 1 | #ifndef PVN_DEPTH_SOURCE_H 2 | #define PVN_DEPTH_SOURCE_H 3 | 4 | #include "depth_source.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "util/string_format.h" 17 | 18 | #include "util/image_io.h" 19 | #include "util/mirrored_memory.h" 20 | #include "vector_types.h" 21 | 22 | namespace dart{ 23 | 24 | template 25 | class PangoDepthSource : public DepthSource { 26 | public: 27 | PangoDepthSource(); 28 | 29 | ~PangoDepthSource(); 30 | 31 | bool initialize(const std::string & pangoFilename, 32 | const float2 focalLength, 33 | const float2 principalPoint = make_float2(0,0), 34 | const uint depthWidth = 0, 35 | const uint depthHeight = 0, 36 | const float scaleToMeters = 1.f); 37 | 38 | #ifdef CUDA_BUILD 39 | const DepthType * getDepth() const { return _depthData->hostPtr(); } 40 | const DepthType * getDeviceDepth() const { return _depthData->devicePtr(); } 41 | #else 42 | const DepthType * getDepth() const { return _depthData; } 43 | const DepthType * getDeviceDepth() const { return 0; } 44 | #endif // CUDA_BUILD 45 | 46 | //const ColorType * getColor() const { return _colorData; } 47 | 48 | //ColorLayout getColorLayout() const { return LAYOUT_RGB; } 49 | 50 | uint64_t getDepthTime() const { return _depthTimes[this->_frame]; } 51 | 52 | //uint64_t getColorTime() const { return _colorTimes[this->_frame]; } 53 | 54 | void setFrame(const uint frame); 55 | 56 | void advance(); 57 | 58 | bool hasRadialDistortionParams() const { return false; } 59 | 60 | float getScaleToMeters() const { return _scaleToMeters; } 61 | 62 | private: 63 | 64 | void readDepth(); 65 | //void readColor(); 66 | 67 | #ifdef CUDA_BUILD 68 | MirroredVector * _depthData; 69 | #else 70 | DepthType * _depthData; 71 | #endif // CUDA_BUILD 72 | uint _firstDepthFrame; 73 | uint _lastDepthFrame; 74 | std::string _pangoFilename; 75 | float _scaleToMeters; 76 | std::vector _depthTimes; 77 | 78 | pangolin::VideoRecordRepeat _video; 79 | }; 80 | 81 | // Implementation 82 | template 83 | PangoDepthSource::PangoDepthSource() : 84 | DepthSource(), 85 | _firstDepthFrame(0), 86 | _depthData(0) {} 87 | 88 | template 89 | PangoDepthSource::~PangoDepthSource() { 90 | #ifdef CUDA_BUILD 91 | delete _depthData; 92 | #else 93 | delete [] _depthData; 94 | #endif // CUDA_BUILD 95 | } 96 | 97 | template 98 | bool PangoDepthSource::initialize( 99 | const std::string & pangoFilename, 100 | const float2 focalLength, 101 | const float2 principalPoint, 102 | const uint depthWidth, 103 | const uint depthHeight, 104 | const float scaleToMeters){ 105 | 106 | this->_frame = 0; 107 | _pangoFilename = pangoFilename; 108 | this->_focalLength = focalLength; 109 | _scaleToMeters = scaleToMeters; 110 | 111 | struct stat buffer; 112 | bool exists = (stat(pangoFilename.c_str(), &buffer) == 0); 113 | if(!exists){ 114 | std::cerr << "pangolin file not found" << std::endl; 115 | return false; 116 | } 117 | 118 | std::string uri = std::string("file://") + _pangoFilename; 119 | _video.Open(uri); 120 | 121 | // set depth dimensions 122 | if(depthWidth > 0 && depthHeight > 0) { 123 | this->_depthWidth = depthWidth; 124 | this->_depthHeight = depthHeight; 125 | } 126 | else{ 127 | this->_depthWidth = _video.Width(); 128 | this->_depthHeight = _video.Height(); 129 | } 130 | 131 | // set principal point 132 | if(principalPoint.x == 0) { 133 | this->_principalPoint = make_float2( 134 | this->_depthWidth/2, this->_depthHeight/2); 135 | } else { 136 | this->_principalPoint = principalPoint; 137 | } 138 | 139 | // allocate data 140 | #ifdef CUDA_BUILD 141 | _depthData = new MirroredVector( 142 | this->_depthWidth*this->_depthHeight); 143 | #else 144 | _depthData = new DepthType[this->_depthWidth*this->_depthHeight]; 145 | #endif // CUDA_BUILD 146 | 147 | return true; 148 | } 149 | 150 | template 151 | void PangoDepthSource::setFrame(const uint frame) { 152 | //this->_frame = frame; 153 | pangolin::VideoPlaybackInterface * playback = 154 | _video.Cast(); 155 | playback->Seek(frame); 156 | 157 | readDepth(); 158 | #ifdef CUDA_BUILD 159 | _depthData->syncHostToDevice(); 160 | #endif // CUDA_BUILD 161 | 162 | this->_frame = playback->GetCurrentFrameId(); 163 | } 164 | 165 | template 166 | void PangoDepthSource::advance() { 167 | pangolin::VideoPlaybackInterface * playback = 168 | _video.Cast(); 169 | if(this->_frame == playback->GetTotalFrames()){ 170 | //if(this->_frame == 90){ 171 | playback->Seek(0); 172 | } 173 | 174 | readDepth(); 175 | #ifdef CUDA_BUILD 176 | _depthData->syncHostToDevice(); 177 | #endif // CUDA_BUILD 178 | 179 | this->_frame = playback->GetCurrentFrameId(); 180 | } 181 | 182 | template 183 | void PangoDepthSource::readDepth() { 184 | std::vector > tmp(1); 185 | _video.Grab((unsigned char*) _depthData->hostPtr(), tmp); 186 | } 187 | 188 | } 189 | 190 | #endif 191 | -------------------------------------------------------------------------------- /src/geometry/geometry.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_H 2 | #define GEOMETRY_H 3 | 4 | //#include 5 | #include 6 | #include "util/vector_type_template.h" 7 | 8 | namespace dart { 9 | 10 | //---------------------------------------------------------------------------- 11 | // Distance to ellipsoid functions 12 | //---------------------------------------------------------------------------- 13 | 14 | //---------------------------------------------------------------------------- 15 | // The ellipse is (x0/e0)^2 + (x1/e1)^2 = 1 with e0 >= e1. The query point is 16 | // (y0,y1) with y0 >= 0 and y1 >= 0. The function returns the distance from 17 | // the query point to the ellipse. It also computes the ellipse point (x0,x1) 18 | // in the first quadrant that is closest to (y0,y1). 19 | //---------------------------------------------------------------------------- 20 | template 21 | Real distancePointEllipseSpecial(const Real e[2], const Real y[2], Real x[2]); 22 | 23 | //---------------------------------------------------------------------------- 24 | // The ellipse is (x0/e0)^2 + (x1/e1)^2 = 1. The query point is (y0,y1). 25 | // The function returns the distance from the query point to the ellipse. 26 | // It also computes the ellipse point (x0,x1) that is closest to (y0,y1). 27 | //---------------------------------------------------------------------------- 28 | template 29 | Real distancePointEllipse(const Real e[2], const Real y[2], Real x[2]); 30 | 31 | //---------------------------------------------------------------------------- 32 | // The ellipsoid is (x0/e0)^2 + (x1/e1)^2 + (x2/e2)^2 = 1 with e0 >= e1 >= e2. 33 | // The query point is (y0,y1,y2) with y0 >= 0, y1 >= 0, and y2 >= 0. The 34 | // function returns the distance from the query point to the ellipsoid. It 35 | // also computes the ellipsoid point (x0,x1,x2) in the first octant that is 36 | // closest to (y0,y1,y2). 37 | //---------------------------------------------------------------------------- 38 | template 39 | Real distancePointEllipsoidSpecial(const Real e[3], const Real y[3], Real x[3]); 40 | 41 | //---------------------------------------------------------------------------- 42 | // The ellipsoid is (x0/e0)^2 + (x1/e1)^2 + (x2/e2)^2 = 1. The query point is 43 | // (y0,y1,y2). The function returns the distance from the query point to the 44 | // ellipsoid. It also computes the ellipsoid point (x0,x1,x2) that is 45 | // closest to (y0,y1,y2). 46 | //---------------------------------------------------------------------------- 47 | template 48 | Real distancePointEllipsoid(const Real e[3], const Real y[3], Real x[3]); 49 | 50 | 51 | float distancePointTriangle(const float3 p, const float3 A, const float3 B, const float3 C); 52 | 53 | float distancePointTriangle(const float3 p, const float3 A, const float3 B, const float3 C, float & s, float & t); 54 | 55 | float distancePointTriangle(const float3 p, const float3 A, const float3 B, const float3 C, float3 & D); 56 | 57 | template 58 | Real distancePointLineSegment2D(const typename VectorTypeTemplate::type2 p, 59 | const typename VectorTypeTemplate::type2 a, 60 | const typename VectorTypeTemplate::type2 b ); 61 | 62 | template 63 | Real signedDistancePointLineSegment2D(const typename VectorTypeTemplate::type2 p, 64 | const typename VectorTypeTemplate::type2 a, 65 | const typename VectorTypeTemplate::type2 b ); 66 | 67 | 68 | //---------------------------------------------------------------------------- 69 | // Rodrigues (axis-angle) matrix functions 70 | //---------------------------------------------------------------------------- 71 | //template 72 | //inline void rotationMatrixFromRodrigues(const Real w[3], Eigen::MatrixBase const &R); 73 | 74 | //template 75 | //inline void rodriguesFromRotationMatrix(Real w[3], Eigen::MatrixBase const &R); 76 | 77 | //---------------------------------------------------------------------------- 78 | // This calculates the derivative of a rotation matrix derived from the given 79 | // set of Rodrigues parameters, relative to those three parameters. The 80 | // computed Jacobian is stacked such that the top 3x3 matrix is the derivative 81 | // of the rotation w.r.t. wx, the next 3x3 matrix is the derivative w.r.t. wy, 82 | // and the bottom 3x3 is the derivative w.r.t. wz. 83 | //---------------------------------------------------------------------------- 84 | //template 85 | //void rotationMatrixJacobianFromRodrigues(const Real w[3], Eigen::Matrix &J); 86 | 87 | //---------------------------------------------------------------------------- 88 | // Axis-aligned bounding box (AABB) functions 89 | //---------------------------------------------------------------------------- 90 | 91 | //---------------------------------------------------------------------------- 92 | // This function calculates the axis aligned bounding box of an ellipse 93 | // specified by ((x0-c0)/e0)^2 + ((x1-c1)/e1)^2 + ((x2-c2)/e2)^2 <= 1, which 94 | // has also undergone a rotation specified by Rodrigues parameters (wx,wy,wz). 95 | // The bounding box has origin (o0,o1,o2) and size (s0,s1,s2). 96 | //---------------------------------------------------------------------------- 97 | template 98 | void aabbEllipsoid(const Real e[3], const Real c[3], const Real w[3], Real o[3], Real s[3]); 99 | 100 | //---------------------------------------------------------------------------- 101 | // This function calculates the axis aligned bounding box of an elliptic 102 | // cylinder with end caps specified by ((x0-c0)/e0)^2 + ((x1-c1)/e1)^2 <= 1. 103 | // The end caps are centered at (c0,c1,0) and (c0,c1,h), and the cylinder has 104 | // also undergone a rotation specified by Rodrigues parameters (wx,wy,wx). It 105 | // is assumed the rotation is about the point (c0,c1,0), i.e. the center of 106 | // the lower end cap. 107 | //---------------------------------------------------------------------------- 108 | template 109 | void aabbEllipticCylinder(const Real e[2], const Real h, const Real c[3], const Real w[3], Real o[3], Real s[3]); 110 | 111 | template 112 | void aabbRectangularPrism(const Real l[3], const Real c[3], const Real w[3], Real o[3], Real s[3]); 113 | 114 | //---------------------------------------------------------------------------- 115 | // Geometry-generating functions 116 | //---------------------------------------------------------------------------- 117 | void generateUnitIcosphere(float3 * & verts, int3 * & indxs, int & nverts, int & nfaces, const int splits); 118 | 119 | } 120 | 121 | #endif // GEOMETRY_H 122 | -------------------------------------------------------------------------------- /src/geometry/grid_2d.h: -------------------------------------------------------------------------------- 1 | #ifndef GRID_2D_H 2 | #define GRID_2D_H 3 | 4 | #include 5 | #include 6 | #include "util/vector_operators.h" 7 | #include "util/vector_type_template.h" 8 | #include "util/prefix.h" 9 | 10 | namespace dart { 11 | 12 | template 13 | class Grid2D { 14 | 15 | private: 16 | typedef typename VectorTypeTemplate::type2 T2; 17 | 18 | public: 19 | 20 | // ===== constructors / destructor ===== 21 | PREFIX Grid2D() : dim(make_uint2(0,0)), offset(make_float2(0,0)), resolution(0), data(0) {} 22 | PREFIX Grid2D(uint2 dim) : dim(dim), offset(make_float2(0,0)), resolution(0), data(new T[dim.x*dim.y]) {} 23 | PREFIX Grid2D(uint2 dim, float2 offset, float resolution) : dim(dim), offset(offset), resolution(resolution), data(new T[dim.x*dim.y]) {} 24 | PREFIX Grid2D(const Grid2D & grid) : dim(grid.dim), offset(grid.offset), resolution(grid.resolution), data(new T[grid.dim.x*grid.dim.y]) { 25 | memcpy(data, grid.data, dim.x*dim.y*sizeof(T)); 26 | } 27 | PREFIX ~Grid2D() { delete [] data; } 28 | 29 | // ===== inline member functions ===== 30 | inline PREFIX Grid2D & operator= (const Grid2D & grid) { 31 | if (this == &grid) { 32 | return *this; 33 | } 34 | delete [] data; 35 | dim = grid.dim; 36 | offset = grid.offset; 37 | resolution = grid.resolution; 38 | data = new T[dim.x*dim.y]; 39 | memcpy(grid.data,data,dim.x*dim.y*sizeof(T)); 40 | return *this; 41 | } 42 | 43 | inline PREFIX float2 getGridCoords(const float2 & pWorld) const { 44 | return (pWorld - offset)/resolution; 45 | } 46 | 47 | inline PREFIX float2 getWorldCoords(const float2 & pGrid) const { 48 | return resolution*pGrid + offset; 49 | } 50 | 51 | inline PREFIX bool isInBounds(const float2 & pGrid) const { 52 | return (pGrid.x > 0 && pGrid.x < dim.x && 53 | pGrid.y > 0 && pGrid.y < dim.y); 54 | } 55 | 56 | inline PREFIX bool isInBoundsInterp(const float2 & pGrid) const { 57 | return (pGrid.x > 0.50001 && pGrid.x < dim.x - 0.50001 && 58 | pGrid.y > 0.50001 && pGrid.y < dim.y - 0.50001); 59 | } 60 | 61 | inline PREFIX bool isInBoundsGradient(const float2 & pGrid) const { 62 | return (pGrid.x > 1.50001 && pGrid.x < dim.x - 1.50001 && 63 | pGrid.y > 1.50001 && pGrid.y < dim.y - 1.50001); 64 | } 65 | 66 | // TODO: figure out what these values should really be 67 | inline PREFIX bool isInBoundsGradientInterp(const float2 & pGrid) const { 68 | return (pGrid.x > 2.50001 && pGrid.x < dim.x - 2.50001 && 69 | pGrid.y > 2.50001 && pGrid.y < dim.y - 2.50001); 70 | } 71 | 72 | inline PREFIX T & getValue(const int2 & v) const { 73 | return data[v.x + dim.x*v.y]; 74 | } 75 | 76 | inline PREFIX T getValueInterpolated(const float2 & pGrid) const { 77 | 78 | const int x0 = (int)(pGrid.x-0.5); const float fx = (pGrid.x-0.5) - x0; 79 | const int y0 = (int)(pGrid.y-0.5); const float fy = (pGrid.y-0.5) - y0; 80 | 81 | const int x1 = x0 + 1; 82 | const int y1 = y0 + 1; 83 | 84 | if ( !(x0 >= 0 && x1 < dim.x && y0 >= 0 && y1 < dim.y) ) { 85 | printf("nope (%d)\n",isInBoundsInterp(pGrid)); 86 | } 87 | 88 | const float dx0 = lerp( getValue(make_int2(x0,y0)), getValue(make_int2(x1,y0)), fx); 89 | const float dx1 = lerp( getValue(make_int2(x0,y1)), getValue(make_int2(x1,y1)), fx); 90 | const float dxy = lerp( dx0, dx1, fy ); 91 | 92 | return dxy; 93 | 94 | } 95 | 96 | inline PREFIX T2 getGradient(const int2 & v) const { 97 | 98 | T2 grad; 99 | 100 | if (v.x == 0) { 101 | grad.x = data[v.x+1 + v.y*dim.x] - data[v.x + v.y*dim.x]; 102 | } else if (v.x == dim.x - 1) { 103 | grad.x = data[v.x + v.y*dim.x] - data[v.x-1 + v.y*dim.x]; 104 | } else { 105 | grad.x = 0.5*(data[v.x+1 + v.y*dim.x] - data[v.x-1 + v.y*dim.x]); 106 | } 107 | 108 | if (v.y == 0) { 109 | grad.y = data[v.x + (v.y+1)*dim.x] - data[v.x + v.y*dim.x]; 110 | } else if (v.y == dim.y - 1) { 111 | grad.y = data[v.x + v.y*dim.x] - data[v.x + (v.y-1)*dim.x]; 112 | } else { 113 | grad.y = 0.5*(data[v.x + (v.y+1)*dim.x] - data[v.x + (v.y-1)*dim.x]); 114 | } 115 | 116 | return grad; 117 | 118 | } 119 | 120 | inline PREFIX T2 getGradientInterpolated(const float2 & pGrid) const { 121 | 122 | T f_px = getValueInterpolated(pGrid + make_float2(1,0)); 123 | T f_py = getValueInterpolated(pGrid + make_float2(0,1)); 124 | 125 | T f_mx = getValueInterpolated(pGrid - make_float2(1,0)); 126 | T f_my = getValueInterpolated(pGrid - make_float2(0,1)); 127 | 128 | T2 grad; 129 | grad.x = 0.5*(f_px - f_mx); 130 | grad.y = 0.5*(f_py - f_my); 131 | return grad; 132 | 133 | } 134 | 135 | // ===== data members ===== 136 | uint2 dim; 137 | float2 offset; 138 | float resolution; 139 | T * data; 140 | 141 | }; 142 | 143 | } // namespace dart 144 | 145 | #endif // GRID_2D_H 146 | -------------------------------------------------------------------------------- /src/geometry/plane_fitting.cpp: -------------------------------------------------------------------------------- 1 | #include "plane_fitting.h" 2 | 3 | #include 4 | #include "util/mirrored_memory.h" 5 | 6 | namespace dart { 7 | 8 | void fitPlane(float3 & planeNormal, 9 | float & planeIntercept, 10 | const float4 * dObsVertMap, 11 | const float4 * dObsNormMap, 12 | const int width, 13 | const int height, 14 | const float distanceThreshold, 15 | const float normalThreshold, 16 | const int maxIters, 17 | const float regularization, 18 | int * dbgAssociated) { 19 | 20 | dart::MirroredVector result(4 + 16 + 1); 21 | 22 | 23 | for (int iter=0; iter 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dart { 9 | 10 | // -=-=-=-=- kernel -=-=-=-=- 11 | template 12 | __global__ void gpu_fitPlane(const float3 planeNormal, 13 | const float planeIntercept, 14 | const float4 * obsVertMap, 15 | const float4 * obsNormMap, 16 | const int width, 17 | const int height, 18 | const float distanceThreshold, 19 | const float normalThreshold, 20 | float * result, 21 | int * dbgAssociated) { 22 | 23 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 24 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 25 | 26 | if (x >= width || y >= height) { 27 | return; 28 | } 29 | 30 | const int index = x + y*width; 31 | if (dbgAssoc) { dbgAssociated[index] = 0; } 32 | 33 | if (obsVertMap[index].w == 0 || obsNormMap[index].w == 0) { 34 | return; 35 | } 36 | 37 | float3 v = make_float3(obsVertMap[index]); 38 | 39 | float dist = dot(planeNormal,v) - planeIntercept; 40 | if (fabs(dist) > distanceThreshold) { 41 | return; 42 | } 43 | 44 | float3 n = make_float3(obsNormMap[index]); 45 | if (dot(planeNormal,n) < normalThreshold) { 46 | return; 47 | } 48 | 49 | if (dbgAssoc) { dbgAssociated[index] = 1; } 50 | 51 | float J[4]; 52 | J[0] = v.x; 53 | J[1] = v.y; 54 | J[2] = v.z; 55 | J[3] = -1; 56 | 57 | float * eJ = result; 58 | float * JTJ = &result[4]; 59 | float * e = &result[4 + 16]; 60 | 61 | for (int i=0; i<4; ++i) { 62 | float ejVal = dist*J[i]; 63 | atomicAdd(&eJ[i],ejVal); 64 | for (int j=0; j<4; ++j) { 65 | float jtjVal = J[i]*J[j]; 66 | atomicAdd(&JTJ[i*4 + j],jtjVal); 67 | } 68 | } 69 | 70 | atomicAdd(e,dist*dist); 71 | } 72 | 73 | __global__ void gpu_subtractPlane(float4 * obsVertMap, 74 | float4 * obsNormMap, 75 | const int width, 76 | const int height, 77 | const float3 planeNormal, 78 | const float planeIntercept, 79 | const float distanceThreshold, 80 | const float normalThreshold) { 81 | 82 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 83 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 84 | 85 | if (x >= width || y >= height) { 86 | return; 87 | } 88 | 89 | const int index = x + y*width; 90 | 91 | float3 v = make_float3(obsVertMap[index]); 92 | 93 | float dist = dot(planeNormal,v) - planeIntercept; 94 | //if (fabs(dist) > distanceThreshold) { 95 | if (dist > distanceThreshold) { 96 | return; 97 | } 98 | 99 | if (obsNormMap[index].w == 1.0) { 100 | float3 n = make_float3(obsNormMap[index]); 101 | if (dot(planeNormal,n) < normalThreshold) { 102 | return; 103 | } 104 | } 105 | 106 | obsVertMap[index].w = -1.0f; 107 | obsNormMap[index].w = -1.0f; 108 | 109 | } 110 | 111 | // -=-=-=-=- interface -=-=-=-=- 112 | void fitPlaneIter(const float3 planeNormal, 113 | const float planeIntercept, 114 | const float4 * dObsVertMap, 115 | const float4 * dObsNormMap, 116 | const int width, 117 | const int height, 118 | const float distanceThreshold, 119 | const float normalThreshold, 120 | const float regularization, 121 | float * dResult, 122 | int * dbgAssociated) { 123 | 124 | dim3 block(16,8,1); 125 | dim3 grid( ceil( width / (float)block.x), ceil(height / (float)block.y )); 126 | 127 | if (dbgAssociated == 0) { 128 | gpu_fitPlane<<>>(planeNormal, 129 | planeIntercept, 130 | dObsVertMap, 131 | dObsNormMap, 132 | width, 133 | height, 134 | distanceThreshold, 135 | normalThreshold, 136 | dResult, 137 | dbgAssociated); 138 | } else { 139 | gpu_fitPlane<<>>(planeNormal, 140 | planeIntercept, 141 | dObsVertMap, 142 | dObsNormMap, 143 | width, 144 | height, 145 | distanceThreshold, 146 | normalThreshold, 147 | dResult, 148 | dbgAssociated); 149 | } 150 | 151 | 152 | } 153 | 154 | void subtractPlane_(float4 * dObsVertMap, 155 | float4 * dObsNormMap, 156 | const int width, 157 | const int height, 158 | const float3 planeNormal, 159 | const float planeIntercept, 160 | const float distanceThreshold, 161 | const float normalThreshold) { 162 | 163 | dim3 block(16,8,1); 164 | dim3 grid( ceil( width / (float)block.x), ceil(height / (float)block.y )); 165 | 166 | gpu_subtractPlane<<>>(dObsVertMap, 167 | dObsNormMap, 168 | width, 169 | height, 170 | planeNormal, 171 | planeIntercept, 172 | distanceThreshold, 173 | normalThreshold); 174 | 175 | } 176 | 177 | } 178 | -------------------------------------------------------------------------------- /src/geometry/plane_fitting.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANE_FITTING_H 2 | #define PLANE_FITTING_H 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | void fitPlane(float3 & planeNormal, 9 | float & planeIntercept, 10 | const float4 * dObsVertMap, 11 | const float4 * dObsNormMap, 12 | const int width, 13 | const int height, 14 | const float distanceThreshold, 15 | const float normalThreshold, 16 | const int maxIters, 17 | const float regularization, 18 | int * dbgAssociated = 0); 19 | 20 | void fitPlaneIter(const float3 planeNormal, 21 | const float planeIntercept, 22 | const float4 * dObsVertMap, 23 | const float4 * dObsNormMap, 24 | const int width, 25 | const int height, 26 | const float distanceThreshold, 27 | const float normalThreshold, 28 | const float regularization, 29 | float * dResult, 30 | int * dbgAssocaited); 31 | 32 | void subtractPlane_(float4 * dObsVertMap, 33 | float4 * dObsNormMap, 34 | const int width, 35 | const int height, 36 | const float3 planeNormal, 37 | const float planeIntercept, 38 | const float distanceThreshold, 39 | const float normalThreshold); 40 | 41 | } 42 | 43 | #endif // PLANE_FITTING_H 44 | -------------------------------------------------------------------------------- /src/geometry/sdf.h: -------------------------------------------------------------------------------- 1 | #ifndef SDF_H 2 | #define SDF_H 3 | 4 | #include "geometry/SE3.h" 5 | #include "grid_3d.h" 6 | #include "mesh/mesh.h" 7 | 8 | namespace dart { 9 | 10 | void projectToSdfSurface(const Grid3D & sdf, float3 & pointGrid, const float threshold = 1e-5, const int maxIters = 100); 11 | 12 | void analyticMeshSdf(Grid3D & sdf, const Mesh & mesh); 13 | 14 | void analyticBoxSdf(Grid3D & sdf, SE3 T_bg, const float3 boxMin, const float3 boxMax); 15 | 16 | void analyticSphereSdf(Grid3D & sdf, SE3 T_sg, const float sphereRadius); 17 | 18 | } 19 | 20 | #endif // SDF_H 21 | -------------------------------------------------------------------------------- /src/img_proc/bilateral_filter.cu: -------------------------------------------------------------------------------- 1 | #include "bilateral_filter.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace dart { 7 | 8 | template 9 | __global__ void gpu_bilateralFilter(const T * depthIn, 10 | float * depthOut, 11 | const int width, 12 | const int height, 13 | const float domainFactor, 14 | const float rangeFactor) { 15 | 16 | const int twidth = 16; 17 | const int theight = 16; 18 | const int swidth = twidth+8; 19 | const int sheight = theight+8; 20 | const int nreads = (swidth*sheight)/(twidth*theight); 21 | 22 | __shared__ T sdata[swidth*sheight]; 23 | 24 | const int tid = threadIdx.y*blockDim.x + threadIdx.x; 25 | 26 | for (int i=0; i<=nreads; i++) { 27 | const int readi = tid + i*twidth*theight; 28 | if (readi < swidth*sheight) { 29 | const int row = readi / swidth; 30 | const int col = readi % swidth; 31 | const int x = blockIdx.x*blockDim.x - 4 + col; 32 | const int y = blockIdx.y*blockDim.y - 4 + row; 33 | if (x >= 0 && x < width && y >=0 && y < height) { 34 | sdata[readi] = depthIn[x + y*width]; 35 | } 36 | } 37 | } 38 | 39 | __syncthreads(); 40 | 41 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 42 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 43 | const int index = x + y*width; 44 | 45 | const int bx = threadIdx.x + 4; 46 | const int by = threadIdx.y + 4; 47 | 48 | float d = sdata[bx + by*swidth]; 49 | 50 | if (d > 0) { 51 | 52 | float new_d = 0; 53 | float total_weight = 0; 54 | 55 | int min_dx = max(-4,-x); 56 | int max_dx = min(4,width-x-1); 57 | int min_dy = max(-4,-y); 58 | int max_dy = min(4,height-y-1); 59 | 60 | for (int dy = min_dy; dy <= max_dy; dy++) { 61 | for (int dx = min_dx; dx <= max_dx; dx++) { 62 | 63 | float dd = sdata[bx + dx + (by+dy)*swidth]; 64 | 65 | if ( dd > 0 ) { 66 | 67 | float rangeDist2 = (d-dd)*(d-dd); 68 | float domainDist2 = dx*dx + dy*dy; 69 | 70 | float weight = expf( -domainDist2*domainFactor - rangeDist2*rangeFactor); 71 | 72 | new_d += (weight*dd); 73 | total_weight += weight; 74 | 75 | } 76 | 77 | } 78 | } 79 | 80 | depthOut[index] = (new_d / total_weight); 81 | 82 | } else { 83 | depthOut[index] = 0; 84 | } 85 | 86 | } 87 | 88 | template 89 | void bilateralFilter(const T * depthIn, float * depthOut, const int width, const int height, const float sigmaDomain, const float sigmaRange) { 90 | 91 | dim3 block(16,16,1); 92 | dim3 grid( (width) / block.x, (height) / block.y); 93 | 94 | gpu_bilateralFilter<<>>(depthIn, depthOut, width, height, 1.0/(2*sigmaDomain*sigmaDomain), 1.0/(2*sigmaRange*sigmaRange)); 95 | 96 | cudaDeviceSynchronize(); 97 | 98 | cudaError_t err = cudaGetLastError(); 99 | if (err != cudaSuccess) { 100 | std::cerr << "Error: " << cudaGetErrorString(err) << std::endl; 101 | } 102 | 103 | } 104 | 105 | template void bilateralFilter(const float * depthIn, float * depthOut, const int width, const int height, const float sigmaDomain, const float sigmaRange); 106 | template void bilateralFilter(const double * depthIn, float * depthOut, const int width, const int height, const float sigmaDomain, const float sigmaRange); 107 | template void bilateralFilter(const ushort * depthIn, float * depthOut, const int width, const int height, const float sigmaDomain, const float sigmaRange); 108 | 109 | } 110 | -------------------------------------------------------------------------------- /src/img_proc/bilateral_filter.h: -------------------------------------------------------------------------------- 1 | #ifndef BILATERAL_FILTER_H 2 | #define BILATERAL_FILTER_H 3 | 4 | namespace dart { 5 | 6 | template 7 | void bilateralFilter(const T * depthIn, float * depthOut, const int width, const int height, const float sigmaDomain, const float sigmaRange); 8 | 9 | } 10 | 11 | #endif // BILATERAL_FILTER_H 12 | -------------------------------------------------------------------------------- /src/img_proc/img_ops.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/img_proc/img_ops.cpp -------------------------------------------------------------------------------- /src/img_proc/img_ops.cu: -------------------------------------------------------------------------------- 1 | #include "img_ops.h" 2 | 3 | namespace dart { 4 | 5 | // -=-=-=-=-=-=-=-=-=- kernels -=-=-=-=-=-=-=-=-=- 6 | __global__ void gpu_imageSquare(float * out, const float * in, const int width, const int height) { 7 | 8 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 9 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 10 | 11 | if (x >= width || y >= height) { 12 | return; 13 | } 14 | 15 | int index = x + y*width; 16 | out[index] = in[index]*in[index]; 17 | 18 | } 19 | 20 | __global__ void gpu_imageSqrt(float * out, const float * in, const int width, const int height) { 21 | 22 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 23 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 24 | 25 | if (x >= width || y >= height) { 26 | return; 27 | } 28 | 29 | int index = x + y*width; 30 | out[index] = sqrtf(in[index]); 31 | 32 | } 33 | 34 | template 35 | __global__ void gpu_imageFlipX(T * out, const T * in, const int width, const int height) { 36 | 37 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 38 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 39 | 40 | if (x >= width || y >= height) { 41 | return; 42 | } 43 | 44 | out[x + y*width] = in[width - x + y*width]; 45 | 46 | } 47 | 48 | template 49 | __global__ void gpu_imageFlipXInPlace(T * img, const int width, const int height) { 50 | 51 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 52 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 53 | 54 | if (x >= width/2 || y >= height) { 55 | return; 56 | } 57 | 58 | T tmp = img[x + y*width]; 59 | img[x + y*width] = img[width - x + y*width]; 60 | img[width - x + y*width] = tmp; 61 | 62 | } 63 | 64 | template 65 | __global__ void gpu_imageFlipY(T * out, const T * in, const int width, const int height) { 66 | 67 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 68 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 69 | 70 | if (x >= width || y >= height) { 71 | return; 72 | } 73 | 74 | out[x + y*width] = in[x + (height-y)*width]; 75 | 76 | } 77 | 78 | template 79 | __global__ void gpu_imageFlipYInPlace(T * img, const int width, const int height) { 80 | 81 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 82 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 83 | 84 | if (x >= width || y >= height/2) { 85 | return; 86 | } 87 | 88 | T tmp = img[x + y*width]; 89 | img[x + y*width] = img[x + (height-y)*width]; 90 | img[x + (height-y)*width] = tmp; 91 | 92 | } 93 | 94 | template 95 | __global__ void gpu_unitNormalize(const T * in, T * out, const int width, const int height, const T zeroVal, const T range) { 96 | 97 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 98 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 99 | 100 | const int index = x + y*width; 101 | out[index] = fmaxf(fminf((in[index] - zeroVal)/range,(T)1),(T)0); 102 | 103 | } 104 | 105 | // -=-=-=-=-=-=-=-=-=- interface -=-=-=-=-=-=-=-=-=- 106 | void imageSquare(float * out, const float * in, const int width, const int height) { 107 | 108 | dim3 block(16,8,1); 109 | dim3 grid( ceil( width / (float)block.x), ceil( height / (float)block.y )); 110 | 111 | gpu_imageSquare<<>>(out,in,width,height); 112 | } 113 | 114 | void imageSqrt(float * out, const float * in, const int width, const int height) { 115 | 116 | dim3 block(16,8,1); 117 | dim3 grid( ceil( width / (float)block.x), ceil( height / (float)block.y )); 118 | 119 | gpu_imageSqrt<<>>(out,in,width,height); 120 | 121 | } 122 | 123 | template 124 | void imageFlipX(T * out, const T * in, const int width, const int height) { 125 | 126 | dim3 block(16,8,1); 127 | 128 | if (out == in) { 129 | dim3 grid( ceil( width/2 / (float)block.x), ceil( height / (float)block.y )); 130 | gpu_imageFlipXInPlace<<>>(out,width,height); 131 | } 132 | else { 133 | dim3 grid( ceil( width / (float)block.x), ceil( height / (float)block.y )); 134 | gpu_imageFlipX<<>>(out,in,width,height); 135 | } 136 | 137 | } 138 | 139 | template 140 | void imageFlipY(T * out, const T * in, const int width, const int height) { 141 | 142 | dim3 block(16,8,1); 143 | 144 | if (out == in) { 145 | dim3 grid( ceil( width / (float)block.x), ceil( height/2 / (float)block.y )); 146 | gpu_imageFlipYInPlace<<>>(out,width,height); 147 | } 148 | else { 149 | dim3 grid( ceil( width / (float)block.x), ceil( height / (float)block.y )); 150 | gpu_imageFlipY<<>>(out,in,width,height); 151 | } 152 | 153 | } 154 | 155 | template 156 | void unitNormalize(const T * in, T * out, const int width, const int height, const T zeroVal, const T oneVal) { 157 | 158 | dim3 block(16,16,1); 159 | dim3 grid( ceil((float)width / block.x), ceil((float)height / block.y)); 160 | 161 | T range = oneVal - zeroVal; 162 | 163 | gpu_unitNormalize<<>>(in,out,width,height,zeroVal,range); 164 | 165 | cudaDeviceSynchronize(); 166 | 167 | } 168 | 169 | 170 | #define COMPILE_IMAGE_OPS(type) \ 171 | template void imageFlipX(type * out, const type * in, const int width, const int height); \ 172 | template void imageFlipY(type * out, const type * in, const int width, const int height); \ 173 | template void unitNormalize(const type * in, type * out, const int width, const int height, const type zeroVal, const type oneVal); 174 | 175 | COMPILE_IMAGE_OPS(float) 176 | COMPILE_IMAGE_OPS(ushort) 177 | //COMPILE_IMAGE_OPS(uchar3) 178 | 179 | } 180 | -------------------------------------------------------------------------------- /src/img_proc/img_ops.h: -------------------------------------------------------------------------------- 1 | #ifndef IMG_MATH_H 2 | #define IMG_MATH_H 3 | 4 | namespace dart { 5 | 6 | void imageSquare(float * out, const float * in, const int width, const int height); 7 | 8 | void imageSqrt(float * out, const float * in, const int width, const int height); 9 | 10 | template 11 | void imageFlipX(T * out, const T * in, const int width, const int height); 12 | 13 | template 14 | void imageFlipY(T * out, const T * in, const int width, const int height); 15 | 16 | template 17 | void unitNormalize(const T * in, T * out, const int width, const int height, const T zeroVal, const T oneVal); 18 | 19 | 20 | } 21 | 22 | #endif // IMG_MATH_H 23 | -------------------------------------------------------------------------------- /src/img_proc/organized_point_cloud.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/img_proc/organized_point_cloud.cpp -------------------------------------------------------------------------------- /src/img_proc/organized_point_cloud.h: -------------------------------------------------------------------------------- 1 | #ifndef ORGANIZED_POINT_CLOUD_H 2 | #define ORGANIZED_POINT_CLOUD_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace dart { 9 | 10 | template 11 | void depthToVertices(const DepthType * depthIn, float4 * vertOut, const int width, const int height, const float2 pp, const float2 fl, const float2 range = make_float2(0,std::numeric_limits::infinity())); 12 | 13 | template 14 | void depthToVertices(const DepthType * depthIn, float4 * vertOut, const int width, const int height, const float2 pp, const float2 fl, const float2 range, const float scale); 15 | 16 | template 17 | void depthToVertices(const DepthType * depthIn, float4 * vertOut, const int width, const int height, const float * calibrationParams, const float2 range); 18 | 19 | template 20 | void depthToVertices(const DepthType * depthIn, float4 * vertOut, const int width, const int height, const float * calibrationParams, const float2 range, const float scale); 21 | 22 | void verticesToNormals(const float4 * vertIn, float4 * normOut, const int width, const int height); 23 | 24 | void eliminatePlane(float4 * verts, const float4 * norms, const int width, const int height, const float3 planeNormal, const float planeD, const float epsDist = 0.01, const float epsNorm = 0.1); 25 | 26 | void cropBox(float4 * verts, const int width, const int height, const float3 & boxMin, const float3 & boxMax); 27 | 28 | void maskPointCloud(float4 * verts, const int width, const int height, const int * mask); 29 | 30 | } 31 | 32 | #endif // ORGANIZED_POINT_CLOUD_H 33 | -------------------------------------------------------------------------------- /src/img_proc/resampling.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/img_proc/resampling.cpp -------------------------------------------------------------------------------- /src/img_proc/resampling.h: -------------------------------------------------------------------------------- 1 | #ifndef RESAMPLING_H 2 | #define RESAMPLING_H 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | void downsampleAreaAverage(const float * imgIn, const uint2 dimIn, float * imgOut, const int factor); 9 | 10 | void downsampleAreaAverage(const uchar3 * imgIn, const uint2 dimIn, uchar3 * imgOut, const int factor); 11 | 12 | void downsampleAreaAverage(const uchar4 * imgIn, const uint2 dimIn, uchar4 * imgOut, const int factor); 13 | 14 | void downsampleNearest(const float * imgIn, const uint2 dimIn, float * imgOut, const int factor); 15 | 16 | void downsampleNearest(const uchar3 * imgIn, const uint2 dimIn, uchar3 * imgOut, const int factor); 17 | 18 | void downsampleMin(const float * imgIn, const uint2 dimIn, float * imgOut, const int factor, bool ignoreZero = true); 19 | 20 | } 21 | 22 | #endif // RESAMPLING_H 23 | 24 | -------------------------------------------------------------------------------- /src/mesh/assimp_mesh_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "assimp_mesh_reader.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace dart { 10 | 11 | dart::Mesh * AssimpMeshReader::readMesh(const std::string filename) const { 12 | 13 | const struct aiScene * scene = aiImportFile(filename.c_str(),0); //aiProcess_JoinIdenticalVertices); 14 | if (scene == 0) { 15 | std::cerr << "error: " << aiGetErrorString() << std::endl; 16 | return 0; 17 | } 18 | 19 | if (scene->mNumMeshes != 1) { 20 | std::cerr << "there are " << scene->mNumMeshes << " meshes in " << filename << std::endl; 21 | aiReleaseImport(scene); 22 | return 0; 23 | } 24 | 25 | aiMesh * aiMesh = scene->mMeshes[0]; 26 | 27 | dart::Mesh * mesh = new dart::Mesh(aiMesh->mNumVertices,aiMesh->mNumFaces); 28 | 29 | for (int f=0; fnFaces; ++f) { 30 | aiFace& face = aiMesh->mFaces[f]; 31 | if (face.mNumIndices != 3) { 32 | std::cerr << filename << " is not a triangle mesh" << std::endl; 33 | delete mesh; 34 | aiReleaseImport(scene); 35 | return 0; 36 | } 37 | mesh->faces[f] = make_int3(face.mIndices[0],face.mIndices[1],face.mIndices[2]); 38 | } 39 | for (int v=0; vnVertices; ++v) { 40 | aiVector3D & vertex = aiMesh->mVertices[v]; 41 | mesh->vertices[v] = make_float3(vertex.x,vertex.y,vertex.z); 42 | aiVector3D & normal = aiMesh->mNormals[v]; 43 | mesh->normals[v] = make_float3(normal.x,normal.y,normal.z); 44 | } 45 | 46 | aiReleaseImport(scene); 47 | 48 | return mesh; 49 | 50 | } 51 | 52 | } 53 | -------------------------------------------------------------------------------- /src/mesh/assimp_mesh_reader.h: -------------------------------------------------------------------------------- 1 | #ifndef ASSIMP_MESH_READER_H 2 | #define ASSIMP_MESH_READER_H 3 | 4 | #include "util/model_renderer.h" 5 | 6 | namespace dart { 7 | 8 | class AssimpMeshReader : public dart::MeshReader { 9 | public: 10 | dart::Mesh * readMesh(const std::string filename) const; 11 | }; 12 | 13 | } 14 | 15 | #endif // ASSIMP_MESH_READER_H 16 | -------------------------------------------------------------------------------- /src/mesh/mesh.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace dart { 7 | 8 | void Mesh::writeToObjFile(const char * filename) { 9 | 10 | std::ofstream stream(filename); 11 | std::cout << "writing " << nVertices << " vertices to " << filename << std::endl; 12 | for (int v=0; v= nVertices || faces[f].y < 0 || faces[f].y >= nVertices || faces[f].z < 0 || faces[f].z >= nVertices) { 18 | std::cout << "face " << f << " has out-of-bounds vertices" << std::endl; 19 | } 20 | if (faces[f].x == faces[f].y || faces[f].y == faces[f].z || faces[f].z == faces[f].x) { 21 | std::cout << "face " << f << " has repeated vertices" << std::endl; 22 | } 23 | stream << "f " << faces[f].x+1 << "//" << faces[f].x+1 << " " << faces[f].y+1 << "//" << faces[f].y+1 << " " << faces[f].z+1 << "//" << faces[f].z+1 << std::endl; 24 | } 25 | stream.close(); 26 | 27 | } 28 | 29 | } 30 | -------------------------------------------------------------------------------- /src/mesh/mesh.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_H 2 | #define MESH_H 3 | 4 | #include 5 | #include 6 | 7 | namespace dart { 8 | 9 | class Mesh { 10 | public: 11 | 12 | /** 13 | * The default mesh constructor. 14 | * All data members are initialized to null pointers, and will need to be allocated later. 15 | */ 16 | Mesh() : faces(0), vertices(0), normals(0), nVertices(0), nFaces(0) { } 17 | 18 | /** 19 | * Constructs a mesh with preallocated mesh data. 20 | * The arrays for vertices, normals, and faces are allocated to the appropriate size, but the values are undefined. 21 | * @param numVertices The number of vertices in the mesh. 22 | * @param numFaces The number of faces in the mesh. 23 | */ 24 | Mesh(const int numVertices, const int numFaces) : 25 | faces(new int3[numFaces]), 26 | vertices(new float3[numVertices]), 27 | normals(new float3[numVertices]), 28 | nVertices(numVertices), 29 | nFaces(numFaces) { } 30 | 31 | Mesh(const Mesh & copy) : 32 | faces(new int3[copy.nFaces]), 33 | vertices(new float3[copy.nVertices]), 34 | normals(new float3[copy.nVertices]), 35 | nVertices(copy.nVertices), 36 | nFaces(copy.nFaces) { 37 | memcpy(faces,copy.faces,copy.nFaces*sizeof(int3)); 38 | memcpy(vertices,copy.vertices,copy.nVertices*sizeof(float3)); 39 | memcpy(normals,copy.normals,copy.nVertices*sizeof(float3)); 40 | } 41 | 42 | ~Mesh() { delete faces; delete vertices; delete normals; } 43 | 44 | void writeToObjFile(const char * filename); 45 | 46 | /** 47 | * The face data array. Each entry defines a face consisting of three indices into the vertex array. 48 | */ 49 | int3 * faces; 50 | 51 | /** 52 | * The vertex data array. Each entry defines a vertex. 53 | */ 54 | float3 * vertices; 55 | 56 | /** 57 | * The normal data array. Each entry defines the normal of the corresponding entry in the vertex data array. 58 | */ 59 | float3 * normals; 60 | 61 | /** 62 | * The number of vertices in the mesh. 63 | */ 64 | int nVertices; 65 | 66 | /** 67 | * The number of faces in the mesh. 68 | */ 69 | int nFaces; 70 | }; 71 | 72 | } 73 | 74 | #endif // MESH_H 75 | -------------------------------------------------------------------------------- /src/mesh/mesh_proc.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_proc.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace dart { 14 | 15 | void removeDuplicatedVertices(Mesh & mesh) { 16 | const int originalVerts = mesh.nVertices; 17 | std::vector newVerts; 18 | std::vector newNorms; 19 | std::map vertMapping; 20 | for (int i=0; i nondegenerateFaces; 54 | for (int f=0; f > vertFaces(src.nVertices); 73 | for (int i=0; i disconnectedVerts; 81 | std::set disconnectedFaces; 82 | disconnectedVerts.insert(startV); 83 | 84 | std::stack toExpandList; 85 | toExpandList.push(startV); 86 | while (toExpandList.size() > 0) { 87 | int toExpand = toExpandList.top(); 88 | // std::cout << "expanding " << toExpand << std::endl; 89 | toExpandList.pop(); 90 | std::vector & faces = vertFaces[toExpand]; 91 | for (int i=0; i vertMapping; 111 | int i = 0; 112 | for (std::set::iterator it = disconnectedVerts.begin(); it != disconnectedVerts.end(); ++it) { 113 | int v = *it; 114 | vertMapping[v] = i; 115 | split.vertices[i] = src.vertices[v]; 116 | ++i; 117 | } 118 | 119 | i = 0; 120 | for (std::set::iterator it = disconnectedFaces.begin(); it != disconnectedFaces.end(); ++it) { 121 | int3 &face = src.faces[*it]; 122 | split.faces[i] = make_int3(vertMapping[face.x],vertMapping[face.y],vertMapping[face.z]); 123 | ++i; 124 | } 125 | } 126 | 127 | std::vector vertsLeft; 128 | std::vector vertMapping(src.nVertices,-1); 129 | 130 | for (int i=0; i facesLeft; 138 | for (int f=0; f= 0 && vertMapping[y] >= 0 && vertMapping[z] >= 0) { 144 | facesLeft.push_back(make_int3(vertMapping[x],vertMapping[y],vertMapping[z])); 145 | } 146 | } 147 | 148 | src.nVertices = vertsLeft.size(); 149 | src.nFaces = facesLeft.size(); 150 | 151 | delete [] src.vertices; 152 | delete [] src.normals; 153 | delete [] src.faces; 154 | src.vertices = new float3[vertsLeft.size()]; 155 | src.normals = new float3[vertsLeft.size()]; 156 | src.faces = new int3[facesLeft.size()]; 157 | 158 | memcpy(src.vertices,vertsLeft.data(),vertsLeft.size()*sizeof(float3)); 159 | memcpy(src.faces,facesLeft.data(),facesLeft.size()*sizeof(int3)); 160 | } 161 | 162 | void transformMesh(Mesh & mesh, const SE3 & transform) { 163 | 164 | for (int v=0; v 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace dart { 12 | 13 | void sampleMesh(std::vector & sampledPoints, const Mesh & mesh, const float sampleDensity) { 14 | 15 | // compute cumulative and total surface area 16 | std::vector cumulativeSurfaceArea(mesh.nFaces); 17 | for (int f=0; f 5 | #include "mesh.h" 6 | 7 | namespace dart { 8 | 9 | void sampleMesh(std::vector & sampledPoints, const Mesh & mesh, const float sampleDensity); 10 | 11 | } 12 | 13 | #endif // MESH_SAMPLE_H 14 | -------------------------------------------------------------------------------- /src/mesh/mesh_splat.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_SPLAT_H 2 | #define MESH_SPLAT_H 3 | 4 | #include "mesh.h" 5 | #include "geometry/grid_3d.h" 6 | 7 | namespace dart { 8 | 9 | void splatSolidMesh(const Mesh & mesh, Grid3D & sdf); 10 | 11 | } 12 | 13 | #endif // MESH_SPLAT_H 14 | -------------------------------------------------------------------------------- /src/mesh/primitive_meshing.h: -------------------------------------------------------------------------------- 1 | #ifndef PRIMITIVE_MESHING_H 2 | #define PRIMITIVE_MESHING_H 3 | 4 | #include "mesh.h" 5 | 6 | namespace dart { 7 | 8 | Mesh * generateUnitIcosphereMesh(const int splits); 9 | 10 | Mesh * generateCylinderMesh(const int slices); 11 | 12 | Mesh * generateCubeMesh(); 13 | 14 | } 15 | 16 | #endif // PRIMITIVE_MESHING_H 17 | -------------------------------------------------------------------------------- /src/model/host_only_model.h: -------------------------------------------------------------------------------- 1 | #ifndef HOST_ONLY_MODEL_H 2 | #define HOST_ONLY_MODEL_H 3 | 4 | #include "model/model.h" 5 | //#include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "geometry/geometry.h" 14 | #include "geometry/grid_3d.h" 15 | #include "geometry/SE3.h" 16 | #include "mesh/mesh.h" 17 | #include "pose/pose.h" 18 | 19 | namespace dart { 20 | 21 | class HostOnlyModel : public Model { 22 | public: 23 | HostOnlyModel(); 24 | ~HostOnlyModel(); 25 | 26 | // model setup 27 | void addGeometry(int frame, GeomType geometryType, std::string, std::string sy, std::string sz, std::string tx, std::string ty, std::string tz, std::string rx, std::string ry, std::string rz, unsigned char red, unsigned char green, unsigned char blue, const std::string meshFilename = ""); 28 | void addGeometry(int frame, GeomType geometryType, float sx, float sy, float sz, float tx = 0, float ty = 0, float tz = 0, float rx =0, float ry = 0, float rz = 0, unsigned char red = 0xff, unsigned char green = 0xff, unsigned char blue = 0xff, const std::string meshFilename = ""); 29 | 30 | int addFrame(int parent, JointType type, std::string posX, std::string posY, std::string posZ, std::string orX, std::string orY, std::string orZ, std::string axisX, std::string axisY, std::string axisZ, std::string jointMin, std::string jointMax, std::string jointName); 31 | 32 | void computeStructure(); 33 | void voxelize(float resolution, float padding = 0.0f, std::string cacheFile = ""); 34 | void voxelize2(float resolution, float padding, std::string cacheFile = ""); 35 | 36 | void setJointLimits(const int joint, const float min, const float max); 37 | 38 | void setGeometryColor(int geomNumber, unsigned char color[3]); 39 | void setGeometryColor(int geomNumber, unsigned char red, unsigned char green, unsigned char blue); 40 | 41 | // model queries 42 | inline uint getNumFrames() const { return _nFrames; } 43 | 44 | inline int getFrameParent(const int frame) const { return _parents[frame]; } 45 | 46 | inline int getFrameNumChildren(const int frame) const { return _children[frame].size(); } 47 | const inline int * getFrameChildren(const int frame) const { return _children[frame].data(); } 48 | 49 | int getFrameJoint(int frame) const { for (int i=0; i & getSdf(const int sdfNum) const { return _sdfs[sdfNum]; } 79 | const Grid3D * getSdfs() const { return _sdfs.data(); } 80 | inline uint getSdfFrameNumber(const int sdfNum) const { return _sdfFrames[sdfNum]; } 81 | inline uchar3 getSdfColor(const int sdfNum) const { return _sdfColors[sdfNum]; } 82 | 83 | void setVoxelGrid(Grid3D & grid,const int link); 84 | 85 | // set parameters of DH model 86 | void setArticulation(const float * pose); 87 | void setPose(const Pose & pose); 88 | void setParam(const int link, const int param, const float value); 89 | void setGeometryScale(const int geom, const float3 scale); 90 | void setGeometryTransform(const int geom, const SE3 & T); 91 | void setLinkParent(const int link, const int parent); 92 | 93 | void addSizeParam(std::string name, float value) { _sizeParams[name] = value; } 94 | const std::map & getSizeParams() const { return _sizeParams; } 95 | void setSizeParam(const std::string param, const float value) { 96 | std::map::const_iterator it = _sizeParams.find(param); 97 | if (it != _sizeParams.end()) { 98 | _sizeParams[param] = value; 99 | } 100 | } 101 | float getSizeParam(const std::string param) const { 102 | std::map::const_iterator it = _sizeParams.find(param); 103 | if (it != _sizeParams.end()) { 104 | return it->second; 105 | } 106 | return 0.0; 107 | } 108 | 109 | inline void setSdfColor(const int sdfNum, const uchar3 color) { _sdfColors[sdfNum] = color; } 110 | inline void setGeometryColor(const int geomNum, const uchar3 color) { _geomColors[geomNum] = color; } 111 | 112 | inline const SE3 getTransformJointAxisToParent(const int joint) const { return _T_pf[joint]; } 113 | 114 | protected: 115 | int _nFrames; 116 | 117 | std::vector _parents; 118 | std::vector > _children; 119 | std::vector _jointTypes; 120 | std::vector _T_mf; 121 | std::vector _T_fm; 122 | std::vector _T_pf; 123 | std::map _meshFilenames; 124 | 125 | std::vector _sdfFrames; 126 | 127 | std::vector _geomColors; 128 | std::vector _sdfColors; 129 | 130 | std::vector > _sdfs; 131 | 132 | std::vector _dependencies; 133 | 134 | std::map _sizeParams; 135 | 136 | std::vector _axes; 137 | std::vector _positions; 138 | std::vector _orientations; 139 | std::vector > _frameParamExpressions; 140 | std::vector > _geomParamExpressions; 141 | 142 | 143 | 144 | void voxelizeFrame(Grid3D & vg, const int frame, const float fg, const float bg, const float resolution, const float padding, const float Rc = 1.0); 145 | 146 | float evaluateExpression(const std::string & expression); 147 | }; 148 | 149 | } 150 | 151 | #endif // HOST_ONLY_MODEL_H 152 | -------------------------------------------------------------------------------- /src/model/mirrored_model.h: -------------------------------------------------------------------------------- 1 | #ifndef MIRRORED_MODEL_H 2 | #define MIRRORED_MODEL_H 3 | 4 | #include "model/model.h" 5 | #include "model/host_only_model.h" 6 | #include "util/mirrored_memory.h" 7 | #include "geometry/grid_3d.h" 8 | 9 | namespace dart { 10 | 11 | class MirroredModel : public Model { 12 | public: 13 | MirroredModel(const HostOnlyModel & copy, 14 | const uint3 obsSdfDim, 15 | const float obsSdfRes, 16 | const float3 obsSdfOffset = make_float3(0,0,0), 17 | const int modelID = -1); 18 | ~MirroredModel(); 19 | 20 | void setArticulation(const float * pose); 21 | void setPose(const Pose & pose); 22 | 23 | const inline SE3 & getTransformFrameToModel(const int frame) const { return _T_mf->hostPtr()[frame]; } 24 | const inline SE3 & getTransformModelToFrame(const int frame) const { return _T_fm->hostPtr()[frame]; } 25 | const inline SE3 * getTransformsFrameToModel() const { return _T_mf->hostPtr(); } 26 | const inline SE3 * getTransformsModelToFrame() const { return _T_fm->hostPtr(); } 27 | 28 | inline uint getNumFrames() const { return _frameParents->length(); } 29 | inline int getFrameParent(const int frame) const { return _frameParents->hostPtr()[frame]; } 30 | 31 | inline uint getNumSdfs() const { return _sdfFrames->length(); } 32 | const inline Grid3D & getSdf(const int sdfNum) const { return _sdfs->hostGrids()[sdfNum]; } 33 | const inline Grid3D * getSdfs() const { return _sdfs->hostGrids(); } 34 | inline uint getSdfFrameNumber(const int sdfNum) const { return _sdfFrames->hostPtr()[sdfNum]; } 35 | inline uchar3 getSdfColor(const int sdfNum) const { return _sdfColors->hostPtr()[sdfNum]; } 36 | 37 | inline uint getNumJoints() const { return getNumFrames()-1; } 38 | inline int getJointFrame(const int joint) const { return joint+1; } 39 | inline JointType getJointType(const int joint) const { return _jointTypes->hostPtr()[joint]; } 40 | inline float3 getJointAxis(const int joint) const { return _jointAxes->hostPtr()[joint]; } 41 | inline float3 & getJointPosition(const int joint) { return _jointPositions->hostPtr()[joint]; } 42 | inline float3 & getJointOrientation(const int joint) { return _jointOrientations->hostPtr()[joint]; } 43 | inline const float3 & getJointPosition(const int joint) const { return _jointPositions->hostPtr()[joint]; } 44 | inline const float3 & getJointOrientation(const int joint) const { return _jointOrientations->hostPtr()[joint]; } 45 | 46 | inline uchar3 getGeometryColor(const int geomNumber) const { return _geomColors->hostPtr()[geomNumber]; } 47 | 48 | const inline int * getDependencies() const { return _dependencies->hostPtr(); } 49 | inline int getDependency(const int frame, const int joint) const { return _dependencies->hostPtr()[frame * getNumJoints() + joint]; } 50 | 51 | inline Grid3D * getObsSdf() { return _obsSdf->hostGrid(); } 52 | inline void syncObsSdfHostToDevice() { _obsSdf->syncHostToDevice(); } 53 | inline void syncObsSdfDeviceToHost() { _obsSdf->syncDeviceToHost(); } 54 | inline float3 getObsSdfoffset() const { return _obsSdfOffset; } 55 | 56 | inline uint getModelID() const { return _modelID; } 57 | 58 | inline void setSdfColor(const int sdfNum, const uchar3 color) { _sdfColors->hostPtr()[sdfNum] = color; _sdfColors->syncHostToDevice(); } 59 | inline void setGeometryColor(const int geomNum, const uchar3 color) { _geomColors->hostPtr()[geomNum] = color; _geomColors->syncHostToDevice(); } 60 | 61 | // ------------- device getters ----------------------- 62 | 63 | inline SE3 * getDeviceTransformsFrameToModel() { return _T_mf->devicePtr(); } 64 | inline SE3 * getDeviceTransformsModelToFrame() { return _T_fm->devicePtr(); } 65 | inline const SE3 * getDeviceTransformsFrameToModel() const { return _T_mf->devicePtr(); } 66 | inline const SE3 * getDeviceTransformsModelToFrame() const { return _T_fm->devicePtr(); } 67 | 68 | inline const int * getDeviceFrameParents() const { return _frameParents->devicePtr(); } 69 | 70 | inline int * getDeviceSdfFrames() { return _sdfFrames->devicePtr(); } 71 | inline const int * getDeviceSdfFrames() const { return _sdfFrames->devicePtr(); } 72 | 73 | inline const Grid3D * getDeviceSdfs() const { return _sdfs->deviceGrids(); } 74 | inline const uchar3 * getDeviceSdfColors() const { return _sdfColors->devicePtr(); } 75 | 76 | inline const JointType * getDeviceJointTypes() const { return _jointTypes->devicePtr(); } 77 | inline const float3 * getDeviceJointAxes() const { return _jointAxes->devicePtr(); } 78 | 79 | const inline uchar3 * getDeviceGeometryColors() const { return _geomColors->devicePtr(); } 80 | 81 | const inline int * getDeviceDependencies() const { return _dependencies->devicePtr(); } 82 | 83 | inline Grid3D * getDeviceObsSdf() { return _obsSdf->deviceGrid(); } 84 | inline const Grid3D * getDeviceObsSdf() const { return _obsSdf->deviceGrid(); } 85 | inline float * getDeviceObsSdfData() { return _obsSdf->deviceData(); } 86 | 87 | inline const SE3 getTransformJointAxisToParent(const int joint) const { return _T_pf->hostPtr()[joint]; } 88 | 89 | private: 90 | 91 | uint _modelID; 92 | 93 | // transforms 94 | MirroredVector * _T_mf; 95 | MirroredVector * _T_fm; 96 | MirroredVector * _T_pf; 97 | 98 | // frame data 99 | MirroredVector * _frameParents; 100 | 101 | // sdf data 102 | MirroredVector * _sdfFrames; 103 | MirroredGrid3DVector * _sdfs; 104 | MirroredVector * _sdfColors; 105 | 106 | // joint data 107 | MirroredVector * _jointPositions; 108 | MirroredVector * _jointOrientations; 109 | MirroredVector * _jointAxes; 110 | MirroredVector * _jointTypes; 111 | 112 | // geometry data 113 | MirroredVector * _geomColors; 114 | 115 | // dependency data 116 | MirroredVector * _dependencies; 117 | 118 | // obs sdf data 119 | MirroredGrid3D * _obsSdf; 120 | float3 _obsSdfOffset; 121 | 122 | }; 123 | 124 | } 125 | 126 | #endif // MIRRORED_MODEL_H 127 | -------------------------------------------------------------------------------- /src/model/model.h: -------------------------------------------------------------------------------- 1 | #ifndef MODELS_H 2 | #define MODELS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | //#include 11 | 12 | #include 13 | #include 14 | 15 | #include "geometry/SE3.h" 16 | #include "geometry/grid_3d.h" 17 | #include "pose/pose.h" 18 | #include "util/dart_types.h" 19 | #include "util/model_renderer.h" 20 | 21 | namespace dart { 22 | 23 | class ModelRenderer; 24 | 25 | class Model { 26 | public: 27 | 28 | Model(const int dimensionality) { _dimensionality = dimensionality; } 29 | 30 | Model(const Model & copy); 31 | 32 | int getPoseDimensionality() const { return _dimensionality; } 33 | 34 | virtual void setArticulation(const float * pose) = 0; 35 | 36 | virtual void setPose(const Pose & pose) = 0; 37 | 38 | static void initializeRenderer(MeshReader * meshReader = 0); 39 | 40 | static void shutdownRenderer(); 41 | 42 | void render(const char alpha = 0xff) const; 43 | void renderWireframe() const; 44 | void renderSkeleton(const float sphereSize = 0.01, const float lineSize=4) const; 45 | void renderLabeled(const int modelNumber = 0) const; 46 | void renderVoxels(const float levelSet) const; 47 | void renderCoordinateFrames(const float size = 0.1) const; 48 | void renderFrameParents() const; 49 | 50 | virtual uint getNumFrames() const = 0; 51 | 52 | virtual const SE3 & getTransformFrameToModel(const int frame) const = 0; 53 | virtual const SE3 & getTransformModelToFrame(const int frame) const = 0; 54 | virtual const SE3 * getTransformsFrameToModel() const = 0; 55 | virtual const SE3 * getTransformsModelToFrame() const = 0; 56 | const inline SE3 & getTransformCameraToModel() const { return _T_mc; } 57 | const inline SE3 & getTransformModelToCamera() const { return _T_cm; } 58 | const inline SE3 getTransformCameraToFrame(const int frame) const { return getTransformModelToFrame(frame)*getTransformCameraToModel(); } 59 | const inline SE3 getTransformFrameToCamera(const int frame) const { return getTransformModelToCamera()*getTransformFrameToModel(frame); } 60 | 61 | virtual int getFrameParent(const int frame) const = 0; 62 | inline int getFrameSdfNumber(const int frame) const { return _frameSdfNumbers[frame]; } 63 | inline int getFrameNumGeoms(const int frame) const { return _frameGeoms[frame].size(); } 64 | const inline int * getFrameGeoms(const int frame) const { return _frameGeoms[frame].data(); } 65 | 66 | virtual uint getNumSdfs() const = 0; 67 | virtual const Grid3D & getSdf(const int sdfNum) const = 0; 68 | virtual const Grid3D * getSdfs() const = 0; 69 | virtual uint getSdfFrameNumber(const int sdfNum) const = 0; 70 | virtual uchar3 getSdfColor(const int sdfNum) const = 0; 71 | virtual float3 getJointAxis(const int joint) const = 0; 72 | virtual float3 & getJointPosition(const int joint) = 0; 73 | virtual float3 & getJointOrientation(const int joint) = 0; 74 | virtual const float3 & getJointPosition(const int joint) const = 0; 75 | virtual const float3 & getJointOrientation(const int joint) const = 0; 76 | 77 | virtual int getDependency(const int frame, const int joint) const = 0; 78 | virtual const int * getDependencies() const = 0; 79 | 80 | inline uint getNumGeoms() const { return _geomTypes.size(); } 81 | inline GeomType getGeometryType(const int geomNumber) const { return _geomTypes[geomNumber]; } 82 | virtual uchar3 getGeometryColor(const int geomNumber) const = 0; 83 | const inline float3 getGeometryScale(const int geomNumber) const { return _geomScales[geomNumber]; } 84 | const inline SE3 getGeometryTransform(const int geomNumber) const { return _geomTransforms[geomNumber]; } 85 | 86 | inline int getMeshNumber(const uint geomNumber) const { 87 | std::map::const_iterator it = _meshNumbers.find(geomNumber); 88 | if (it == _meshNumbers.end()) { 89 | return -1; 90 | } 91 | return it->second; 92 | } 93 | inline const Mesh & getMesh(const int meshNumber) const { return _renderer->getMesh(meshNumber); } 94 | 95 | virtual uint getNumJoints() const = 0; 96 | virtual int getJointFrame(const int joint) const = 0; 97 | virtual JointType getJointType(const int joint) const = 0; 98 | void getModelJacobianOfModelPoint(const float4 & modelPoint, const int associatedFrame, std::vector & J3D) const; 99 | 100 | inline float getJointMin(const int joint) const { return _jointLimits[joint].x; } 101 | inline float getJointMax(const int joint) const { return _jointLimits[joint].y; } 102 | 103 | const std::string & getJointName(const int joint) const { return _jointNames[joint]; } 104 | void renderSdf(const dart::Grid3D & sdf, float levelSet) const; 105 | 106 | void getArticulatedBoundingBox(float3 & mins, float3 & maxs, const float modelSdfPadding, const int nSweepPoints = 4); 107 | 108 | virtual void setSdfColor(const int sdfNum, const uchar3 color) = 0; 109 | virtual void setGeometryColor(const int geomNum, const uchar3 color) = 0; 110 | 111 | void setModelVersion(const int modelVersion) { _modelVersion = modelVersion; } 112 | 113 | virtual const SE3 getTransformJointAxisToParent(const int joint) const = 0; 114 | 115 | protected: 116 | int _dimensionality; 117 | 118 | // TODO: maybe move some of this to private? 119 | std::vector > _frameGeoms; 120 | std::vector _geomTypes; 121 | std::vector _jointNames; 122 | 123 | SE3 _T_mc; 124 | SE3 _T_cm; 125 | 126 | std::vector _jointLimits; 127 | 128 | std::vector _geomScales; 129 | std::vector _geomTransforms; 130 | 131 | static ModelRenderer * _renderer; 132 | std::vector _frameSdfNumbers; 133 | 134 | 135 | 136 | inline void setMeshNumber(const int geomNumber, const int meshNumber) { _meshNumbers[geomNumber] = meshNumber; } 137 | 138 | int _modelVersion; 139 | 140 | private: 141 | 142 | std::map _meshNumbers; 143 | 144 | // geometry-level rendering 145 | void renderGeometries(void (Model::*prerenderFunc)(const int, const int, const char *) const, 146 | const char * args = 0, 147 | void (Model::*postrenderFunc)() const = 0) const; 148 | 149 | void renderColoredPrerender(const int frameNumber, const int geomNumber, const char * args) const; 150 | 151 | void renderWireframePrerender(const int frameNumber, const int geomNumber, const char * args) const; 152 | void renderWireFramePostrender() const; 153 | 154 | void renderLabeledPrerender(const int frameNumber, const int geomNumber, const char * args) const; 155 | 156 | // frame-level rendering 157 | void renderFrames(void (Model::*renderFrameFunc)(const int,const char*) const, 158 | const char * args = 0) const; 159 | 160 | void renderVoxelizedFrame(const int frameNumber, const char * args) const; 161 | 162 | 163 | void renderCoordinateFrame(const int frameNumber, const char * args) const; 164 | 165 | }; 166 | 167 | } 168 | 169 | #endif // MODELS_H 170 | -------------------------------------------------------------------------------- /src/optimization/contact_prior.cpp: -------------------------------------------------------------------------------- 1 | #include "priors.h" 2 | #include "geometry/sdf.h" 3 | 4 | namespace dart { 5 | 6 | void ContactPrior::computeContribution(Eigen::SparseMatrix & JTJ, 7 | Eigen::VectorXf & JTe, 8 | const int * modelOffsets, 9 | const int priorParamOffset, 10 | const std::vector & models, 11 | const std::vector & poses, 12 | const OptimizationOptions & opts) { 13 | 14 | if (_weight == 0) { return; } 15 | 16 | const MirroredModel & srcModel = *models[_srcModelID]; 17 | const MirroredModel & dstModel = *models[_dstModelID]; 18 | 19 | const Pose & srcPose = poses[_srcModelID]; 20 | const Pose & dstPose = poses[_dstModelID]; 21 | 22 | const int srcOffset = modelOffsets[_srcModelID]; 23 | const int dstOffset = modelOffsets[_dstModelID]; 24 | 25 | const int srcDims = srcPose.getReducedDimensions(); 26 | const int dstDims = dstPose.getReducedDimensions(); 27 | 28 | const int srcFrame = srcModel.getSdfFrameNumber(_srcSdfNum); 29 | const int dstFrame = dstModel.getSdfFrameNumber(_dstSdfNum); 30 | 31 | const float4 contact_sf = make_float4(_contactPoint,1); 32 | const float4 contact_sm = srcModel.getTransformFrameToModel(srcFrame)*contact_sf; 33 | const float4 contact_c = srcModel.getTransformModelToCamera()*contact_sm; 34 | const float4 contact_dm = dstModel.getTransformCameraToModel()*contact_c; 35 | const float4 contact_df = dstModel.getTransformModelToFrame(dstFrame)*contact_dm; 36 | 37 | const Grid3D & dstSdf = dstModel.getSdf(_dstSdfNum); 38 | const float3 contact_g = dstSdf.getGridCoords(make_float3(contact_df)); 39 | 40 | if (!dstSdf.isInBoundsGradientInterp(contact_g)) { 41 | return; 42 | } 43 | 44 | // compute contact error 45 | const float errContact = dstSdf.getValueInterpolated(contact_g)*dstSdf.resolution; 46 | 47 | if (errContact > opts.contactThreshold) { 48 | return; 49 | } 50 | 51 | // compute contact jacobian 52 | const float3 sdfGrad_df = dstSdf.getGradientInterpolated(contact_g); 53 | const float3 sdfGrad_dm = SE3Rotate(dstModel.getTransformFrameToModel(dstFrame),sdfGrad_df); 54 | const float3 sdfGrad_c = SE3Rotate(dstModel.getTransformModelToCamera(),sdfGrad_dm); 55 | const float3 sdfGrad_sm = SE3Rotate(srcModel.getTransformCameraToModel(),sdfGrad_c); 56 | const float3 sdfGrad_sf = SE3Rotate(srcModel.getTransformModelToFrame(srcFrame),sdfGrad_sm); 57 | 58 | const int subSysSize = srcDims + dstDims + 3; 59 | 60 | // contact point gradient 61 | Eigen::VectorXf subJ = Eigen::VectorXf::Zero(subSysSize); 62 | subJ(srcDims + dstDims + 0) = sdfGrad_sf.x; 63 | subJ(srcDims + dstDims + 1) = sdfGrad_sf.y; 64 | subJ(srcDims + dstDims + 2) = sdfGrad_sf.z; 65 | 66 | // dst pose gradient 67 | // std::vector dstJ3D; 68 | dstModel.getModelJacobianOfModelPoint(contact_dm,dstFrame,dstJ3D); 69 | 70 | for (int i=0; i srcJ3D; 76 | srcModel.getModelJacobianOfModelPoint(contact_sm,srcFrame,srcJ3D); 77 | 78 | for (int i=0; i & models) { 156 | 157 | _contactPoint.x += update[0]; 158 | _contactPoint.y += update[1]; 159 | _contactPoint.z += update[2]; 160 | 161 | const Grid3D sdf = models[_srcModelID]->getSdf(_srcSdfNum); 162 | float3 est_g = sdf.getGridCoords(_contactPoint); 163 | projectToSdfSurface(sdf,est_g,1e-9); 164 | _contactPoint = sdf.getWorldCoords(est_g); 165 | 166 | } 167 | 168 | 169 | } 170 | -------------------------------------------------------------------------------- /src/optimization/kernels/intersection.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERSECTION_H 2 | #define INTERSECTION_H 3 | 4 | #include "geometry/grid_3d.h" 5 | #include "geometry/SE3.h" 6 | #include "model/mirrored_model.h" 7 | #include "util/dart_types.h" 8 | 9 | namespace dart { 10 | 11 | int countSelfIntersections(const float4 * testSites, 12 | const int nSites, 13 | const SE3 * T_mfs, 14 | const SE3 * T_fms, 15 | const int * sdfFrames, 16 | const Grid3D * sdfs, 17 | const int nSdfs, 18 | const int * potentialIntersection); 19 | 20 | void normEqnsSelfIntersection(const float4 * testSites, 21 | const int nSites, 22 | const int dims, 23 | const MirroredModel & model, 24 | const int * potentialIntersection, 25 | float * result, 26 | float * debugError = 0); 27 | 28 | void normEqnsSelfIntersectionReduced(const float4 * testSites, 29 | const int nSites, 30 | const int fullDims, 31 | const int redDims, 32 | const MirroredModel & model, 33 | const float * dtheta_dalpha, 34 | const int * potentialIntersection, 35 | float * result); 36 | 37 | void normEqnsSelfIntersectionParamMap(const float4 * testSites, 38 | const int nSites, 39 | const int fullDims, 40 | const int redDims, 41 | const MirroredModel & model, 42 | const int * dMapping, 43 | const int * potentialIntersection, 44 | float * result); 45 | 46 | void normEqnsIntersection(const float4 * testSites, 47 | const int nSites, 48 | const int dims, 49 | const SE3 T_ds, 50 | const SE3 T_sd, 51 | const MirroredModel & srcModel, 52 | const MirroredModel & dstModel, 53 | float * result, 54 | float * debugError = 0); 55 | 56 | void normEqnsIntersectionReduced(const float4 * testSites, 57 | const int nSites, 58 | const int fullDims, 59 | const int redDims, 60 | const SE3 T_ds, 61 | const SE3 T_sd, 62 | const MirroredModel & srcModel, 63 | const MirroredModel & dstModel, 64 | const float * dtheta_dalpha_src, 65 | float * result); 66 | 67 | void normEqnsIntersectionParamMap(const float4 * testSites, 68 | const int nSites, 69 | const int fullDims, 70 | const int redDims, 71 | const SE3 T_ds, 72 | const SE3 T_sd, 73 | const MirroredModel & srcModel, 74 | const MirroredModel & dstModel, 75 | const int * dMapping_src, 76 | float * result); 77 | 78 | void initDebugIntersectionError(float * debugError, 79 | const int nSites); 80 | 81 | } 82 | 83 | #endif // INTERSECTION_H 84 | -------------------------------------------------------------------------------- /src/optimization/kernels/kernel_common.h: -------------------------------------------------------------------------------- 1 | #ifndef KERNEL_COMMON_H 2 | #define KERNEL_COMMON_H 3 | 4 | #include "geometry/SE3.h" 5 | #include "util/dart_types.h" 6 | 7 | #include 8 | #include 9 | 10 | namespace dart { 11 | 12 | __device__ inline void getErrorJacobianOfModelPoint(float * J, const float4 & point_m, const int frame, const float3 & errorGrad3D_m, 13 | const int dims, const int * dependencies, const JointType * jointTypes, 14 | const float3 * jointAxes, const SE3 * T_fms, const SE3 * T_mfs) { 15 | 16 | J[0] = dot(errorGrad3D_m,make_float3(-1, 0, 0)); 17 | J[1] = dot(errorGrad3D_m,make_float3( 0,-1, 0)); 18 | J[2] = dot(errorGrad3D_m,make_float3( 0, 0,-1)); 19 | 20 | // rotation 21 | J[3] = dot(errorGrad3D_m,make_float3( 0, point_m.z,-point_m.y)); 22 | J[4] = dot(errorGrad3D_m,make_float3(-point_m.z, 0, point_m.x)); 23 | J[5] = dot(errorGrad3D_m,make_float3( point_m.y,-point_m.x, 0)); 24 | 25 | #pragma unroll 26 | for (int i=0; i<(dims-6); i++) { 27 | 28 | if (dependencies[frame*(dims-6) + i] == 0) { 29 | J[i+6] = 0; 30 | continue; 31 | } 32 | 33 | const int jointFrame = i+1; 34 | if (jointTypes[i] == RotationalJoint) { 35 | const float4 axisRelativePoint = T_fms[jointFrame]*point_m; 36 | const float3 dx_a = cross(jointAxes[i],make_float3(axisRelativePoint)); 37 | const float3 dx = SE3Rotate(T_mfs[jointFrame],dx_a); 38 | J[i+6] = dot(errorGrad3D_m,dx); 39 | } else if (jointTypes[i] == PrismaticJoint) { 40 | const float3 axis_m = SE3Rotate(T_mfs[jointFrame],jointAxes[i]); 41 | J[i+6] = dot(errorGrad3D_m,axis_m); 42 | } 43 | 44 | } 45 | } 46 | 47 | __device__ inline void getErrorJacobianOfModelPointArticulationOnly(float * J, const float4 & point_m, const int frame, const float3 & errorGrad3D_m, 48 | const int dims, const int * dependencies, const JointType * jointTypes, 49 | const float3 * jointAxes, const SE3 * T_fms, const SE3 * T_mfs) { 50 | 51 | #pragma unroll 52 | for (int i=0; i<(dims-6); i++) { 53 | 54 | if (dependencies[frame*(dims-6) + i] == 0) { 55 | J[i] = 0; 56 | continue; 57 | } 58 | 59 | const int jointFrame = i+1; 60 | if (jointTypes[i] == RotationalJoint) { 61 | float4 axisRelativePoint = T_fms[jointFrame]*point_m; 62 | float3 dx_a = cross(jointAxes[i],make_float3(axisRelativePoint)); 63 | float3 dx = SE3Rotate(T_mfs[jointFrame],dx_a); 64 | J[i] = dot(errorGrad3D_m,dx); 65 | } else if (jointTypes[i] == PrismaticJoint) { 66 | const float3 axis_m = SE3Rotate(T_mfs[jointFrame],jointAxes[i]); 67 | J[i] = dot(errorGrad3D_m,axis_m); 68 | } 69 | 70 | } 71 | } 72 | 73 | __device__ inline void doPoseGradientReduction(float * J, const float * de_dtheta, const float * dtheta_dalpha, const int fullDims, const int redDims) { 74 | 75 | // copy over 6DoF gradients [TODO: this is wasteful] 76 | for (int r=0; r<6; ++r) { 77 | J[r] = de_dtheta[r]; 78 | } 79 | 80 | // articulation reduction 81 | for (int r=6; r>1) + j],v2); 150 | } 151 | } 152 | atomicAdd(e,0.5*residual*residual); 153 | } 154 | 155 | } 156 | 157 | #endif // KERNEL_COMMON_H 158 | -------------------------------------------------------------------------------- /src/optimization/kernels/modToObs.h: -------------------------------------------------------------------------------- 1 | #ifndef MODTOOBS_H 2 | #define MODTOOBS_H 3 | 4 | #include 5 | 6 | #include "geometry/grid_3d.h" 7 | #include "geometry/SE3.h" 8 | #include "model/mirrored_model.h" 9 | #include "util/dart_types.h" 10 | 11 | namespace dart { 12 | 13 | float errorModToObs(const float4 * dLabeledPredictedVertMap, 14 | const int width, 15 | const int height, 16 | const Grid3D * dObsSdf); 17 | 18 | void normEqnsModToObs(const int dimensions, 19 | const float4 * dLabeledPredictedVertMap, 20 | const int width, 21 | const int height, 22 | const MirroredModel & model, 23 | const SE3 T_gc, 24 | float * dResult, 25 | int * numPredictions = 0, 26 | int * debugDataAssociation = 0, 27 | float * debugError = 0, 28 | float4 * debugNorm = 0); 29 | 30 | void normEqnsModToObsReduced(const int dims, 31 | const int reductionDims, 32 | const float * d_dtheta_dalpha, 33 | const float4 * dLabeledPredictedVertMap, 34 | const int width, 35 | const int height, 36 | const MirroredModel & model, 37 | float * dResult, 38 | int * numPredictions, 39 | int * debugDataAssociation = 0, 40 | float * debugError = 0, 41 | float4 * debugNorm = 0); 42 | 43 | void normEqnsModToObsParamMap(const int dims, 44 | const int reductionDims, 45 | const int * dMapping, 46 | const float4 * dLabeledPredictedVertMap, 47 | const int width, 48 | const int height, 49 | const MirroredModel & model, 50 | float * dResult, 51 | int * numPredictions, 52 | int * debugDataAssociation = 0, 53 | float * debugError = 0, 54 | float4 * debugNorm = 0); 55 | 56 | void normEqnsModToObsTruncated(const int dimensions, 57 | const float4 * dLabeledPredictedVertMap, 58 | const int width, 59 | const int height, 60 | const MirroredModel & model, 61 | const float truncationDistance, 62 | float * dResult, 63 | int * numPredictions = 0, 64 | int * debugDataAssociation = 0, 65 | float * debugError = 0, 66 | float4 * debugNorm = 0); 67 | 68 | void splatObsSdfZeros(const float4 * dObsVertMap, 69 | const int width, 70 | const int height, 71 | const SE3 & T_cm, 72 | const Grid3D * dObsSdf, 73 | const uint3 sdfDim, 74 | const float focalLength); 75 | 76 | void computeTruncatedObsSdf(const float4 * dObsVertMap, 77 | const int width, 78 | const int height, 79 | const SE3 & T_mc, 80 | const Grid3D * dObsSdf, 81 | const uint3 sdfDim, 82 | const float truncationDist); 83 | 84 | void cullUnobservable_(float4 * predVertMap, 85 | const int predWidth, 86 | const int predHeight, 87 | const float4 * obsVertMap, 88 | const int obsWidth, 89 | const int obsHeight, 90 | const cudaStream_t stream = 0); 91 | 92 | 93 | } 94 | 95 | #endif // MODTOOBS_H 96 | -------------------------------------------------------------------------------- /src/optimization/kernels/obsToMod.h: -------------------------------------------------------------------------------- 1 | #ifndef OBSTOMOD_H 2 | #define OBSTOMOD_H 3 | 4 | #include "geometry/grid_3d.h" 5 | #include "geometry/SE3.h" 6 | #include "optimization/optimization.h" 7 | #include "model/mirrored_model.h" 8 | #include "util/dart_types.h" 9 | 10 | namespace dart { 11 | 12 | void normEqnsObsToMod(const int dims, 13 | const float4 * dObsVertMap, 14 | const int width, 15 | const int height, 16 | const MirroredModel & model, 17 | const OptimizationOptions & opts, 18 | DataAssociatedPoint * dPts, 19 | int nElements, 20 | float * dResult, 21 | float4 * debugJs = 0); 22 | 23 | void normEqnsObsToModReduced(const int dims, 24 | const int reductionDims, 25 | const float * d_dtheta_dalpha, 26 | const float4 * dObsVertMap, 27 | const int width, 28 | const int height, 29 | const MirroredModel & model, 30 | const OptimizationOptions & opts, 31 | DataAssociatedPoint * dPts, 32 | int nElements, 33 | float * dResult); 34 | 35 | void normEqnsObsToModParamMap(const int dims, 36 | const int reductionDims, 37 | const int * dMapping, 38 | const float4 * dObsVertMap, 39 | const int width, 40 | const int height, 41 | const MirroredModel & model, 42 | const OptimizationOptions & opts, 43 | DataAssociatedPoint * dPts, 44 | int nElements, 45 | float * dResult); 46 | 47 | void normEqnsObsToModAndPinkyPos(const int dims, 48 | const float4 * dObsVertMap, 49 | const float4 * dObsNormMap, 50 | const int width, 51 | const int height, 52 | const SE3 T_mc, 53 | const SE3 * dT_fms, 54 | const SE3 * dT_mfs, 55 | const SE3 T_pm, 56 | const int * dSdfFrames, 57 | const Grid3D * dSdfs, 58 | const int nSdfs, 59 | const float distanceThreshold, 60 | const float normalThreshold, 61 | const int * dDependencies, 62 | const JointType * dJointTypes, 63 | const float3 * dJointAxes, 64 | const int * dFrameParents, 65 | const float planeOffset, 66 | const float3 planeNormal, 67 | DataAssociatedPoint * dPts, 68 | int * dLastElement, 69 | int * hLastElement, 70 | float * dResult, 71 | int * debugDataAssociation, 72 | float * debugError, 73 | float4 * debugNorm); 74 | 75 | void errorAndDataAssociation(const float4 * dObsVertMap, 76 | const float4 * dObsNormMap, 77 | const int width, 78 | const int height, 79 | const MirroredModel & model, 80 | const OptimizationOptions & opts, 81 | DataAssociatedPoint * dPts, 82 | int * dLastElement, 83 | int * hLastElement, 84 | int * debugDataAssociation = 0, 85 | float * debugError = 0, 86 | float4 * debugNorm = 0); 87 | 88 | void errorAndDataAssociationMultiModel(const float4 * dObsVertMap, 89 | const float4 * dObsNormMap, 90 | const int width, 91 | const int height, 92 | const int nModels, 93 | const SE3 * T_mcs, 94 | const SE3 * const * T_fms, 95 | const int * const * sdfFrames, 96 | const Grid3D * const * sdfs, 97 | const int * nSdfs, 98 | const float * distanceThresholds, 99 | const float * normalThresholds, 100 | const float * planeOffsets, 101 | const float3 * planeNormals, 102 | int * lastElements, 103 | DataAssociatedPoint * * pts, 104 | int * dDebugDataAssociation, 105 | float * dDebugError, 106 | float4 * dDebugNorm, 107 | cudaStream_t stream = 0); 108 | 109 | 110 | __device__ void getErrorJacobianOfModelPoint(float * J, const float4 & point_m, const int frame, const float3 & errorGrad3D_m, 111 | const int dims, const int * dependencies, const JointType * jointTypes, 112 | const float3 * jointAxes, const SE3 * T_fms, const SE3 * T_mfs); 113 | 114 | } 115 | 116 | #endif // OBSTOMOD_H 117 | -------------------------------------------------------------------------------- /src/optimization/kernels/raycast.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST_H 2 | #define RAYCAST_H 3 | 4 | #include "geometry/grid_3d.h" 5 | #include "geometry/SE3.h" 6 | 7 | namespace dart { 8 | 9 | void raycastPrediction(float2 fl, 10 | float2 pp, 11 | const int width, 12 | const int height, 13 | const int modelNum, 14 | const SE3 T_mc, 15 | const SE3 * T_fms, 16 | const SE3 * T_mfs, 17 | const int * sdfFrames, 18 | const Grid3D * sdfs, 19 | const int nSdfs, 20 | float4 * prediction, 21 | const float levelSet, 22 | cudaStream_t stream); 23 | 24 | void raycastPredictionDebugRay(float2 fl, 25 | float2 pp, 26 | const int x, 27 | const int y, 28 | const int width, 29 | const int modelNum, 30 | const SE3 T_mc, 31 | const SE3 * T_fms, 32 | const SE3 * T_mfs, 33 | const int * sdfFrames, 34 | const Grid3D * sdfs, 35 | const int nSdfs, 36 | float4 * prediction, 37 | const float levelSet, 38 | float3 * boxIntersects, 39 | float2 * raySteps, 40 | const int maxRaySteps); 41 | 42 | } 43 | 44 | 45 | #endif // RAYCAST_H 46 | -------------------------------------------------------------------------------- /src/optimization/optimization.h: -------------------------------------------------------------------------------- 1 | #ifndef OPTIMIZATION_H 2 | #define OPTIMIZATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace dart { 8 | 9 | struct OptimizationOptions { 10 | 11 | int numIterations; /**< The number of solver iterations run. */ 12 | std::vector distThreshold; /**< The maximum distance (in meters) at which data association of an observed point may be considered valid (one value per model). */ 13 | std::vector regularization; /**< The amount of regularization to be applied at each iteration of the solver. This value times the identity matrix will be added to the Hessian approximation before solving the normal equations (one value per model). */ 14 | float lambdaObsToMod; /**< Determines the weight applied to errors induced by observed points in the model SDF. */ 15 | float lambdaModToObs; /**< Determines the weight applied to errors induced by predicted model poins in the observation SDF. */ 16 | std::vector lambdaIntersection; /**< Determines the weight applied to errors induced by model points intersecting other model SDFs. If N models have been added to the Tracker, there should be N2 values, where lambdaIntersection[i + j*N] gives the weight assigned to model i intersecting model j. */ 17 | std::vector planeOffset; /**< Together with planeNormal, this allows for specifying a clipping plane for rejecting data association. If the dot product of an observed vertex (transformed into model space) with the planeNormal is less than the planeOffset, the point is rejected (one per model). */ 18 | std::vector planeNormal; /**< Together with planeNormal, this allows for specifying a clipping plane for rejecting data association. If the dot product of an observed vertex (transformed into model space) with the planeNormal is less than the planeOffset, the point is rejected (one per model). */ 19 | std::vector regularizationScaled; /**< The amount of (scaled) regularization to be applied at each iteration of the solver. This value times the diagonal of the Hessian approximation will be added to the Hessian approximation before solving the normal equations (one value per model). */ 20 | 21 | float focalLength; // TODO: remove? 22 | float normThreshold; // TODO: remove? 23 | float huberDelta; 24 | float contactThreshold; // TODO: remove? 25 | 26 | // debugging options 27 | bool debugObsToModDA; /**< Tells the optimizer whether to compute a dense data association map for debugging. If true, the map can be accessed with Optimizer::getDeviceDebugDataAssociationObsToMod(). */ 28 | bool debugObsToModErr; /**< Tells the optimizer whether to compute a dense error map for debugging. If true, the map can be accessed with Optimizer::getDeviceDebugErrorObsToMod(). */ 29 | bool debugModToObsDA; /**< Tells the optimizer whether to compute a dense data association map for debugging. If true, the map can be accessed with Optimizer::getDeviceDebugDataAssociationModToObs(). */ 30 | bool debugModToObsErr; /**< Tells the optimizer whether to compute a dense error map for debugging. If true, the map can be accessed with Optimizer::getDeviceDebugErrorModToObs(). */ 31 | bool debugObsToModNorm; 32 | bool debugModToObsNorm; 33 | bool debugObsToModJs; 34 | bool debugIntersectionErr; 35 | bool debugJTJ; 36 | 37 | OptimizationOptions() : 38 | numIterations(5), 39 | focalLength(537.0), 40 | normThreshold(-0.1), 41 | distThreshold(1,0.03), 42 | regularization(1,1e-20), 43 | lambdaObsToMod(1.0), 44 | lambdaModToObs(1.0), 45 | lambdaIntersection(1,0.0), 46 | planeOffset(1,-0.03), 47 | planeNormal(1,make_float3(0,0,0)), 48 | 49 | regularizationScaled(1,1), 50 | 51 | debugObsToModDA(false), 52 | debugObsToModErr(false), 53 | debugModToObsDA(false), 54 | debugModToObsErr(false), 55 | debugObsToModNorm(false), 56 | debugModToObsNorm(false), 57 | 58 | debugJTJ(false), 59 | 60 | contactThreshold(0.01), 61 | huberDelta(0.02) 62 | 63 | { } 64 | }; 65 | 66 | __host__ __device__ inline int JTJSize(const int dimensions) { return ((dimensions*(dimensions+1))>>1); } 67 | 68 | __host__ __device__ inline int JTJIndex(const int i, const int j){ 69 | if(i >= j){ 70 | return ((i*(i+1))>>1) + j; 71 | } 72 | else{ 73 | return ((j*(j+1))>>1) + i; 74 | } 75 | } 76 | 77 | struct DataAssociatedPoint { 78 | int index; 79 | int dataAssociation; 80 | float error; 81 | }; 82 | 83 | } 84 | 85 | #endif // OPTIMIZATION_H 86 | -------------------------------------------------------------------------------- /src/optimization/point_2d_3d_prior.cpp: -------------------------------------------------------------------------------- 1 | #include "priors.h" 2 | #include "geometry/sdf.h" 3 | 4 | namespace dart { 5 | 6 | void Point2D3DPrior::computeContribution(Eigen::SparseMatrix & JTJ, 7 | Eigen::VectorXf & JTe, 8 | const int * modelOffsets, 9 | const int priorParamOffset, 10 | const std::vector & models, 11 | const std::vector & poses, 12 | const OptimizationOptions & opts) { 13 | 14 | if (_weight == 0) { return; } 15 | 16 | const MirroredModel & srcModel = *models[_srcModelID]; 17 | 18 | const Pose & srcPose = poses[_srcModelID]; 19 | 20 | const int srcOffset = modelOffsets[_srcModelID]; 21 | 22 | const int srcDims = srcPose.getReducedDimensions(); 23 | 24 | const float3 srcPoint_c = SE3Transform(srcModel.getTransformFrameToCamera(_srcFrame),_point_f); 25 | 26 | const float2 projectedSrcPoint = make_float2( srcPoint_c.x*_focalLength.x/srcPoint_c.z + _principalPoint.x, 27 | srcPoint_c.y*_focalLength.y/srcPoint_c.z + _principalPoint.y); 28 | 29 | std::cout << _principalPoint.x << ", " << _principalPoint.y << std::endl; 30 | std::cout << _focalLength.x << ", " << _focalLength.y << std::endl; 31 | 32 | std::cout << projectedSrcPoint.x << ", " << projectedSrcPoint.y << std::endl; 33 | 34 | // compute error 35 | const float2 pixelDiff = projectedSrcPoint - _point_c; 36 | const float pixelDist = length(pixelDiff); 37 | 38 | std::cout << pixelDist << std::endl; 39 | std::cout << std::endl; 40 | 41 | if (pixelDist == 0) { return; } 42 | 43 | const float2 pixelGrad = pixelDiff / pixelDist; 44 | 45 | // const float3 pointGrad_c = make_float3(dot(pixelGrad,make_float2( _focalLength.x / srcPoint_c.z, 0 )), 46 | // dot(pixelGrad,make_float2( 0 ,_focalLength.y / srcPoint_c.z )), 47 | // dot(pixelGrad,make_float2(-(srcPoint_c.x)*_focalLength.x / (srcPoint_c.z*srcPoint_c.z), 48 | // -(srcPoint_c.y)*_focalLength.y / (srcPoint_c.z*srcPoint_c.z)))); 49 | // const float3 distGrad_m = SE3Rotate(srcModel.getTransformCameraToModel(),distGrad_c); 50 | // pointGrad_m = SE3Rotate(srcModel.getTransformCameraToModel(),pointGrad_c); 51 | 52 | 53 | // src pose gradient 54 | // std::vector srcJ3D; 55 | float4 point_m = srcModel.getTransformFrameToModel(_srcFrame)*make_float4(_point_f,1); 56 | srcModel.getModelJacobianOfModelPoint(point_m,_srcFrame,srcJ3D); 57 | 58 | Eigen::MatrixXf subJ = Eigen::MatrixXf::Zero(2,srcDims); 59 | for (int i=0; i & JTJ, 7 | Eigen::VectorXf & JTe, 8 | const int * modelOffsets, 9 | const int priorParamOffset, 10 | const std::vector & models, 11 | const std::vector & poses, 12 | const OptimizationOptions & opts) { 13 | 14 | if (_weight == 0) { return; } 15 | 16 | const MirroredModel & srcModel = *models[_srcModelID]; 17 | 18 | const Pose & srcPose = poses[_srcModelID]; 19 | 20 | const int srcOffset = modelOffsets[_srcModelID]; 21 | 22 | const int srcDims = srcPose.getReducedDimensions(); 23 | 24 | const float3 srcPoint_c = SE3Transform(srcModel.getTransformFrameToCamera(_srcFrame),_point_f); 25 | 26 | // compute error 27 | const float3 diff_c = srcPoint_c - _point_c; 28 | const float3 diff_m = SE3Rotate(srcModel.getTransformCameraToModel(),diff_c); 29 | const float dist = length(diff_c); 30 | 31 | if (dist == 0) { return; } 32 | // const float3 distGrad_c = diff_c / dist; 33 | // const float3 distGrad_m = SE3Rotate(srcModel.getTransformCameraToModel(),distGrad_c); 34 | // distGrad_m = SE3Rotate(srcModel.getTransformCameraToModel(),distGrad_c); 35 | 36 | // compute jacobian 37 | 38 | // src pose gradient 39 | // std::vector srcJ3D; 40 | float4 point_m = srcModel.getTransformFrameToModel(_srcFrame)*make_float4(_point_f,1); 41 | srcModel.getModelJacobianOfModelPoint(point_m,_srcFrame,srcJ3D); 42 | 43 | Eigen::MatrixXf subJ = Eigen::MatrixXf::Zero(3,srcDims); // TODO: this is kind of a waste 44 | for (int i=0; i 5 | #include 6 | 7 | #include "model/model.h" 8 | #include "model/mirrored_model.h" 9 | #include "util/mirrored_memory.h" 10 | 11 | namespace dart { 12 | 13 | class PredictionRenderer { 14 | public: 15 | PredictionRenderer(const int width, const int height, const float2 focalLength); 16 | 17 | ~PredictionRenderer(); 18 | 19 | int getWidth() const { return _width; } 20 | 21 | int getHeight() const { return _height; } 22 | 23 | const float4 * getDevicePrediction() const { cudaDeviceSynchronize(); return _dPrediction; } 24 | // void renderPrediction(const Model & model,cudaStream_t & stream); 25 | // void renderPrediction(const std::vector & models, cudaStream_t & stream); 26 | 27 | void raytracePrediction(const MirroredModel & model, cudaStream_t & stream) ; 28 | 29 | void raytracePrediction(const std::vector & models, cudaStream_t & stream); 30 | 31 | void cullUnobservable(const float4 * dObsVertMap, const int width, const int height, const cudaStream_t stream = 0); 32 | 33 | const unsigned char * getDebugBoxIntersection() const { return _debugBoxIntersections.hostPtr(); } 34 | 35 | void debugPredictionRay(const std::vector & models, const int x, const int y, std::vector > & boxIntersects, std::vector > & raySteps); 36 | private: 37 | 38 | int _width, _height; 39 | float2 _focalLength; 40 | // double _glK[16]; 41 | 42 | // GLuint _tidRgb; 43 | // cudaGraphicsResource * _resRgb; 44 | 45 | // GLuint _rbid; 46 | // GLuint _fbid; 47 | 48 | float4 * _dPrediction; 49 | // GLenum _program; 50 | // GLhandleARB _fragShader; 51 | // GLhandleARB _vertShader; 52 | 53 | MirroredVector _debugBoxIntersections; 54 | }; 55 | 56 | } 57 | 58 | #endif // PREDICTION_RENDERER_H 59 | -------------------------------------------------------------------------------- /src/optimization/priors.h: -------------------------------------------------------------------------------- 1 | #ifndef PRIORS_H 2 | #define PRIORS_H 3 | 4 | #include 5 | #include "optimization.h" 6 | #include "model/mirrored_model.h" 7 | 8 | namespace dart { 9 | 10 | class Prior { 11 | public: 12 | 13 | virtual int getNumPriorParams() const { return 0; } 14 | 15 | virtual float * getPriorParams() { return 0; } 16 | 17 | virtual void computeContribution(Eigen::SparseMatrix & JTJ, 18 | Eigen::VectorXf & JTe, 19 | const int * modelOffsets, 20 | const int priorParamOffset, 21 | const std::vector & models, 22 | const std::vector & poses, 23 | const OptimizationOptions & opts) = 0; 24 | 25 | virtual void updatePriorParams(const float * update, 26 | const std::vector & models) { }; 27 | }; 28 | 29 | class ContactPrior : public Prior { 30 | public: 31 | 32 | ContactPrior(const int srcModelID, const int dstModelID, const int srcSdfNum, const int dstSdfNum, const float weight, const float3 initialContact, float regularization = 100.f) : 33 | _srcModelID(srcModelID), _dstModelID(dstModelID), _srcSdfNum(srcSdfNum), _dstSdfNum(dstSdfNum), _contactPoint(initialContact), _weight(weight), _regularization(regularization), _error(0) { } 34 | 35 | inline int getNumPriorParams() const { return 3; } 36 | 37 | float * getPriorParams() { return (float *) &_contactPoint; } 38 | 39 | void computeContribution(Eigen::SparseMatrix & JTJ, 40 | Eigen::VectorXf & JTe, 41 | const int * modelOffsets, 42 | const int priorParamOffset, 43 | const std::vector & models, 44 | const std::vector & poses, 45 | const OptimizationOptions & opts); 46 | 47 | void updatePriorParams(const float * update, 48 | const std::vector & models); 49 | 50 | void setWeight(float weight) { _weight = weight; } 51 | 52 | float3 getContactPoint() const { return _contactPoint; } 53 | int getSourceModel() const { return _srcModelID; } 54 | int getDestinationModel() const { return _dstModelID; } 55 | int getSourceSdfNum() const { return _srcSdfNum; } 56 | int getDestinationSdfNum() const { return _dstSdfNum; } 57 | float getWeight() const { return _weight; } 58 | float getError() const { return _error; } 59 | 60 | std::vector & getSrcJ3D() { return srcJ3D; } // TODO: remove 61 | std::vector & getDstJ3D() { return dstJ3D; } // TODO: remove 62 | 63 | private: 64 | int _srcModelID, _dstModelID; 65 | int _srcSdfNum, _dstSdfNum; 66 | float3 _contactPoint; 67 | float _weight, _regularization, _error; 68 | std::vector srcJ3D, dstJ3D; // TODO: remove 69 | }; 70 | 71 | class Point3D3DPrior : public Prior { 72 | public: 73 | Point3D3DPrior(const int srcModelID, const int srcFrame, const float3 point_c, const float3 point_f, const float weight) : 74 | _srcModelID(srcModelID), _srcFrame(srcFrame), _point_c(point_c), _point_f(point_f), _weight(weight) { } 75 | 76 | void computeContribution(Eigen::SparseMatrix & JTJ, 77 | Eigen::VectorXf & JTe, 78 | const int * modelOffsets, 79 | const int priorParamOffset, 80 | const std::vector & models, 81 | const std::vector & poses, 82 | const OptimizationOptions & opts); 83 | 84 | void setWeight(float weight) { _weight = weight; } 85 | void setTargetCameraPoint(float3 point_c) { _point_c = point_c; } 86 | 87 | float3 getTargetCameraPoint() const { return _point_c; } 88 | float3 getSourceFramePoint() const { return _point_f; } 89 | int getSourceModelID() const { return _srcModelID; } 90 | int getSourceFrame() const { return _srcFrame; } 91 | 92 | float3 getDistGrad_m() const { return distGrad_m; } // TODO: remove 93 | std::vector & getSrcJ3D() { return srcJ3D; } // TODO: remove 94 | 95 | private: 96 | int _srcModelID, _srcFrame; 97 | float _weight; 98 | float3 _point_c, _point_f; 99 | float3 distGrad_m; // TODO: remove 100 | std::vector srcJ3D; // TODO: remove 101 | }; 102 | 103 | class Point2D3DPrior : public Prior { 104 | public: 105 | Point2D3DPrior(const int srcModelID, const int srcFrame, const float2 point_c, const float3 point_f, const float2 principalPoint, const float2 focalLength, const float weight) : 106 | _srcModelID(srcModelID), _srcFrame(srcFrame), _point_c(point_c), _point_f(point_f), _principalPoint(principalPoint), _focalLength(focalLength), _weight(weight) { } 107 | 108 | void computeContribution(Eigen::SparseMatrix & JTJ, 109 | Eigen::VectorXf & JTe, 110 | const int * modelOffsets, 111 | const int priorParamOffset, 112 | const std::vector & models, 113 | const std::vector & poses, 114 | const OptimizationOptions & opts); 115 | 116 | void setWeight(float weight) { _weight = weight; } 117 | void setTargetCameraPoint(float2 point_c) { _point_c = point_c; } 118 | 119 | float2 getTargetCameraPoint() const { return _point_c; } 120 | float3 getSourceFramePoint() const { return _point_f; } 121 | int getSourceModelID() const { return _srcModelID; } 122 | int getSourceFrame() const { return _srcFrame; } 123 | 124 | float3 getDistGrad_m() const { return pointGrad_m; } // TODO: remove 125 | std::vector & getSrcJ3D() { return srcJ3D; } // TODO: remove 126 | 127 | private: 128 | int _srcModelID, _srcFrame; 129 | float _weight; 130 | float2 _point_c; 131 | float3 _point_f; 132 | float2 _principalPoint, _focalLength; 133 | float3 pointGrad_m; // TODO: remove 134 | std::vector srcJ3D; // TODO: remove 135 | }; 136 | 137 | } // namespace dart 138 | 139 | #endif // PRIORS_H 140 | -------------------------------------------------------------------------------- /src/pose/pose.cpp: -------------------------------------------------------------------------------- 1 | #include "pose.h" 2 | 3 | namespace dart { 4 | 5 | Pose::Pose(PoseReduction * reduction) : 6 | _reduction(reduction), 7 | _fullArticulation(new float[reduction->getFullDimensions()]) { 8 | 9 | if (reduction->isNull()) { 10 | _reducedArticulation = _fullArticulation; 11 | } else { 12 | _reducedArticulation = new float[reduction->getReducedDimensions()]; 13 | } 14 | 15 | } 16 | 17 | Pose::Pose(const Pose & pose) : 18 | _reduction(pose._reduction), 19 | _fullArticulation(new float[pose._reduction->getFullDimensions()]) { 20 | 21 | if (pose._fullArticulation == pose._reducedArticulation) { 22 | _reducedArticulation = _fullArticulation; 23 | } else { 24 | _reducedArticulation = new float[pose._reduction->getReducedDimensions()]; 25 | } 26 | memcpy(_reducedArticulation,pose._reducedArticulation,(pose._reduction->getReducedDimensions())*sizeof(float)); 27 | _T_cm = pose._T_cm; 28 | _T_mc = pose._T_mc; 29 | } 30 | 31 | Pose::~Pose() { 32 | 33 | delete [] _fullArticulation; 34 | if (_reducedArticulation != _fullArticulation) { 35 | delete [] _reducedArticulation; 36 | } 37 | 38 | } 39 | 40 | Pose & Pose::operator=(const Pose & rhs) { 41 | memcpy(_reducedArticulation,rhs._reducedArticulation,getReducedArticulatedDimensions()*sizeof(float)); 42 | memcpy(_fullArticulation,rhs._fullArticulation,getArticulatedDimensions()*sizeof(float)); 43 | _T_mc = rhs._T_mc; 44 | _T_cm = rhs._T_cm; 45 | return *this; 46 | } 47 | 48 | LinearPoseReduction::LinearPoseReduction(const int fullDims, const int redDims) 49 | : PoseReduction(fullDims,redDims), _A(fullDims*redDims), _b(new float[fullDims]) { 50 | 51 | } 52 | 53 | LinearPoseReduction::LinearPoseReduction(const int fullDims, const int redDims, float * A, float * b, float * mins, float * maxs, std::string * names) 54 | : PoseReduction(fullDims,redDims), _A(fullDims*redDims), _b(new float[fullDims]) { 55 | init(A,b,mins,maxs,names); 56 | } 57 | 58 | void LinearPoseReduction::init(float * A, float * b, float * mins, float * maxs, std::string * names) { 59 | memcpy(_b,b,_fullDims*sizeof(float)); 60 | memcpy(_A.hostPtr(),A,_fullDims*_redDims*sizeof(float)); _A.syncHostToDevice(); 61 | memcpy(_mins.data(),mins,_redDims*sizeof(float)); 62 | memcpy(_maxs.data(),maxs,_redDims*sizeof(float)); 63 | for (int i=0; i<_redDims; ++i) { _names[i] = names[i]; } 64 | } 65 | 66 | void LinearPoseReduction::projectReducedToFull(const float * reduced, float * full) const { 67 | for (int f=0; f<_fullDims; ++f) { 68 | full[f] = _b[f]; 69 | for (int r=0; r<_redDims; ++r) { 70 | full[f] += reduced[r]*_A.hostPtr()[r + f*_redDims]; 71 | } 72 | } 73 | } 74 | 75 | ParamMapPoseReduction::ParamMapPoseReduction(const int fullDims, const int redDims, const int * mapping, float * mins, float * maxs, std::string * names) : 76 | LinearPoseReduction(fullDims,redDims), _mapping(fullDims) { 77 | std::vector b(fullDims,0.f); 78 | std::vector A(fullDims*redDims,0.f); 79 | for (int f=0; f 5 | #include 6 | #include 7 | #include "pose_reduction.h" 8 | #include "util/mirrored_memory.h" 9 | #include "geometry/SE3.h" 10 | 11 | namespace dart { 12 | 13 | class Pose { 14 | public: 15 | Pose(PoseReduction * reduction); 16 | 17 | Pose(const Pose & pose); 18 | 19 | ~Pose(); 20 | 21 | inline bool isReduced() const { return !_reduction->isNull(); } 22 | 23 | inline int getDimensions() const { return _reduction->getFullDimensions() + 6; } 24 | 25 | inline int getArticulatedDimensions() const { return _reduction->getFullDimensions(); } 26 | 27 | inline int getReducedDimensions() const { return _reduction->getReducedDimensions() + 6; } 28 | 29 | inline int getReducedArticulatedDimensions() const { return _reduction->getReducedDimensions(); } 30 | 31 | inline float * getArticulation() { return _fullArticulation; } 32 | 33 | inline float * getReducedArticulation() { return _reducedArticulation; } 34 | 35 | inline const float * getArticulation() const { return _fullArticulation; } 36 | 37 | inline const float * getReducedArticulation() const { return _reducedArticulation; } 38 | 39 | virtual void projectReducedToFull() { return _reduction->projectReducedToFull(_reducedArticulation,_fullArticulation); } 40 | 41 | inline const float * getFirstDerivatives() const { return _reduction->getFirstDerivatives(); } 42 | 43 | inline const float * getDeviceFirstDerivatives() const { return _reduction->getDeviceFirstDerivatives(); } 44 | 45 | inline float getReducedMin(const int dim) const { return _reduction->getMin(dim); } 46 | 47 | inline float getReducedMax(const int dim) const { return _reduction->getMax(dim); } 48 | 49 | inline const std::string & getReducedName(const int dim) const { return _reduction->getName(dim); } 50 | 51 | Pose & operator=(const Pose & rhs); 52 | 53 | inline void zero() { memset(_reducedArticulation,0,getReducedArticulatedDimensions()*sizeof(float)); } 54 | 55 | inline SE3 & getTransformCameraToModel() { return _T_mc; } 56 | 57 | inline const SE3 & getTransformCameraToModel() const { return _T_mc; } 58 | 59 | inline SE3 & getTransformModelToCamera() { return _T_cm; } 60 | 61 | inline const SE3 & getTransformModelToCamera() const { return _T_cm; } 62 | 63 | inline void setTransformCameraToModel(const SE3 T_mc) { _T_mc = T_mc; _T_cm = SE3Invert(T_mc); } 64 | 65 | inline void setTransformModelToCamera(const SE3 T_cm) { _T_cm = T_cm; _T_mc = SE3Invert(T_cm); } 66 | 67 | inline const PoseReduction * getReduction() const { return _reduction; } 68 | 69 | protected: 70 | float * _fullArticulation; 71 | float * _reducedArticulation; 72 | PoseReduction * _reduction; 73 | SE3 _T_mc; 74 | SE3 _T_cm; 75 | }; 76 | 77 | 78 | class PosePrior { 79 | public: 80 | PosePrior(const int length) : _pose(length), _weights(length) { } 81 | int getLength() const { return _pose.size(); } 82 | float * getPose() { return _pose.data(); } 83 | const float * getPose() const { return _pose.data(); } 84 | float * getWeights() { return _weights.data(); } 85 | const float * getWeights() const { return _weights.data(); } 86 | private: 87 | std::vector _pose; 88 | std::vector _weights; 89 | }; 90 | 91 | } 92 | 93 | 94 | #endif // POSE_H 95 | -------------------------------------------------------------------------------- /src/pose/pose_reduction.h: -------------------------------------------------------------------------------- 1 | #ifndef POSE_REDUCTION_H 2 | #define POSE_REDUCTION_H 3 | 4 | #include 5 | #include 6 | #include "util/mirrored_memory.h" 7 | 8 | namespace dart { 9 | 10 | class PoseReduction { 11 | public: 12 | PoseReduction(int fullDims, int redDims) : 13 | _fullDims(fullDims), _redDims(redDims), 14 | _mins(redDims), _maxs(redDims), _names(redDims) { } 15 | inline int getFullDimensions() { return _fullDims; } 16 | inline int getReducedDimensions() { return _redDims; } 17 | virtual void projectReducedToFull(const float * reduced, float * full) const = 0; 18 | virtual const float * getFirstDerivatives() const = 0; 19 | virtual const float * getDeviceFirstDerivatives() const = 0; 20 | virtual bool isNull() const { return false; } 21 | 22 | inline float getMin(const int dim) const { return _mins[dim]; } 23 | inline float getMax(const int dim) const { return _maxs[dim]; } 24 | inline const std::string & getName(const int dim) const { return _names[dim]; } 25 | protected: 26 | int _fullDims; 27 | int _redDims; 28 | std::vector _mins; 29 | std::vector _maxs; 30 | std::vector _names; 31 | }; 32 | 33 | class LinearPoseReduction : public PoseReduction { 34 | public: 35 | LinearPoseReduction(const int fullDims, const int redDims); 36 | 37 | LinearPoseReduction(const int fullDims, const int redDims, float * A, float * b, float * mins, float * maxs, std::string * names); 38 | 39 | ~LinearPoseReduction() { delete [] _b; } 40 | 41 | void init(float * A, float * b, float * mins, float * maxs, std::string * names); 42 | 43 | virtual void projectReducedToFull(const float * reduced, float * full) const; 44 | 45 | inline const float * getFirstDerivatives() const { return _A.hostPtr(); } 46 | 47 | inline float getFirstDerivative(const int reducedDim, const int fullDim) { return _A[reducedDim + fullDim*_redDims]; } 48 | 49 | inline const float * getDeviceFirstDerivatives() const { return _A.devicePtr(); } 50 | 51 | inline virtual bool isParamMap() const { return false; } 52 | 53 | protected: 54 | MirroredVector _A; 55 | float * _b; 56 | }; 57 | 58 | class ParamMapPoseReduction : public LinearPoseReduction { 59 | public: 60 | ParamMapPoseReduction(const int fullDims, const int redDims, const int * mapping, float * mins, float * maxs, std::string * names); 61 | 62 | ~ParamMapPoseReduction(); 63 | 64 | void projectReducedToFull(const float * reduced, float * full) const; 65 | 66 | inline const int * getMapping() const { return _mapping.hostPtr(); } 67 | 68 | inline const int * getDeviceMapping() const { return _mapping.devicePtr(); } 69 | 70 | inline bool isParamMap() const { return true; } 71 | 72 | private: 73 | 74 | MirroredVector _mapping; 75 | }; 76 | 77 | class NullReduction : public PoseReduction { 78 | public: 79 | NullReduction(const int dims, const float * mins, const float * maxs, const std::string * names) : PoseReduction(dims,dims) { 80 | memcpy(_mins.data(),mins,dims*sizeof(float)); 81 | memcpy(_maxs.data(),maxs,dims*sizeof(float)); 82 | for (int i=0; i 5 | #include 6 | 7 | #define CheckCudaDieOnError() _CheckCudaDieOnError( __FILE__, __LINE__ ); 8 | namespace dart { 9 | inline void _CheckCudaDieOnError( const char * sFile, const int nLine ) { 10 | cudaError_t error = cudaGetLastError(); 11 | if (error != cudaSuccess) { 12 | std::string errorString(cudaGetErrorString(error)); 13 | std::cerr << "CUDA error: " << errorString << std::endl; 14 | std::cerr << "from line " << nLine << " of file " << sFile << std::endl; 15 | exit(1); 16 | } 17 | } 18 | } // namespace dart 19 | 20 | 21 | #endif // CUDA_UTILS_H 22 | -------------------------------------------------------------------------------- /src/util/dart_io.h: -------------------------------------------------------------------------------- 1 | #ifndef DART_IO_H 2 | #define DART_IO_H 3 | 4 | #include 5 | #include "model/host_only_model.h" 6 | #include "pose/pose.h" 7 | 8 | namespace dart { 9 | 10 | void writeModelXML(const HostOnlyModel & model, const char * filename); 11 | bool readModelXML(const char * filename, HostOnlyModel & model); 12 | 13 | // TODO: maybe make this look nicer? 14 | void saveState(const float * pose, const int dimensions, const int frame, std::string filename); 15 | void loadState(float * pose, const int dimensions, int & frame, std::string filename); 16 | 17 | LinearPoseReduction * loadLinearPoseReduction(std::string filename); 18 | 19 | ParamMapPoseReduction * loadParamMapPoseReduction(std::string filename); 20 | 21 | //// TODO: put the number of dimensions in the file? 22 | ///** 23 | // * @brief loadLinearPoseReduction Loads a LinearPoseReduction instance from a text file. The returned pointer is owned by the caller and will therefore need to be deleted by the caller. 24 | // * @param filename The name of the file storing the LinearPoseReduction data. 25 | // * @param fullDimensions The dimensionality of the space the reduction projects to. 26 | // * @param reducedDimensions The dimensionality of the reduced pose space. 27 | // * @return A pointer to the newly allocated LinearPoseReduction instance. 28 | // */ 29 | //LinearPoseReduction * loadLinearPoseReduction(std::string filename, const int fullDimensions, const int reducedDimensions); 30 | 31 | int * loadSelfIntersectionMatrix(const std::string filename, const int numSdfs); 32 | 33 | } 34 | 35 | #endif // DART_IO_H 36 | -------------------------------------------------------------------------------- /src/util/dart_types.h: -------------------------------------------------------------------------------- 1 | #ifndef DART_TYPES_H 2 | #define DART_TYPES_H 3 | 4 | namespace dart { 5 | 6 | enum JointType { 7 | RotationalJoint = 0, 8 | PrismaticJoint = 1 9 | }; 10 | 11 | enum GeomType { 12 | PrimitiveSphereType = 0, 13 | PrimitiveCylinderType = 1, 14 | PrimitiveCubeType = 2, 15 | NumPrimitives = 3, 16 | MeshType = 4 17 | }; 18 | 19 | enum LossFunctionType { 20 | SquaredLoss, 21 | HuberLoss 22 | }; 23 | 24 | } 25 | 26 | #endif // DART_TYPES_H 27 | -------------------------------------------------------------------------------- /src/util/gl_dart.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "gl_dart.h" 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | void glLoadSE3(const dart::SE3 & mx) { 9 | float mxData[16] = { mx.r0.x, mx.r1.x, mx.r2.x, 0, 10 | mx.r0.y, mx.r1.y, mx.r2.y, 0, 11 | mx.r0.z, mx.r1.z, mx.r2.z, 0, 12 | mx.r0.w, mx.r1.w, mx.r2.w, 1 }; 13 | glLoadMatrixf(mxData); 14 | } 15 | 16 | void glMultSE3(const dart::SE3 & mx) { 17 | float mxData[16] = { mx.r0.x, mx.r1.x, mx.r2.x, 0, 18 | mx.r0.y, mx.r1.y, mx.r2.y, 0, 19 | mx.r0.z, mx.r1.z, mx.r2.z, 0, 20 | mx.r0.w, mx.r1.w, mx.r2.w, 1 }; 21 | glMultMatrixf(mxData); 22 | } 23 | 24 | } 25 | -------------------------------------------------------------------------------- /src/util/gl_dart.h: -------------------------------------------------------------------------------- 1 | #ifndef GL_DART_H 2 | #define GL_DART_H 3 | 4 | #include 5 | #include "geometry/SE3.h" 6 | 7 | namespace dart { 8 | 9 | /** 10 | * @brief glLoadSE3 This function loads a 6DoF transformation matrix stored in an SE3 instance. The semantics are the same as glLoadMatrix. 11 | * @param mx The 6DoF transformation matrix to load. 12 | */ 13 | void glLoadSE3(const dart::SE3 & mx); 14 | 15 | /** 16 | * @brief glMultSE3 This function multiplies a 6DoF transformation matrix stored in an SE3 instance with the current GL matrix stack. The semantics are the same as glMultMatrix. 17 | * @param mx The 6DoF transformation matrix to load. 18 | */ 19 | void glMultSE3(const dart::SE3 & mx); 20 | 21 | /** 22 | * @brief glVertexFloat4 This function places a 3D vertex at the location given in the float4 struct. The homogenous coordinate is ignored. 23 | * @param vertex The vertex location. 24 | */ 25 | inline void glVertexFloat4(const float4 & vertex) { glVertex3f(vertex.x, vertex.y, vertex.z); } 26 | 27 | /** 28 | * @brief glColorUchar3 This function sets the current GL color to that given by the uchar3 struct. 29 | * @param color The color values (0-255). 30 | */ 31 | inline void glColorUchar3(const uchar3 & color) { glColor3ub(color.x, color.y, color.z); } 32 | 33 | } 34 | 35 | #endif // GL_DART_H 36 | -------------------------------------------------------------------------------- /src/util/image_io.cpp: -------------------------------------------------------------------------------- 1 | #include "image_io.h" 2 | 3 | #include 4 | 5 | namespace dart { 6 | 7 | void pngErrorHandler(png_structp pngPtr, png_const_charp msg) { 8 | jmp_buf * buff; 9 | 10 | std::cerr << "libpng error: " << msg << std::endl; 11 | 12 | buff = (jmp_buf *)png_get_error_ptr(pngPtr); 13 | if (buff == NULL) { 14 | std::cerr << "png severe error: jmpbuf not recoverable; terminating" << std::endl; 15 | exit(99); 16 | } 17 | 18 | longjmp(*buff, 1); 19 | } 20 | 21 | void writePNG(const char * filename, const uchar3 * imgData, const int width, const int height) { 22 | 23 | FILE * fp = fopen(filename,"wb"); 24 | 25 | png_byte * rowPointers[height]; 26 | 27 | jmp_buf buff; 28 | png_structp pngPtr = png_create_write_struct(PNG_LIBPNG_VER_STRING, &buff, pngErrorHandler, NULL); 29 | png_infop infoPtr = png_create_info_struct(pngPtr); 30 | 31 | if (setjmp(buff)) { 32 | goto png_failure; 33 | } 34 | 35 | png_set_IHDR( pngPtr, 36 | infoPtr, 37 | width, 38 | height, 39 | sizeof(unsigned char)*8, 40 | PNG_COLOR_TYPE_RGB, 41 | PNG_INTERLACE_NONE, 42 | PNG_COMPRESSION_TYPE_DEFAULT, 43 | PNG_FILTER_TYPE_DEFAULT); 44 | 45 | 46 | for (int y = 0; y 5 | #include "vector_types.h" 6 | #include 7 | 8 | namespace dart { 9 | 10 | void pngErrorHandler(png_structp pngPtr, png_const_charp msg); 11 | 12 | void writePNG(const char * filename, const uchar3 * imgData, const int width, const int height); 13 | 14 | void writePNG(const char * filename, const ushort * imgData, const int width, const int height); 15 | 16 | unsigned char * readPNG(const char * filename, int & width, int & height, int & channels); 17 | 18 | } 19 | 20 | #endif // IMAGE_IO_H 21 | -------------------------------------------------------------------------------- /src/util/model_renderer.cpp: -------------------------------------------------------------------------------- 1 | #include "model_renderer.h" 2 | 3 | #include 4 | #include "mesh/primitive_meshing.h" 5 | #include 6 | 7 | namespace dart { 8 | 9 | static const int cylinderSlices = 20; 10 | static const int sphereSplits = 2; 11 | 12 | ModelRenderer::ModelRenderer(MeshReader * meshReader) { 13 | 14 | // check if a GLContext is available 15 | GLXContext glxContext = glXGetCurrentContext(); 16 | if (glxContext == NULL) { 17 | std::cerr << "gl context is null; please intitialize a gl context before instantiation of dart::Tracker" << std::endl; 18 | } 19 | 20 | // generate the primitive meshes 21 | Mesh * primitiveMeshes[NumPrimitives]; 22 | primitiveMeshes[PrimitiveSphereType] = generateUnitIcosphereMesh(sphereSplits); 23 | primitiveMeshes[PrimitiveCylinderType] = generateCylinderMesh(cylinderSlices); 24 | primitiveMeshes[PrimitiveCubeType] = generateCubeMesh(); 25 | 26 | // generate the buffers 27 | glGenBuffersARB(NumPrimitives,_primitiveVBOs); 28 | glGenBuffersARB(NumPrimitives,_primitiveNBOs); 29 | glGenBuffersARB(NumPrimitives,_primitiveIBOs); 30 | 31 | // copy data to buffers 32 | for (int i=0; inVertices*sizeof(float3),primitive->vertices,GL_STATIC_DRAW_ARB); 38 | 39 | glBindBufferARB(GL_ARRAY_BUFFER_ARB,_primitiveNBOs[i]); 40 | glBufferDataARB(GL_ARRAY_BUFFER_ARB,primitive->nVertices*sizeof(float3),primitive->normals,GL_STATIC_DRAW_ARB); 41 | 42 | glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB,_primitiveIBOs[i]); 43 | glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB,primitive->nFaces*sizeof(int3),primitive->faces,GL_STATIC_DRAW_ARB); 44 | 45 | _nPrimitiveFaces[i] = primitive->nFaces; 46 | 47 | delete primitive; 48 | 49 | } 50 | 51 | // initialize mesh reader 52 | _meshReader = meshReader; 53 | 54 | } 55 | 56 | ModelRenderer::~ModelRenderer() { 57 | 58 | // free buffers 59 | glDeleteBuffersARB(NumPrimitives,_primitiveVBOs); 60 | glDeleteBuffersARB(NumPrimitives,_primitiveNBOs); 61 | glDeleteBuffersARB(NumPrimitives,_primitiveIBOs); 62 | 63 | if (_meshNumbers.size() > 0) { 64 | glDeleteBuffersARB(_meshNumbers.size(),_meshVBOs.data()); 65 | glDeleteBuffersARB(_meshNumbers.size(),_meshNBOs.data()); 66 | glDeleteBuffersARB(_meshNumbers.size(),_meshIBOs.data()); 67 | } 68 | 69 | // free meshes 70 | for (int i=0; i<_meshes.size(); ++i) { 71 | delete _meshes[i]; 72 | } 73 | 74 | // free mesh reader 75 | delete _meshReader; 76 | } 77 | 78 | int ModelRenderer::getMeshNumber(const std::string meshFilename) { 79 | 80 | if (_meshReader == 0) { 81 | std::cerr << "mesh reader was not set; therefore, meshes may not be read." << std::endl; 82 | return -1; 83 | } 84 | 85 | if (_meshNumbers.find(meshFilename) != _meshNumbers.end()) { 86 | return _meshNumbers[meshFilename]; 87 | } 88 | 89 | Mesh * mesh = _meshReader->readMesh(meshFilename); 90 | 91 | if (mesh == 0) { 92 | std::cerr << "could not read " + meshFilename << std::endl; 93 | return -1; 94 | } 95 | 96 | int meshNum = _meshNumbers.size(); 97 | _meshNumbers[meshFilename] = meshNum; 98 | 99 | _meshVBOs.resize(meshNum + 1); 100 | _meshNBOs.resize(meshNum + 1); 101 | _meshIBOs.resize(meshNum + 1); 102 | 103 | glGenBuffersARB(1,&_meshVBOs[meshNum]); 104 | glGenBuffersARB(1,&_meshNBOs[meshNum]); 105 | glGenBuffersARB(1,&_meshIBOs[meshNum]); 106 | 107 | glBindBufferARB(GL_ARRAY_BUFFER_ARB,_meshVBOs[meshNum]); 108 | glBufferDataARB(GL_ARRAY_BUFFER_ARB,mesh->nVertices*sizeof(float3),mesh->vertices,GL_STATIC_DRAW_ARB); 109 | 110 | glBindBufferARB(GL_ARRAY_BUFFER_ARB,_meshNBOs[meshNum]); 111 | glBufferDataARB(GL_ARRAY_BUFFER_ARB,mesh->nVertices*sizeof(float3),mesh->normals,GL_STATIC_DRAW_ARB); 112 | 113 | glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB,_meshIBOs[meshNum]); 114 | glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB,mesh->nFaces*sizeof(int3),mesh->faces,GL_STATIC_DRAW_ARB); 115 | 116 | _nMeshFaces.push_back(mesh->nFaces); 117 | 118 | _meshes.push_back(mesh); 119 | 120 | return meshNum; 121 | 122 | } 123 | 124 | void ModelRenderer::renderPrimitive(const GeomType type) const { 125 | 126 | glBindBufferARB(GL_ARRAY_BUFFER_ARB, _primitiveNBOs[type]); 127 | glNormalPointer(GL_FLOAT,0,0); 128 | 129 | glBindBufferARB(GL_ARRAY_BUFFER_ARB, _primitiveVBOs[type]); 130 | glVertexPointer(3, GL_FLOAT, 0, 0); 131 | 132 | glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, _primitiveIBOs[type]); 133 | 134 | glEnableClientState(GL_NORMAL_ARRAY); 135 | glEnableClientState(GL_VERTEX_ARRAY); 136 | 137 | glDrawElements(GL_TRIANGLES,3*_nPrimitiveFaces[type],GL_UNSIGNED_INT,0); 138 | 139 | glDisableClientState(GL_VERTEX_ARRAY); 140 | glDisableClientState(GL_NORMAL_ARRAY); 141 | 142 | glBindBufferARB(GL_ARRAY_BUFFER_ARB,0); 143 | glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB,0); 144 | 145 | } 146 | 147 | void ModelRenderer::renderMesh(const uint meshNum) const { 148 | 149 | glBindBufferARB(GL_ARRAY_BUFFER_ARB, _meshNBOs[meshNum]); 150 | glNormalPointer(GL_FLOAT,0,0); 151 | 152 | glBindBufferARB(GL_ARRAY_BUFFER_ARB, _meshVBOs[meshNum]); 153 | glVertexPointer(3, GL_FLOAT, 0, 0); 154 | 155 | glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, _meshIBOs[meshNum]); 156 | 157 | glEnableClientState(GL_NORMAL_ARRAY); 158 | glEnableClientState(GL_VERTEX_ARRAY); 159 | 160 | glDrawElements(GL_TRIANGLES,3*_nMeshFaces[meshNum],GL_UNSIGNED_INT,0); 161 | 162 | glDisableClientState(GL_VERTEX_ARRAY); 163 | glDisableClientState(GL_NORMAL_ARRAY); 164 | 165 | glBindBufferARB(GL_ARRAY_BUFFER_ARB,0); 166 | glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB,0); 167 | 168 | } 169 | 170 | const Mesh & ModelRenderer::getMesh(const uint meshNum) const { 171 | 172 | return *_meshes[meshNum]; 173 | 174 | } 175 | 176 | } 177 | -------------------------------------------------------------------------------- /src/util/model_renderer.h: -------------------------------------------------------------------------------- 1 | #ifndef MODEL_RENDERER_H 2 | #define MODEL_RENDERER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "mesh/mesh.h" 11 | #include "dart_types.h" 12 | //#include "models.h" 13 | 14 | namespace dart { 15 | 16 | class MeshReader { 17 | public: 18 | virtual Mesh * readMesh(const std::string filename) const = 0; 19 | }; 20 | 21 | class ModelRenderer { 22 | public: 23 | ModelRenderer(MeshReader * meshReader); 24 | ~ModelRenderer(); 25 | 26 | /** 27 | Returns the mesh number for a given mesh. If the mesh has not been seen before, a new mesh number is 28 | assigned and the mesh is loaded into memory. 29 | @param meshFilename The name of the file in which the mesh is stored. 30 | @return The unique mesh identifier if successful, otherwise -1. 31 | */ 32 | int getMeshNumber(const std::string meshFilename); 33 | 34 | void renderPrimitive(const GeomType type) const; 35 | 36 | /** 37 | Renders a mesh that is already loaded into memory. 38 | @param meshNum The unique identifier associated with the mesh. 39 | @see getMeshNumber() 40 | */ 41 | void renderMesh(const uint meshNum) const; 42 | 43 | const Mesh & getMesh(const uint meshNum) const; 44 | 45 | private: 46 | 47 | // primitive data 48 | GLuint _primitiveVBOs[NumPrimitives]; 49 | GLuint _primitiveNBOs[NumPrimitives]; 50 | GLuint _primitiveIBOs[NumPrimitives]; 51 | int _nPrimitiveFaces[NumPrimitives]; 52 | 53 | // mesh data 54 | std::map _meshNumbers; 55 | std::vector _meshVBOs; 56 | std::vector _meshNBOs; 57 | std::vector _meshIBOs; 58 | std::vector _nMeshFaces; 59 | std::vector _meshes; 60 | 61 | // mesh loader 62 | MeshReader * _meshReader; 63 | }; 64 | 65 | } 66 | 67 | #endif // MODEL_RENDERER_H 68 | -------------------------------------------------------------------------------- /src/util/ostream_operators.cpp: -------------------------------------------------------------------------------- 1 | #include "ostream_operators.h" 2 | 3 | //namespace dart { 4 | 5 | std::ostream & operator<<(std::ostream & os, const float3 & v) { 6 | os << v.x << ", " << v.y << ", " << v.z; 7 | return os; 8 | } 9 | 10 | std::ostream & operator<<(std::ostream & os, const float4 & v) { 11 | os << v.x << ", " << v.y << ", " << v.z << ", " << v.w; 12 | return os; 13 | } 14 | 15 | std::ostream & operator<<(std::ostream & os, const dart::SE3 & v) { 16 | os << v.r0 << std::endl << v.r1 << std::endl << v.r2; 17 | return os; 18 | } 19 | 20 | std::ostream & operator<<(std::ostream & os, const dart::se3 & v) { 21 | os << v.p[0] << ", " << v.p[1] << ", " << v.p[2] << "; " << v.p[3] << ", " << v.p[4] << ", " << v.p[5]; 22 | return os; 23 | } 24 | 25 | //} // namespace dart 26 | -------------------------------------------------------------------------------- /src/util/ostream_operators.h: -------------------------------------------------------------------------------- 1 | #ifndef OSTREAM_OPERATORS_H 2 | #define OSTREAM_OPERATORS_H 3 | 4 | #include 5 | 6 | #include "geometry/SE3.h" 7 | 8 | //namespace dart { 9 | 10 | std::ostream & operator<<(std::ostream & os, const float3 & v); 11 | 12 | std::ostream & operator<<(std::ostream & os, const float4 & v); 13 | 14 | std::ostream & operator<<(std::ostream & os, const dart::SE3 & v); 15 | 16 | std::ostream & operator<<(std::ostream & os, const dart::se3 & v); 17 | 18 | //} // namespace dart 19 | 20 | #endif // OSTREAM_OPERATORS_H 21 | -------------------------------------------------------------------------------- /src/util/prefix.h: -------------------------------------------------------------------------------- 1 | #ifndef PREFIX_H 2 | #define PREFIX_H 3 | 4 | #include 5 | 6 | #ifdef CUDA_BUILD 7 | #define PREFIX __device__ __host__ 8 | #else 9 | #define PREFIX 10 | #endif // CUDA_BUILD 11 | 12 | #endif // PREFIX_H 13 | -------------------------------------------------------------------------------- /src/util/string_format.cpp: -------------------------------------------------------------------------------- 1 | #include "string_format.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace dart { 7 | 8 | std::string stringFormat(const std::string fmt, ...) { 9 | int size = 100; 10 | std::string str; 11 | va_list ap; 12 | while (1) { 13 | str.resize(size); 14 | va_start(ap, fmt); 15 | int n = vsnprintf((char *)str.c_str(), size, fmt.c_str(), ap); 16 | va_end(ap); 17 | if (n > -1 && n < size) { 18 | str.resize(n); 19 | return str; 20 | } 21 | if (n > -1) 22 | size = n + 1; 23 | else 24 | size *= 2; 25 | } 26 | return str; 27 | } 28 | 29 | } 30 | -------------------------------------------------------------------------------- /src/util/string_format.h: -------------------------------------------------------------------------------- 1 | #ifndef STRING_FORMAT_H 2 | #define STRING_FORMAT_H 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | /** 9 | * @namespace dart 10 | * This function mimics the behavior of sprintf, but returns a std::string rather than printing to a buffer. This can be useful if, for example, the size of the string after the format is unknown a priori. 11 | * @param fmt The format, using the same semantics as the standard printf. 12 | * @return The formatted string. 13 | */ 14 | std::string stringFormat(const std::string fmt, ...); 15 | 16 | } 17 | 18 | #endif // STRING_FORMAT_H 19 | -------------------------------------------------------------------------------- /src/util/vector_type_template.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR_TYPE_TEMPLATE_H 2 | #define VECTOR_TYPE_TEMPLATE_H 3 | 4 | namespace dart { 5 | 6 | template 7 | struct VectorTypeTemplate {}; 8 | 9 | template <> 10 | struct VectorTypeTemplate { 11 | typedef float2 type2; 12 | typedef float3 type3; 13 | typedef float4 type4; 14 | static float2 (*const2)(float,float); 15 | static float3 (*const3)(float,float,float); 16 | static float4 (*const4)(float,float,float,float); 17 | }; 18 | 19 | template <> 20 | struct VectorTypeTemplate { 21 | typedef double2 type2; 22 | typedef double3 type3; 23 | typedef double4 type4; 24 | static double2 (*const2)(double,double); 25 | static double3 (*const3)(double,double,double); 26 | static double4 (*const4)(double,double,double,double); 27 | }; 28 | 29 | } 30 | 31 | #endif // VECTOR_TYPE_TEMPLATE_H 32 | -------------------------------------------------------------------------------- /src/visualization/color_ramps.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/visualization/color_ramps.cpp -------------------------------------------------------------------------------- /src/visualization/color_ramps.h: -------------------------------------------------------------------------------- 1 | #ifndef COLOR_RAMPS_H 2 | #define COLOR_RAMPS_H 3 | 4 | namespace dart { 5 | 6 | void colorRampHeatMap(uchar3 * colored, 7 | const float * vals, 8 | const int width, 9 | const int height, 10 | const float minVal = 0.0f, 11 | const float maxVal = 1.0f); 12 | 13 | void colorRampHeatMap(uchar4 * colored, 14 | const float * vals, 15 | const int width, 16 | const int height, 17 | const float minVal = 0.0f, 18 | const float maxVal = 1.0f); 19 | 20 | void colorRampHeatMapUnsat(uchar3 * colored, 21 | const float * vals, 22 | const int width, 23 | const int height, 24 | const float minVal, 25 | const float maxVal); 26 | 27 | void colorRampHeatMapUnsat(uchar4 * colored, 28 | const float * vals, 29 | const int width, 30 | const int height, 31 | const float minVal, 32 | const float maxVal); 33 | 34 | void colorRampTopographic(uchar4 * colored, 35 | const float * vals, 36 | const int width, 37 | const int height, 38 | const float lineThickness, 39 | const float lineSpacing, 40 | const bool showZeroLevel = true); 41 | 42 | void colorRamp2DGradient(uchar4 * color, 43 | const float2 * grad, 44 | const int width, 45 | const int height, 46 | const bool normalize = true); 47 | 48 | void colorRamp3DGradient(uchar4 * color, 49 | const float3 * grad, 50 | const int width, 51 | const int height, 52 | const bool normalize = true); 53 | 54 | } 55 | 56 | #endif // COLOR_RAMPS_H 57 | -------------------------------------------------------------------------------- /src/visualization/data_association_viz.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/visualization/data_association_viz.cpp -------------------------------------------------------------------------------- /src/visualization/data_association_viz.cu: -------------------------------------------------------------------------------- 1 | #include "data_association_viz.h" 2 | 3 | #include 4 | \ 5 | namespace dart { 6 | 7 | // -=-=-=-=-=-=-=-=-=- kernels -=-=-=-=-=-=-=-=-=- 8 | __global__ void gpu_colorDataAssociation(uchar3 * coloredAssociation, 9 | const int * dataAssociation, 10 | const uchar3 * colors, 11 | const int width, 12 | const int height, 13 | const uchar3 unassociatedColor) { 14 | 15 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 16 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 17 | 18 | if (x >= width || y >= height) { 19 | return; 20 | } 21 | 22 | const int index = x + y*width; 23 | const int association = dataAssociation[index]; 24 | if (association < 0) { 25 | coloredAssociation[index] = unassociatedColor; 26 | } 27 | else { 28 | coloredAssociation[index] = colors[association]; 29 | } 30 | // coloredAssociation[index] = make_uchar3(0,1,0); 31 | 32 | } 33 | 34 | __global__ void gpu_colorDataAssociationMultiModel(uchar3 * coloredAssociation, 35 | const int * dataAssociation, 36 | const uchar3 * * colors, 37 | const int width, 38 | const int height, 39 | const uchar3 unassociatedColor) { 40 | 41 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 42 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 43 | 44 | if (x >= width || y >= height) { 45 | return; 46 | } 47 | 48 | const int index = x + y*width; 49 | if (dataAssociation[index] < 0) { 50 | coloredAssociation[index] = unassociatedColor; 51 | } 52 | else { 53 | const int association = 0xffff & dataAssociation[index]; 54 | const int model = dataAssociation[index] >> 16; 55 | coloredAssociation[index] = colors[model][association]; 56 | } 57 | 58 | } 59 | 60 | // -=-=-=-=-=-=-=-=-=- interface -=-=-=-=-=-=-=-=-=- 61 | void colorDataAssociation(uchar3 * coloredAssociation, 62 | const int * dataAssociation, 63 | const uchar3 * colors, 64 | const int width, 65 | const int height, 66 | const uchar3 unassociatedColor) { 67 | 68 | dim3 block(16,8,1); 69 | dim3 grid( ceil( width / (float)block.x), ceil(height / (float)block.y )); 70 | 71 | gpu_colorDataAssociation<<>>(coloredAssociation,dataAssociation,colors,width,height,unassociatedColor); 72 | } 73 | 74 | void colorDataAssociationMultiModel(uchar3 * coloredAssociation, 75 | const int * dataAssociation, 76 | const uchar3 * * colors, 77 | const int width, 78 | const int height, 79 | const uchar3 unassociatedColor) { 80 | 81 | dim3 block(16,8,1); 82 | dim3 grid( ceil( width / (float)block.x), ceil(height / (float)block.y )); 83 | 84 | gpu_colorDataAssociationMultiModel<<>>(coloredAssociation,dataAssociation,colors,width,height,unassociatedColor); 85 | 86 | } 87 | 88 | } 89 | -------------------------------------------------------------------------------- /src/visualization/data_association_viz.h: -------------------------------------------------------------------------------- 1 | #ifndef DATA_ASSOCIATION_VIZ_H 2 | #define DATA_ASSOCIATION_VIZ_H 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | void colorDataAssociation(uchar3 * coloredAssociation, 9 | const int * dataAssociation, 10 | const uchar3 * colors, 11 | const int width, 12 | const int height, 13 | const uchar3 unassociatedColor = make_uchar3(0,0,0)); 14 | 15 | void colorDataAssociationMultiModel(uchar3 * coloredAssociation, 16 | const int * dataAssociation, 17 | const uchar3 * * colors, 18 | const int width, 19 | const int height, 20 | const uchar3 unassociatedColor = make_uchar3(0,0,0)); 21 | 22 | } 23 | 24 | #endif // DATA_ASSOCIATION_VIZ_H 25 | -------------------------------------------------------------------------------- /src/visualization/gradient_viz.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/visualization/gradient_viz.cpp -------------------------------------------------------------------------------- /src/visualization/gradient_viz.cu: -------------------------------------------------------------------------------- 1 | #include "gradient_viz.h" 2 | 3 | #include 4 | 5 | namespace dart { 6 | 7 | inline __host__ __device__ unsigned char clamp(int c) { 8 | return min(max(0,c),255); 9 | } 10 | 11 | inline __host__ __device__ uchar3 hsv2rgb(float h, float s, float v) { 12 | float c = v*s; 13 | float hPrime = h/60.0f; 14 | float x = c*(1 - fabs(fmodf(hPrime,2) - 1)); 15 | float m = c-v; 16 | int hPrimeInt = hPrime; 17 | switch (hPrimeInt) { 18 | case 0: 19 | return make_uchar3(255*(c+m),255*(x+m),255*(m)); 20 | case 1: 21 | return make_uchar3(255*(x+m),255*(c+m),255*(m)); 22 | case 2: 23 | return make_uchar3(255*(m),255*(c+m),255*(x+m)); 24 | case 3: 25 | return make_uchar3(255*(m),255*(x+m),255*(c+m)); 26 | case 4: 27 | return make_uchar3(255*(x+m),255*(m),255*(c+m)); 28 | case 5: 29 | return make_uchar3(255*(c+m),255*(m),255*(x+m)); 30 | } 31 | return make_uchar3(0,0,0); 32 | } 33 | 34 | // -=-=-=-=-=-=-=-=-=- kernels -=-=-=-=-=-=-=-=-=- 35 | __global__ void gpu_visualizeImageGradient(const float2 * imgGradient, uchar3 * gradientViz, const int width, const int height, const float minMag, const float maxMag) { 36 | 37 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 38 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 39 | 40 | if (x >= width || y >= height) { 41 | return; 42 | } 43 | 44 | const int index = x + y*width; 45 | 46 | float2 grad = imgGradient[index]; 47 | const float angle = atan2(grad.y,grad.x); 48 | const float mag = length(grad); 49 | grad = grad / mag; 50 | 51 | float h = angle * 180 / M_PI; 52 | if (h < 0) { 53 | h += 360; 54 | } 55 | const float v = min(max(0.0f,(mag-minMag)/(maxMag-minMag)),1.0f); 56 | 57 | gradientViz[index] = hsv2rgb(h,1.0,v); 58 | 59 | } 60 | 61 | // -=-=-=-=-=-=-=-=-=- interface -=-=-=-=-=-=-=-=-=- 62 | void visualizeImageGradient(const float2 * imgGradient, uchar3 * gradientViz, const int width, const int height, const float minMag, const float maxMag) { 63 | 64 | dim3 block(16,8,1); 65 | dim3 grid( ceil( width / (float)block.x), ceil(height / (float)block.y )); 66 | 67 | gpu_visualizeImageGradient<<>>(imgGradient,gradientViz,width,height,minMag,maxMag); 68 | 69 | } 70 | 71 | } 72 | -------------------------------------------------------------------------------- /src/visualization/gradient_viz.h: -------------------------------------------------------------------------------- 1 | #ifndef GRADIENT_VIZ_H 2 | #define GRADIENT_VIZ_H 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | void visualizeImageGradient(const float2 * imgGradient, uchar3 * gradientViz, const int width, const int height, const float minMag = 0.0, const float maxMag = 1.0); 9 | 10 | } 11 | 12 | #endif // GRADIENT_VIZ_H 13 | -------------------------------------------------------------------------------- /src/visualization/matrix_viz.cu: -------------------------------------------------------------------------------- 1 | #include "matrix_viz.h" 2 | 3 | #include 4 | 5 | namespace dart { 6 | 7 | // -=-=-=-=-=-=-=-=-=- kernels -=-=-=-=-=-=-=-=-=- 8 | __global__ void gpu_visualizeMatrix( 9 | const float * mxData, 10 | const int mxCols, 11 | const int mxRows, 12 | uchar3 * img, 13 | const int width, 14 | const int height, 15 | const uchar3 zeroColor, 16 | const float minVal, 17 | const float maxVal) { 18 | 19 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 20 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 21 | 22 | if (x >= width || y >= height) { 23 | return; 24 | } 25 | 26 | int mxCol = (float)mxCols*x/width; 27 | int mxRow = (float)mxRows*y/height; 28 | 29 | mxCol = min(max(0,mxCol),mxCols-1); 30 | mxRow = min(max(0,mxRow),mxRows-1); 31 | 32 | // printf("%d,%d\n",mxRow,mxCol); 33 | const float val = mxData[mxRow*mxCols + mxCol]; 34 | //const int val = 0.0f; 35 | 36 | if (val == 0.0f) { 37 | img[x + y*width] = zeroColor; 38 | return; 39 | } 40 | 41 | float a = min(max(0.0f,(val - minVal)/(val - maxVal)),1.0f); 42 | img[x + y*width] = make_uchar3(255*a,255*a,255*a); 43 | 44 | } 45 | 46 | // -=-=-=-=-=-=-=-=-=- interface -=-=-=-=-=-=-=-=-=- 47 | void visualizeMatrix(const float * mxData, 48 | const int mxCols, 49 | const int mxRows, 50 | uchar3 * img, 51 | const int width, 52 | const int height, 53 | const uchar3 zeroColor, 54 | const float minVal, 55 | const float maxVal) { 56 | 57 | dim3 block(16,8,1); 58 | dim3 grid( ceil( width / (float)block.x), ceil(height / (float)block.y )); 59 | 60 | gpu_visualizeMatrix<<>>(mxData,mxCols,mxRows,img,width,height,zeroColor,minVal,maxVal); 61 | 62 | } 63 | 64 | } 65 | -------------------------------------------------------------------------------- /src/visualization/matrix_viz.h: -------------------------------------------------------------------------------- 1 | #ifndef MATRIX_VIZ_H 2 | #define MATRIX_VIZ_H 3 | 4 | #include 5 | 6 | namespace dart { 7 | 8 | void visualizeMatrix(const float * mxData, 9 | const int mxCols, 10 | const int mxRows, 11 | uchar3 * img, 12 | const int width, 13 | const int height, 14 | const uchar3 zeroColor, 15 | const float minVal, 16 | const float maxVal); 17 | 18 | } 19 | 20 | #endif // MATRIX_VIZ_H 21 | -------------------------------------------------------------------------------- /src/visualization/sdf_viz.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tschmidt23/dart/37adc66580a15c5b438986d77621ebd8f5f450c3/src/visualization/sdf_viz.cpp -------------------------------------------------------------------------------- /src/visualization/sdf_viz.h: -------------------------------------------------------------------------------- 1 | #ifndef SDF_VIZ 2 | #define SDF_VIZ 3 | 4 | #include "geometry/grid_3d.h" 5 | #include "geometry/SE3.h" 6 | #include "model/mirrored_model.h" 7 | 8 | namespace dart { 9 | 10 | enum ColorRamp { 11 | ColorRampGrayscale, 12 | ColorRampHeatMap, 13 | ColorRampRedGreen 14 | }; 15 | 16 | void visualizeModelSdfPlane(uchar3 * img, 17 | const int width, 18 | const int height, 19 | const float2 origin, 20 | const float2 size, 21 | const SE3 & T_mc, 22 | const SE3 * T_fms, 23 | const int * sdfFrames, 24 | const Grid3D * sdfs, 25 | const int nSdfs, 26 | const float planeDepth, 27 | const float minVal, 28 | const float maxVal, 29 | const ColorRamp ramp = ColorRampHeatMap); 30 | 31 | void visualizeModelSdfPlaneProjective(uchar3 * img, 32 | const int width, 33 | const int height, 34 | const SE3 & T_mc, 35 | const SE3 * T_fms, 36 | const int * sdfFrames, 37 | const Grid3D * sdfs, 38 | const int nSdfs, 39 | const float planeDepth, 40 | const float focalLength, 41 | const float minVal, 42 | const float maxVal, 43 | const ColorRamp ramp = ColorRampHeatMap); 44 | 45 | void getSdfSlice(float * sdfSlice, 46 | const int width, 47 | const int height, 48 | const float2 origin, 49 | const float2 size, 50 | const SE3 & T_sp, 51 | const Grid3D * deviceSdf); 52 | 53 | void getModelSdfSlice(float * sdfSlice, 54 | const int width, 55 | const int height, 56 | const float2 origin, 57 | const float2 size, 58 | const SE3 & T_pm, 59 | const MirroredModel & model); 60 | 61 | void getMultiModelSdfSlice(float * sdfSlice, 62 | const int width, 63 | const int height, 64 | const float2 origin, 65 | const float2 size, 66 | const std::vector & T_pm, 67 | const std::vector & models); 68 | 69 | void getModelSdfPlaneProjective(float * sdf, 70 | const int width, 71 | const int height, 72 | const SE3 & T_mc, 73 | const SE3 * T_fms, 74 | const int * sdfFrames, 75 | const Grid3D * sdfs, 76 | const int nSdfs, 77 | const float planeDepth, 78 | const float focalLength); 79 | 80 | void getModelSdfGradientPlaneProjective(float3 * grad, 81 | const int width, 82 | const int height, 83 | const SE3 & T_mc, 84 | const SE3 * T_fms, 85 | const int * sdfFrames, 86 | const Grid3D * sdfs, 87 | const int nSdfs, 88 | const float planeDepth, 89 | const float focalLength); 90 | 91 | void getObservationSdfPlane(float * sdfVals, 92 | const int width, 93 | const int height, 94 | const Grid3D * sdf, 95 | const float planeDepth); 96 | 97 | void getObservationSdfPlaneProjective(float * sdfVals, 98 | const int width, 99 | const int height, 100 | const Grid3D * sdf, 101 | const float planeDepth, 102 | const float focalLength); 103 | 104 | void visualizeDataAssociationPlane(uchar3 * img, 105 | const int width, 106 | const int height, 107 | const float2 origin, 108 | const float2 size, 109 | const SE3 & T_mc, 110 | const SE3 * T_fms, 111 | const int * sdfFrames, 112 | const Grid3D * sdfs, 113 | const int nSdfs, 114 | const uchar3 * sdfColors, 115 | const float planeDepth); 116 | 117 | void visualizeDataAssociationPlaneProjective(uchar3 * img, 118 | const int width, 119 | const int height, 120 | const SE3 & T_mc, 121 | const SE3 * T_fms, 122 | const int * sdfFrames, 123 | const Grid3D * sdfs, 124 | const int nSdfs, 125 | const uchar3 * sdfColors, 126 | const float planeDepth, 127 | const float focalLength); 128 | 129 | void visualizeDataAssociationPlaneProjective(uchar4 * img, 130 | const int width, 131 | const int height, 132 | const SE3 & T_mc, 133 | const SE3 * T_fms, 134 | const int * sdfFrames, 135 | const Grid3D * sdfs, 136 | const int nSdfs, 137 | const uchar3 * sdfColors, 138 | const float planeDepth, 139 | const float focalLength); 140 | 141 | } 142 | 143 | #endif // SDF_VIZ 144 | -------------------------------------------------------------------------------- /test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include 4 | #include 5 | 6 | int main(int argc, char * * argv) { 7 | ::testing::InitGoogleTest(&argc, argv); 8 | 9 | glutInit(&argc,argv); 10 | glutCreateWindow("useless"); 11 | 12 | glewInit(); 13 | 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /test/test_mirrored_model.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "mesh/assimp_mesh_reader.h" 4 | #include "model/mirrored_model.h" 5 | #include "util/dart_io.h" 6 | 7 | namespace { 8 | 9 | TEST(TestMirroredModel,TestMirredModelConstructorDestructor) { 10 | 11 | std::vector testModels; 12 | testModels.push_back("../models/leftHand/leftHand.xml"); 13 | 14 | cudaError_t err = cudaGetLastError(); 15 | 16 | dart::Model::initializeRenderer(new dart::AssimpMeshReader()); 17 | 18 | 19 | for (int m=0; m 4 | #include 5 | 6 | #include "model/host_only_model.h" 7 | #include "util/dart_io.h" 8 | #include "mesh/assimp_mesh_reader.h" 9 | 10 | namespace { 11 | 12 | TEST(TestModelJacobian,TestModelArticulationJacobian) { 13 | 14 | const float dPose = 1e-3; 15 | const float magTolerance = 1e-4; 16 | const int nPoses = 20; 17 | 18 | std::vector testModels; 19 | testModels.push_back("../models/leftHand/leftHand.xml"); 20 | 21 | for (int m=0; m testPoints_f; 31 | testPoints_f.push_back(make_float4(0,0,0,1)); 32 | testPoints_f.push_back(make_float4(0.1,0.1,0.1,1)); 33 | 34 | for (int n=0; n J3Danalytic; 45 | float tmpPose[dims]; 46 | 47 | for (int frame=0; frame < model.getNumFrames(); ++frame) { 48 | for (int p=0; p testModels; 91 | testModels.push_back("../models/leftHand.xml"); 92 | 93 | for (int m=0; m testPoints_f; 102 | testPoints_f.push_back(make_float4(0,0,0,1)); 103 | testPoints_f.push_back(make_float4(0.1,0.1,0.1,1)); 104 | 105 | std::vector mins(dims); 106 | std::vector maxs(dims); 107 | std::vector names(dims); 108 | dart::Pose pose(new dart::NullReduction(dims,mins.data(),maxs.data(),names.data())); 109 | 110 | for (int n=0; n J3Danalytic; 128 | 129 | for (int frame=0; frame < model.getNumFrames(); ++frame) { 130 | for (int p=0; p 4 | 5 | #include "mesh/assimp_mesh_reader.h" 6 | #include "optimization/kernels/obsToMod.h" 7 | #include "model/host_only_model.h" 8 | #include "model/mirrored_model.h" 9 | #include "util/ostream_operators.h" 10 | #include "util/string_format.h" 11 | 12 | namespace { 13 | 14 | TEST(TestObsToModKernels,TestErrorAndDataAssociationObsToMod) { 15 | 16 | dart::Model::initializeRenderer(new dart::AssimpMeshReader()); 17 | 18 | dart::HostOnlyModel hostModel; 19 | const float sphereRadius = 0.1; 20 | const float sdfResolution = 0.005; 21 | const std::string sphereRadiusStr = dart::stringFormat("%f",sphereRadius); 22 | hostModel.addGeometry(0,dart::PrimitiveSphereType,sphereRadiusStr,sphereRadiusStr,sphereRadiusStr,"0","0","0","0","0","0",255,255,255); 23 | 24 | hostModel.computeStructure(); 25 | hostModel.voxelize2(sdfResolution,0.1); 26 | 27 | dart::MirroredModel model(hostModel,make_uint3(64,64,64),0.01); 28 | 29 | ASSERT_EQ(hostModel.getPoseDimensionality(),6); 30 | 31 | // memory setup 32 | const int obsWidth = 10; 33 | const int obsHeight = 10; 34 | dart::MirroredVector obsVertMap(obsWidth*obsHeight); 35 | dart::MirroredVector obsNormMap(obsWidth*obsHeight); 36 | dart::OptimizationOptions opts; 37 | dart::MirroredVector associatedPoints(obsWidth*obsHeight); 38 | dart::MirroredVector lastElement(1); 39 | dart::MirroredVector debugDataAssociation(obsWidth*obsHeight); 40 | dart::MirroredVector debugError(obsWidth*obsHeight); 41 | 42 | std::default_random_engine generator; 43 | std::bernoulli_distribution validDistribution(0.8); 44 | std::normal_distribution normalDistribution(0,0.1); 45 | 46 | // fill observation map 47 | for (int i=0; i opts.distThreshold[0]) { 95 | EXPECT_EQ(-1,debugDataAssociation[i]); 96 | EXPECT_TRUE(std::isnan(debugError[i])); 97 | } else { 98 | EXPECT_EQ( 0,debugDataAssociation[i]); 99 | EXPECT_NEAR( dist, debugError[i], sdfResolution / 8 ); 100 | } 101 | } 102 | } 103 | 104 | 105 | dart::Model::shutdownRenderer(); 106 | 107 | delete reduction; 108 | 109 | } 110 | 111 | 112 | } // namespace 113 | 114 | -------------------------------------------------------------------------------- /test/test_pose_reduction.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "pose/pose.h" 4 | #include "util/string_format.h" 5 | 6 | namespace { 7 | 8 | TEST(TestPoseReduction,TestNullPoseReduction) { 9 | 10 | const int nDims = 10; 11 | const int nArticulatedDims = nDims-6; 12 | 13 | std::vector mins(nArticulatedDims); 14 | std::vector maxs(nArticulatedDims); 15 | std::vector names(nArticulatedDims); 16 | for (int i=0; i 4 | 5 | #include "model/host_only_model.h" 6 | #include "model/mirrored_model.h" 7 | #include "util/string_format.h" 8 | 9 | namespace { 10 | 11 | TEST(TestVoxelize,TestSphereVoxelize) { 12 | 13 | dart::Model::initializeRenderer(); 14 | 15 | dart::HostOnlyModel hostModel; 16 | const float sphereRadius = 0.1; 17 | const float sdfResolution = 0.005; 18 | const std::string sphereRadiusStr = dart::stringFormat("%f",sphereRadius); 19 | hostModel.addGeometry(0,dart::PrimitiveSphereType,sphereRadiusStr,sphereRadiusStr,sphereRadiusStr,"0","0","0","0","0","0",255,255,255); 20 | 21 | hostModel.computeStructure(); 22 | hostModel.voxelize2(sdfResolution,0.1); 23 | 24 | dart::MirroredModel model(hostModel,make_uint3(64,64,64),0.01); 25 | 26 | ASSERT_EQ(6,hostModel.getPoseDimensionality()); 27 | ASSERT_EQ(6,model.getPoseDimensionality()); 28 | 29 | ASSERT_EQ(1,model.getNumSdfs()); 30 | const dart::Grid3D & sdf = model.getSdf(0); 31 | 32 | ASSERT_EQ(sdfResolution,sdf.resolution); 33 | 34 | for (int z=0; z