├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── gpfactor.h ├── gpfactor ├── CMakeLists.txt └── gp │ ├── CMakeLists.txt │ └── GaussianProcessSpatialPrior.h ├── matlab ├── shape-map │ ├── Map2DGPGraph.m │ ├── MapGelSight.m │ ├── MapGelSightReal.m │ ├── Viz2DGPGraph.m │ ├── Viz3DGPGraph.m │ ├── Viz3DGPGraphReal.m │ ├── mapYCB.m │ ├── optimizeGraphBatch.m │ ├── optimizeGraphIncremental.m │ └── vizHeightMapsTest.m └── utils │ ├── ScaleAndOffset.m │ ├── addNoise.m │ ├── cleanUp.m │ ├── plotPointsAndNormals3D.m │ ├── slice3D.m │ ├── splitFV.m │ ├── stlread.m │ └── stlwrite.m └── media ├── cover.jpg ├── robotouch.png ├── rpl.png └── shape-map.gif /.gitignore: -------------------------------------------------------------------------------- 1 | ### folders ### 2 | dev/* 3 | env/* 4 | results/* 5 | mex/build/ 6 | .vscode/* 7 | .ipynb_checkpoints 8 | data/ 9 | build/ 10 | 11 | ### dev chosen ### 12 | # *.gif 13 | # *.mat 14 | *.zip 15 | *.pyc 16 | *.jpeg 17 | *.png 18 | *.pdf 19 | 20 | ### Git ### 21 | *.orig 22 | 23 | ### Linux ### 24 | *~ 25 | 26 | # temporary files which can be created if a process still has a handle open of a deleted file 27 | .fuse_hidden* 28 | 29 | # KDE directory preferences 30 | .directory 31 | 32 | # Linux trash folder which might appear on any partition or disk 33 | .Trash-* 34 | 35 | # .nfs files are created when an open file is removed but is still being accessed 36 | .nfs* 37 | 38 | ### macOS ### 39 | *.DS_Store 40 | .AppleDouble 41 | .LSOverride 42 | 43 | # Icon must end with two \r 44 | Icon 45 | 46 | # Thumbnails 47 | ._* 48 | 49 | # Files that might appear in the root of a volume 50 | .DocumentRevisions-V100 51 | .fseventsd 52 | .Spotlight-V100 53 | .TemporaryItems 54 | .Trashes 55 | .VolumeIcon.icns 56 | .com.apple.timemachine.donotpresent 57 | 58 | # Directories potentially created on remote AFP share 59 | .AppleDB 60 | .AppleDesktop 61 | Network Trash Folder 62 | Temporary Items 63 | .apdisk 64 | 65 | ### Matlab ### 66 | ##--------------------------------------------------- 67 | ## Remove autosaves generated by the Matlab editor 68 | ## We have git for backups! 69 | ##--------------------------------------------------- 70 | 71 | # Windows default autosave extension 72 | *.asv 73 | 74 | # OSX / *nix default autosave extension 75 | *.m~ 76 | 77 | # Compiled MEX binaries (all platforms) 78 | *.mex* 79 | 80 | # Simulink Code Generation 81 | slprj/ 82 | 83 | # Session info 84 | octave-workspace 85 | 86 | # Simulink autosave extension 87 | *.autosave 88 | 89 | ### Windows ### 90 | # Windows thumbnail cache files 91 | Thumbs.db 92 | ehthumbs.db 93 | ehthumbs_vista.db 94 | 95 | # Folder config file 96 | Desktop.ini 97 | 98 | # Recycle Bin used on file shares 99 | $RECYCLE.BIN/ 100 | 101 | # Windows Installer files 102 | *.cab 103 | *.msi 104 | *.msm 105 | *.msp 106 | 107 | # Windows shortcuts 108 | *.lnk 109 | 110 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # cmake structure for GTSAM + MATLAB inspired from: https://github.com/gtrll/gpslam/blob/master/CMakeLists.txt 2 | 3 | cmake_minimum_required(VERSION 3.0) 4 | project(gpfactor CXX C) 5 | 6 | set(GPFACTOR_VERSION_MAJOR 1) 7 | set(GPFACTOR_VERSION_MINOR 0) 8 | set(GPFACTOR_VERSION_PATCH 0) 9 | set(gpfactor_VERSION_STRING "${GPSLAM_VERSION_MAJOR}.${GPSLAM_VERSION_MINOR}.${GPSLAM_VERSION_PATCH}") 10 | 11 | # GTSAM-specific 12 | find_package(GTSAM REQUIRED) 13 | include_directories(${GTSAM_INCLUDE_DIR}) 14 | set(GTSAM_LIBS gtsam) 15 | 16 | find_package(GTSAMCMakeTools) 17 | include(GtsamMakeConfigFile) 18 | include(GtsamBuildTypes) 19 | include(GtsamTesting) 20 | 21 | option(GPFACTOR_BUILD_TOOLBOX "Wrap and install matlab toolbox?" OFF) 22 | set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools") 23 | 24 | # Boost-specific 25 | find_package(Boost 1.46 REQUIRED) 26 | find_package(Boost COMPONENTS system REQUIRED) 27 | find_package(Boost COMPONENTS filesystem REQUIRED) 28 | find_package(Boost COMPONENTS serialization REQUIRED) 29 | find_package(Boost COMPONENTS thread REQUIRED) 30 | include_directories(${Boost_INCLUDE_DIR}) 31 | 32 | include_directories(BEFORE ${CMAKE_CURRENT_SOURCE_DIR}) 33 | add_subdirectory(gpfactor) 34 | 35 | # MATLAB wrap 36 | if(GPFACTOR_BUILD_TOOLBOX) 37 | include(GtsamMatlabWrap) 38 | include_directories("/home/suddhu/packages/MATLAB/extern/include") 39 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 40 | wrap_and_install_library(gpfactor.h ${PROJECT_NAME} "${CMAKE_CURRENT_SOURCE_DIR}" "") 41 | endif() 42 | 43 | GtsamMakeConfigFile(gpfactor) 44 | export(TARGETS ${ggpfactor_EXPORTED_TARGETS} FILE gpfactor-exports.cmake) 45 | 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The X11 License (X11) 2 | 3 | Copyright (c) 2021 Carnegie Mellon University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | Except as contained in this notice, the name(s) of the above copyright holders shall not be used in advertising or otherwise to promote the sale, use or other dealings in this Software without prior written authorization. 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Shape map 3-D: Efficient shape mapping through dense touch and vision 2 | 3 | [![License: X11](https://img.shields.io/badge/license-X11-yellowgreen)](https://github.com/rpl-cmu/shape-map-3D/blob/master/LICENSE)   RPL-logo    Robotouch-logo 4 | 5 | ![cover](/media/cover.jpg)     ![shape-map](/media/shape-map.gif) 6 | 7 | Base library for visuo-tactile shape mapping with gelsight and depth camera. For more information, consider our work [Efficient shape mapping through dense touch and vision](https://www.cs.cmu.edu/~sudhars1/shape-map/). 8 | 9 | ## Roadmap 10 | The library is still being actively updated, please check back soon for a stable version! 11 | - [x] Initial code upload 12 | - [ ] Data upload 13 | - [ ] Compilation fixes 14 | - [ ] Enhanced documentation 15 | 16 | ## Folder structure 17 | - `matlab/`: Executable and utility scripts for mapping 18 | - `shape-map/`: 3-D and 2-D mapping scripts for both simulated and real-world data 19 | - `utils/`: helpers scripts 20 | - `gpfactor/`: cpp header wrapped with the gtsam + MATLAB library 21 | 22 | ## Requirements 23 | - [gtsam](https://github.com/borglab/gtsam) 24 | - [MATLAB](https://www.mathworks.com/products/matlab.html) 25 | 26 | ## Other dependencies 27 | - [spatialmath-matlab](https://github.com/petercorke/spatialmath-matlab) 28 | 29 | ## Third-party scripts 30 | - [splitFV](https://www.mathworks.com/matlabcentral/fileexchange/27667-splitfv-split-a-mesh) 31 | - [stlwrite](https://www.mathworks.com/matlabcentral/fileexchange/20922-stlwrite-write-ascii-or-binary-stl-files) 32 | 33 | ## Compilation 34 | - **GTSAM**: Checkout gtsam 4.0.2 `master` branch (not `develop`): 35 | - Using the default CMakeLists without modification 36 | ``` 37 | cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WITH_TBB=OFF -DGTSAM_INSTALL_MATLAB_TOOLBOX=ON .. 38 | sudo make install -j 39 | ``` 40 | - MATLAB toolbox check 41 | - Open MATLAB 42 | - add `addpath /usr/local/gtsam_toolbox` to the `startup.m` 43 | - Test `gtsamExamples` 44 | - C++ header compilation: 45 | ``` 46 | mkdir build 47 | cd build 48 | cmake -DGPFACTOR_BUILD_TOOLBOX:OPTION=ON -DCMAKE_BUILD_TYPE=Release -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=/usr/local/gtsam_toolbox .. 49 | sudo make install 50 | ``` 51 | - `sudo ldconfig` in terminal 52 | - open MATLAB here 53 | - Run `.m` files 54 | 55 | ## Citation 56 | Feel free to use the library as you please. If you find it helpful, please consider referencing: 57 | 58 | ```BibTeX 59 | @misc{suresh2021efficient, 60 | title={Efficient shape mapping through dense touch and vision}, 61 | author={Sudharshan Suresh and Zilin Si and Joshua G. Mangelson and Wenzhen Yuan and Michael Kaess}, 62 | year={2021}, 63 | eprint={2109.09884}, 64 | archivePrefix={arXiv}, 65 | primaryClass={cs.RO} 66 | } 67 | ``` 68 | -------------------------------------------------------------------------------- /gpfactor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gpfactor.h 3 | * @brief Wrapper to build cpp header with MATLAB 4 | * @author Sudharshan Suresh 5 | * @date May 2021 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 21 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | class gtsam::Vector2; class gtsam::Vector3; 30 | class gtsam::Rot2; class gtsam::Rot3; 31 | class gtsam::Pose2; class gtsam::Pose3; 32 | 33 | class gtsam::Matrix3; class gtsam::Matrix4; 34 | 35 | class gtsam::GaussianFactorGraph; class gtsam::Values; 36 | 37 | virtual class gtsam::noiseModel::Base; 38 | virtual class gtsam::NonlinearFactor; 39 | virtual class gtsam::NonlinearFactorGraph; 40 | virtual class gtsam::NumericalDerivative; 41 | virtual class gtsam::NoiseModelFactor; 42 | 43 | namespace gpfactor { 44 | #include 45 | 46 | // 2-D and 3-D GP-SG 47 | template 48 | virtual class GaussianProcessSpatialPrior : gtsam::NoiseModelFactor { 49 | GaussianProcessSpatialPrior(size_t yKey, const Matrix& xm, const Matrix& xq, 50 | const Vector& ym, const Vector& theta, const Vector& measurementNoise); 51 | }; 52 | } 53 | 54 | -------------------------------------------------------------------------------- /gpfactor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(gpfactor_dirs gp) 2 | set(gpfactor_srcs) 3 | 4 | # Loop over only gp folder 5 | foreach(subdir ${gpfactor_dirs}) 6 | file(GLOB subdir_srcs "${subdir}/*.cpp" "${subdir}/*.h") 7 | list(APPEND gpfactor_srcs ${subdir_srcs}) 8 | message(STATUS "Building Module: ${subdir_srcs}") 9 | add_subdirectory(${subdir}) 10 | endforeach(subdir) 11 | 12 | add_library(${PROJECT_NAME} SHARED ${gpfactor_srcs}) 13 | set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) 14 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${GTSAM_LIBS}) 15 | install(TARGETS ${PROJECT_NAME} EXPORT gpfactor-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) 16 | 17 | 18 | -------------------------------------------------------------------------------- /gpfactor/gp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Install headers 2 | file(GLOB gp_headers "*.h") 3 | install(FILES ${gp_headers} DESTINATION include/gpfactor/gp) 4 | -------------------------------------------------------------------------------- /gpfactor/gp/GaussianProcessSpatialPrior.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GaussianProcessSpatialPrior.h 3 | * @brief Unary Gaussian potential for surface measurements in spatial graph 4 | * @author Sudharshan Suresh 5 | * @date May 2021 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 21 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace gpfactor { 40 | 41 | /** Spatial GP prior (2D or 3D) */ 42 | template 43 | class GaussianProcessSpatialPrior: public gtsam::NoiseModelFactor1 > { 44 | 45 | private: 46 | 47 | typedef Eigen::Matrix YDim; 48 | typedef GaussianProcessSpatialPrior This; 49 | typedef gtsam::NoiseModelFactor1 Base; 50 | double kernType_; // 0 - thinplate, 1 - RBF 51 | Eigen::Vector2d kernParams_; 52 | YDim ypred_; 53 | 54 | public: 55 | 56 | // constructor and destructor 57 | GaussianProcessSpatialPrior() {} 58 | GaussianProcessSpatialPrior(gtsam::Key yKey, const gtsam::Matrix& xq, const gtsam::Matrix& xm, 59 | const gtsam::Vector& ym, const gtsam::Vector& theta, const gtsam::Vector& measurementNoise) : 60 | Base(gtsam::noiseModel::Gaussian::Covariance(getMeanAndReturnVariance(xq, xm, ym, theta, measurementNoise)), yKey) {} 61 | virtual ~GaussianProcessSpatialPrior() {} 62 | 63 | // Deep copy 64 | virtual gtsam::NonlinearFactor::shared_ptr clone() const { 65 | return boost::static_pointer_cast( 66 | gtsam::NonlinearFactor::shared_ptr(new This(*this))); } 67 | 68 | /// Error function 69 | gtsam::Vector evaluateError(const YDim& y, boost::optional H1 = boost::none) const { 70 | auto err = [&] (const YDim& yq) 71 | { 72 | return (ypred_ - yq); 73 | }; 74 | // Jacobians 75 | // if (H1) *H1 = gtsam::numericalDerivative11(err, y); 76 | if (H1) *H1 = -Eigen::Matrix::Identity(); 77 | return err(y); 78 | } 79 | size_t dim() const { return Dim; } 80 | 81 | // print 82 | void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { 83 | std::cout << s << "Gaussian Process Spatial Prior<" << Dim << ">" << std::endl; 84 | Base::print("", keyFormatter); 85 | } 86 | 87 | private: 88 | // get GP mean and covariance from measurements 89 | gtsam::Matrix getMeanAndReturnVariance(const gtsam::Matrix& xq, const gtsam::Matrix& xm, const gtsam::Vector& ym, const gtsam::Vector& theta, const gtsam::Vector& measurementNoise) { 90 | kernParams_ << theta(1), theta(2); 91 | kernType_ = theta(0); 92 | gtsam::Matrix mN = measurementNoise.asDiagonal(); 93 | gtsam::Matrix Kmm = kernel(xm, xm) + mN; 94 | gtsam::Matrix Kqm = kernel(xq, xm); 95 | gtsam::Matrix Kqq = kernel(xq, xq); 96 | 97 | gtsam::Matrix Kmm_L = Eigen::LLT(Kmm).matrixL(); // cholesky with PSD 98 | gtsam::Matrix x = Kmm_L.triangularView().solve(ym); 99 | 100 | // compute mean 101 | gtsam::Matrix Kmm_alpha = (Kmm_L.triangularView().transpose()).solve(x); 102 | ypred_ = Kqm * Kmm_alpha; 103 | 104 | // compute variance 105 | gtsam::Matrix V = Kmm_L.triangularView().solve(Kqm.transpose()); 106 | gtsam::Matrix C = Kqq - V.transpose()*V; 107 | return C; 108 | } 109 | 110 | // Get the Kxstarxstar kernel (3, 3) 111 | gtsam::Matrix kernel(const gtsam::Matrix& x1, const gtsam::Matrix& x2) const{ 112 | int n = static_cast(x1.rows()); 113 | int m = static_cast(x2.rows()); 114 | Eigen::MatrixXd K = Eigen::MatrixXd::Zero((Dim + 1)*n,(Dim + 1)*m); 115 | for (int i = 0; i < n; i++) { 116 | for (int j = 0; j < m; j++) { 117 | if (kernType_ == 0) K.block<(Dim + 1),(Dim + 1)>((Dim + 1)*i, (Dim + 1)*j) = kernelBlockThinPlate(x1.row(i), x2.row(j)); 118 | else if (kernType_ == 1) K.block<(Dim + 1),(Dim + 1)>((Dim + 1)*i, (Dim + 1)*j) = kernelBlockGaussian(x1.row(i), x2.row(j)); 119 | } 120 | } 121 | return K; 122 | } 123 | 124 | // individual kernel elements: thinplate 125 | gtsam::Matrix kernelBlockThinPlate(const gtsam::Matrix& x1, const gtsam::Matrix& x2) const { 126 | Eigen::MatrixXd block = Eigen::MatrixXd::Zero(Dim + 1, Dim + 1); 127 | double xyNorm = Norm(x1, x2); 128 | double R = kernParams_(0); 129 | double sigma = kernParams_(1); 130 | 131 | if (xyNorm > 1e-4) { 132 | // x1 != x2 133 | block(0,0) = 2.0 * std::pow(xyNorm, 3) - 3.0 * R * std::pow(xyNorm, 2) + std::pow(R, 3); // cov(di, dj) 134 | block(0,1) = 2.0 * 3.0 * xyNorm * (-x1(0) + x2(0)) - 3.0 * R * 2.0 * (-x1(0) + x2(0)); // cov(di, w1j) 135 | block(0,2) = 2.0 * 3.0 * xyNorm * (-x1(1) + x2(1)) - 3.0 * R * 2.0 * (-x1(1) + x2(1)); // cov(di, w2j) 136 | 137 | block(1,0) = -block(0,1); // cov(w1i, dj) 138 | block(1,1) = 2.0 * 3.0 * ((-x1(0) + x2(0))/xyNorm) * (x1(0) - x2(0)) + 2.0 * 3.0 * xyNorm * (-1.0) - 3.0 * R * 2.0 * (-1.0); // cov(w1i, w1j) 139 | block(1,2) = 2.0 * 3.0 * ((-x1(1) + x2(1))/xyNorm) * (x1(0) - x2(0)); // cov(w1i, w2j) 140 | 141 | block(2,0) = -block(0,2); // cov(w2i, dj) 142 | block(2,1) = block(1,2); // cov(w2i, w1j) 143 | block(2,2) = 2.0 * 3.0 * ((-x1(1) + x2(1))/xyNorm) * (x1(1) - x2(1)) + 2.0 * 3.0 * xyNorm * (-1.0) - 3.0 * R * 2.0 * (-1.0); // cov(w2i, w2j) 144 | 145 | // 3d data 146 | if (Dim == 3) { 147 | block(0,3) = 2.0 * 3.0 * xyNorm * (-x1(2) + x2(2)) - 3.0 * R * 2.0 * (-x1(2) + x2(2)); // cov(di, w3j) 148 | block(1,3) = 2.0 * 3.0 * ((-x1(2) + x2(2))/xyNorm) * (x1(0) - x2(0)); // cov(w1i, w3j) 149 | block(2,3) = 2.0 * 3.0 * ((-x1(2) + x2(2))/xyNorm) * (x1(1) - x2(1)); // cov(w2i, w3j) 150 | 151 | block(3,0) = -block(0,3); // cov(w3i, dj) 152 | block(3,1) = block(1,3); // cov(w3i, w1j) 153 | block(3,2) = block(2,3); // cov(w3i, w2j) 154 | block(3,3) = 2.0 * 3.0 * ((-x1(2) + x2(2))/xyNorm) * (x1(2) - x2(2)) + 2.0 * 3.0 * xyNorm * (-1.0) - 3.0 * R * 2.0 * (-1.0); // cov(w3i, w3j) 155 | } 156 | } else { 157 | // x1 == x2 diag 158 | block(0,0) = 2.0 * std::pow(xyNorm, 3) - 3.0 * R * std::pow(xyNorm, 2) + std::pow(R, 3); 159 | block(1,1) = 6.0 * R; 160 | block(2,2) = 6.0 * R; 161 | if (Dim == 3) { 162 | block(3,3) = 6.0 * R; 163 | } 164 | } 165 | return std::pow(sigma, 2) * block; 166 | } 167 | 168 | // individual kernel elements: SE kernel (BUGGY, not tested) 169 | gtsam::Matrix kernelBlockGaussian(const gtsam::Matrix& x1, const gtsam::Matrix& x2) const{ 170 | Eigen::MatrixXd block = Eigen::MatrixXd::Zero(3,3); 171 | double l2_inv = 1.0 / std::pow(kernParams_(0), 2); 172 | double sigma = kernParams_(1); 173 | 174 | double diffX = x1(0) - x2(0); 175 | double diffY = x1(1) - x2(1); 176 | 177 | block(0,0) = 1.0; 178 | 179 | block(1,1) = l2_inv * (1 - std::pow(diffX,2) * l2_inv); 180 | block(2,2) = l2_inv * (1 - std::pow(diffY,2) * l2_inv); 181 | block(1,2) = -std::pow(l2_inv,2) * diffX * diffY; 182 | 183 | block(0,1) = l2_inv * diffX; 184 | block(0,2) = l2_inv * diffY; 185 | 186 | block(1,0) = -block(0,1); 187 | block(2,0) = -block(0,2); 188 | block(2,1) = block(1,2); 189 | 190 | block *= std::pow(sigma, 2) * std::exp(-0.5 * Norm(x1, x2) * l2_inv); 191 | return block; 192 | } 193 | 194 | // Euclidean distance norm Norm(x - y) 195 | double Norm(const gtsam::Matrix& x1, const gtsam::Matrix& x2) const { 196 | if (Dim == 3) { 197 | return std::sqrt(std::pow(x1(0) - x2(0), 2) + std::pow(x1(1) - x2(1), 2) + std::pow(x1(2) - x2(2), 2)); 198 | } else { 199 | return std::sqrt(std::pow(x1(0) - x2(0), 2) + std::pow(x1(1) - x2(1), 2)); 200 | } 201 | } 202 | 203 | // serialization 204 | friend class boost::serialization::access; 205 | template 206 | void serialize(ARCHIVE & ar, const unsigned int version) { 207 | ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); 208 | ar & BOOST_SERIALIZATION_NVP(kernType_); 209 | ar & BOOST_SERIALIZATION_NVP(kernParams_); 210 | ar & BOOST_SERIALIZATION_NVP(ypred_); 211 | } 212 | 213 | }; // GaussianProcessSpatialPrior 214 | 215 | } // namespace gpfactor 216 | 217 | /// traits 218 | namespace gtsam { 219 | template 220 | struct traits > : public Testable< 221 | gpfactor::GaussianProcessSpatialPrior > {}; 222 | } 223 | -------------------------------------------------------------------------------- /matlab/shape-map/Map2DGPGraph.m: -------------------------------------------------------------------------------- 1 | %% 2D GPIS via GP-SG with 2-D surface and normal measurements 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function Map2DGPGraph(varargin) 28 | close all; 29 | gpisRootPath = getenv('GPIS_PATH'); 30 | addpath('/usr/local/lib'); 31 | addpath(fullfile(gpisRootPath, 'matlab', 'standalone','utils')) 32 | 33 | %% hyperparameters 34 | params = inputParser; 35 | addParameter(params,'debugFlag',true); 36 | addParameter(params,'MeshN',30); 37 | addParameter(params,'dim',2); 38 | addParameter(params,'kernelType','thinplate'); 39 | addParameter(params,'measNoise',[1e-2, 1e-2]); 40 | addParameter(params,'inputNoise',[0, 0]); 41 | addParameter(params,'depthNoise',[5e-4, 1e-3, 1e-3]); 42 | addParameter(params,'compact',true); 43 | addParameter(params,'compRadius',0.3); 44 | addParameter(params,'training','batch'); 45 | addParameter(params,'shape','bunny'); 46 | addParameter(params,'fileID','20210331-2217'); 47 | parse(params, varargin{:}); 48 | params = params.Results; 49 | 50 | % load data and ground truth (x, y, nx, ny) 51 | dataPath = strcat(fullfile(gpisRootPath, 'data', 'contacts'), '/contacts-', params.shape,'-', params.fileID, '.txt'); 52 | shapePath = strcat(fullfile(gpisRootPath, 'data', 'shapes', params.shape, params.shape), '.mat'); 53 | params.shape_data = struct2array(load(shapePath)); 54 | 55 | data = importdata(dataPath); 56 | data = data(randperm(size(data, 1)), :); % shuffle data 57 | X = data(:,1:2); Y = normr(data(:,3:4)); 58 | 59 | % scale [0, 1] and normalize data 60 | params.shape_data = ScaleAndOffset(params.shape_data); 61 | X = ScaleAndOffset(X); 62 | Y = normr(Y); 63 | 64 | % add noise after offset and normalize 65 | [X, Y] = addNoise(X, Y, params.measNoise, params.inputNoise); 66 | 67 | % load ground-truth 68 | if strcmp(params.kernelType, "thinplate") 69 | params.theta = [0, max(pdist(X,'euclidean')), 1]; 70 | elseif strcmp(params.kernelType, "rbf") 71 | params.theta = [1, 0.25, 0.01]; 72 | end 73 | 74 | fprintf('Training data points: %d, Query points: %d\n', size(X, 1), params.MeshN^2); 75 | 76 | % generate 2D mesh query points 77 | params.meshLim = [-0.1 1.1; -0.1 1.1]; 78 | Grid = {linspace(params.meshLim(1, 1), params.meshLim(1, 2), params.MeshN),... 79 | linspace(params.meshLim(2, 1), params.meshLim(2, 2), params.MeshN)}; 80 | [xt, yt] = meshgrid(Grid{1}, Grid{2}); 81 | 82 | % visualize measurements 83 | viz = Viz2DGPGraph(params); 84 | viz.groundTruthModel(); 85 | drawnow; 86 | 87 | params 88 | 89 | if strcmp(params.training, 'batch') 90 | opt = optimizeGraphBatch(params); 91 | else 92 | opt = optimizeGraphIncremental(params); 93 | end 94 | 95 | allNodeEdges = []; 96 | if strcmp(params.training, 'batch') % run batch 97 | %% update with all measurements 98 | tic; 99 | opt.Update(X, Y); 100 | updateTime = toc; 101 | 102 | %% query grid 103 | tic; 104 | [fMean, fVar] = opt.Query(); 105 | queryTime = toc; 106 | fprintf('Update time: %.04f secs, Query time: %.04f secs\n', updateTime, queryTime); 107 | 108 | %% get level-set and delete outliers 109 | C = contourc(Grid{1},Grid{2},fMean, [0 0]); 110 | thresh = 1; idx = any(C > thresh,1); C(:,idx) = []; 111 | else % run incremental 112 | for i = 1:size(X, 1) 113 | x = X(i, :); y = Y(i, :); 114 | %% update with new measurement 115 | tic; 116 | nodeEdges = opt.Update(x, y); 117 | allNodeEdges = [allNodeEdges; nodeEdges]; 118 | updateTime = toc; 119 | 120 | %% query grid 121 | tic; 122 | [fMean, fVar] = opt.Query(); 123 | queryTime = toc; 124 | fprintf('#%.d Update time: %.04f secs, Query time: %.04f secs\n', i, updateTime, queryTime); 125 | 126 | %% get level-set and delete outliers 127 | C = contourc(Grid{1},Grid{2},fMean, [0 0]); 128 | thresh = 1; idx = any(C > thresh,1); C(:,idx) = []; 129 | 130 | %% plot results 131 | % viz.measurements(X(1:i, :), Y(1:i, :)); 132 | % viz.levelSet(C, {xt, yt}, fMean, fVar, false); 133 | % viz.SDFMean(C, {xt, yt}, fMean, false); 134 | % viz.SDFVar(C, {xt, yt}, fVar, false); 135 | % drawnow; 136 | end 137 | end 138 | 139 | viz.measurements(X, Y); 140 | viz.levelSet(C, {xt, yt}, fMean, fVar, true); 141 | viz.SDFMean(C, {xt, yt}, fMean, true); 142 | viz.SDFVar(C, {xt, yt}, fVar, true); 143 | drawnow; 144 | 145 | if ~params.debugFlag 146 | saveFolder = fullfile(gpisRootPath, 'results', [params.shape, '2D']); 147 | if ~exist(saveFolder, 'dir') 148 | mkdir(saveFolder); 149 | end 150 | viz.exportAll(saveFolder, params) 151 | end 152 | end 153 | 154 | -------------------------------------------------------------------------------- /matlab/shape-map/MapGelSight.m: -------------------------------------------------------------------------------- 1 | %% 3-D shape mapping with GelSight and Depth camera, simulation 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function MapGelSight(varargin) 28 | close all; 29 | gpisRootPath = getenv('GPIS_PATH'); 30 | addpath('/usr/local/lib'); 31 | addpath(genpath(fullfile(gpisRootPath, 'matlab'))); 32 | 33 | %% hyperparameters 34 | params = inputParser; 35 | addParameter(params,'debugFlag',false); 36 | addParameter(params,'MeshN',16); 37 | addParameter(params,'dim',3); 38 | addParameter(params,'kernelType',"thinplate"); 39 | addParameter(params,'poseNoise',[1e-3*ones(1, 3), 1e-1*ones(1, 3)],); 40 | addParameter(params,'measNoise',[6e-4, 1e-3, 1e-3, 1e-3]); 41 | addParameter(params,'depthNoise',[1e-3, 1e-3, 1e-3, 1e-3]); 42 | addParameter(params,'inputNoise',[0, 0, 0]); 43 | addParameter(params,'compact',true); 44 | addParameter(params,'depthRes',5e-3); 45 | addParameter(params,'measRes',2e-3); 46 | addParameter(params,'training','incremental'); 47 | addParameter(params,'shape','010_potted_meat_can'); 48 | addParameter(params,'genType','fcrn'); 49 | addParameter(params,'elev',30); 50 | parse(params, varargin{:}); 51 | params = params.Results; 52 | 53 | %% load pointcloud data: x, y, z, nx, ny, nz 54 | datasetRoot = '/home/suddhu/projects/shape-map/Shape_Mapping_Tactile/'; 55 | datasetName = 'textured_60sampled'; 56 | 57 | gelsightFile = fullfile(datasetRoot, 'gelsight_data', datasetName, params.shape, 'gelsight_data.mat'); load(gelsightFile); 58 | if ~strcmp(params.genType, 'gt') 59 | genFile = fullfile(datasetRoot, 'generated_data', datasetName, params.genType, params.shape, strcat(params.genType, '_data.mat')); load(genFile); 60 | end 61 | modelFile = fullfile(datasetRoot, 'models', params.shape, 'textured.mat'); load(modelFile); 62 | XObj = vertices; YObj = normals; YObj = normr(YObj); TriObj = faces; 63 | numMeas = length(poses); 64 | 65 | %% extract patches from point cloud 66 | fprintf('Extracting tactile + vision data\n'); 67 | patches = []; 68 | sensorPoses = {}; 69 | for i = 1:numMeas 70 | poses(i, [4, 5, 6, 7]) = poses(i, [7, 4, 5, 6]); % x y z w -> w x y z 71 | sensorPose = SE3(quat2rotm(poses(i, 4:end)), poses(i, 1:3)); 72 | 73 | % add noise to sensor poses 74 | if ~strcmp(params.genType, 'gt') 75 | noise = params.poseNoise.*randn(size(params.poseNoise)); 76 | noiseSE3 = SE3(eul2rotm(noise(4:end)), noise(1:3)); 77 | sensorPose = sensorPose * noiseSE3; 78 | end 79 | sensorPoses{i} = sensorPose; 80 | 81 | % get heightmaps and contactmasks 82 | if strcmp(params.genType, 'gt') 83 | heightmap = reshape(gt_heightmaps(:, i),[480,640]); 84 | normalmap = reshape(gt_normalmaps(:, 3*(i-1) + 1 : 3*(i-1) + 3),[480*640, 3]); 85 | contactmask = reshape(gt_contactmasks(:, i),[480,640]); 86 | elseif strcmp(params.genType, 'lookup') 87 | heightmap = reshape(lookup_heightmaps(:, i),[480,640]); 88 | normalmap = reshape(lookup_normalmaps(:, 3*(i-1) + 1 : 3*(i-1) + 3),[480*640, 3]); 89 | contactmask = reshape(est_contactmasks(:, i),[480,640]); 90 | elseif strcmp(params.genType, 'fcrn') 91 | heightmap = reshape(fcrn_heightmaps(:, i),[480,640]); 92 | normalmap = reshape(fcrn_normalmaps(:, 3*(i-1) + 1 : 3*(i-1) + 3),[480*640, 3]); 93 | contactmask = reshape(est_contactmasks(:, i),[480,640]); 94 | end 95 | 96 | %% get tactile images 97 | tactileimage = reshape(tactileimages(:, 3*(i-1) + 1 : 3*(i-1) + 3),[480,640, 3]); 98 | tactileimage = rescale(tactileimage); 99 | normalmap = ((sensorPose.SO3) * normalmap')'; 100 | 101 | world_point_cloud = heightMap2Cloud(heightmap, normalmap, contactmask, sensorPose); 102 | world_point_cloud = pcdownsample(world_point_cloud,'gridAverage', params.measRes); 103 | 104 | patches(end + 1).X = world_point_cloud.Location; 105 | patches(end).Y = world_point_cloud.Normal; 106 | 107 | % add noise after offset and normalize 108 | [patches(i).X, patches(i).Y] = addNoise(patches(i).X, patches(i).Y, params.measNoise(2:end), params.inputNoise); 109 | end 110 | 111 | %% vision measurements 112 | depth_map = pointCloud(depth_map); 113 | origin = mean(XObj); 114 | depth_map.Normal = depth_map.Location - origin; 115 | depth_map.Normal = (depth_map.Normal)./vecnorm(depth_map.Normal, 2, 2); 116 | depth_map = pcdownsample(depth_map,'gridAverage',params.depthRes); 117 | 118 | %% kernel parameters 119 | if strcmp(params.kernelType, "thinplate") 120 | params('theta') = [0, 0.25 , 1]; 121 | elseif strcmp(params.kernelType, "rbf") 122 | params('theta') = [1, 0.25, 0.01]; 123 | end 124 | 125 | fprintf('Training data points: %d, Query points: %d\n', numMeas, params.MeshN^3); 126 | %% generate 3D mesh query points 127 | ml = [ min(XObj(:, 1)) max(XObj(:, 1)); 128 | min(XObj(:, 2)) max(XObj(:, 2)); 129 | min(XObj(:, 3)) max(XObj(:, 3))] + [-2e-2 2e-2]; 130 | 131 | params.compRadius = 0.15*mean(ml(:, 2) - ml(:,1)); 132 | params.cleanRadius = params.compRadius; 133 | 134 | Grid = {linspace(ml(1, 1), ml(1, 2), params.MeshN),... 135 | linspace(ml(2, 1), ml(2, 2), params.MeshN)... 136 | linspace(ml(3, 1), ml(3, 2), params.MeshN)}; 137 | [xt, yt, zt] = meshgrid(Grid{1}, Grid{2}, Grid{3}); 138 | 139 | params('meshLim') = ml; 140 | %% visualize measurements 141 | viz = Viz3DGPGraph(params); 142 | viz.groundTruthModel(TriObj, XObj); 143 | viz.measurements(TriObj, XObj); 144 | drawnow; 145 | 146 | params 147 | 148 | if strcmp(params.training, 'batch') 149 | opt = optimizeGraphBatch(params, depth_map); 150 | elseif strcmp(params.training, 'incremental') 151 | opt = optimizeGraphIncremental(params); 152 | end 153 | 154 | if ~params.debugFlag 155 | shapeFolder = fullfile(gpisRootPath, 'results', params.shape); 156 | exportString = sprintf('k=%s_t=%s_g=%s', params.kernelType,params.training, params.genType); 157 | saveFolder = fullfile(shapeFolder, exportString); 158 | if ~exist(saveFolder, 'dir') 159 | mkdir(saveFolder); 160 | end 161 | end 162 | 163 | updateTimes = zeros(1, numMeas + 1); queryTimes = zeros(1, numMeas + 1); 164 | tic; 165 | opt.UpdateDepth(depth_map); 166 | updateTimes(1) = toc; 167 | fprintf('Depth prior update time: %.04f secs\n', updateTimes(1)); 168 | 169 | %% query grid 170 | tic; 171 | [fMean, fVar] = opt.Query(); 172 | queryTimes(1) = toc; 173 | fprintf('Depth prior query time: %.04f secs\n', queryTimes(1)); 174 | 175 | %% get isosurface 176 | iso = isosurface(xt,yt,zt,fMean, 0); 177 | iso = cleanUp(depth_map.Location, iso, params.cleanRadius); 178 | [iso, ~] = splitFV(iso); 179 | 180 | viz.addDepthMeasurements(depth_map.Location); 181 | viz.plotMesh(iso, {xt, yt, zt}, fMean, fVar); 182 | viz.plotSDF(iso, Grid, {xt, yt, zt}, fMean); 183 | drawnow; 184 | 185 | if ~params.debugFlag 186 | fprintf('Saving images...\n'); 187 | viz.exportAll(saveFolder, 0); 188 | exportIso(saveFolder, iso, 0); 189 | end 190 | 191 | if strcmp(params.training, 'batch') % run batch 192 | %% update with all measurements 193 | X = []; Y = []; 194 | for i = 1:length(patches) 195 | X = [X; patches(i).X]; 196 | Y = [Y; patches(i).Y]; 197 | end 198 | 199 | viz.addSensorMeasurements(X, Y); drawnow; 200 | 201 | tic; 202 | opt.Update(X, Y); 203 | updateTime = toc; 204 | 205 | %% query grid 206 | tic; 207 | [fMean, fVar] = opt.Query(); 208 | queryTime = toc; 209 | fprintf('Update time: %.04f secs, Query time: %.04f secs\n', updateTime, queryTime); 210 | 211 | %% get isosurface 212 | iso = isosurface(xt,yt,zt,fMean, 0); 213 | iso = cleanUp([opt.X; depth_map.Location], iso, params.cleanRadius); 214 | [iso, ~] = splitFV(iso); 215 | 216 | viz.plotMesh(iso, {xt, yt, zt}, fMean, fVar); 217 | viz.plotSDF(iso, Grid, {xt, yt, zt}, fMean); 218 | drawnow; 219 | else % run incremental 220 | for i = 1:length(patches) 221 | x = patches(i).X; y = patches(i).Y; 222 | fprintf("Update size: %d\n", length(x)); 223 | 224 | %% update with new measurement 225 | tic; 226 | opt.Update(x, y); 227 | updateTimes(i + 1) = toc; 228 | 229 | %% query grid 230 | tic; 231 | [fMean, fVar] = opt.Query(); 232 | queryTimes(i + 1) = toc; 233 | fprintf('#%.d Update time: %.04f secs, Query time: %.04f secs\n', i, updateTimes(i + 1), queryTimes(i + 1)); 234 | 235 | %% get isosurface 236 | iso = isosurface(xt,yt,zt,fMean, 0); 237 | iso = cleanUp([opt.X; depth_map.Location], iso, params.cleanRadius); 238 | [iso, ~] = splitFV(iso); 239 | 240 | %% plot results 241 | viz.addSensorMeasurements(x, y, sensorPoses{i}); 242 | viz.plotMesh(iso, {xt, yt, zt}, fMean, fVar); 243 | viz.plotSDF(iso, Grid, {xt, yt, zt}, fMean); 244 | drawnow; 245 | 246 | if ~params.debugFlag 247 | fprintf('Saving images...\n'); 248 | viz.exportAll(saveFolder, i); 249 | exportIso(saveFolder, iso, i); 250 | end 251 | end 252 | if ~params.debugFlag 253 | save(fullfile(saveFolder,'timing.mat'), 'updateTimes', 'queryTimes'); 254 | end 255 | end 256 | end 257 | 258 | function exportIso(saveFolder, iso, idx) 259 | stlFolder = fullfile(saveFolder, 'stl'); 260 | if ~exist(stlFolder, 'dir') 261 | mkdir(stlFolder); 262 | end 263 | stlwrite(fullfile(stlFolder,sprintf('%s.stl', num2str(idx))), iso); 264 | end 265 | 266 | %% convert heightmap to 3-D pointcloud in world frame 267 | function world_point_cloud = heightMap2Cloud(heightmap, normalmap, contactmask, sensorPose) 268 | %% gelsight parameters 269 | pixmm = 0.0295; % heightmap (pix) -> 3D conversion (mm) 270 | max_depth = 1.0; %mm 271 | max_z = max_depth/pixmm; % pix 272 | 273 | % ground truth heightmap 274 | heightmap = -1 * (heightmap - max_z); % invert gelsight heightmap 275 | heightmapValid = heightmap .* contactmask; % apply contact mask 276 | [x,y] = meshgrid((1:size(heightmapValid,2)) - size(heightmapValid,2)/2,... 277 | (1:size(heightmapValid,1)) - size(heightmapValid,1)/2); 278 | heightmap_3d = [x(:), y(:), heightmapValid(:)]; 279 | normalmap(heightmap_3d(:,3) == 0, :) = []; 280 | heightmap_3d(heightmap_3d(:,3) == 0, :) = []; 281 | local_point_cloud = pointCloud(heightmap_3d*pixmm/1000); % heightmap (pix) -> 3D conversion (m) 282 | 283 | world_point_cloud = pointCloud((sensorPose * local_point_cloud.Location')'); % convert to global cooridnates via sensorPose 284 | world_point_cloud.Normal = normalmap; 285 | end 286 | -------------------------------------------------------------------------------- /matlab/shape-map/MapGelSightReal.m: -------------------------------------------------------------------------------- 1 | %% 3-D shape mapping with GelSight and Depth camera, real 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function MapGelSightReal(varargin) 28 | close all; 29 | gpisRootPath = getenv('GPIS_PATH'); 30 | addpath('/usr/local/lib'); 31 | addpath(genpath(fullfile(gpisRootPath, 'matlab'))); 32 | shapeMappingRootPath = getenv('SHAPE_MAPPING_ROOT'); 33 | addpath([shapeMappingRootPath,'/scripts', '/matlab']) 34 | 35 | %% hyperparameters 36 | params = inputParser; 37 | addParameter(params,'debugFlag',false); 38 | addParameter(params,'MeshN',16); 39 | addParameter(params,'dim',3); 40 | addParameter(params,'kernelType',"thinplate"); 41 | addParameter(params,'poseNoise',[1e-3*ones(1, 3), 1e-1*ones(1, 3)],); 42 | addParameter(params,'measNoise',[6e-4, 1e-3, 1e-3, 1e-3]); 43 | addParameter(params,'depthNoise',[1e-3, 1e-3, 1e-3, 1e-3]); 44 | addParameter(params,'inputNoise',[0, 0, 0]); 45 | addParameter(params,'compact',true); 46 | addParameter(params,'depthRes',3e-3); 47 | addParameter(params,'measRes',1e-3); 48 | addParameter(params,'training','incremental'); 49 | addParameter(params,'shape','010_potted_meat_can'); 50 | addParameter(params,'genType','fcrn'); 51 | addParameter(params,'elev',30); 52 | parse(params, varargin{:}); 53 | params = params.Results; 54 | 55 | %% load pointcloud data: x, y, z, nx, ny, nz 56 | datasetRoot = "/media/suddhu/Backup Plus/suddhu/rpl/datasets/tactile_mapping/"; 57 | datasetName = 'textured_60sampled'; 58 | 59 | gelsightFile = fullfile(datasetRoot, 'ycbSight2', params.shape, 'gelsight_data.mat'); load(gelsightFile); 60 | 61 | %% get generated data 62 | if strcmp(params.genType, 'fcrn') 63 | genFile = fullfile(datasetRoot, 'generated_data', 'fcrn', params.shape, 'fcrn_data.mat'); 64 | genData = load(genFile); gen_heightmaps = genData.fcrn_heightmaps; gen_normalmaps = genData.fcrn_normalmaps; 65 | elseif strcmp(dataSource, 'lookup') 66 | genFile = fullfile(datasetRoot, 'generated_data', 'lookup', params.shape, 'lookup_data.mat'); 67 | genData = load(genFile); gen_heightmaps = genData.lookup_heightmaps; gen_normalmaps = genData.lookup_normalmaps; 68 | end 69 | 70 | modelFile = fullfile('/home/suddhu/projects/Shape_Mapping_Tactile/', 'aligned_models', strcat(params.shape, '.stl')); 71 | [TriObj, XObj] = stlread(modelFile); 72 | numMeas = length(poses)/2; 73 | 74 | %% transforms 75 | world2azure = toPose(world2azure); 76 | world2object = toPose(world2object).inv; 77 | gripper2gelsight = toPose(gripper2gelsight); 78 | gripper2gelsight.t(2) = gripper2gelsight.t(2) + 5e-3; % 5mm offset compensation 79 | 80 | sensorPoses = {}; sensorPositions = zeros(numMeas, 3); 81 | 82 | %% extract patches from point cloud 83 | fprintf('Extracting tactile + vision data\n'); 84 | patches = []; sensorPoses = {}; 85 | 86 | count = 1; 87 | for i = 2:2:2*numMeas 88 | poses(i, [4, 5, 6, 7]) = poses(i, [7, 4, 5, 6]); % x y z w -> w x y z 89 | 90 | %% get sensorpose 91 | sensorPose = SE3(quat2rotm(poses(i, 4:end)), poses(i, 1:3)); 92 | gelsightTrans = SE3(eye(3), gripper2gelsight.t); 93 | sensorPose = sensorPose * gelsightTrans; 94 | gelsightRot = SE3(gripper2gelsight.R, zeros(1, 3)); 95 | sensorPose = sensorPose * gelsightRot; 96 | 97 | sensorPose = world2object * sensorPose; 98 | sensorPoses{count} = sensorPose; 99 | sensorPositions(count, :) = sensorPose.t'; 100 | count = count + 1; 101 | 102 | %% get 2-D image information 103 | heightmap = reshape(gen_heightmaps(:, i),[480,640]); 104 | normalmap = reshape(gen_normalmaps(:, 3*(i-1) + 1 : 3*(i-1) + 3),[480*640, 3]); 105 | contactmask = reshape(est_contactmasks(:, i),[480,640]); 106 | tactileimage = reshape(tactileimages(:, 3*(i-1) + 1 : 3*(i-1) + 3),[480,640, 3]); 107 | tactileimage = rescale(tactileimage); 108 | 109 | %% get world point cloud 110 | world_point_cloud = heightMap2Cloud(heightmap, normalmap, contactmask, sensorPose); 111 | world_point_cloud = pcdownsample(world_point_cloud,'gridAverage', params.measRes); 112 | 113 | patches(end + 1).X = world_point_cloud.Location; 114 | patches(end).Y = world_point_cloud.Normal; 115 | patches(end).Y = normr(patches(end).Y); 116 | end 117 | 118 | %% get object depth in bounding box 119 | world_depth_map = pointCloud((world2azure * depth_map')'); 120 | object_depth_map = pointCloud((world2object * world_depth_map.Location')'); 121 | 122 | xmin = min(sensorPositions(:, 1)); xmax = max(sensorPositions(:, 1)); 123 | ymin = min(sensorPositions(:, 2)); ymax = max(sensorPositions(:, 2)); 124 | zmin = min(sensorPositions(:, 3)); zmax = max(sensorPositions(:, 3)); 125 | roi = [xmin xmax ymin ymax zmin zmax] + [-1 1 -1 1 -1 3]*1e-2; 126 | indices = findPointsInROI(object_depth_map,roi); 127 | object_depth_map = select(object_depth_map,indices); 128 | 129 | origin = mean(sensorPositions); 130 | object_depth_map.Normal = object_depth_map.Location - origin; 131 | object_depth_map.Normal = (object_depth_map.Normal)./vecnorm(object_depth_map.Normal, 2, 2); 132 | object_depth_map = pcdownsample(object_depth_map,'gridAverage',params.depthRes); 133 | 134 | %% Hallucinate base measurements 135 | x = sensorPositions(5:5:end, 1)'; x(end + 1) = x(1); 136 | y = sensorPositions(5:5:end, 2)'; y(end + 1) = y(1); 137 | meshx = (min(x)):3e-3:(max(x)); meshy = (min(y)):3e-3:(max(y)); 138 | [xm,ym] = meshgrid(meshx,meshy); 139 | xm = xm(:)'; ym = ym(:)'; 140 | 141 | in1 = inpolygon(xm,ym,x, y); % get the contour 142 | bottomPts = [xm(in1)',ym(in1)', abs(world2object.t(3))*ones(size(xm(in1)'))]; 143 | in2 = inpolygon(xm,ym,x - sign(x)*5e-3, y - sign(y)*5e-3); 144 | sidePtsXY = [xm(~in2)',ym(~in2)']; % get the side points 145 | 146 | z = mean(sensorPositions(5:5:end, 3)); 147 | hs = linspace(abs(world2object.t(3)), z, 6); 148 | sidePtsZ = [repmat(hs(1), size(sidePtsXY, 1), 1); repmat(hs(2), size(sidePtsXY, 1), 1);... 149 | repmat(hs(3), size(sidePtsXY, 1), 1); repmat(hs(4), size(sidePtsXY, 1), 1);... 150 | repmat(hs(5), size(sidePtsXY, 1), 1); repmat(hs(6), size(sidePtsXY, 1), 1)]; 151 | 152 | sidePts = [repmat(sidePtsXY, 6, 1), sidePtsZ]; 153 | sideNormals = sidePts - [origin(1), origin(2), mean(sidePtsZ)]; 154 | sideNormals = (sideNormals)./vecnorm(sideNormals, 2, 2); 155 | 156 | bottomPts = [bottomPts; sidePts]; 157 | bottomPts = pointCloud(bottomPts); 158 | bottomPts.Normal = [repmat([0 0 -1],bottomPts.Count - size(sidePts, 1),1); sideNormals]; 159 | 160 | %% kernel parameters 161 | if strcmp(params.kernelType, "thinplate") 162 | params.theta = [0, 0.25, 1]; 163 | elseif strcmp(params.kernelType, "rbf") 164 | params.theta = [1, 0.25, 0.01]; 165 | end 166 | 167 | fprintf('Training data points: %d, Query points: %d\n', numMeas, params.MeshN^3); 168 | %% generate 3D mesh query points 169 | params.meshLim = [min(sensorPositions(:, 1)) - 2e-2 max(sensorPositions(:, 1)) + 1.5e-2; 170 | min(sensorPositions(:, 2)) - 2e-2 max(sensorPositions(:, 2)) + 1.5e-2; 171 | abs(world2object.t(3)) - 2e-2 max(sensorPositions(:, 3)) + 3e-2;]; 172 | 173 | % assign compact radius: tuning for each object (can work well with fixed value too) 174 | if (strcmp(params.shape, "010_potted_meat_can")) 175 | params.compRadius = 0.15*max(params.meshLim(:, 2) - params.meshLim(:,1)); 176 | elseif (strcmp(params.shape, "021_bleach_cleanser")) 177 | params.compRadius = 0.16*mean(params.meshLim(:, 2) - params.meshLim(:,1)); 178 | else 179 | params.compRadius = 0.15*mean(params.meshLim(:, 2) - params.meshLim(:,1)); 180 | end 181 | params.cleanRadius = params.compRadius; 182 | 183 | Grid = {linspace(params.meshLim(1, 1), params.meshLim(1, 2), params.MeshN),... 184 | linspace(params.meshLim(2, 1), params.meshLim(2, 2), params.MeshN)... 185 | linspace(params.meshLim(3, 1), params.meshLim(3, 2), params.MeshN)}; 186 | [xt, yt, zt] = meshgrid(Grid{1}, Grid{2}, Grid{3}); 187 | 188 | %% visualize measurements 189 | viz = Viz3DGPGraphReal(params); 190 | viz.groundTruthModel(TriObj, XObj); 191 | viz.measurements(); 192 | drawnow; 193 | 194 | params 195 | 196 | if strcmp(params.training, 'batch') 197 | opt = optimizeGraphBatch(params, object_depth_map); 198 | elseif strcmp(params.training, 'incremental') 199 | opt = optimizeGraphIncremental(params); 200 | end 201 | 202 | if ~params.debugFlag 203 | shapeFolder = fullfile(gpisRootPath, 'results', params.shape); 204 | exportString = sprintf('real_k=%s_t=%s_g=%s', params.kernelType,params.training, params.genType); 205 | saveFolder = fullfile(shapeFolder, exportString); 206 | if ~exist(saveFolder, 'dir') 207 | mkdir(saveFolder); 208 | end 209 | end 210 | 211 | updateTimes = zeros(1, numMeas + 1); queryTimes = zeros(1, numMeas + 1); 212 | tic; 213 | opt.UpdateDepth(object_depth_map); 214 | updateTimes(1) = toc; 215 | fprintf('Depth prior update time: %.04f secs\n', updateTimes(1)); 216 | 217 | %% query grid 218 | tic; 219 | [fMean, fVar] = opt.Query(); 220 | queryTimes(1) = toc; 221 | fprintf('Depth prior query time: %.04f secs\n', queryTimes(1)); 222 | 223 | %% Bottom points 224 | opt.UpdateDepth(bottomPts); 225 | %% query grid 226 | [fMean, fVar] = opt.Query(); 227 | 228 | %% get isosurface 229 | iso = isosurface(xt,yt,zt,fMean, 0); 230 | iso = cleanUp([object_depth_map.Location; bottomPts.Location], iso, params.cleanRadius); 231 | [iso, ~] = splitFV(iso); 232 | 233 | viz.addDepthMeasurements(object_depth_map.Location); 234 | viz.plotMesh(iso, {xt, yt, zt}, fMean, fVar); 235 | viz.plotSDF(iso, Grid, {xt, yt, zt}, fMean); 236 | drawnow; 237 | 238 | if ~params.debugFlag 239 | fprintf('Saving images...\n'); 240 | viz.exportAll(saveFolder, 0); 241 | exportIso(saveFolder, iso, 0); 242 | end 243 | 244 | if strcmp(params.training, 'batch') % run batch 245 | %% update with all measurements 246 | X = []; Y = []; 247 | for i = 1:length(patches) 248 | X = [X; patches(i).X]; 249 | Y = [Y; patches(i).Y]; 250 | end 251 | 252 | viz.addSensorMeasurements(X, Y); drawnow; 253 | 254 | %% update step 255 | tic; 256 | opt.Update(X, Y); 257 | updateTime = toc; 258 | 259 | %% query grid 260 | tic; 261 | [fMean, fVar] = opt.Query(); 262 | queryTime = toc; 263 | fprintf('Update time: %.04f secs, Query time: %.04f secs\n', updateTime, queryTime); 264 | 265 | %% get isosurface 266 | iso = isosurface(xt,yt,zt,fMean, 0); 267 | iso = cleanUp([opt.X; object_depth_map.Location; bottomPts.Location], iso, params.cleanRadius); 268 | [iso, ~] = splitFV(iso); 269 | 270 | viz.plotMesh(iso, {xt, yt, zt}, fMean, fVar); 271 | viz.plotSDF(iso, Grid, {xt, yt, zt}, fMean); 272 | drawnow; 273 | else % run incremental 274 | for i = 1:length(patches) 275 | x = patches(i).X; y = patches(i).Y; 276 | fprintf("Update size: %d\n", length(x)); 277 | 278 | %% update step 279 | tic; 280 | opt.Update(x, y); 281 | updateTimes(i + 1) = toc; 282 | 283 | %% query step 284 | tic; 285 | [fMean, fVar] = opt.Query(); 286 | queryTimes(i + 1) = toc; 287 | fprintf('#%.d Update time: %.04f secs, Query time: %.04f secs\n', i, updateTimes(i + 1), queryTimes(i + 1)); 288 | 289 | %% get isosurface 290 | iso = isosurface(xt,yt,zt,fMean, 0); 291 | iso = cleanUp([opt.X; object_depth_map.Location; bottomPts.Location], iso, params.cleanRadius); 292 | [iso, ~] = splitFV(iso); 293 | 294 | %% plot results 295 | viz.addSensorMeasurements(x, y, sensorPoses{i}); 296 | viz.plotMesh(iso, {xt, yt, zt}, fMean, fVar); 297 | viz.plotSDF(iso, Grid, {xt, yt, zt}, fMean); 298 | drawnow; 299 | 300 | if (~params.debugFlag) 301 | fprintf('Saving images...\n'); 302 | viz.exportAll(saveFolder, i); 303 | exportIso(saveFolder, iso, i); 304 | end 305 | end 306 | 307 | if ~params.debugFlag 308 | save(fullfile(saveFolder,'timing.mat'), 'updateTimes', 'queryTimes'); 309 | end 310 | end 311 | end 312 | 313 | function exportIso(saveFolder, iso, idx) 314 | stlFolder = fullfile(saveFolder, 'stl'); 315 | if ~exist(stlFolder, 'dir') 316 | mkdir(stlFolder); 317 | end 318 | stlwrite(fullfile(stlFolder,sprintf('%s.stl', num2str(idx))), iso); 319 | end 320 | 321 | %% convert heightmap to 3-D pointcloud in world frame 322 | function world_point_cloud = heightMap2Cloud(heightmap, normalmap, contactmask, sensorPose) 323 | %% gelsight parameters 324 | pixmm = 0.0295; % heightmap (pix) -> 3D conversion (mm) 325 | max_depth = 1.0; %mm 326 | max_z = max_depth/pixmm; % pix 327 | 328 | % ground truth heightmap 329 | heightmap = -1 * (heightmap - max_z); % invert gelsight heightmap 330 | heightmapValid = heightmap .* contactmask; % apply contact mask 331 | [x,y] = meshgrid((1:size(heightmapValid,2)) - size(heightmapValid,2)/2,... 332 | (1:size(heightmapValid,1)) - size(heightmapValid,1)/2); 333 | heightmap_3d = [x(:), y(:), heightmapValid(:)]; 334 | 335 | normalmap = ((sensorPose.SO3) * normalmap')'; 336 | 337 | normalmap(heightmap_3d(:,3) == 0, :) = []; 338 | heightmap_3d(heightmap_3d(:,3) == 0, :) = []; 339 | local_point_cloud = pointCloud(heightmap_3d*pixmm/1000); % heightmap (pix) -> 3D conversion (m) 340 | 341 | world_point_cloud = pointCloud((sensorPose * local_point_cloud.Location')'); % convert to global cooridnates via sensorPose 342 | world_point_cloud.Normal = normalmap; 343 | end 344 | 345 | %% quat to SE3 conversion 346 | function p = toPose(xyzquat) 347 | R = quat2rotm(xyzquat([7, 4, 5, 6])); 348 | t = xyzquat(1:3); 349 | p = SE3(R, t); 350 | end 351 | -------------------------------------------------------------------------------- /matlab/shape-map/Viz2DGPGraph.m: -------------------------------------------------------------------------------- 1 | %% Visualizer, 2-D GPIS 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | classdef Viz2DGPGraph < handle 28 | properties 29 | f; % figure handle 30 | s1; s2; s3; s4, s5, s6; % subplot handles 31 | meshLim; 32 | elev; 33 | count; 34 | gt; 35 | xMesh; 36 | yMesh; 37 | end 38 | 39 | methods 40 | function obj = Viz2DGPGraph(params) 41 | % define figure handle 42 | obj.f = figure('units','normalized','outerposition',[0 0 1 1]); 43 | set(gca, 'FontName', 'Times New Roman'); 44 | obj.meshLim = params.meshLim; 45 | obj.gt = params.shape_data; 46 | Grid = {linspace(params.meshLim(1), params.meshLim(2), params.MeshN),... 47 | linspace(params.meshLim(1), params.meshLim(2), params.MeshN)}; 48 | [obj.xMesh, obj.yMesh] = meshgrid(Grid{1}, Grid{2}); 49 | obj.count = 1; 50 | end 51 | 52 | %% plot original model 53 | function groundTruthModel(obj) 54 | set(0,'CurrentFigure',obj.f); 55 | obj.s1 = subplot(3, 2, 1); 56 | plot(obj.gt(:,1), obj.gt(:,2), 'k-.', 'LineWidth', 3); 57 | axis([obj.meshLim(1, :), obj.meshLim(2, :)]); 58 | axis equal off 59 | end 60 | 61 | %% plot measurements and isosurface 62 | function measurements(obj, X, Y) 63 | set(0,'CurrentFigure',obj.f); 64 | obj.s2 = subplot(3, 2, 2); 65 | cla(); 66 | plot(obj.gt(:,1), obj.gt(:,2), 'k-.', 'LineWidth', 1); 67 | hold on; 68 | plot(X(:,1),X(:,2),'o','color',[0 0.392157 0],'MarkerSize',4, 'MarkerFaceColor',[0 0.392157 0]); 69 | quiver(X(:,1),X(:,2),Y(:,1),Y(:,2), 0.3, 'color',[0 0.392157 0]); 70 | axis([obj.meshLim(1, :), obj.meshLim(2, :)]); 71 | axis equal off 72 | hold off; 73 | end 74 | 75 | %% plot isosurface, measurements and SDF with mean 76 | function SDFMean(obj, C, xy, M, drawC) 77 | set(0,'CurrentFigure',obj.f); 78 | obj.s3 = subplot(3, 2, 3); 79 | cla(); 80 | if drawC 81 | plot(C(1, 1:end), C(2, 1:end), 'k-', 'linewidth', 2); % object contour 82 | end 83 | hold on; 84 | axis equal off; 85 | s = surf(xy{1},xy{2},M - 0.5, 'FaceColor', 'interp'); % plot GP 86 | axis([obj.meshLim(1, :), obj.meshLim(2, :)]); 87 | colormap(gca,jet); 88 | q = quantile(M(:),[0.01 0.25]); 89 | shading interp 90 | view(2); 91 | hold off; 92 | end 93 | 94 | %% plot isosurface, measurements and SDF with variance 95 | function SDFVar(obj, C, xy, V, drawC) 96 | set(0,'CurrentFigure',obj.f); 97 | obj.s4 = subplot(3, 2, 4); 98 | cla(); 99 | 100 | if drawC 101 | plot(C(1, 1:end), C(2, 1:end), 'k-', 'linewidth', 2); % object contour 102 | end 103 | hold on; 104 | axis equal off; 105 | s = surf(xy{1},xy{2},V - 0.5, 'FaceColor', 'interp'); % plot GP 106 | axis([obj.meshLim(1, :), obj.meshLim(2, :)]); 107 | colormap(gca,parula); 108 | % q = quantile(V(:),[0.01 0.1]); 109 | % caxis([q(1), q(2)]); 110 | shading interp 111 | view(2); 112 | hold off; 113 | end 114 | 115 | function levelSet(obj, C, xy, M, V, drawC) 116 | set(0,'CurrentFigure',obj.f); 117 | obj.s5 = subplot(3, 2, 5); 118 | cla(); 119 | if drawC 120 | plot(C(1, 2:end), C(2, 2:end), 'g-', 'linewidth', 5); % object contour 121 | end 122 | hold on; 123 | axis equal off; 124 | s = surf(xy{1},xy{2},M, 'FaceColor', 'interp'); % plot GP 125 | axis([obj.meshLim(1, :), obj.meshLim(2, :)]); 126 | colormap(gca,parula); 127 | s.CData = V; 128 | % q = quantile(V(:),[0.01 0.5]); 129 | % caxis([q(1), q(2)]); 130 | set(gca, 'ZDir','reverse'); 131 | zlim([min(M(:)) max(M(:))]); 132 | 133 | % plot 0 plane 134 | Z = zeros(size(xy{1})); 135 | CO(:,:,1) = zeros(size(xy{1},1)); 136 | CO(:,:,2) = zeros(size(xy{1},1)); 137 | CO(:,:,3) = zeros(size(xy{1},1)); 138 | s = surf(xy{1},xy{2},Z, CO); 139 | alpha(s,.7); 140 | view(3); 141 | hold off; 142 | end 143 | 144 | function graph(obj, X, xy, fs, M) 145 | set(0,'CurrentFigure',obj.f); 146 | obj.s6 = subplot(3, 2, 6); 147 | 148 | xt = xy{1}; yt = xy{2}; 149 | QueryPts = [xt(:), yt(:)]; 150 | % connectivity 151 | 152 | %% plot variance 153 | z = -2*ones(size(xy{1})); 154 | s = surf(xy{1},xy{2}, z, M, 'FaceColor', 'interp', 'FaceAlpha',0.1); % plot GP 155 | s.EdgeColor = 'none'; 156 | axis([obj.meshLim(1, :), obj.meshLim(2, :)]); 157 | axis equal off 158 | colormap(gca,jet); 159 | view(2); 160 | hold on; 161 | 162 | %% plot graph connections 163 | linesX = []; linesY = []; 164 | for i=1:size(fs, 1) 165 | qp = [QueryPts(fs(i, 1), 1), QueryPts(fs(i, 1), 2)]; 166 | mp = X(fs(i, 2),:); 167 | linesX(end + 1, :) = [qp(1),mp(1)]; 168 | linesY(end + 1, :) = [qp(2),mp(2)]; 169 | end 170 | 171 | z = -1*ones(size(linesY)); 172 | connPlot = plot3(linesX', linesY', z', 'b-', 'LineWidth', 0.5); 173 | 174 | %% plot ground truth contour 175 | gtPlot = plot(obj.gt(:,1), obj.gt(:,2), 'k--', 'LineWidth', 0.5); 176 | %% plot query points 177 | queryPlot = plot(QueryPts(:, 1), QueryPts(:, 2), 'o', 'color','k','MarkerSize',2, 'MarkerFaceColor','r'); 178 | %% plot measurement points 179 | measurementPlot = plot(X(:,1),X(:,2),'s','color','k','MarkerSize',4, 'MarkerFaceColor',[0 0.392157 0]); 180 | end 181 | 182 | function save(obj) 183 | saveas(obj.f,sprintf('images/Fig_%d.png',obj.count)); 184 | obj.count = obj.count + 1; 185 | end 186 | 187 | function exportAll(obj, saveFolder, params) 188 | %% export 189 | if params.compact 190 | exportString = sprintf('%s_c=%s_%s_%s', params.kernelType, string(params.compact),... 191 | string(params.compactRadius), params.training); 192 | else 193 | exportString = sprintf('%s_c=%s_%s', params.kernelType, string(params.compact),... 194 | params.training); 195 | end 196 | 197 | % ground truth model 198 | fig = figure('visible', 'off'); 199 | h = copyobj(obj.s1,fig); 200 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 201 | disp(['Saving ', sprintf('%s/%s_%s.png', saveFolder, exportString, 'model')]); 202 | exportgraphics(h,sprintf('%s/%s_%s.png', saveFolder, exportString, 'model'), 'Resolution',1000); 203 | close(fig); 204 | 205 | % measurements 206 | fig = figure('visible', 'off'); 207 | h = copyobj(obj.s2,fig); 208 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 209 | disp(['Saving ', sprintf('%s/%s_%s.png', saveFolder, exportString, 'measurements')]); 210 | exportgraphics(h,sprintf('%s/%s_%s.png', saveFolder, exportString, 'measurements'), 'Resolution',1000); 211 | close(fig); 212 | 213 | % SDF mean 214 | fig = figure('visible', 'off'); 215 | h = copyobj(obj.s3,fig); 216 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 217 | disp(['Saving ', sprintf('%s/%s_%s.png', saveFolder, exportString, 'sdfmean')]); 218 | exportgraphics(h,sprintf('%s/%s_%s.png', saveFolder, exportString, 'sdfmean'), 'Resolution',1000); 219 | close(fig); 220 | 221 | % SDF var 222 | fig = figure('visible', 'off'); 223 | h = copyobj(obj.s4,fig); 224 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 225 | disp(['Saving ', sprintf('%s/%s_%s.png', saveFolder, exportString, 'sdfvar')]); 226 | exportgraphics(h,sprintf('%s/%s_%s.png', saveFolder, exportString, 'sdfvar'), 'Resolution',1000); 227 | close(fig); 228 | 229 | % 3d level-set 230 | fig = figure('visible', 'off'); 231 | h = copyobj(obj.s5,fig); 232 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 233 | disp(['Saving ', sprintf('%s/%s_%s.png', saveFolder, exportString, 'levelset')]); 234 | exportgraphics(h,sprintf('%s/%s_%s.png', saveFolder, exportString, 'levelset'), 'Resolution',1000); 235 | close(fig); 236 | 237 | % graph 238 | fig = figure('visible', 'off'); 239 | h = copyobj(obj.s6,fig); 240 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 241 | disp(['Saving ', sprintf('%s/%s_%s.png', saveFolder, exportString, 'graph')]); 242 | exportgraphics(h,sprintf('%s/%s_%s.png', saveFolder, exportString, 'graph'), 'Resolution',1000); 243 | close(fig); 244 | end 245 | end 246 | end 247 | 248 | -------------------------------------------------------------------------------- /matlab/shape-map/Viz3DGPGraph.m: -------------------------------------------------------------------------------- 1 | %% Visualizer, 3-D GPIS for sim data 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | classdef Viz3DGPGraph < handle 28 | properties 29 | f; % figure handle 30 | s1; s2; s3; s4; % subplot handles 31 | limits; 32 | meshLim; 33 | elev; 34 | count; 35 | xMesh; yMesh; zMesh; 36 | model_name; 37 | end 38 | 39 | methods 40 | function obj = Viz3DGPGraph(params) 41 | % define figure handle 42 | obj.f = figure('units','normalized','outerposition',[0 0 1 1]); 43 | set(gcf,'color','w'); 44 | set(gca, 'FontName', 'Times New Roman'); 45 | obj.meshLim = params.meshLim; 46 | Grid = {linspace(params.meshLim(1), params.meshLim(2), params.MeshN),... 47 | linspace(params.meshLim(1), params.meshLim(2), params.MeshN),... 48 | linspace(params.meshLim(1), params.meshLim(2), params.MeshN)}; 49 | [obj.xMesh, obj.yMesh, obj.zMesh] = meshgrid(Grid{1}, Grid{2}, Grid{3}); 50 | obj.count = 1; 51 | obj.elev = params.elev; 52 | obj.model_name = params.shape; 53 | end 54 | 55 | %% plot original model 56 | function groundTruthModel(obj, TriObj, XObj) 57 | set(0,'CurrentFigure',obj.f); 58 | obj.s1 = subplot(2, 2, 1); 59 | 60 | hold on; 61 | trisurf(TriObj, XObj(:,1), XObj(:,2), XObj(:,3), 'EdgeColor', 'none'); 62 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 63 | axis equal off 64 | shading interp 65 | camlight; lighting phong 66 | % colormap(gca,gray) 67 | view(-37.5, obj.elev); 68 | hold off; 69 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 70 | end 71 | 72 | %% plot measurements and isosurface 73 | function measurements(obj, TriObj, XObj) 74 | set(0,'CurrentFigure',obj.f); 75 | obj.s2 = subplot(2, 2, 2); 76 | hold on; 77 | 78 | trisurf(TriObj, XObj(:,1), XObj(:,2), XObj(:,3), 'EdgeColor', 'none', 'FaceAlpha', 0.3); 79 | 80 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 81 | axis equal off 82 | shading interp 83 | % colormap(gca,gray); 84 | view(-37.5, obj.elev); 85 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 86 | hold off; 87 | end 88 | 89 | function addDepthMeasurements(obj, x) 90 | set(0,'CurrentFigure',obj.f); 91 | obj.s2 = subplot(2, 2, 2); 92 | hold on; 93 | plotPointsAndNormals3D(x, [], 1e-2, 'b'); 94 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 95 | hold off; 96 | end 97 | 98 | function addSensorMeasurements(obj, x, y, sensorPose) 99 | set(0,'CurrentFigure',obj.f); 100 | obj.s2 = subplot(2, 2, 2); 101 | hold on; 102 | plotPointsAndNormals3D(x, y, 1e-2, 'g'); 103 | 104 | if nargin > 3 105 | sensorPose.plot('rgb', 'length', 0.005, 'labels', ' ', 'thick', 0.5); 106 | end 107 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 108 | hold off; 109 | end 110 | 111 | function plotMesh(obj, iso, xyz, M, V) 112 | set(0,'CurrentFigure',obj.f); 113 | obj.s3 = subplot(2, 2, 3); 114 | cla(); 115 | 116 | psurf1=patch(iso, 'EdgeColor','none'); 117 | hold on; 118 | isonormals(xyz{1},xyz{2},xyz{3},M,psurf1); 119 | isocolors(xyz{1},xyz{2},xyz{3},V,psurf1); 120 | % trisurf(iso.faces, iso.vertices(:,1), iso.vertices(:,2), iso.vertices(:,3), 'EdgeColor', 'none'); 121 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 122 | axis equal off 123 | camlight; lighting gouraud; 124 | shading interp; 125 | customMap = jet; 126 | customMap(end, :) = ones(1, 3); 127 | colormap(gca,jet); 128 | set(psurf1,'linestyle','none'); 129 | 130 | %% hardcoded for better visibility (paper figures) 131 | if strcmp(obj.model_name, "002_master_chef_can") 132 | caxis(obj.s3, [1e-6 2.5e-5]); % 002_master_chef_can 133 | elseif strcmp(obj.model_name, "004_sugar_box") 134 | caxis(obj.s3, [1e-6 1.8e-5]); % 004_sugar_box 135 | elseif strcmp(obj.model_name, "005_tomato_soup_can") 136 | caxis(obj.s3, [1e-6 1.8e-5]); % 005_tomato_soup_can 137 | elseif strcmp(obj.model_name, "010_potted_meat_can") 138 | caxis(obj.s3, [1e-6 2e-5]); % 010_potted_meat_can 139 | elseif strcmp(obj.model_name, "021_bleach_cleanser") 140 | caxis(obj.s3, [1e-6 2e-5]); % 021_bleach_cleanser 141 | elseif strcmp(obj.model_name, "036_wood_block") 142 | caxis(obj.s3, [1e-6 1.6e-5]); % 036_wood_block 143 | else 144 | v = V(:); v(v == max(v)) = []; 145 | q = quantile(v,[0 0.8]); 146 | caxis(obj.s3, [q(1), q(2)]); 147 | end 148 | 149 | view(-37.5, obj.elev); 150 | psurf1.FaceAlpha = 0.85; 151 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 152 | hold off; 153 | end 154 | 155 | %% plot isosurface, measurements and SDF 156 | function plotSDF(obj, iso, grid, xyz, M) 157 | set(0,'CurrentFigure',obj.f); 158 | obj.s4 = subplot(2, 2, 4); 159 | cla(); 160 | 161 | slice = [mean(grid{1}), mean(grid{2}), mean(grid{3})]; 162 | slice3D(xyz{1}, xyz{2}, xyz{3}, M, slice); 163 | hold on; 164 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 165 | axis equal off 166 | camlight; lighting gouraud; 167 | colormap(gca,flipud(jet)); 168 | % q = quantile(M(:),[0.01 0.25]); 169 | caxis([0, 1e-2]); 170 | % caxis(obj.s4, [q(1), q(2)]); 171 | 172 | trisurf(iso.faces, iso.vertices(:,1), iso.vertices(:,2), iso.vertices(:,3),... 173 | 'EdgeColor', 'none', 'LineStyle', 'none', 'FaceColor', 'white'); 174 | shading interp 175 | view(-37.5, obj.elev); 176 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 177 | hold off; 178 | end 179 | 180 | function save(obj) 181 | saveas(obj.f,sprintf('images/Fig_%d.png',obj.count)); 182 | obj.count = obj.count + 1; 183 | end 184 | 185 | function exportAll(obj, saveFolder, idx) 186 | % ground truth model 187 | modelFolder = fullfile(saveFolder, 'model'); 188 | if ~exist(modelFolder, 'dir') 189 | mkdir(modelFolder); 190 | fig = figure('visible', 'off'); 191 | h = copyobj(obj.s1,fig); 192 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 193 | exportgraphics(h,fullfile(modelFolder,'model.png'), 'Resolution',500); 194 | close(fig); 195 | end 196 | 197 | % measurements 198 | measFolder = fullfile(saveFolder, 'meas'); 199 | if ~exist(measFolder, 'dir') 200 | mkdir(measFolder); 201 | end 202 | fig = figure('visible', 'off'); 203 | h = copyobj(obj.s2,fig); 204 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 205 | exportgraphics(h,fullfile(measFolder,sprintf('%s.png', num2str(idx))), 'Resolution',500); 206 | close(fig); 207 | 208 | % isosurface 209 | isoFolder = fullfile(saveFolder, 'iso'); 210 | if ~exist(isoFolder, 'dir') 211 | mkdir(isoFolder); 212 | end 213 | fig = figure('visible', 'off'); 214 | h = copyobj(obj.s3,fig); 215 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 216 | exportgraphics(h,fullfile(isoFolder,sprintf('%s.png', num2str(idx))), 'Resolution',500); 217 | close(fig); 218 | 219 | % SDF 220 | sdfFolder = fullfile(saveFolder, 'sdf'); 221 | if ~exist(sdfFolder, 'dir') 222 | mkdir(sdfFolder); 223 | end 224 | fig = figure('visible', 'off'); 225 | h = copyobj(obj.s4,fig); 226 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 227 | exportgraphics(h,fullfile(sdfFolder,sprintf('%s.png', num2str(idx))), 'Resolution',500); 228 | close(fig); 229 | end 230 | end 231 | end 232 | 233 | -------------------------------------------------------------------------------- /matlab/shape-map/Viz3DGPGraphReal.m: -------------------------------------------------------------------------------- 1 | %% Visualizer, 3-D GPIS for real data 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | classdef Viz3DGPGraphReal < handle 28 | properties 29 | f; % figure handle 30 | s1; s2; s3; s4; % subplot handles 31 | limits; 32 | meshLim; 33 | elev; 34 | count; 35 | xMesh; yMesh; zMesh; 36 | model_name; 37 | end 38 | 39 | methods 40 | function obj = Viz3DGPGraphReal(params) 41 | % define figure handle 42 | obj.f = figure('units','normalized','outerposition',[0 0 1 1]); 43 | set(gcf,'color','w'); 44 | set(gca, 'FontName', 'Times New Roman'); 45 | obj.meshLim = params.meshLim; 46 | Grid = {linspace(params.meshLim(1), params.meshLim(2), params.MeshN),... 47 | linspace(params.meshLim(1), params.meshLim(2), params.MeshN),... 48 | linspace(params.meshLim(1), params.meshLim(2), params.MeshN)}; 49 | [obj.xMesh, obj.yMesh, obj.zMesh] = meshgrid(Grid{1}, Grid{2}, Grid{3}); 50 | obj.count = 1; 51 | obj.elev = params.elev; 52 | obj.model_name = params.shape; 53 | end 54 | 55 | %% plot original model 56 | function groundTruthModel(obj, TriObj, XObj) 57 | set(0,'CurrentFigure',obj.f); 58 | obj.s1 = subplot(2, 2, 1); 59 | 60 | hold on; 61 | trisurf(TriObj, XObj(:,1), XObj(:,2), XObj(:,3), 'EdgeColor', 'none'); 62 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 63 | axis equal off 64 | shading interp 65 | camlight; lighting phong 66 | % colormap(gca,gray) 67 | view(130, obj.elev); 68 | hold off; 69 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 70 | end 71 | 72 | %% plot measurements and isosurface 73 | function measurements(obj) 74 | set(0,'CurrentFigure',obj.f); 75 | obj.s2 = subplot(2, 2, 2); 76 | 77 | axis equal off 78 | shading interp 79 | view(130, obj.elev); 80 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 81 | hold off; 82 | end 83 | 84 | function addDepthMeasurements(obj, x) 85 | set(0,'CurrentFigure',obj.f); 86 | obj.s2 = subplot(2, 2, 2); 87 | hold on; 88 | plotPointsAndNormals3D(x, [], 1e-2, 'b'); 89 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 90 | hold off; 91 | end 92 | 93 | function addSensorMeasurements(obj, x, y, sensorPose) 94 | set(0,'CurrentFigure',obj.f); 95 | obj.s2 = subplot(2, 2, 2); 96 | hold on; 97 | plotPointsAndNormals3D(x, y, 1e-2, 'g'); 98 | 99 | if nargin > 3 100 | sensorPose.plot('rgb', 'length', 0.005, 'labels', ' ', 'thick', 0.5); 101 | end 102 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 103 | hold off; 104 | end 105 | 106 | function plotMesh(obj, iso, xyz, M, V) 107 | set(0,'CurrentFigure',obj.f); 108 | obj.s3 = subplot(2, 2, 3); 109 | cla(); 110 | 111 | psurf1=patch(iso, 'EdgeColor','none'); 112 | hold on; 113 | isonormals(xyz{1},xyz{2},xyz{3},M,psurf1); 114 | isocolors(xyz{1},xyz{2},xyz{3},V,psurf1); 115 | % trisurf(iso.faces, iso.vertices(:,1), iso.vertices(:,2), iso.vertices(:,3), 'EdgeColor', 'none'); 116 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 117 | axis equal off 118 | camlight; lighting gouraud; 119 | shading interp; 120 | % customMap = jet; 121 | % customMap(end, :) = ones(1, 3); 122 | % colormap(gca,customMap); 123 | colormap(gca,jet); 124 | 125 | set(psurf1,'linestyle','none'); 126 | 127 | %% hardcoded for Sep 10th 128 | if strcmp(obj.model_name, "002_master_chef_can") 129 | caxis(obj.s3, [1e-7 3e-5]); % 002_master_chef_can 130 | elseif strcmp(obj.model_name, "004_sugar_box") 131 | caxis(obj.s3, [1e-7 3e-5]); % 004_sugar_box 132 | elseif strcmp(obj.model_name, "005_tomato_soup_can") 133 | caxis(obj.s3, [1e-7 3e-5]); % 005_tomato_soup_can 134 | elseif strcmp(obj.model_name, "010_potted_meat_can") 135 | caxis(obj.s3, [1e-7 3e-5]); % 010_potted_meat_can 136 | elseif strcmp(obj.model_name, "021_bleach_cleanser") 137 | caxis(obj.s3, [1e-7 3e-5]); % 021_bleach_cleanser 138 | elseif strcmp(obj.model_name, "036_wood_block") 139 | caxis(obj.s3, [1e-7 3e-5]); % 036_wood_block 140 | else 141 | v = V(:); v(v == max(v)) = []; 142 | q = quantile(v,[0 0.8]); 143 | caxis(obj.s3, [q(1), q(2)]); 144 | end 145 | 146 | view(130, obj.elev); 147 | psurf1.FaceAlpha = 0.85; 148 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 149 | hold off; 150 | end 151 | 152 | %% plot isosurface, measurements and SDF 153 | function plotSDF(obj, iso, grid, xyz, M) 154 | set(0,'CurrentFigure',obj.f); 155 | obj.s4 = subplot(2, 2, 4); 156 | cla(); 157 | 158 | slice = [mean(grid{1}), mean(grid{2}), mean(grid{3})]; 159 | slice3D(xyz{1}, xyz{2}, xyz{3}, M, slice); 160 | hold on; 161 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]) 162 | axis equal off 163 | camlight; lighting gouraud; 164 | colormap(gca,flipud(jet)); 165 | % q = quantile(M(:),[0.01 0.25]); 166 | caxis([0, 1e-2]); 167 | % caxis(obj.s4, [q(1), q(2)]); 168 | 169 | trisurf(iso.faces, iso.vertices(:,1), iso.vertices(:,2), iso.vertices(:,3),... 170 | 'EdgeColor', 'None', 'LineStyle', 'None', 'FaceColor', 'white'); 171 | shading interp 172 | 173 | view(130, obj.elev); 174 | axis([obj.meshLim(1,:), obj.meshLim(2,:), obj.meshLim(3,:)]); 175 | hold off; 176 | end 177 | 178 | function save(obj) 179 | saveas(obj.f,sprintf('images/Fig_%d.png',obj.count)); 180 | obj.count = obj.count + 1; 181 | end 182 | 183 | 184 | function exportAll(obj, saveFolder, idx) 185 | 186 | % ground truth model 187 | modelFolder = fullfile(saveFolder, 'model'); 188 | if ~exist(modelFolder, 'dir') 189 | mkdir(modelFolder); 190 | fig = figure('visible', 'off'); 191 | h = copyobj(obj.s1,fig); 192 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 193 | exportgraphics(h,fullfile(modelFolder,'model.png'), 'Resolution',500); 194 | close(fig); 195 | end 196 | 197 | % measurements 198 | measFolder = fullfile(saveFolder, 'meas'); 199 | if ~exist(measFolder, 'dir') 200 | mkdir(measFolder); 201 | end 202 | fig = figure('visible', 'off'); 203 | h = copyobj(obj.s2,fig); 204 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 205 | exportgraphics(h,fullfile(measFolder,sprintf('%s.png', num2str(idx))), 'Resolution',500); 206 | close(fig); 207 | 208 | % isosurface 209 | isoFolder = fullfile(saveFolder, 'iso'); 210 | if ~exist(isoFolder, 'dir') 211 | mkdir(isoFolder); 212 | end 213 | fig = figure('visible', 'off'); 214 | h = copyobj(obj.s3,fig); 215 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 216 | exportgraphics(h,fullfile(isoFolder,sprintf('%s.png', num2str(idx))), 'Resolution',500); 217 | close(fig); 218 | 219 | % SDF 220 | sdfFolder = fullfile(saveFolder, 'sdf'); 221 | if ~exist(sdfFolder, 'dir') 222 | mkdir(sdfFolder); 223 | end 224 | fig = figure('visible', 'off'); 225 | h = copyobj(obj.s4,fig); 226 | set(h, 'pos', [0.23162 0.2233 0.72058 0.63107]); 227 | exportgraphics(h,fullfile(sdfFolder,sprintf('%s.png', num2str(idx))), 'Resolution',500); 228 | close(fig); 229 | end 230 | end 231 | end 232 | 233 | -------------------------------------------------------------------------------- /matlab/shape-map/mapYCB.m: -------------------------------------------------------------------------------- 1 | %% batch script to map YCB objects (both sim and real) 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | clc; clear; close all; 28 | 29 | objectList = ["002_master_chef_can","004_sugar_box", "005_tomato_soup_can",... 30 | "010_potted_meat_can", "021_bleach_cleanser", "036_wood_block"]; 31 | 32 | flag = "sim"; 33 | debug = true; 34 | 35 | for obj = objectList 36 | fprintf('\n'); 37 | if (strcmp(flag, "sim")) 38 | MapGelSight('shape', obj, 'debugFlag', debug, 'MeshN', 16, 'dim', 3, 'kernelType', 'thinplate',... 39 | 'poseNoise', [1e-3*ones(1, 3), 1e-1*ones(1, 3)], 'measNoise', [6e-4, 1e-3, 1e-3, 1e-3], 'depthNoise', [1e-3, 1e-3, 1e-3, 1e-3],... 40 | 'compact', true, 'depthRes', 5e-3, 'measRes', 2e-3,... 41 | 'training', 'incremental', 'genType', 'fcrn', 'elev', 30); 42 | else 43 | MapGelSightReal('shape', obj, 'debugFlag', debug, 'MeshN', 16, 'dim', 3, 'kernelType', 'thinplate',... 44 | 'poseNoise', [1e-3*ones(1, 3), 1e-1*ones(1, 3)], 'measNoise', [6e-4, 1e-3, 1e-3, 1e-3], 'depthNoise', [1e-3, 1e-3, 1e-3, 1e-3],... 45 | 'compact', true, 'depthRes', 3e-3, 'measRes', 1e-3,... 46 | 'training', 'incremental', 'genType', 'fcrn', 'elev', 30); 47 | end 48 | pause(1); 49 | end 50 | -------------------------------------------------------------------------------- /matlab/shape-map/optimizeGraphBatch.m: -------------------------------------------------------------------------------- 1 | %% GP-SG batch optimizer 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | classdef optimizeGraphBatch < handle 28 | properties 29 | %% query points 30 | queryPoints; 31 | numNodes; meshLength; 32 | numMeasurements; numDepthPrior; 33 | inputDim; outputDim; 34 | 35 | %% parameters 36 | noiseModels = struct('meas', [], 'prior', []); 37 | compact = struct('isCompact', [], 'compRadius', []); 38 | theta; 39 | 40 | %% solver 41 | solver = struct('useGaussNewton', [], 'optParams', [], 'optimizeStopRelErr', []); 42 | Graph; initValues; 43 | mu; sigma; 44 | X; Y; 45 | end 46 | 47 | methods 48 | %% constructor 49 | function obj = optimizeGraphBatch(params, depth_map) 50 | obj.inputDim = params.dim; 51 | obj.outputDim = obj.inputDim + 1; 52 | 53 | %% get query grid 54 | if (obj.inputDim == 2) 55 | Grid = {linspace(params.meshLim(1, 1), params.meshLim(1, 2), params.MeshN),... 56 | linspace(params.meshLim(2, 1), params.meshLim(2, 2), params.MeshN)}; 57 | [xt, yt] = meshgrid(Grid{1}, Grid{2}); 58 | obj.queryPoints = [xt(:), yt(:)]; 59 | obj.numNodes = params.MeshN^2; 60 | elseif (obj.inputDim == 3) 61 | Grid = {linspace(params.meshLim(1, 1), params.meshLim(1, 2), params.MeshN),... 62 | linspace(params.meshLim(2, 1), params.meshLim(2, 2), params.MeshN)... 63 | linspace(params.meshLim(3, 1), params.meshLim(3, 2), params.MeshN)}; 64 | [xt, yt, zt] = meshgrid(Grid{1}, Grid{2}, Grid{3}); 65 | obj.queryPoints = [xt(:), yt(:), zt(:)]; 66 | obj.numNodes = params.MeshN^3; 67 | end 68 | 69 | obj.meshLength = params.MeshN; 70 | obj.numMeasurements = 0; 71 | obj.theta = params.theta'; 72 | 73 | %% solver 74 | obj.solver.useGaussNewton = false; 75 | obj.solver.optimizeStopRelErr = 1e-6; 76 | if (obj.solver.useGaussNewton) 77 | obj.solver.optParams = gtsam.GaussNewtonParams; 78 | else 79 | obj.solver.optParams = gtsam.LevenbergMarquardtParams; 80 | end 81 | 82 | %% graph 83 | obj.Graph = gtsam.NonlinearFactorGraph; 84 | obj.initValues = gtsam.Values; 85 | %% noiseModels 86 | obj.noiseModels.meas = params.measNoise'; 87 | if nargin > 1 88 | obj.noiseModels.depth = params.depthNoise'; 89 | end 90 | obj.noiseModels.prior = gtsam.noiseModel.Diagonal.Sigmas([1e-1; ones(obj.inputDim, 1) * 1e-1]); 91 | 92 | %% compact parameters 93 | obj.compact.isCompact = params.compact; 94 | obj.compact.compRadius = params.compRadius; 95 | 96 | %% build prior graph 97 | for idx = 1:obj.numNodes 98 | obj.initValues.insert(gtsam.symbol('y', idx), [1e-2, zeros(obj.inputDim, 1)']'); % init variables 99 | obj.Graph.add(gtsam.PriorFactorVector(gtsam.symbol('y', idx), [1e-2, zeros(obj.inputDim, 1)']', obj.noiseModels.prior)) 100 | end 101 | 102 | %% add depth cloud 103 | if nargin > 1 104 | depthPrior = depth_map.Location; 105 | normalPrior = depth_map.Normal; 106 | 107 | obj.numDepthPrior = size(depthPrior, 1); 108 | fprintf("Intializing with %d depth points\n", obj.numDepthPrior); 109 | 110 | for idx = 1:obj.numNodes 111 | dist2Measurements = pdist2(obj.queryPoints(idx, :), depthPrior); 112 | % get elements of compact set (all if params.compact = false) 113 | compactSet = find((dist2Measurements < obj.compact.compRadius) | repmat(~obj.compact.isCompact, 1, obj.numDepthPrior)); 114 | for cs = compactSet 115 | y_aug = reshape([zeros(size(depthPrior(cs, :),1), 1)' ; normalPrior(cs, :)'], [], 1); 116 | % add GP spatial factors 117 | if (obj.inputDim == 2) 118 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior2(gtsam.symbol('y', idx), obj.queryPoints(idx, :),... 119 | depthPrior(cs, :), y_aug, obj.theta, obj.noiseModels.depth)); 120 | else 121 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior3(gtsam.symbol('y', idx), obj.queryPoints(idx, :),... 122 | depthPrior(cs, :), y_aug, obj.theta, obj.noiseModels.depth)); 123 | end 124 | end 125 | end 126 | end 127 | fprintf("Adding %d factors\n", obj.Graph.size); 128 | obj.mu = zeros(obj.numNodes, obj.outputDim); 129 | obj.sigma = 1e-1 * ones(obj.numNodes, 1); 130 | obj.X = []; 131 | end 132 | 133 | function Update(obj, x, y) 134 | obj.numMeasurements = size(x, 1); 135 | 136 | obj.X = [obj.X; x]; 137 | fprintf('Updating %d nodes\n', obj.numNodes); 138 | 139 | for idx = 1:obj.numNodes 140 | dist2Measurements = pdist2(obj.queryPoints(idx, :), x); 141 | % get elements of compact set (all if params.compact = false) 142 | compactSet = find((dist2Measurements < obj.compact.compRadius) | repmat(~obj.compact.isCompact, 1, obj.numMeasurements)); 143 | for cs = compactSet 144 | % add GP spatial factors 145 | y_aug = reshape([zeros(size(y(cs, :),1), 1)' ; y(cs, :)'], [], 1); 146 | if (obj.inputDim == 2) 147 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior2(gtsam.symbol('y', idx), obj.queryPoints(idx, :),... 148 | x(cs, :), y_aug, obj.theta, obj.noiseModels.meas)); 149 | else 150 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior3(gtsam.symbol('y', idx), obj.queryPoints(idx, :),... 151 | x(cs, :), y_aug, obj.theta, obj.noiseModels.meas)); 152 | end 153 | end 154 | end 155 | fprintf("Adding %d tactile measurements\n", obj.numMeasurements); 156 | fprintf("Adding %d factors\n", obj.Graph.size); 157 | end 158 | 159 | function [fMean, fVar] = Query(obj) 160 | [results, marginals] = Optimize(obj); 161 | obj.mu = zeros(obj.numNodes, obj.outputDim); obj.sigma = zeros(obj.numNodes, 1); 162 | 163 | for idx = 1:obj.numNodes 164 | temp = results.atVector(gtsam.symbol('y', idx)); % mean 165 | obj.mu(idx, :) = temp'; 166 | mc = marginals.marginalCovariance(gtsam.symbol('y', idx)); % cov 167 | obj.sigma(idx, 1) = mc(1, 1); 168 | end 169 | 170 | fMean = reshape(obj.mu(:, 1), repelem(obj.meshLength, obj.inputDim)); 171 | fVar = reshape(obj.sigma(:, 1), repelem(obj.meshLength, obj.inputDim)); 172 | end 173 | 174 | function [results, marginals] = Optimize(obj) 175 | if obj.solver.useGaussNewton 176 | optimizer = gtsam.GaussNewtonOptimizer(obj.Graph, obj.initValues, obj.solver.optParams); 177 | else 178 | optimizer = gtsam.LevenbergMarquardtOptimizer(obj.Graph, obj.initValues, obj.solver.optParams); 179 | end 180 | 181 | num_iters = 0; 182 | curr_error = 1e10; 183 | while (curr_error - optimizer.error()) / curr_error > obj.solver.optimizeStopRelErr 184 | curr_error = optimizer.error(); 185 | tic; 186 | optimizer.iterate(); 187 | num_iters = num_iters + toc; 188 | fprintf('Updated error: %d\n', optimizer.error()); 189 | end 190 | results = optimizer.values; % mean 191 | marginals = gtsam.Marginals(obj.Graph, results); % cov 192 | end 193 | end 194 | end 195 | -------------------------------------------------------------------------------- /matlab/shape-map/optimizeGraphIncremental.m: -------------------------------------------------------------------------------- 1 | %% GP-SG incremental optimizer 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | classdef optimizeGraphIncremental < handle 28 | properties 29 | %% query points 30 | queryPoints; 31 | numNodes; meshLength; 32 | numMeasurements; 33 | numDepthPrior; 34 | 35 | inputDim; outputDim; 36 | 37 | %% parameters 38 | noiseModels = struct('meas', [], 'prior', []); 39 | compact = struct('isCompact', [], 'compRadius', []); 40 | 41 | theta; 42 | compactSet; 43 | 44 | %% solver 45 | solver = struct('useGaussNewton', [], 'optParams', [], 'optimizeStopRelErr', [], 'isam', []); 46 | Graph; initValues; allValues; 47 | 48 | mu; sigma; 49 | X; Y; 50 | end 51 | 52 | methods 53 | %% constructor 54 | function obj = optimizeGraphIncremental(params) 55 | obj.inputDim = params.dim; 56 | obj.outputDim = obj.inputDim + 1; 57 | %% get query grid 58 | if (obj.inputDim == 2) 59 | Grid = {linspace(params.meshLim(1, 1), params.meshLim(1, 2), params.MeshN),... 60 | linspace(params.meshLim(2, 1), params.meshLim(2, 2), params.MeshN)}; 61 | [xt, yt] = meshgrid(Grid{1}, Grid{2}); 62 | obj.queryPoints = [xt(:), yt(:)]; 63 | obj.numNodes = params.MeshN^2; 64 | elseif (obj.inputDim == 3) 65 | Grid = {linspace(params.meshLim(1, 1), params.meshLim(1, 2), params.MeshN),... 66 | linspace(params.meshLim(2, 1), params.meshLim(2, 2), params.MeshN)... 67 | linspace(params.meshLim(3, 1), params.meshLim(3, 2), params.MeshN)}; 68 | [xt, yt, zt] = meshgrid(Grid{1}, Grid{2}, Grid{3}); 69 | obj.queryPoints = [xt(:), yt(:), zt(:)]; 70 | obj.numNodes = params.MeshN^3; 71 | end 72 | 73 | obj.meshLength = params.MeshN; 74 | obj.numMeasurements = 0; 75 | obj.theta = params.theta'; 76 | 77 | %% isam2 solver object 78 | isamParams = gtsam.ISAM2Params; 79 | obj.solver.isam = gtsam.ISAM2(isamParams); 80 | obj.Graph = gtsam.NonlinearFactorGraph; 81 | obj.initValues = gtsam.Values; 82 | obj.allValues = gtsam.Values; 83 | 84 | %% noiseModels 85 | obj.noiseModels.meas = params.measNoise'; 86 | obj.noiseModels.depth = params.depthNoise'; 87 | obj.noiseModels.prior = gtsam.noiseModel.Diagonal.Sigmas([1e-1; ones(obj.inputDim, 1) * 1e-1]); 88 | 89 | %% compact stuff 90 | obj.compact.isCompact = params.compact; 91 | obj.compact.compRadius = params.compRadius; 92 | 93 | %% build prior graph 94 | for idx = 1:obj.numNodes 95 | obj.initValues.insert(gtsam.symbol('y', idx), [1e-2, zeros(obj.inputDim, 1)']'); % init variables 96 | obj.Graph.add(gtsam.PriorFactorVector(gtsam.symbol('y', idx), [1e-2, zeros(obj.inputDim, 1)']', obj.noiseModels.prior)) 97 | end 98 | 99 | fprintf("Adding %d factors\n", obj.Graph.size); 100 | obj.mu = zeros(obj.numNodes, obj.outputDim); 101 | obj.sigma = 1e-1 * ones(obj.numNodes, 1); 102 | obj.X = []; 103 | end 104 | 105 | function UpdateDepth(obj, depth_map) 106 | depthPrior = depth_map.Location; 107 | normalPrior = depth_map.Normal; 108 | 109 | obj.numDepthPrior = size(depthPrior, 1); 110 | fprintf("Intializing with %d depth points\n", obj.numDepthPrior); 111 | 112 | for idx = 1:obj.numNodes 113 | dist2Measurements = pdist2(obj.queryPoints(idx, :), depthPrior); 114 | % get elements of compact set (all if params.compact = false) 115 | obj.compactSet = find((dist2Measurements < obj.compact.compRadius) | repmat(~obj.compact.isCompact, 1, obj.numDepthPrior)); 116 | for cs = obj.compactSet 117 | % add GP spatial factors 118 | y_aug = reshape([zeros(size(depthPrior(cs, :),1), 1)' ; normalPrior(cs, :)'], [], 1); 119 | if (obj.inputDim == 2) 120 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior2(gtsam.symbol('y', idx), obj.queryPoints(idx, :),... 121 | depthPrior(cs, :), y_aug, obj.theta, obj.noiseModels.depth)); 122 | else 123 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior3(gtsam.symbol('y', idx), obj.queryPoints(idx, :),... 124 | depthPrior(cs, :), y_aug, obj.theta, obj.noiseModels.depth)); 125 | end 126 | end 127 | end 128 | end 129 | 130 | function [nodeEdges] = Update(obj, x, y) 131 | numUpdate = size(x, 1); 132 | obj.numMeasurements = obj.numMeasurements + 1; 133 | 134 | obj.X = [obj.X; x]; 135 | 136 | dist2Measurements = pdist2(obj.queryPoints, x); 137 | % get elements of compact set (all if params.compact = false) 138 | [r, c] = find((dist2Measurements < obj.compact.compRadius) | repmat(~obj.compact.isCompact, obj.numNodes, numUpdate)); 139 | obj.compactSet = [ r, c]; 140 | nodeEdges = zeros(size(obj.compactSet, 1), 2); 141 | 142 | count = 1; 143 | tic; 144 | for cs = obj.compactSet' 145 | % add GP spatial factors 146 | y_aug = reshape([0 ; y(cs(2), :)'], [], 1); 147 | if (obj.inputDim == 2) 148 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior2(gtsam.symbol('y', cs(1)),... 149 | obj.queryPoints(cs(1), :), x(cs(2), :), y_aug, obj.theta, obj.noiseModels.meas)); 150 | else 151 | obj.Graph.add(gpgraph.GaussianProcessSpatialPrior3(gtsam.symbol('y', cs(1)),... 152 | obj.queryPoints(cs(1), :), x(cs(2), :), y_aug, obj.theta, obj.noiseModels.meas)); 153 | end 154 | nodeEdges(count, :) = [cs(1), cs(2)]; 155 | count = count + 1; 156 | end 157 | 158 | fprintf("%f secs, Adding %d factors\n", toc, obj.Graph.size); 159 | obj.solver.isam.update(obj.Graph, obj.initValues); 160 | obj.Graph = gtsam.NonlinearFactorGraph; 161 | obj.initValues = gtsam.Values; 162 | end 163 | 164 | function [fMean, fVar] = Query(obj) 165 | if obj.numMeasurements > 0 166 | set = obj.compactSet'; 167 | else 168 | obj.solver.isam.update(obj.Graph, obj.initValues); 169 | obj.Graph = gtsam.NonlinearFactorGraph; 170 | obj.initValues = gtsam.Values; 171 | set = 1:obj.numNodes; 172 | end 173 | 174 | %% retrieve only modified nodes 175 | for idx = set 176 | temp = obj.solver.isam.calculateEstimateVector(gtsam.symbol('y', idx(1))); % mean 177 | obj.mu(idx(1), :) = temp'; 178 | mc = obj.solver.isam.marginalCovariance(gtsam.symbol('y', idx(1))); % cov 179 | obj.sigma(idx(1), 1) = mc(1, 1); 180 | end 181 | 182 | fMean = reshape(obj.mu(:, 1), repelem(obj.meshLength, obj.inputDim)); 183 | fVar = reshape(obj.sigma(:, 1), repelem(obj.meshLength, obj.inputDim)); 184 | end 185 | end 186 | end -------------------------------------------------------------------------------- /matlab/shape-map/vizHeightMapsTest.m: -------------------------------------------------------------------------------- 1 | %% Test script, visualize heightmaps and contact masks 2 | 3 | % Copyright (c) 2021 Sudharshan Suresh 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 | % 1. Redistributions of source code must retain the above copyright notice, this 10 | % list of conditions and the following disclaimer. 11 | 12 | % 2. 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" AND 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 20 | % ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | clc; clear; close all; 28 | 29 | % load gelsight data 30 | load('/home/suddhu/projects/Shape_Mapping_Tactile/processed_data/mustard_bottle/mustard_bottle_gelsight.mat'); 31 | % load the model and sample points 32 | load('/home/suddhu/projects/Shape_Mapping_Tactile/data/mustard_bottle/mustard_bottle_50sampled.mat'); 33 | elev = 30; 34 | 35 | N = size(heightmaps,2); 36 | 37 | figure('Name', 'Gelsight output'); 38 | 39 | s1 = subplot(2, 2, [1 2]); 40 | hold on; 41 | trisurf(faces, vertices(:,1), vertices(:,2), vertices(:,3), 'EdgeColor', 'none', 'FaceAlpha', 0.7); 42 | axis equal off 43 | shading interp 44 | camlight; lighting phong 45 | colormap(s1,gray); 46 | view(-37.5, elev); 47 | xlim manual; ylim manual; zlim manual; 48 | 49 | for i = 1:N 50 | subplot(2, 2, [1 2]); 51 | poses(i, [4, 5, 6, 7]) = poses(i, [7, 4, 5, 6]); % x y z w -> w x y z 52 | sensorPose = SE3(quat2rotm(poses(i, 4:end))', poses(i, 1:3)); 53 | sensorPose.plot('rgb', 'length', 0.03, 'labels', ' ', 'thick', 1); 54 | plot3(samplePoints(i, 1), samplePoints(i, 2), samplePoints(i, 3), 'yo'); 55 | 56 | heightmap = reshape(heightmaps(:, i),[480,640]); 57 | s2 = subplot(2,2,3); 58 | imshow(heightmap, []); 59 | title(strcat('Heightmap #', num2str(i))) 60 | colormap(s2, jet); 61 | 62 | contactmask = reshape(contactmasks(:, i),[480,640]); 63 | s3 = subplot(2,2,4); 64 | imshow(contactmask, []); 65 | title(strcat('Contactmask #', num2str(i))) 66 | pause(1); 67 | end -------------------------------------------------------------------------------- /matlab/utils/ScaleAndOffset.m: -------------------------------------------------------------------------------- 1 | %% ScaleAndOffset.m: scale and offset measurements 2 | function [X] = ScaleAndOffset(X) 3 | Xmin = min(X); Xmax = max(X); 4 | scale = Xmax - Xmin; 5 | X = (X - Xmin)./scale; 6 | end 7 | 8 | -------------------------------------------------------------------------------- /matlab/utils/addNoise.m: -------------------------------------------------------------------------------- 1 | %% addNoise.m: adds noise to X and Y data 2 | function [X,Y] = addNoise(X,Y, mn, in) 3 | X = X + in.*randn(size(X)); 4 | Y = Y + mn.*randn(size(Y)); 5 | Y = normr(Y); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /matlab/utils/cleanUp.m: -------------------------------------------------------------------------------- 1 | %% cleanUp.m: prune mesh away from points 2 | function iso = cleanUp(X, iso, thresh) 3 | D = pdist2(X, iso.vertices, 'euclidean', 'Smallest', 1)'; 4 | pruneVertices = find(mean(D,2) > thresh)'; 5 | vertices = iso.vertices; 6 | vertices(pruneVertices,:) = []; % remove 7 | 8 | [~, newIdx] = ismember(iso.vertices, vertices, 'rows'); 9 | 10 | newFaces = iso.faces(all([iso.faces(:,1) ~= pruneVertices, iso.faces(:,2) ~= pruneVertices, iso.faces(:,3) ~= pruneVertices], 2),:); 11 | newFaces = newIdx(newFaces); 12 | iso.vertices = vertices; iso.faces = newFaces; 13 | end 14 | 15 | -------------------------------------------------------------------------------- /matlab/utils/plotPointsAndNormals3D.m: -------------------------------------------------------------------------------- 1 | %% plotPointsAndNormals3D.m: visualizes surface measurement and normal 2 | function plotPointsAndNormals3D(X, Y, sc, c) 3 | rc = [rand,rand,rand]; 4 | plot3(X(:,1),X(:,2),X(:,3),'.','MarkerSize',1, 'Color', c, 'MarkerFaceColor', c); 5 | if ~isempty(Y) 6 | quiver3(X(:,1),X(:,2),X(:,3),sc*Y(:,1),sc*Y(:,2),sc*Y(:,3), 'Color', c); 7 | end 8 | end 9 | 10 | -------------------------------------------------------------------------------- /matlab/utils/slice3D.m: -------------------------------------------------------------------------------- 1 | %% slice3D.m: slice across the SDF volume 2 | function slice3D(x, y, z, val, xyzslice) 3 | % cross-sections 4 | xslice = xyzslice(1); yslice = xyzslice(2); zslice = xyzslice(3); 5 | s = slice(x, y, z, val, xslice, yslice, zslice); % slice it! 6 | alpha(s, 0.5); 7 | set(s,'linewidth',1); 8 | caxis([0, 0.1]); 9 | end 10 | 11 | -------------------------------------------------------------------------------- /matlab/utils/splitFV.m: -------------------------------------------------------------------------------- 1 | function [iso, maxSet] = splitFV( f, v ) 2 | %SPLITFV Splits faces and vertices into connected pieces 3 | % FVOUT = SPLITFV(F,V) separates disconnected pieces inside a patch defined by faces (F) and 4 | % vertices (V). FVOUT is a structure array with fields "faces" and "vertices". Each element of 5 | % this array indicates a separately connected patch. 6 | % 7 | % FVOUT = SPLITFV(FV) takes in FV as a structure with fields "faces" and "vertices" 8 | % 9 | % For example: 10 | % fullpatch.vertices = [2 4; 2 8; 8 4; 8 0; 0 4; 2 6; 2 2; 4 2; 4 0; 5 2; 5 0]; 11 | % fullpatch.faces = [1 2 3; 1 3 4; 5 6 1; 7 8 9; 11 10 4]; 12 | % figure, subplot(2,1,1), patch(fullpatch,'facecolor','r'), title('Unsplit mesh'); 13 | % splitpatch = splitFV(fullpatch); 14 | % colours = lines(length(splitpatch)); 15 | % subplot(2,1,2), hold on, title('Split mesh'); 16 | % for i=1:length(splitpatch) 17 | % patch(splitpatch(i),'facecolor',colours(i,:)); 18 | % end 19 | % 20 | % Note: faces and vertices should be defined such that faces sharing a coincident vertex reference 21 | % the same vertex number, rather than having a separate vertice defined for each face (yet at the 22 | % same vertex location). In other words, running the following command: size(unique(v,'rows') == 23 | % size(v) should return TRUE. An explicit test for this has not been included in this function so 24 | % as to allow for the deliberate splitting of a mesh at a given location by simply duplicating 25 | % those vertices. 26 | % 27 | % See also PATCH 28 | % Copyright Sven Holcombe 29 | % $Date: 2010/05/19 $ 30 | %% Extract f and v 31 | if nargin==1 && isstruct(f) && all(isfield(f,{'faces','vertices'})) 32 | v = f.vertices; 33 | f = f.faces; 34 | elseif nargin==2 35 | % f and v are already defined 36 | else 37 | error('splitFV:badArgs','splitFV takes a faces/vertices structure, or these fields passed individually') 38 | end 39 | %% Organise faces into connected fSets that share nodes 40 | fSets = zeros(size(f,1),1,'uint32'); 41 | currentSet = 0; 42 | while any(fSets==0) 43 | currentSet = currentSet + 1; 44 | % fprintf('Connecting set #%d vertices...',currentSet); 45 | nextAvailFace = find(fSets==0,1,'first'); 46 | openVertices = f(nextAvailFace,:); 47 | while ~isempty(openVertices) 48 | availFaceInds = find(fSets==0); 49 | [availFaceSub, ~] = find(ismember(f(availFaceInds,:), openVertices)); 50 | fSets(availFaceInds(availFaceSub)) = currentSet; 51 | openVertices = f(availFaceInds(availFaceSub),:); 52 | end 53 | % fprintf(' done! Set #%d has %d faces.\n',currentSet,nnz(fSets==currentSet)); 54 | end 55 | numSets = currentSet; 56 | %% Create separate faces/vertices structures for each fSet 57 | fvOut = repmat(struct('faces',[],'vertices',[]),numSets,1); 58 | 59 | maxCount = 0; 60 | for currentSet = 1:numSets 61 | setF = f(fSets==currentSet,:); 62 | [unqVertIds, ~, newVertIndices] = unique(setF); 63 | fvOut(currentSet).faces = reshape(newVertIndices,size(setF)); 64 | fvOut(currentSet).vertices = v(unqVertIds,:); 65 | 66 | if (nnz(fSets==currentSet) > maxCount) 67 | maxSet = currentSet; 68 | maxCount = nnz(fSets==currentSet); 69 | end 70 | end 71 | 72 | maxisoLength = 0; maxisoIdx = 1; 73 | for s = 1:length(fvOut) 74 | if length(fvOut(s).faces) > maxisoLength 75 | maxisoLength = length(fvOut(s).faces); 76 | maxisoIdx = s; 77 | end 78 | end 79 | 80 | iso.faces = []; iso.vertices = []; 81 | iso.faces = fvOut(maxisoIdx).faces; 82 | iso.vertices = fvOut(maxisoIdx).vertices; 83 | 84 | -------------------------------------------------------------------------------- /matlab/utils/stlread.m: -------------------------------------------------------------------------------- 1 | function varargout = stlread(file) 2 | % STLREAD imports geometry from an STL file into MATLAB. 3 | % FV = STLREAD(FILENAME) imports triangular faces from the ASCII or binary 4 | % STL file idicated by FILENAME, and returns the patch struct FV, with fields 5 | % 'faces' and 'vertices'. 6 | % 7 | % [F,V] = STLREAD(FILENAME) returns the faces F and vertices V separately. 8 | % 9 | % [F,V,N] = STLREAD(FILENAME) also returns the face normal vectors. 10 | % 11 | % The faces and vertices are arranged in the format used by the PATCH plot 12 | % object. 13 | 14 | % Copyright 2011 The MathWorks, Inc. 15 | 16 | if ~exist(file,'file') 17 | error(['File ''%s'' not found. If the file is not on MATLAB''s path' ... 18 | ', be sure to specify the full path to the file.'], file); 19 | end 20 | 21 | fid = fopen(file,'r'); 22 | if ~isempty(ferror(fid)) 23 | error(lasterror); %#ok 24 | end 25 | 26 | M = fread(fid,inf,'uint8=>uint8'); 27 | fclose(fid); 28 | 29 | [f,v,n] = stlbinary(M); 30 | %if( isbinary(M) ) % This may not be a reliable test 31 | % [f,v,n] = stlbinary(M); 32 | %else 33 | % [f,v,n] = stlascii(M); 34 | %end 35 | 36 | varargout = cell(1,nargout); 37 | switch nargout 38 | case 2 39 | varargout{1} = f; 40 | varargout{2} = v; 41 | case 3 42 | varargout{1} = f; 43 | varargout{2} = v; 44 | varargout{3} = n; 45 | otherwise 46 | varargout{1} = struct('faces',f,'vertices',v); 47 | end 48 | 49 | end 50 | 51 | 52 | function [F,V,N] = stlbinary(M) 53 | 54 | F = []; 55 | V = []; 56 | N = []; 57 | 58 | if length(M) < 84 59 | error('MATLAB:stlread:incorrectFormat', ... 60 | 'Incomplete header information in binary STL file.'); 61 | end 62 | 63 | % Bytes 81-84 are an unsigned 32-bit integer specifying the number of faces 64 | % that follow. 65 | numFaces = typecast(M(81:84),'uint32'); 66 | %numFaces = double(numFaces); 67 | if numFaces == 0 68 | warning('MATLAB:stlread:nodata','No data in STL file.'); 69 | return 70 | end 71 | 72 | T = M(85:end); 73 | F = NaN(numFaces,3); 74 | V = NaN(3*numFaces,3); 75 | N = NaN(numFaces,3); 76 | 77 | numRead = 0; 78 | while numRead < numFaces 79 | % Each facet is 50 bytes 80 | % - Three single precision values specifying the face normal vector 81 | % - Three single precision values specifying the first vertex (XYZ) 82 | % - Three single precision values specifying the second vertex (XYZ) 83 | % - Three single precision values specifying the third vertex (XYZ) 84 | % - Two unused bytes 85 | i1 = 50 * numRead + 1; 86 | i2 = i1 + 50 - 1; 87 | facet = T(i1:i2)'; 88 | 89 | n = typecast(facet(1:12),'single'); 90 | v1 = typecast(facet(13:24),'single'); 91 | v2 = typecast(facet(25:36),'single'); 92 | v3 = typecast(facet(37:48),'single'); 93 | 94 | n = double(n); 95 | v = double([v1; v2; v3]); 96 | 97 | % Figure out where to fit these new vertices, and the face, in the 98 | % larger F and V collections. 99 | fInd = numRead + 1; 100 | vInd1 = 3 * (fInd - 1) + 1; 101 | vInd2 = vInd1 + 3 - 1; 102 | 103 | V(vInd1:vInd2,:) = v; 104 | F(fInd,:) = vInd1:vInd2; 105 | N(fInd,:) = n; 106 | 107 | numRead = numRead + 1; 108 | end 109 | 110 | end 111 | 112 | 113 | function [F,V,N] = stlascii(M) 114 | warning('MATLAB:stlread:ascii','ASCII STL files currently not supported.'); 115 | F = []; 116 | V = []; 117 | N = []; 118 | end 119 | 120 | % TODO: Change the testing criteria! Some binary STL files still begin with 121 | % 'solid'. 122 | function tf = isbinary(A) 123 | % ISBINARY uses the first line of an STL file to identify its format. 124 | if isempty(A) || length(A) < 5 125 | error('MATLAB:stlread:incorrectFormat', ... 126 | 'File does not appear to be an ASCII or binary STL file.'); 127 | end 128 | if strcmpi('solid',char(A(1:5)')) 129 | tf = false; % ASCII 130 | else 131 | tf = true; % Binary 132 | end 133 | end 134 | 135 | 136 | -------------------------------------------------------------------------------- /matlab/utils/stlwrite.m: -------------------------------------------------------------------------------- 1 | function stlwrite(filename, varargin) 2 | %STLWRITE Write STL file from patch or surface data. 3 | % 4 | % STLWRITE(FILE, FV) writes a stereolithography (STL) file to FILE for a 5 | % triangulated patch defined by FV (a structure with fields 'vertices' 6 | % and 'faces'). 7 | % 8 | % STLWRITE(FILE, FACES, VERTICES) takes faces and vertices separately, 9 | % rather than in an FV struct 10 | % 11 | % STLWRITE(FILE, X, Y, Z) creates an STL file from surface data in X, Y, 12 | % and Z. STLWRITE triangulates this gridded data into a triangulated 13 | % surface using triangulation options specified below. X, Y and Z can be 14 | % two-dimensional arrays with the same size. If X and Y are vectors with 15 | % length equal to SIZE(Z,2) and SIZE(Z,1), respectively, they are passed 16 | % through MESHGRID to create gridded data. If X or Y are scalar values, 17 | % they are used to specify the X and Y spacing between grid points. 18 | % 19 | % STLWRITE(...,'PropertyName',VALUE,'PropertyName',VALUE,...) writes an 20 | % STL file using the following property values: 21 | % 22 | % MODE - File is written using 'binary' (default) or 'ascii'. 23 | % 24 | % TITLE - Header text (max 80 chars) written to the STL file. 25 | % 26 | % TRIANGULATION - When used with gridded data, TRIANGULATION is either: 27 | % 'delaunay' - (default) Delaunay triangulation of X, Y 28 | % 'f' - Forward slash division of grid quads 29 | % 'b' - Back slash division of quadrilaterals 30 | % 'x' - Cross division of quadrilaterals 31 | % Note that 'f', 'b', or 't' triangulations now use an 32 | % inbuilt version of FEX entry 28327, "mesh2tri". 33 | % 34 | % FACECOLOR - Single colour (1-by-3) or one-colour-per-face (N-by-3) 35 | % vector of RGB colours, for face/vertex input. RGB range 36 | % is 5 bits (0:31), stored in VisCAM/SolidView format 37 | % (http://en.wikipedia.org/wiki/STL_(file_format)#Color_in_binary_STL) 38 | % 39 | % Example 1: 40 | % % Write binary STL from face/vertex data 41 | % tmpvol = false(20,20,20); % Empty voxel volume 42 | % tmpvol(8:12,8:12,5:15) = 1; % Turn some voxels on 43 | % fv = isosurface(~tmpvol, 0.5); % Make patch w. faces "out" 44 | % stlwrite('test.stl',fv) % Save to binary .stl 45 | % 46 | % Example 2: 47 | % % Write ascii STL from gridded data 48 | % [X,Y] = deal(1:40); % Create grid reference 49 | % Z = peaks(40); % Create grid height 50 | % stlwrite('test.stl',X,Y,Z,'mode','ascii') 51 | % 52 | % Example 3: 53 | % % Write binary STL with coloured faces 54 | % cVals = fv.vertices(fv.faces(:,1),3); % Colour by Z height. 55 | % cLims = [min(cVals) max(cVals)]; % Transform height values 56 | % nCols = 255; cMap = jet(nCols); % onto an 8-bit colour map 57 | % fColsDbl = interp1(linspace(cLims(1),cLims(2),nCols),cMap,cVals); 58 | % fCols8bit = fColsDbl*255; % Pass cols in 8bit (0-255) RGB triplets 59 | % stlwrite('testCol.stl',fv,'FaceColor',fCols8bit) 60 | 61 | % Original idea adapted from surf2stl by Bill McDonald. Huge speed 62 | % improvements implemented by Oliver Woodford. Non-Delaunay triangulation 63 | % of quadrilateral surface courtesy of Kevin Moerman. FaceColor 64 | % implementation by Grant Lohsen. 65 | % 66 | % Author: Sven Holcombe, 11-24-11 67 | 68 | 69 | % Check valid filename path 70 | path = fileparts(filename); 71 | if ~isempty(path) && ~exist(path,'dir') 72 | error('Directory "%s" does not exist.',path); 73 | end 74 | 75 | % Get faces, vertices, and user-defined options for writing 76 | [faces, vertices, options] = parseInputs(varargin{:}); 77 | asciiMode = strcmp( options.mode ,'ascii'); 78 | 79 | % Create the facets 80 | facets = single(vertices'); 81 | facets = reshape(facets(:,faces'), 3, 3, []); 82 | 83 | % Compute their normals 84 | V1 = squeeze(facets(:,2,:) - facets(:,1,:)); 85 | V2 = squeeze(facets(:,3,:) - facets(:,1,:)); 86 | normals = V1([2 3 1],:) .* V2([3 1 2],:) - V2([2 3 1],:) .* V1([3 1 2],:); 87 | clear V1 V2 88 | normals = bsxfun(@times, normals, 1 ./ sqrt(sum(normals .* normals, 1))); 89 | facets = cat(2, reshape(normals, 3, 1, []), facets); 90 | clear normals 91 | 92 | % Open the file for writing 93 | permissions = {'w','wb+'}; 94 | fid = fopen(filename, permissions{asciiMode+1}); 95 | if (fid == -1) 96 | error('stlwrite:cannotWriteFile', 'Unable to write to %s', filename); 97 | end 98 | 99 | % Write the file contents 100 | if asciiMode 101 | % Write HEADER 102 | fprintf(fid,'solid %s\r\n',options.title); 103 | % Write DATA 104 | fprintf(fid,[... 105 | 'facet normal %.7E %.7E %.7E\r\n' ... 106 | 'outer loop\r\n' ... 107 | 'vertex %.7E %.7E %.7E\r\n' ... 108 | 'vertex %.7E %.7E %.7E\r\n' ... 109 | 'vertex %.7E %.7E %.7E\r\n' ... 110 | 'endloop\r\n' ... 111 | 'endfacet\r\n'], facets); 112 | % Write FOOTER 113 | fprintf(fid,'endsolid %s\r\n',options.title); 114 | 115 | else % BINARY 116 | % Write HEADER 117 | fprintf(fid, '%-80s', options.title); % Title 118 | fwrite(fid, size(facets, 3), 'uint32'); % Number of facets 119 | % Write DATA 120 | % Add one uint16(0) to the end of each facet using a typecasting trick 121 | facets = reshape(typecast(facets(:), 'uint16'), 12*2, []); 122 | % Set the last bit to 0 (default) or supplied RGB 123 | facets(end+1,:) = options.facecolor; 124 | fwrite(fid, facets, 'uint16'); 125 | end 126 | 127 | % Close the file 128 | fclose(fid); 129 | % fprintf('Wrote %d faces\n',size(faces, 2)); 130 | 131 | 132 | %% Input handling subfunctions 133 | function [faces, vertices, options] = parseInputs(varargin) 134 | % Determine input type 135 | if isstruct(varargin{1}) % stlwrite('file', FVstruct, ...) 136 | if ~all(isfield(varargin{1},{'vertices','faces'})) 137 | error( 'Variable p must be a faces/vertices structure' ); 138 | end 139 | faces = varargin{1}.faces; 140 | vertices = varargin{1}.vertices; 141 | options = parseOptions(varargin{2:end}); 142 | 143 | elseif isnumeric(varargin{1}) 144 | firstNumInput = cellfun(@isnumeric,varargin); 145 | firstNumInput(find(~firstNumInput,1):end) = 0; % Only consider numerical input PRIOR to the first non-numeric 146 | numericInputCnt = nnz(firstNumInput); 147 | 148 | options = parseOptions(varargin{numericInputCnt+1:end}); 149 | switch numericInputCnt 150 | case 3 % stlwrite('file', X, Y, Z, ...) 151 | % Extract the matrix Z 152 | Z = varargin{3}; 153 | 154 | % Convert scalar XY to vectors 155 | ZsizeXY = fliplr(size(Z)); 156 | for i = 1:2 157 | if isscalar(varargin{i}) 158 | varargin{i} = (0:ZsizeXY(i)-1) * varargin{i}; 159 | end 160 | end 161 | 162 | % Extract X and Y 163 | if isequal(size(Z), size(varargin{1}), size(varargin{2})) 164 | % X,Y,Z were all provided as matrices 165 | [X,Y] = varargin{1:2}; 166 | elseif numel(varargin{1})==ZsizeXY(1) && numel(varargin{2})==ZsizeXY(2) 167 | % Convert vector XY to meshgrid 168 | [X,Y] = meshgrid(varargin{1}, varargin{2}); 169 | else 170 | error('stlwrite:badinput', 'Unable to resolve X and Y variables'); 171 | end 172 | 173 | % Convert to faces/vertices 174 | if strcmp(options.triangulation,'delaunay') 175 | faces = delaunay(X,Y); 176 | vertices = [X(:) Y(:) Z(:)]; 177 | else 178 | error('stlwrite:missing', '"mesh2tri" is required to convert X,Y,Z matrices to STL. It can be downloaded from:\n%s\n',... 179 | 'http://www.mathworks.com/matlabcentral/fileexchange/28327') 180 | end 181 | 182 | case 2 % stlwrite('file', FACES, VERTICES, ...) 183 | faces = varargin{1}; 184 | vertices = varargin{2}; 185 | 186 | otherwise 187 | error('stlwrite:badinput', 'Unable to resolve input types.'); 188 | end 189 | end 190 | 191 | if size(faces,2)~=3 192 | errorMsg = { 193 | sprintf('The FACES input array should hold triangular faces (N x 3), but was detected as N x %d.',size(faces,2)) 194 | 'The STL format is for triangulated surfaces (i.e., surfaces made from 3-sided triangles).' 195 | 'The Geom3d package (https://www.mathworks.com/matlabcentral/fileexchange/24484-geom3d) contains' 196 | 'a "triangulateFaces" function which can be used convert your faces into triangles.' 197 | }; 198 | error('stlwrite:nonTriangles', '%s\n',errorMsg{:}) 199 | end 200 | 201 | if ~isempty(options.facecolor) % Handle colour preparation 202 | facecolor = uint16(options.facecolor); 203 | %Set the Valid Color bit (bit 15) 204 | c0 = bitshift(ones(size(faces,1),1,'uint16'),15); 205 | %Red color (10:15), Blue color (5:9), Green color (0:4) 206 | c0 = bitor(bitshift(bitand(2^6-1, facecolor(:,1)),10),c0); 207 | c0 = bitor(bitshift(bitand(2^11-1, facecolor(:,2)),5),c0); 208 | c0 = bitor(bitand(2^6-1, facecolor(:,3)),c0); 209 | options.facecolor = c0; 210 | else 211 | options.facecolor = 0; 212 | end 213 | 214 | function options = parseOptions(varargin) 215 | IP = inputParser; 216 | IP.addParamValue('mode', 'binary', @ischar) 217 | IP.addParamValue('title', sprintf('Created by stlwrite.m %s',datestr(now)), @ischar); 218 | IP.addParamValue('triangulation', 'delaunay', @ischar); 219 | IP.addParamValue('facecolor',[], @isnumeric) 220 | IP.addParamValue('facecolour',[], @isnumeric) 221 | IP.parse(varargin{:}); 222 | options = IP.Results; 223 | if ~isempty(options.facecolour) 224 | options.facecolor = options.facecolour; 225 | end 226 | 227 | V=[X(:),Y(:),Z(:)]; 228 | -------------------------------------------------------------------------------- /media/cover.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/shape-map-3D/983b48b1bab433b876294f31abde3f2f70b7ef23/media/cover.jpg -------------------------------------------------------------------------------- /media/robotouch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/shape-map-3D/983b48b1bab433b876294f31abde3f2f70b7ef23/media/robotouch.png -------------------------------------------------------------------------------- /media/rpl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/shape-map-3D/983b48b1bab433b876294f31abde3f2f70b7ef23/media/rpl.png -------------------------------------------------------------------------------- /media/shape-map.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/shape-map-3D/983b48b1bab433b876294f31abde3f2f70b7ef23/media/shape-map.gif --------------------------------------------------------------------------------