├── .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 | [](https://github.com/rpl-cmu/shape-map-3D/blob/master/LICENSE)
4 |
5 |  
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
--------------------------------------------------------------------------------