├── CMakeLists.txt ├── README.md ├── data ├── bunny │ ├── README.bunny.txt │ ├── README.md │ ├── bun000.ply │ ├── bun000mesh.ply │ ├── bun045.ply │ └── bun045mesh.ply └── pcl_data │ ├── LICENSE │ ├── README.md │ ├── milk.pcd │ └── milk_cartoon_all_small_clorox.pcd └── src ├── icp1_simple.cpp ├── icp2_iterative_view.cpp ├── icp3_with_normal_iterative_view.cpp ├── icp4_after_feature_registration.cpp ├── transform_estimation.cpp ├── visualize_correspondences.cpp └── visualize_correspondences.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | set(CMAKE_VERBOSE_MAKEFILE 1) # to see all make process 3 | 4 | project( icp_samples ) 5 | 6 | find_package(PCL 1.7 REQUIRED) 7 | 8 | include_directories(${PCL_INCLUDE_DIRS}) 9 | link_directories(${PCL_LIBRARY_DIRS}) 10 | add_definitions(${PCL_DEFINITIONS}) 11 | 12 | add_executable (icp1_simple src/icp1_simple.cpp) 13 | target_link_libraries (icp1_simple ${PCL_LIBRARIES}) 14 | 15 | add_executable (icp2_iterative_view src/icp2_iterative_view.cpp) 16 | target_link_libraries (icp2_iterative_view ${PCL_LIBRARIES}) 17 | 18 | add_executable (icp3_with_normal_iterative_view src/icp3_with_normal_iterative_view.cpp) 19 | target_link_libraries (icp3_with_normal_iterative_view ${PCL_LIBRARIES}) 20 | 21 | add_executable (icp4_after_feature_registration src/icp4_after_feature_registration.cpp src/visualize_correspondences.cpp) 22 | target_link_libraries (icp4_after_feature_registration ${PCL_LIBRARIES}) 23 | 24 | 25 | add_executable (transform_estimation src/transform_estimation.cpp) 26 | target_link_libraries (transform_estimation ${PCL_LIBRARIES}) 27 | 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ICP-test 2 | ======== 3 | 4 | This is simple ICP registration codes by using PCL 1.7 APIs. 5 | 6 | Requirements 7 | ------------ 8 | - pcl > 1.7 9 | - cmake > 2.8 10 | - eigen > 3 11 | 12 | Codes 13 | ----- 14 | - `icp1_simple.cpp` 15 | simple registration with ICP. Only results are shown. 16 | - `icp2_iterative_view.cpp` 17 | simple registration with ICP. Animated registration process is shown. 18 | - `icp3_with_normal_iterative_view.cpp` 19 | registration by using ICP with normal vector information. Animated registration process is shown. 20 | - `icp4_after_feature_registration.cpp` 21 | registration by using feature detection, description, matching, followed by ICP for fine registration. 22 | - `transform_estimation` 23 | estimation of R and t (and scale) from given two sets of points 24 | 25 | Build 26 | ----- 27 | 28 | ``` 29 | mkdir build 30 | cd build 31 | cmake .. 32 | make 33 | ``` 34 | 35 | Run 36 | ---- 37 | 38 | ``` 39 | cd build 40 | ``` 41 | 42 | ### sample 1 43 | 44 | Simple registration with ICP. Only results are shown. 45 | 46 | ``` 47 | $ ./icp1_simple ../data/bunny/bun{000,045}mesh.ply 48 | Converged. score =1.19601e-05 49 | 0.827246 0.00948285 -0.561763 0.0341258 50 | -0.012711 0.99992 -0.00183846 0.000735376 51 | 0.5617 0.00866113 0.827298 0.0383121 52 | 0 0 0 1 53 | ``` 54 | 55 | ### sample 2 56 | 57 | Simple registration with ICP. Animated registration process is shown. 58 | 59 | ``` 60 | $ ./icp2_iterative_view ../data/bunny/bun{000,045}mesh.ply 61 | 0.000115243 62 | 6.63744e-05 63 | 3.57769e-05 64 | 2.65457e-05 65 | 1.99575e-05 66 | 1.64426e-05 67 | 1.42573e-05 68 | 1.27708e-05 69 | 1.17543e-05 70 | 1.10658e-05 71 | 1.0597e-05 72 | 1.02805e-05 73 | 1.00704e-05 74 | 9.92269e-06 75 | 9.82098e-06 76 | 9.74741e-06 77 | ........(omit) 78 | ``` 79 | press `q` to stop. 80 | 81 | 82 | ### sample 3 83 | 84 | Registration by using ICP with normal vector information. Animated registration process is shown. 85 | 86 | ``` 87 | $ ./icp3_with_normal_iterative_view ../data/bunny/bun{000,045}mesh.ply 88 | 0.000228403 89 | 8.08289e-05 90 | 2.11145e-05 91 | 2.11849e-05 92 | 1.23206e-05 93 | 1.10054e-05 94 | 1.09729e-05 95 | 1.09652e-05 96 | 1.09648e-05 97 | 1.09648e-05 98 | 1.09648e-05 99 | 1.09648e-05 100 | 1.09648e-05 101 | 1.09648e-05 102 | 1.09648e-05 103 | ........(omit) 104 | ``` 105 | press `q` to stop. 106 | 107 | 108 | ### sample 4 109 | 110 | Registration by using ICP with normal vector information. Animated registration process is shown. 111 | 112 | ``` 113 | $ ./icp4_after_feature_registration ../data/pcl_data/milk.pcd ../data/pcl_data/milk_cartoon_all_small_clorox.pcd 114 | scale: 0.00639819 115 | detection 116 | number of source keypoints found: 224 117 | number of target keypoints found: 29130 118 | description 119 | Estimating transformation 120 | 0.968323 -0.130764 0.212724 -0.16276 121 | 0.129827 0.991365 0.0184329 0.205639 122 | -0.213298 0.00976849 0.976938 -0.0389752 123 | 0 0 0 1 124 | ``` 125 | 126 | + Correspondences (matches) are shown with lines. 127 | There are many outliers. 128 | press `q` to proceed. 129 | + Outlier matches are rejected. 130 | press `q` to proceed. 131 | + Aligned the milk to the scene with matched points. 132 | press `q` to proceed. 133 | + Final refinement by PCL. 134 | Estimated R and t are shown. 135 | press `q` to stop. 136 | 137 | 138 | 139 | 140 | ### sample : R and t estimation 141 | 142 | Estimation of R and t (and scale) from given two sets of points. 143 | - One is randomly generated points, and 144 | - the other is transfomed by R, t (and optionaly s). 145 | 146 | Options: 147 | - `-s 1` : scale is estimated. otherwise R and t only. default: `-s 0` 148 | - `-r 1` : the result changes every time because randomly generated points, R and t are used. otherwise fixed result. default: `-r 0` 149 | - `-m` : select method: `-m 0` svd, `-m 1` dual quaternion, `-m 2` nonlinear optimization (LM). default: `-m 0` 150 | 151 | + R, t and s are estimatd if `-s 1` is given. 152 | 153 | ``` 154 | $ ./transform_estimation -s 1 155 | method: SVD 156 | use scale: true 157 | forse SVD. 158 | use random seed: false 159 | true R 160 | 0.382339 -0.813161 0.438847 0 161 | 0.327021 -0.325114 -0.887332 0 162 | 0.864219 0.482773 0.141617 0 163 | 0 0 0 1 164 | true T 165 | 1.41126 166 | 2.84138 167 | 1.76013 168 | true sR 169 | 0.568925 -1.20999 0.653009 0 170 | 0.486611 -0.483774 -1.32036 0 171 | 1.28597 0.718373 0.210728 0 172 | 0 0 0 1 173 | true scale 1.48801 174 | true transformation 175 | 0.568925 -1.20999 0.653009 1.41126 176 | 0.486611 -0.483774 -1.32036 2.84138 177 | 1.28597 0.718373 0.210728 1.76013 178 | 0 0 0 1 179 | estimated scale 1.48801 180 | estimated transformation 181 | 0.568925 -1.20999 0.65301 1.41126 182 | 0.486611 -0.483774 -1.32036 2.84138 183 | 1.28597 0.718373 0.210728 1.76013 184 | 0 0 0 1 185 | ``` 186 | 187 | 188 | + R, t are estimatd if `-s 0` (or no option) is given (default) 189 | 190 | ``` 191 | $ ./transform_estimation 192 | method: SVD 193 | use scale: false 194 | use random seed: false 195 | true R 196 | 0.382339 -0.813161 0.438847 0 197 | 0.327021 -0.325114 -0.887332 0 198 | 0.864219 0.482773 0.141617 0 199 | 0 0 0 1 200 | true T 201 | 1.41126 202 | 2.84138 203 | 1.76013 204 | true transformation 205 | 0.382339 -0.813161 0.438847 1.41126 206 | 0.327021 -0.325114 -0.887332 2.84138 207 | 0.864219 0.482773 0.141617 1.76013 208 | 0 0 0 1 209 | estimated transformation 210 | 0.382339 -0.813161 0.438847 1.41126 211 | 0.327021 -0.325114 -0.887332 2.84138 212 | 0.864219 0.482773 0.141617 1.76013 213 | 0 0 0 1 214 | ``` 215 | -------------------------------------------------------------------------------- /data/bunny/README.bunny.txt: -------------------------------------------------------------------------------- 1 | Range Data 2 | 3 | Stanford Range Repository 4 | Computer Graphics Laboratory 5 | Stanford University 6 | 7 | August 4, 1996 8 | 9 | 10 | These data files were obtained with a Cyberware 3030MS optical 11 | triangulation scanner. They are stored as range images in the "ply" 12 | format. The ".conf" file contains the transformations required to 13 | bring each range image into a single coordinate system. 14 | 15 | For more information, consult the web pages of the Stanford Graphics 16 | Laboratory: 17 | 18 | http://www-graphics.stanford.edu 19 | 20 | -------------------------------------------------------------------------------- /data/bunny/README.md: -------------------------------------------------------------------------------- 1 | This folder contains the standford bunny data (bun000.ply, bun045.ply) 2 | obtained from http://graphics.stanford.edu/data/3Dscanrep/ . 3 | 4 | From these files, bun000mesh.ply and bun045mesh.ply are created by 5 | - reading the ply file by meshlab, and 6 | - exporting the mesh into another ply file by meshlab (select "File"->"Export mesh as" then choose options "Vert: Flags, Normal" and "Face: Flags") so that pcl::io::loadPolygonFile() can read the ply file. 7 | - Note: pcl::io::loadPolygonFile() can not load the original bun{000,045}.ply files because the face element does not exist. 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /data/pcl_data/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013, Point Cloud Library (PCL) 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | Redistributions in binary form must reproduce the above copyright notice, this 11 | list of conditions and the following disclaimer in the documentation and/or 12 | other materials provided with the distribution. 13 | 14 | Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 22 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 25 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /data/pcl_data/README.md: -------------------------------------------------------------------------------- 1 | This folder contains the standford bunny data 2 | obtained from the PCL data github as follows: 3 | ``` 4 | wget https://github.com/PointCloudLibrary/data/raw/master/tutorials/correspondence_grouping/milk.pcd 5 | wget https://github.com/PointCloudLibrary/data/raw/master/tutorials/correspondence_grouping/milk_cartoon_all_small_clorox.pcd 6 | ``` 7 | -------------------------------------------------------------------------------- /data/pcl_data/milk.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tttamaki/ICP-test/946f484aa74f1edbf251770a96fedebd31ea341b/data/pcl_data/milk.pcd -------------------------------------------------------------------------------- /data/pcl_data/milk_cartoon_all_small_clorox.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tttamaki/ICP-test/946f484aa74f1edbf251770a96fedebd31ea341b/data/pcl_data/milk_cartoon_all_small_clorox.pcd -------------------------------------------------------------------------------- /src/icp1_simple.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | void 18 | loadFile(const char* fileName, 19 | pcl::PointCloud &cloud 20 | ) 21 | { 22 | pcl::PolygonMesh mesh; 23 | 24 | if ( pcl::io::loadPolygonFile ( fileName, mesh ) == -1 ) 25 | { 26 | PCL_ERROR ( "loadFile faild." ); 27 | return; 28 | } 29 | else 30 | pcl::fromPCLPointCloud2 ( mesh.cloud, cloud ); 31 | 32 | // remove points having values of nan 33 | std::vector index; 34 | pcl::removeNaNFromPointCloud ( cloud, cloud, index ); 35 | } 36 | 37 | 38 | int main ( int argc, char** argv ) 39 | { 40 | 41 | 42 | pcl::PointCloud::Ptr cloud_source ( new pcl::PointCloud () ); 43 | pcl::PointCloud::Ptr cloud_target ( new pcl::PointCloud () ); 44 | 45 | { 46 | // load source 47 | loadFile ( argv[1], *cloud_source ); 48 | // load target 49 | loadFile ( argv[2], *cloud_target ); 50 | } 51 | 52 | 53 | // transformed source ---> target 54 | pcl::PointCloud::Ptr cloud_source_trans ( new pcl::PointCloud () ); 55 | 56 | 57 | { // ICP registration 58 | pcl::IterativeClosestPoint icp; 59 | 60 | icp.setInputSource ( cloud_source ); 61 | icp.setInputTarget ( cloud_target ); 62 | 63 | // registration 64 | icp.align ( *cloud_source_trans ); 65 | 66 | 67 | if ( icp.hasConverged() ) 68 | { 69 | std::cout << "Converged. score =" << icp.getFitnessScore() << std::endl; 70 | 71 | Eigen::Matrix4f transformation = icp.getFinalTransformation(); 72 | std::cout << transformation << std::endl; 73 | } 74 | else 75 | std::cout << "Not converged." << std::endl; 76 | } 77 | 78 | 79 | 80 | { // visualization 81 | boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer ( new pcl::visualization::PCLVisualizer ("3D Viewer") ); 82 | viewer->setBackgroundColor (0, 0, 0); 83 | 84 | pcl::visualization::PointCloudColorHandlerCustom source_color ( cloud_source, 0, 255, 0 ); 85 | viewer->addPointCloud (cloud_source, source_color, "source"); 86 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source"); 87 | 88 | pcl::visualization::PointCloudColorHandlerCustom target_color ( cloud_target, 255, 255, 255 ); 89 | viewer->addPointCloud ( cloud_target, target_color, "target"); 90 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target" ); 91 | 92 | pcl::visualization::PointCloudColorHandlerCustom source_trans_color ( cloud_source_trans, 255, 0, 255 ); 93 | viewer->addPointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 94 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source trans" ); 95 | 96 | 97 | // orthographic (parallel) projection; same with pressing key 'o' 98 | viewer->getRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection( 1 ); 99 | 100 | viewer->resetCamera(); 101 | 102 | viewer->spin (); 103 | 104 | 105 | } 106 | 107 | return(0); 108 | } 109 | 110 | -------------------------------------------------------------------------------- /src/icp2_iterative_view.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | // #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | void 17 | loadFile(const char* fileName, 18 | pcl::PointCloud &cloud 19 | ) 20 | { 21 | pcl::PolygonMesh mesh; 22 | 23 | if ( pcl::io::loadPolygonFile ( fileName, mesh ) == -1 ) 24 | { 25 | PCL_ERROR ( "loadFile faild." ); 26 | return; 27 | } 28 | else 29 | pcl::fromPCLPointCloud2 ( mesh.cloud, cloud ); 30 | 31 | // remove points having values of nan 32 | std::vector index; 33 | pcl::removeNaNFromPointCloud ( cloud, cloud, index ); 34 | } 35 | 36 | int main (int argc, char** argv) 37 | { 38 | 39 | 40 | pcl::PointCloud::Ptr cloud_source ( new pcl::PointCloud () ); 41 | pcl::PointCloud::Ptr cloud_target ( new pcl::PointCloud () ); 42 | 43 | { 44 | // load source 45 | loadFile ( argv[1], *cloud_source ); 46 | // load target 47 | loadFile ( argv[2], *cloud_target ); 48 | } 49 | 50 | 51 | // transformed source ---> target 52 | pcl::PointCloud::Ptr cloud_source_trans ( new pcl::PointCloud () ); 53 | cloud_source_trans = cloud_source; 54 | 55 | 56 | boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer ( new pcl::visualization::PCLVisualizer ("3D Viewer") ); 57 | viewer->setBackgroundColor (0, 0, 0); 58 | 59 | pcl::visualization::PointCloudColorHandlerCustom source_color ( cloud_source, 0, 255, 0 ); 60 | viewer->addPointCloud (cloud_source, source_color, "source"); 61 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source"); 62 | 63 | pcl::visualization::PointCloudColorHandlerCustom target_color ( cloud_target, 255, 255, 255 ); 64 | viewer->addPointCloud ( cloud_target, target_color, "target"); 65 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target" ); 66 | 67 | pcl::visualization::PointCloudColorHandlerCustom source_trans_color ( cloud_source_trans, 255, 0, 255 ); 68 | viewer->addPointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 69 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source trans" ); 70 | 71 | 72 | // orthographic (parallel) projection; same with pressing key 'o' 73 | viewer->getRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection( 1 ); 74 | 75 | viewer->resetCamera(); 76 | 77 | 78 | 79 | 80 | pcl::IterativeClosestPoint::Ptr icp ( new pcl::IterativeClosestPoint () ); 81 | // pcl::IterativeClosestPointNonLinear::Ptr icp ( new pcl::IterativeClosestPointNonLinear ); 82 | icp->setMaximumIterations ( 1 ); 83 | icp->setInputSource ( cloud_source_trans ); // not cloud_source, but cloud_source_trans! 84 | icp->setInputTarget ( cloud_target ); 85 | 86 | 87 | 88 | 89 | while ( !viewer->wasStopped() ) 90 | { 91 | // registration 92 | icp->align ( *cloud_source_trans ); 93 | 94 | if ( icp->hasConverged() ) 95 | { 96 | viewer->updatePointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 97 | std::cout << icp->getFitnessScore() << std::endl; 98 | } 99 | else 100 | std::cout << "Not converged." << std::endl; 101 | 102 | viewer->spinOnce(); 103 | } 104 | 105 | 106 | return( 0 ); 107 | } 108 | 109 | -------------------------------------------------------------------------------- /src/icp3_with_normal_iterative_view.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | 17 | void addNormal(pcl::PointCloud::Ptr cloud, 18 | pcl::PointCloud::Ptr cloud_with_normals 19 | ) 20 | { 21 | pcl::PointCloud::Ptr normals ( new pcl::PointCloud ); 22 | 23 | pcl::search::KdTree::Ptr searchTree (new pcl::search::KdTree); 24 | searchTree->setInputCloud ( cloud ); 25 | 26 | pcl::NormalEstimation normalEstimator; 27 | normalEstimator.setInputCloud ( cloud ); 28 | normalEstimator.setSearchMethod ( searchTree ); 29 | normalEstimator.setKSearch ( 15 ); 30 | normalEstimator.compute ( *normals ); 31 | 32 | pcl::concatenateFields( *cloud, *normals, *cloud_with_normals ); 33 | } 34 | 35 | void 36 | loadFile(const char* fileName, 37 | pcl::PointCloud &cloud 38 | ) 39 | { 40 | pcl::PolygonMesh mesh; 41 | 42 | if ( pcl::io::loadPolygonFile ( fileName, mesh ) == -1 ) 43 | { 44 | PCL_ERROR ( "loadFile faild." ); 45 | return; 46 | } 47 | else 48 | pcl::fromPCLPointCloud2 ( mesh.cloud, cloud ); 49 | 50 | // remove points having values of nan 51 | std::vector index; 52 | pcl::removeNaNFromPointCloud ( cloud, cloud, index ); 53 | } 54 | 55 | int main (int argc, char** argv) 56 | { 57 | 58 | 59 | pcl::PointCloud::Ptr cloud_source ( new pcl::PointCloud () ); 60 | pcl::PointCloud::Ptr cloud_target ( new pcl::PointCloud () ); 61 | 62 | { 63 | // load source 64 | loadFile ( argv[1], *cloud_source ); 65 | // load target 66 | loadFile ( argv[2], *cloud_target ); 67 | } 68 | 69 | 70 | // transformed source ---> target 71 | pcl::PointCloud::Ptr cloud_source_trans ( new pcl::PointCloud () ); 72 | cloud_source_trans = cloud_source; 73 | 74 | 75 | boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer ( new pcl::visualization::PCLVisualizer ("3D Viewer") ); 76 | viewer->setBackgroundColor (0, 0, 0); 77 | 78 | pcl::visualization::PointCloudColorHandlerCustom source_color ( cloud_source, 0, 255, 0 ); 79 | viewer->addPointCloud (cloud_source, source_color, "source"); 80 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source"); 81 | 82 | pcl::visualization::PointCloudColorHandlerCustom target_color ( cloud_target, 255, 255, 255 ); 83 | viewer->addPointCloud ( cloud_target, target_color, "target"); 84 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target" ); 85 | 86 | pcl::visualization::PointCloudColorHandlerCustom source_trans_color ( cloud_source_trans, 255, 0, 255 ); 87 | viewer->addPointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 88 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source trans" ); 89 | 90 | 91 | // orthographic (parallel) projection; same with pressing key 'o' 92 | viewer->getRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection( 1 ); 93 | 94 | viewer->resetCamera(); 95 | 96 | 97 | 98 | // prepare could with normals 99 | pcl::PointCloud::Ptr cloud_source_normals ( new pcl::PointCloud () ); 100 | pcl::PointCloud::Ptr cloud_target_normals ( new pcl::PointCloud () ); 101 | pcl::PointCloud::Ptr cloud_source_trans_normals ( new pcl::PointCloud () ); 102 | 103 | addNormal( cloud_source, cloud_source_normals ); 104 | addNormal( cloud_target, cloud_target_normals ); 105 | addNormal( cloud_source_trans, cloud_source_trans_normals ); 106 | 107 | 108 | 109 | 110 | 111 | pcl::IterativeClosestPointWithNormals::Ptr icp ( new pcl::IterativeClosestPointWithNormals () ); 112 | icp->setMaximumIterations ( 1 ); 113 | icp->setInputSource ( cloud_source_trans_normals ); // not cloud_source, but cloud_source_trans! 114 | icp->setInputTarget ( cloud_target_normals ); 115 | 116 | 117 | 118 | while ( !viewer->wasStopped () ) 119 | { 120 | // registration 121 | icp->align ( *cloud_source_trans_normals ); // use cloud with normals for ICP 122 | 123 | if ( icp->hasConverged() ) 124 | { 125 | // use cloud without normals for visualizatoin 126 | pcl::transformPointCloud ( *cloud_source, *cloud_source_trans, icp->getFinalTransformation() ); 127 | viewer->updatePointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 128 | std::cout << icp->getFitnessScore() << std::endl; 129 | } 130 | else 131 | std::cout << "Not converged." << std::endl; 132 | 133 | viewer->spinOnce(); 134 | } 135 | 136 | 137 | return( 0 ); 138 | } 139 | 140 | -------------------------------------------------------------------------------- /src/icp4_after_feature_registration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "visualize_correspondences.h" 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | 24 | void addNormal(pcl::PointCloud::Ptr cloud, 25 | pcl::PointCloud::Ptr normals, 26 | pcl::PointCloud::Ptr cloud_with_normals 27 | ) 28 | { 29 | pcl::search::KdTree::Ptr searchTree (new pcl::search::KdTree); 30 | searchTree->setInputCloud ( cloud ); 31 | 32 | pcl::NormalEstimation normalEstimator; 33 | normalEstimator.setInputCloud ( cloud ); 34 | normalEstimator.setSearchMethod ( searchTree ); 35 | normalEstimator.setKSearch ( 15 ); 36 | normalEstimator.compute ( *normals ); 37 | 38 | pcl::concatenateFields( *cloud, *normals, *cloud_with_normals ); 39 | } 40 | 41 | 42 | 43 | 44 | void 45 | loadFile(const char* fileName, 46 | pcl::PointCloud &cloud 47 | ) 48 | { 49 | pcl::PolygonMesh mesh; 50 | 51 | if ( pcl::io::loadPolygonFile ( fileName, mesh ) == -1 ) 52 | { 53 | PCL_ERROR ( "loadFile faild." ); 54 | return; 55 | } 56 | else 57 | pcl::fromPCLPointCloud2 ( mesh.cloud, cloud ); 58 | 59 | // remove points having values of nan 60 | std::vector index; 61 | pcl::removeNaNFromPointCloud ( cloud, cloud, index ); 62 | } 63 | 64 | int main (int argc, char** argv) 65 | { 66 | 67 | pcl::PointCloud::Ptr cloud_source ( new pcl::PointCloud () ); 68 | pcl::PointCloud::Ptr cloud_target ( new pcl::PointCloud () ); 69 | 70 | { 71 | // load source 72 | loadFile ( argv[1], *cloud_source ); 73 | // load target 74 | loadFile ( argv[2], *cloud_target ); 75 | } 76 | 77 | 78 | pcl::PointCloud::Ptr cloud_source_trans ( new pcl::PointCloud () ); 79 | 80 | // prepare could with normals 81 | pcl::PointCloud::Ptr cloud_source_normals (new pcl::PointCloud () ); 82 | pcl::PointCloud::Ptr cloud_target_normals (new pcl::PointCloud () ); 83 | pcl::PointCloud::Ptr cloud_source_trans_normals (new pcl::PointCloud () ); 84 | 85 | pcl::PointCloud::Ptr source_normals ( new pcl::PointCloud () ); 86 | pcl::PointCloud::Ptr target_normals ( new pcl::PointCloud () ); 87 | 88 | addNormal ( cloud_source, source_normals, cloud_source_normals ); 89 | addNormal ( cloud_target, target_normals, cloud_target_normals ); 90 | addNormal ( cloud_source, source_normals, cloud_source_trans_normals ); // dummy at this time 91 | 92 | 93 | 94 | 95 | 96 | 97 | float radius; 98 | { 99 | Eigen::Vector4f max_pt, min_pt; 100 | pcl::getMinMax3D ( *cloud_source, min_pt, max_pt ); 101 | radius = (max_pt - min_pt).norm() / 50; // fixed radius (scale) for detector/descriptor 102 | std::cout << "scale: " << radius << std::endl; 103 | } 104 | 105 | 106 | 107 | { // registration 108 | 109 | pcl::PointCloud::Ptr source_keypoints ( new pcl::PointCloud () ); 110 | pcl::PointCloud::Ptr target_keypoints ( new pcl::PointCloud () ); 111 | pcl::PointCloud::Ptr source_keypointsXYZ ( new pcl::PointCloud () ); 112 | pcl::PointCloud::Ptr target_keypointsXYZ ( new pcl::PointCloud () ); 113 | 114 | 115 | { // Harris detector with normal 116 | std::cout << "detection" << std::endl; 117 | 118 | pcl::HarrisKeypoint3D::Ptr detector ( new pcl::HarrisKeypoint3D () ); 119 | detector->setNonMaxSupression ( true ); 120 | detector->setRadius ( radius ); 121 | detector->setRadiusSearch ( radius ); 122 | detector->setMethod ( pcl::HarrisKeypoint3D::CURVATURE ); // HARRIS, NOBLE, LOWE, TOMASI, CURVATURE 123 | 124 | detector->setInputCloud ( cloud_source_normals ); 125 | detector->setSearchSurface ( cloud_source_normals ); 126 | detector->compute ( *source_keypoints ); 127 | cout << "number of source keypoints found: " << source_keypoints->points.size() << endl; 128 | source_keypointsXYZ->points.resize( source_keypoints->points.size() ); 129 | pcl::copyPointCloud( *source_keypoints, *source_keypointsXYZ ); 130 | 131 | detector->setInputCloud ( cloud_target_normals ); 132 | detector->setSearchSurface ( cloud_target_normals ); 133 | detector->compute ( *target_keypoints ); 134 | cout << "number of target keypoints found: " << target_keypoints->points.size() << endl; 135 | source_keypointsXYZ->points.resize( target_keypoints->points.size() ); 136 | pcl::copyPointCloud( *target_keypoints, *target_keypointsXYZ ); 137 | 138 | } 139 | 140 | 141 | 142 | #define useFPFH 143 | 144 | #ifdef useFPFH 145 | #define descriptorType pcl::FPFHSignature33 146 | #else 147 | #define descriptorType pcl::SHOT352 148 | #endif 149 | 150 | pcl::PointCloud::Ptr source_features ( new pcl::PointCloud () ); 151 | pcl::PointCloud::Ptr target_features ( new pcl::PointCloud () ); 152 | 153 | { // descriptor 154 | std::cout << "description" << std::endl; 155 | #ifdef useFPFH 156 | pcl::Feature::Ptr descriptor ( new pcl::FPFHEstimationOMP () ); 157 | #else 158 | pcl::Feature::Ptr descriptor ( new pcl::SHOTEstimationOMP () ); 159 | #endif 160 | descriptor->setSearchMethod ( pcl::search::Search::Ptr ( new pcl::search::KdTree) ); 161 | descriptor->setRadiusSearch ( radius*10 ); 162 | 163 | pcl::FeatureFromNormals::Ptr feature_from_normals = boost::dynamic_pointer_cast > ( descriptor ); 164 | 165 | descriptor->setSearchSurface ( cloud_source ); 166 | descriptor->setInputCloud ( source_keypointsXYZ ); 167 | feature_from_normals->setInputNormals ( source_normals ); 168 | descriptor->compute ( *source_features ); 169 | 170 | descriptor->setSearchSurface ( cloud_target ); 171 | descriptor->setInputCloud ( target_keypointsXYZ ); 172 | feature_from_normals->setInputNormals ( target_normals ); 173 | descriptor->compute ( *target_features ); 174 | 175 | } 176 | 177 | 178 | 179 | std::vector correspondences; 180 | 181 | { // Find matching with Kd-tree 182 | std::cout << "matching" << std::endl; 183 | 184 | correspondences.resize ( source_features->size() ); 185 | 186 | pcl::KdTreeFLANN search_tree; 187 | search_tree.setInputCloud ( target_features ); 188 | 189 | std::vector index(1); 190 | std::vector L2_distance(1); 191 | for (int i = 0; i < source_features->size(); ++i) 192 | { 193 | correspondences[i] = -1; // -1 means no correspondence 194 | 195 | #ifdef useFPFH 196 | if ( isnan ( source_features->points[i].histogram[0] ) ) continue; 197 | #else 198 | if ( isnan ( source_features->points[i].descriptor[0] ) ) continue; 199 | #endif 200 | 201 | search_tree.nearestKSearch ( *source_features, i, 1, index, L2_distance ); 202 | correspondences[i] = index[0]; 203 | 204 | } 205 | 206 | } 207 | visualize_correspondences (cloud_source, source_keypointsXYZ, 208 | cloud_target, target_keypointsXYZ, 209 | correspondences); 210 | 211 | 212 | pcl::CorrespondencesPtr pCorrespondences ( new pcl::Correspondences ); 213 | 214 | { // Refining matching by filtering out wrong correspondence 215 | std::cout << "refineing matching" << std::endl; 216 | 217 | int nCorrespondence = 0; 218 | for (int i = 0; i < correspondences.size(); i++) 219 | if ( correspondences[i] >= 0 ) nCorrespondence++; // do not count "-1" in correspondences 220 | 221 | pCorrespondences->resize ( nCorrespondence ); 222 | for (int i = 0, j = 0; i < correspondences.size(); i++) 223 | { 224 | if ( correspondences[i] > 0 ) 225 | { 226 | (*pCorrespondences)[j].index_query = i; 227 | (*pCorrespondences)[j].index_match = correspondences[i]; 228 | j++; 229 | 230 | } 231 | } 232 | 233 | pcl::registration::CorrespondenceRejectorSampleConsensus refine; 234 | refine.setInputSource ( source_keypointsXYZ ); 235 | refine.setInputTarget ( target_keypointsXYZ ); 236 | refine.setInputCorrespondences ( pCorrespondences ); 237 | refine.getCorrespondences ( *pCorrespondences ); 238 | } 239 | visualize_correspondences (cloud_source, source_keypointsXYZ, 240 | cloud_target, target_keypointsXYZ, 241 | pCorrespondences); 242 | 243 | 244 | 245 | 246 | Eigen::Matrix4f transformation; 247 | 248 | { // Estimating rigid transformation 249 | std::cout << "Estimating transformation" << std::endl; 250 | 251 | pcl::registration::TransformationEstimationSVD< pcl::PointXYZ, pcl::PointXYZ > est; 252 | 253 | std::vector source_index ( source_features->size() ); 254 | for (int i = 0; i < source_features->size(); ++i) source_index[i] = i; 255 | 256 | 257 | est.estimateRigidTransformation ( *source_keypointsXYZ, 258 | *target_keypointsXYZ, *pCorrespondences, 259 | transformation ); 260 | std::cout << transformation << std::endl; 261 | 262 | pcl::transformPointCloud ( *cloud_source, *cloud_source_trans, transformation ); 263 | pcl::transformPointCloud ( *cloud_source_normals, *cloud_source_trans_normals, transformation ); 264 | 265 | } 266 | 267 | 268 | } 269 | 270 | 271 | 272 | { 273 | // visualization 274 | boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer ( new pcl::visualization::PCLVisualizer ("3D Viewer") ); 275 | viewer->setBackgroundColor (0, 0, 0); 276 | 277 | pcl::visualization::PointCloudColorHandlerCustom source_color ( cloud_source, 0, 255, 0 ); 278 | viewer->addPointCloud (cloud_source, source_color, "source"); 279 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source"); 280 | 281 | pcl::visualization::PointCloudColorHandlerCustom target_color ( cloud_target, 255, 255, 255 ); 282 | viewer->addPointCloud ( cloud_target, target_color, "target"); 283 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target" ); 284 | 285 | pcl::visualization::PointCloudColorHandlerCustom source_trans_color ( cloud_source_trans, 255, 0, 255 ); 286 | viewer->addPointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 287 | viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source trans" ); 288 | 289 | 290 | // orthographic (parallel) projection; same with pressing key 'o' 291 | viewer->getRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection( 1 ); 292 | 293 | viewer->resetCamera(); 294 | 295 | viewer->spin (); 296 | 297 | 298 | 299 | 300 | { // addtional refinement wit ICP 301 | pcl::IterativeClosestPointWithNormals icp; 302 | 303 | icp.setInputSource ( cloud_source_trans_normals ); 304 | icp.setInputTarget ( cloud_target_normals ); 305 | 306 | // registration 307 | icp.align ( *cloud_source_trans_normals ); 308 | 309 | Eigen::Matrix4f transformation; 310 | if ( icp.hasConverged() ) 311 | { 312 | pcl::transformPointCloud ( *cloud_source_trans, *cloud_source_trans, icp.getFinalTransformation() ); 313 | viewer->updatePointCloud ( cloud_source_trans, source_trans_color, "source trans" ); 314 | } 315 | else 316 | std::cout << "Not converged." << std::endl; 317 | 318 | viewer->spin (); 319 | } 320 | } 321 | 322 | 323 | return(0); 324 | } 325 | 326 | -------------------------------------------------------------------------------- /src/transform_estimation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | enum methods { 13 | SVD, 14 | DQ, 15 | LM 16 | }; 17 | 18 | 19 | int main (int argc, char** argv) 20 | { 21 | 22 | int method = SVD; 23 | pcl::console::parse_argument (argc, argv, "-m", method); 24 | std::cout << "method: "; 25 | switch (method) { 26 | case SVD: std::cout << "SVD"; break; 27 | case DQ: std::cout << "DQ"; break; 28 | case LM: std::cout << "LM"; break; 29 | default: std::cout << "undefined. ERROR" << std::endl; exit(0); 30 | } 31 | std::cout << std::endl; 32 | 33 | bool use_scale = false; 34 | pcl::console::parse_argument (argc, argv, "-s", use_scale); 35 | std::cout << "use scale: " << (use_scale ? "true" : "false") << std::endl; 36 | if (use_scale) { 37 | std::cout << "forse SVD." << std::endl; 38 | method = SVD; 39 | } 40 | 41 | bool use_rand = false; 42 | pcl::console::parse_argument (argc, argv, "-r", use_rand); 43 | std::cout << "use random seed: " << (use_rand ? "true" : "false") << std::endl; 44 | 45 | 46 | 47 | boost::random::mt19937 gen; // alternative to rand() 48 | if (use_rand) 49 | gen.seed( std::time(0) ); // random seed with current time in second 50 | else 51 | gen.seed( 0 ); // fixed seed 52 | 53 | boost::random::uniform_real_distribution frand( 1.0, 3.2 ); // random gen between 1.0 and 3.2 54 | 55 | 56 | pcl::PointCloud::Ptr cloud_source ( new pcl::PointCloud () ); 57 | pcl::PointCloud::Ptr cloud_target ( new pcl::PointCloud () ); 58 | 59 | // create random source point cloud 60 | for (int i = 0; i < 1000; i++) { 61 | cloud_source->push_back (pcl::PointXYZ (frand(gen), frand(gen), frand(gen) )); 62 | } 63 | 64 | 65 | // create random transformation: R and T 66 | Eigen::Affine3f transformation_true; 67 | { 68 | // random rotation matrix 69 | Eigen::Vector3f axis; 70 | axis.setRandom().normalize(); 71 | float angle = frand( gen ); 72 | Eigen::Affine3f R ( Eigen::AngleAxis ( angle, axis ) ); 73 | 74 | // random translation vector 75 | Eigen::Translation3f T ( frand(gen), frand(gen), frand(gen) ); 76 | 77 | std::cout << "true R" << std::endl << R.matrix() << std::endl 78 | << "true T" << std::endl << T .vector() << std::endl; 79 | 80 | if ( use_scale ) 81 | { 82 | float scale = frand( gen ); 83 | R.matrix().topLeftCorner(3,3) *= scale; 84 | std::cout << "true sR" << std::endl << R.matrix() << std::endl 85 | << "true scale " << scale << std::endl; 86 | } 87 | 88 | // R and T 89 | transformation_true = T * R ; // shoul be in this order if you mean (Rx + T). If R*T, then R(x+t) ! 90 | 91 | } 92 | std::cout << "true transformation" << std::endl << transformation_true.matrix() << std::endl; 93 | 94 | 95 | // create target point cloud 96 | pcl::transformPointCloud ( *cloud_source, *cloud_target, transformation_true ); 97 | 98 | boost::shared_ptr< pcl::registration::TransformationEstimation< pcl::PointXYZ, pcl::PointXYZ > > estPtr; 99 | if ( use_scale ) 100 | // estimator of R and T along with scale 101 | estPtr.reset ( new pcl::registration::TransformationEstimationSVDScale < pcl::PointXYZ, pcl::PointXYZ > () ); 102 | else 103 | // estimator of R and T 104 | switch (method) { 105 | case SVD: 106 | estPtr.reset ( new pcl::registration::TransformationEstimationSVD < pcl::PointXYZ, pcl::PointXYZ > () ); 107 | break; 108 | case DQ: 109 | estPtr.reset ( new pcl::registration::TransformationEstimationDualQuaternion < pcl::PointXYZ, pcl::PointXYZ > () ); 110 | break; 111 | case LM: 112 | estPtr.reset ( new pcl::registration::TransformationEstimationLM < pcl::PointXYZ, pcl::PointXYZ > () ); 113 | break; 114 | } 115 | 116 | 117 | Eigen::Affine3f transformation_est; 118 | estPtr->estimateRigidTransformation ( *cloud_source, 119 | *cloud_target, 120 | transformation_est.matrix() ); 121 | 122 | if ( use_scale ) { 123 | Eigen::Matrix3f R = transformation_est.matrix().topLeftCorner(3,3); 124 | std::cout << "estimated scale " << std::sqrt( (R.transpose() * R).trace() / 3.0 ) << std::endl; 125 | } 126 | std::cout << "estimated transformation " << std::endl << transformation_est.matrix() << std::endl; 127 | 128 | return ( 0 ); 129 | } 130 | 131 | -------------------------------------------------------------------------------- /src/visualize_correspondences.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // taken from https://github.com/PointCloudLibrary/pcl/blob/master/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "visualize_correspondences.h" 12 | 13 | 14 | void 15 | visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1, 16 | const PointCloudPtr points2, const PointCloudPtr keypoints2, 17 | const std::vector &correspondences /*, 18 | const std::vector &correspondence_scores, int max_to_display */) 19 | { 20 | // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them 21 | // by shifting one to the left and the other to the right. Then we'll draw lines between the corresponding points 22 | // Create some new point clouds to hold our transformed data 23 | PointCloudPtr points_left (new PointCloud); 24 | PointCloudPtr keypoints_left (new PointCloud); 25 | PointCloudPtr points_right (new PointCloud); 26 | PointCloudPtr keypoints_right (new PointCloud); 27 | // Shift the first clouds' points to the left 28 | //const Eigen::Vector3f translate (0.0, 0.0, 0.3); 29 | const Eigen::Vector3f translate (0.4, 0.0, 0.0); 30 | const Eigen::Quaternionf no_rotation (0, 0, 0, 0); 31 | pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation); 32 | pcl::transformPointCloud (*keypoints1, *keypoints_left, -translate, no_rotation); 33 | // Shift the second clouds' points to the right 34 | pcl::transformPointCloud (*points2, *points_right, translate, no_rotation); 35 | pcl::transformPointCloud (*keypoints2, *keypoints_right, translate, no_rotation); 36 | // Add the clouds to the visualizer 37 | pcl::visualization::PCLVisualizer vis; 38 | vis.addPointCloud (points_left, "points_left"); 39 | vis.addPointCloud (points_right, "points_right"); 40 | // // Compute the weakest correspondence score to display 41 | // std::vector temp (correspondence_scores); 42 | // std::sort (temp.begin (), temp.end ()); 43 | // if (max_to_display >= temp.size ()) 44 | // max_to_display = temp.size () - 1; 45 | // float threshold = temp[max_to_display]; 46 | // std::cout << max_to_display << std::endl; 47 | // Draw lines between the best corresponding points 48 | for (size_t i = 0; i < keypoints_left->size (); ++i) 49 | { 50 | // if (correspondence_scores[i] > threshold) 51 | // { 52 | // continue; // Don't draw weak correspondences 53 | // } 54 | if ( correspondences[i] < 0) continue; 55 | // Get the pair of points 56 | const PointT & p_left = keypoints_left->points[i]; 57 | const PointT & p_right = keypoints_right->points[correspondences[i]]; 58 | // Generate a random (bright) color 59 | double r = (rand() % 100); 60 | double g = (rand() % 100); 61 | double b = (rand() % 100); 62 | double max_channel = std::max (r, std::max (g, b)); 63 | r /= max_channel; 64 | g /= max_channel; 65 | b /= max_channel; 66 | // Generate a unique string for each line 67 | std::stringstream ss ("line"); 68 | ss << i; 69 | // Draw the line 70 | vis.addLine (p_left, p_right, r, g, b, ss.str ()); 71 | } 72 | vis.resetCamera (); 73 | vis.spin (); 74 | } 75 | void 76 | visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1, 77 | const PointCloudPtr points2, const PointCloudPtr keypoints2, 78 | const pcl::CorrespondencesPtr pCorrespondences 79 | /* std::vector &correspondences , 80 | const std::vector &correspondence_scores, int max_to_display */) 81 | { 82 | // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them 83 | // by shifting one to the left and the other to the right. Then we'll draw lines between the corresponding points 84 | // Create some new point clouds to hold our transformed data 85 | PointCloudPtr points_left (new PointCloud); 86 | PointCloudPtr keypoints_left (new PointCloud); 87 | PointCloudPtr points_right (new PointCloud); 88 | PointCloudPtr keypoints_right (new PointCloud); 89 | // Shift the first clouds' points to the left 90 | //const Eigen::Vector3f translate (0.0, 0.0, 0.3); 91 | const Eigen::Vector3f translate (0.4, 0.0, 0.0); 92 | const Eigen::Quaternionf no_rotation (0, 0, 0, 0); 93 | pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation); 94 | pcl::transformPointCloud (*keypoints1, *keypoints_left, -translate, no_rotation); 95 | // Shift the second clouds' points to the right 96 | pcl::transformPointCloud (*points2, *points_right, translate, no_rotation); 97 | pcl::transformPointCloud (*keypoints2, *keypoints_right, translate, no_rotation); 98 | // Add the clouds to the visualizer 99 | pcl::visualization::PCLVisualizer vis; 100 | vis.addPointCloud (points_left, "points_left"); 101 | vis.addPointCloud (points_right, "points_right"); 102 | // // Compute the weakest correspondence score to display 103 | // std::vector temp (correspondence_scores); 104 | // std::sort (temp.begin (), temp.end ()); 105 | // if (max_to_display >= temp.size ()) 106 | // max_to_display = temp.size () - 1; 107 | // float threshold = temp[max_to_display]; 108 | // std::cout << max_to_display << std::endl; 109 | // Draw lines between the best corresponding points 110 | for (size_t i = 0; i < pCorrespondences->size (); ++i) 111 | { 112 | // if (correspondence_scores[i] > threshold) 113 | // { 114 | // continue; // Don't draw weak correspondences 115 | // } 116 | // Get the pair of points 117 | const PointT & p_left = keypoints_left->points[(*pCorrespondences)[i].index_query]; 118 | const PointT & p_right = keypoints_right->points[(*pCorrespondences)[i].index_match]; 119 | // Generate a random (bright) color 120 | double r = (rand() % 100); 121 | double g = (rand() % 100); 122 | double b = (rand() % 100); 123 | double max_channel = std::max (r, std::max (g, b)); 124 | r /= max_channel; 125 | g /= max_channel; 126 | b /= max_channel; 127 | // Generate a unique string for each line 128 | std::stringstream ss ("line"); 129 | ss << i; 130 | // Draw the line 131 | vis.addLine (p_left, p_right, r, g, b, ss.str ()); 132 | } 133 | vis.resetCamera (); 134 | vis.spin (); 135 | } 136 | -------------------------------------------------------------------------------- /src/visualize_correspondences.h: -------------------------------------------------------------------------------- 1 | #ifndef VISUALIZE_CORRESPONDENCES_H_ 2 | #define VISUALIZE_CORRESPONDENCES_H_ 3 | 4 | typedef pcl::PointCloud::Ptr PointCloudPtr; 5 | typedef pcl::PointCloud PointCloud; 6 | typedef pcl::PointXYZ PointT; 7 | void 8 | visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1, 9 | const PointCloudPtr points2, const PointCloudPtr keypoints2, 10 | const std::vector &correspondences 11 | ); 12 | void 13 | visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1, 14 | const PointCloudPtr points2, const PointCloudPtr keypoints2, 15 | const pcl::CorrespondencesPtr pCorrespondences 16 | ); 17 | 18 | #endif 19 | --------------------------------------------------------------------------------