├── .gitignore ├── README.md ├── cpp ├── CMakeLists.txt ├── pcd_recorder.cpp ├── plane_fitter.cpp ├── plane_fitter_pcd.cpp ├── plane_fitter_pcd.ini ├── plane_fitter_pcd.tls.ini └── plane_fitter_pcd_debug.cpp ├── data ├── andreashaus │ ├── list.txt │ ├── readme.md │ ├── scan000.pcd │ ├── scan001.pcd │ ├── scan002.pcd │ └── scan003.pcd └── stair │ ├── list.txt │ ├── record_00080.pcd │ ├── record_00100.pcd │ └── record_00208.pcd ├── include ├── AHCParamSet.hpp ├── AHCPlaneFitter.hpp ├── AHCPlaneSeg.hpp ├── AHCTypes.hpp ├── AHCUtils.hpp ├── DisjointSet.hpp └── eig33sym.hpp └── matlab ├── Kinect.m ├── createSegImg.m ├── fitAHCPlane.m ├── fitPlane.m ├── frame.mat ├── getDefaultAHCFitterparams.m ├── kinect_ahc.m ├── kinect_record.m ├── mex ├── makefile.m ├── mxEig33Sym.cpp ├── mxEig33Sym.m ├── mxEig33Sym.mexw64 ├── mxFitAHCPlane.cpp ├── mxFitAHCPlane.m └── mxFitAHCPlane.mexw64 ├── myPseudoColor.m ├── plotColorPointCloud.m ├── projectToXY.m ├── reportAHCPlaneFitterParams.m ├── setAHCPlaneFitterParams.m ├── splitXYZ.m ├── viewSeg.m └── xyz2pts.m /.gitignore: -------------------------------------------------------------------------------- 1 | .hg/* 2 | build/* 3 | */3x3-C/* 4 | build*/* 5 | install/* -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Fast Plane Extraction Using Agglomerative Hierarchical Clustering (AHC) 2 | ======================================================================= 3 | 4 | Legal Remarks 5 | ------------- 6 | 7 | Copyright 2014 Mitsubishi Electric Research Laboratories All 8 | Rights Reserved. 9 | 10 | Permission to use, copy and modify this software and its 11 | documentation without fee for educational, research and non-profit 12 | purposes, is hereby granted, provided that the above copyright 13 | notice, this paragraph, and the following three paragraphs appear 14 | in all copies. 15 | 16 | To request permission to incorporate this software into commercial 17 | products contact: Director; Mitsubishi Electric Research 18 | Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 19 | 20 | IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 21 | INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 22 | LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 23 | DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 24 | SUCH DAMAGES. 25 | 26 | MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 27 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 28 | FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 29 | "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 30 | SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 31 | 32 | Overview 33 | -------- 34 | 35 | This source code package contains our C++ implementation of the AHC based fast plane extraction for organized point cloud (point cloud that can be indexed as an image). There are three folders in this package: 36 | 37 | * include 38 | 39 | Our C++ implementation of the algorithm with dependencies on OpenCV and shared_ptr (from C++11 or Boost). 40 | 41 | * cpp 42 | 43 | Two example C++ console applications using our algorithm to extract planes from Kinect-like point cloud (depends on PCL), with a CMake script to help generating project files. 44 | 45 | * matlab 46 | 47 | A matlab interface (fitAHCPlane.m) through MEX for using our algorithm in matlab. We also provide a wrapper class Kinect.m and kinect_ahc.m to do real-time plane extraction in matlab, partially depends on a 3rd-party toolbox [Kinect_Matlab]. 48 | 49 | **If you use this package, please cite our ICRA 2014 paper:** 50 | 51 | Feng, C., Taguchi, Y., and Kamat, V. R. (2014). "Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering." Proceedings of the IEEE International Conference on Robotics and Automation, Hong Kong, China, 6218-6225. 52 | 53 | [Kinect_Matlab]:http://www.mathworks.com/matlabcentral/fileexchange/30242-kinect-matlab 54 | 55 | Version 56 | ------- 57 | 58 | 1.0 59 | 60 | Installation 61 | ------------ 62 | 63 | ##### C++ example 64 | 65 | 1. Install OpenCV, Boost and PCL (If you install PCL using their all-in-one installer, you directly get Boost installed as well). 66 | 67 | 2. Generate project file using CMake under either Windows or Linux. 68 | 69 | 3. Compile. 70 | 71 | 4. Run the compiled process: plane_fitter (first connect a Kinect to your computer!) or plane_fitter_pcd (first modify plane_fitter_pcd.ini accordingly!). 72 | 73 | 5. Enjoy! 74 | 75 | ##### Matlab example 76 | 77 | 1. In matlab: 78 | ``` 79 | cd WHERE_YOU_EXTRACT_THE_PACKAGE/matlab/mex 80 | ``` 81 | 82 | 2. Run makefile.m 83 | 84 | 3. Select the directories for *OpenCV_Include*, *OpenCV_Lib*, and *Boost_Include* respectively 85 | 86 | 4. If everything compiles smoothly: 87 | ``` 88 | cd .. 89 | ``` 90 | 91 | 5. Load a single frame we've prepared for you in matlab by: 92 | ``` 93 | load frame 94 | ``` 95 | 96 | 6. Run our algorithm on the point cloud: 97 | ``` 98 | frame.mbs=fitAHCPlane(frame.xyz); 99 | viewSeg(frame.mbs,640,480) 100 | ``` 101 | 102 | 7. Enjoy! 103 | 104 | 8. If you want to play with the kinect_ahc.m with a Kinect, install [Kinect_Matlab] first. 105 | 106 | Contact 107 | ------- 108 | 109 | Chen Feng 110 | 111 | **Feel free to email any bugs or suggestions to help us improve the code. Thank you!** -------------------------------------------------------------------------------- /cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | project(ahc) 4 | 5 | find_package(OpenCV COMPONENTS core highgui imgproc imgcodecs QUIET) 6 | if(NOT OpenCV_FOUND) 7 | find_package(OpenCV REQUIRED) 8 | endif() 9 | find_package(PCL COMPONENTS io REQUIRED) 10 | 11 | include_directories("${CMAKE_CURRENT_SOURCE_DIR}/../include") 12 | include_directories(${PCL_INCLUDE_DIRS}) 13 | link_directories(${PCL_LIBRARY_DIRS}) 14 | add_definitions(${PCL_DEFINITIONS}) 15 | 16 | list(REMOVE_ITEM PCL_LIBRARIES ${VTK_LIBRARIES} ${QHULL_LIBRARIES} ${FLANN_LIBRARIES}) #we don't need vtk, qhull, flann 17 | message(STATUS "OpenCV_LIBS=${OpenCV_LIBS}") 18 | message(STATUS "PCL_LIBRARIES=${PCL_LIBRARIES}") 19 | 20 | ###### Options 21 | option(OPT_USE_BOOST_SHARED_PTR "use boost::shared_ptr instead of std::shared_ptr (in C++11) as ahc::shared_ptr" ON) 22 | if(OPT_USE_BOOST_SHARED_PTR) 23 | add_definitions( -DUSE_BOOST_SHARED_PTR ) 24 | message(STATUS "use boost::shared_ptr as ahc::shared_ptr") 25 | else() 26 | message(STATUS "use std::shared_ptr (C++11) as ahc::shared_ptr") 27 | endif() 28 | 29 | option(OPT_USE_DSYEVH3 "use dsyevh3 (requires Internet to download package) instead of Eigen::SelfAdjointEigenSolver in LA::eig33sym" OFF) 30 | if(OPT_USE_DSYEVH3) 31 | set(DSYEVH3_URL "http://www.mpi-hd.mpg.de/personalhomes/globes/3x3/download/3x3-C.tar.gz") 32 | set(DSYEVH3_DOWNLOAD_PATH "${CMAKE_BINARY_DIR}/3x3-C.tar.gz") 33 | 34 | file(MAKE_DIRECTORY ${CMAKE_SOURCE_DIR}/../3rdparty) 35 | set(DSYEVH3_ROOT "${CMAKE_SOURCE_DIR}/../3rdparty/3x3-C") 36 | 37 | if(NOT EXISTS "${DSYEVH3_ROOT}/dsyevh3.h") 38 | message(STATUS "downloading...\n src=${DSYEVH3_URL}\n dst=${DSYEVH3_DOWNLOAD_PATH}") 39 | file(DOWNLOAD ${DSYEVH3_URL} ${DSYEVH3_DOWNLOAD_PATH} STATUS status LOG log SHOW_PROGRESS EXPECTED_MD5 dc56d40543c41e6b975dbd91c0b1ddeb) 40 | list(GET status 0 status_code) 41 | list(GET status 1 status_string) 42 | if(NOT status_code EQUAL 0) 43 | message(WARNING "error: downloading ${DSYEVH3_URL} failed\n status_code: ${status_code}\n status_string: ${status_string}\n log: ${log}") 44 | set(OPT_USE_DSYEVH3 OFF) 45 | set(HAS_DSYEVH3 FALSE) 46 | else() 47 | message(STATUS "dsyevh3 downloaded!") 48 | execute_process( 49 | COMMAND ${CMAKE_COMMAND} -E tar xzf ${DSYEVH3_DOWNLOAD_PATH} 50 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/../3rdparty 51 | ) 52 | message(STATUS "dsyevh3 extracted to ${DSYEVH3_ROOT}!") 53 | set(HAS_DSYEVH3 TRUE) 54 | 55 | #now let's slightly modify two files in 3x3-C folder 56 | file(READ "${DSYEVH3_ROOT}/dsytrd3.h" DSYTRD3_H_TEXT) 57 | file(WRITE "${DSYEVH3_ROOT}/dsytrd3.h.orig" "${DSYTRD3_H_TEXT}") 58 | string(REPLACE "inline " "" DSYTRD3_H_TEXT "${DSYTRD3_H_TEXT}") 59 | file(WRITE "${DSYEVH3_ROOT}/dsytrd3.h" "${DSYTRD3_H_TEXT}") 60 | message(STATUS "removed 'inline ' in ${DSYEVH3_ROOT}/dsytrd3.h") 61 | 62 | file(READ "${DSYEVH3_ROOT}/dsytrd3.c" DSYTRD3_C_TEXT) 63 | file(WRITE "${DSYEVH3_ROOT}/dsytrd3.c.orig" "${DSYTRD3_C_TEXT}") 64 | string(REPLACE "inline " "" DSYTRD3_C_TEXT "${DSYTRD3_C_TEXT}") 65 | file(WRITE "${DSYEVH3_ROOT}/dsytrd3.c" "${DSYTRD3_C_TEXT}") 66 | message(STATUS "removed 'inline ' in ${DSYEVH3_ROOT}/dsytrd3.c") 67 | endif() 68 | else() 69 | set(HAS_DSYEVH3 TRUE) 70 | endif() 71 | else() 72 | set(HAS_DSYEVH3 FALSE) 73 | endif() 74 | if(HAS_DSYEVH3) 75 | include_directories(${DSYEVH3_ROOT}) 76 | set(DSYEVH3_SRC ${DSYEVH3_ROOT}/dsyevc3.c ${DSYEVH3_ROOT}/dsyevh3.c ${DSYEVH3_ROOT}/dsyevq3.c ${DSYEVH3_ROOT}/dsytrd3.c) 77 | set_source_files_properties(${DSYEVH3_SRC} PROPERTIES LANGUAGE CXX) 78 | add_definitions( -DUSE_DSYEVH3 ) 79 | 80 | add_library(lib3x3 STATIC ${DSYEVH3_SRC}) 81 | set(LIB3x3 lib3x3) 82 | message(STATUS "added project: ${LIB3x3}") 83 | 84 | message(STATUS "use dsyevh3 in LA::eig33sym") 85 | else() 86 | set(LIB3x3 ) 87 | message(STATUS "use Eigen::SelfAdjointEigenSolver in LA::eig33sym") 88 | endif() 89 | 90 | option(OPT_USE_OPENNI1 "use pcl::OpenNIGrabber instead of pcl::io::OpenNI2Grabber in plane_fitter.cpp" OFF) 91 | if(OPT_USE_OPENNI1) 92 | add_definitions( -DUSE_OPENNI1 ) 93 | message(STATUS "use pcl::OpenNIGrabber in plane_fitter.cpp") 94 | else() 95 | message(STATUS "use pcl::io::OpenNI2Grabber in plane_fitter.cpp") 96 | endif() 97 | 98 | ###### Projects 99 | add_executable(plane_fitter plane_fitter.cpp) 100 | target_link_libraries(plane_fitter ${PCL_LIBRARIES} ${OpenCV_LIBS} ${LIB3x3}) 101 | install(TARGETS plane_fitter DESTINATION bin) 102 | message(STATUS "added project: plane_fitter") 103 | 104 | add_executable(plane_fitter_pcd plane_fitter_pcd.cpp plane_fitter_pcd.ini plane_fitter_pcd.tls.ini) 105 | target_link_libraries(plane_fitter_pcd ${PCL_LIBRARIES} ${OpenCV_LIBS} ${LIB3x3}) 106 | install(TARGETS plane_fitter_pcd DESTINATION bin) 107 | install(FILES plane_fitter_pcd.ini plane_fitter_pcd.tls.ini DESTINATION bin) 108 | message(STATUS "added project: plane_fitter_pcd") 109 | 110 | add_executable(plane_fitter_pcd_debug plane_fitter_pcd_debug.cpp) 111 | target_link_libraries(plane_fitter_pcd_debug ${PCL_LIBRARIES} ${OpenCV_LIBS} ${LIB3x3}) 112 | install(TARGETS plane_fitter_pcd_debug DESTINATION bin) 113 | message(STATUS "added project: plane_fitter_pcd_debug") 114 | 115 | add_executable(pcd_recorder pcd_recorder.cpp) 116 | target_link_libraries(pcd_recorder ${PCL_LIBRARIES} ${OpenCV_LIBS} ${LIB3x3}) 117 | install(TARGETS pcd_recorder DESTINATION bin) 118 | message(STATUS "added project: pcd_recorder") -------------------------------------------------------------------------------- /cpp/pcd_recorder.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma warning(disable: 4996) 28 | #pragma warning(disable: 4819) 29 | #define _CRT_SECURE_NO_WARNINGS 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #ifdef USE_OPENNI1 37 | #include 38 | #else 39 | #include 40 | #include 41 | #endif 42 | #include 43 | 44 | #include "opencv2/opencv.hpp" 45 | 46 | 47 | class MainLoop 48 | { 49 | typedef pcl::PointCloud< pcl::PointXYZRGBA > ColorCloud; 50 | typedef pcl::PointCloud< pcl::PointXYZRGB > RGBCloud; 51 | protected: 52 | ColorCloud::ConstPtr cloud_; 53 | cv::Mat rgb; 54 | bool done; 55 | 56 | std::vector< ColorCloud::Ptr > recorded_cloud; 57 | std::vector recorded_rgb; 58 | int record_id; 59 | boost::mutex record_mutex; 60 | 61 | public: 62 | MainLoop () : done(false), cloud_(), record_id(0)/*, capturing(true), recording(false)*/ {} 63 | 64 | //process a new frame of point cloud 65 | void onNewCloud (const ColorCloud::ConstPtr &cloud) 66 | { 67 | boost::mutex::scoped_lock lock(record_mutex); 68 | cloud_ = cloud; 69 | 70 | //fill RGB 71 | if (rgb.empty() || rgb.rows != cloud->height || rgb.cols != cloud->width) { 72 | rgb.create(cloud->height, cloud->width, CV_8UC3); 73 | } 74 | for (int i = 0; i < (int)cloud->height; ++i) { 75 | for (int j = 0; j < (int)cloud->width; ++j) { 76 | const pcl::PointXYZRGBA& p = cloud->at(j, i); 77 | if (!pcl_isnan(p.z)) { 78 | rgb.at(i, j) = cv::Vec3b(p.b, p.g, p.r); 79 | } else { 80 | static const cv::Vec3b white(255, 255, 255); 81 | rgb.at(i, j) = white;//whiten invalid area 82 | } 83 | } 84 | } 85 | 86 | //show frame rate 87 | std::stringstream stext; 88 | stext << "id=" << recorded_rgb.size(); 89 | cv::putText(rgb, stext.str(), cv::Point(15, 15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0, 1)); 90 | 91 | //cv::imshow("rgb", rgb); //calling imshow inside capture thread seems not a good idea for tablet 92 | } 93 | 94 | //start the main loop 95 | void run () 96 | { 97 | #ifdef USE_OPENNI1 98 | pcl::Grabber* grabber = new pcl::OpenNIGrabber(); 99 | #else 100 | pcl::Grabber* grabber = new pcl::io::OpenNI2Grabber(); 101 | #endif 102 | 103 | boost::function f = 104 | boost::bind (&MainLoop::onNewCloud, this, _1); 105 | 106 | grabber->registerCallback(f); 107 | 108 | cv::namedWindow("rgb"); 109 | cv::namedWindow("recorded"); 110 | const int baseX = 80, baseY = 10; 111 | cv::moveWindow("rgb", baseX, baseY); 112 | cv::moveWindow("recorded", baseX, baseY + 10 + 480); 113 | 114 | //grabbing loop 115 | grabber->start(); 116 | 117 | //GUI loop 118 | while (!done) 119 | { 120 | onKey(cv::waitKey(50)); //update parameter once per second 121 | } 122 | 123 | grabber->stop(); 124 | delete grabber; 125 | } 126 | 127 | void record_current() { 128 | { 129 | boost::mutex::scoped_lock lock(record_mutex); 130 | if (!cloud_) return; 131 | //recording = true; 132 | recorded_rgb.push_back(rgb.clone()); 133 | recorded_cloud.push_back(ColorCloud::Ptr(new ColorCloud(*cloud_))); 134 | } 135 | 136 | if (recorded_rgb.size() <= 0) return; 137 | cv::imshow("recorded", recorded_rgb.back()); 138 | record_id = recorded_rgb.size() - 1; 139 | std::cout << "#recorded=" << recorded_rgb.size() << std::endl; 140 | } 141 | 142 | void save(const std::string& outputPrefix) { 143 | for (int k = 0; k < (int)recorded_cloud.size(); ++k) { 144 | std::string name = outputPrefix + cv::format("/record_%05d.pcd", k); 145 | const ColorCloud::Ptr& cloud = recorded_cloud[k]; 146 | 147 | RGBCloud ocloud(cloud->width, cloud->height); 148 | for (int i = 0; i<(int)cloud->height; ++i) { 149 | for (int j = 0; j<(int)cloud->width; ++j) { 150 | const pcl::PointXYZRGBA& p = cloud->at(j, i); 151 | pcl::PointXYZRGB& po = ocloud.at(j, i); 152 | po.x = p.x; po.y = p.y; po.z = p.z; 153 | po.r = p.r; po.g = p.g; po.b = p.b; 154 | } 155 | } 156 | 157 | pcl::io::savePCDFileBinary(name, ocloud); 158 | std::cout << "saved: " << name << std::endl; 159 | } 160 | } 161 | 162 | //handle keyboard commands 163 | void onKey(const unsigned char key) 164 | { 165 | static bool stop=false; 166 | switch(key) { 167 | case 'q': 168 | this->done=true; 169 | break; 170 | case 'r': 171 | record_current(); 172 | break; 173 | case ',': 174 | if (recorded_rgb.size() > 0) { 175 | --record_id; 176 | if (record_id < 0) record_id = recorded_rgb.size() - 1; 177 | cv::imshow("recorded", recorded_rgb[record_id]); 178 | } 179 | break; 180 | case '.': 181 | if (recorded_rgb.size()>0) { 182 | record_id = (record_id + 1) % recorded_rgb.size(); 183 | cv::imshow("recorded", recorded_rgb[record_id]); 184 | } 185 | break; 186 | case '-': 187 | if (recorded_rgb.size() > 0) { 188 | recorded_rgb.pop_back(); 189 | recorded_cloud.pop_back(); 190 | record_id = recorded_rgb.size() - 1; 191 | if (record_id >= 0) { 192 | cv::imshow("recorded", recorded_rgb[record_id]); 193 | } else { 194 | cv::imshow("recorded", cv::Mat::zeros(480,640,CV_8UC1)); 195 | } 196 | std::cout << "deleted last recorded frame! (" << recorded_rgb.size() << " frames left)." << std::endl; 197 | } 198 | break; 199 | } 200 | if(!rgb.empty()) 201 | cv::imshow("rgb", rgb); 202 | } 203 | }; 204 | 205 | int main (const int argc, const char** argv) 206 | { 207 | if (argc != 2) { 208 | std::cout << "usage: " << argv[0] << " " << std::endl; 209 | return 0; 210 | } 211 | 212 | const std::string outputDir(argv[1]); 213 | {//create outputDir 214 | #ifdef _WIN32 215 | std::string cmd = "mkdir " + outputDir; 216 | #else 217 | std::string cmd = "mkdir -p " + outputDir; 218 | #endif 219 | system(cmd.c_str()); 220 | } 221 | 222 | MainLoop loop; 223 | loop.run(); 224 | 225 | loop.save(outputDir); 226 | return 0; 227 | } -------------------------------------------------------------------------------- /cpp/plane_fitter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma warning(disable: 4996) 28 | #pragma warning(disable: 4819) 29 | #define _CRT_SECURE_NO_WARNINGS 30 | 31 | #include 32 | #include 33 | #ifdef USE_OPENNI1 34 | #include 35 | #else 36 | #include 37 | #include 38 | #endif 39 | 40 | #include "opencv2/opencv.hpp" 41 | 42 | #include "AHCPlaneFitter.hpp" 43 | 44 | using ahc::utils::Timer; 45 | 46 | // pcl::PointCloud interface for our ahc::PlaneFitter 47 | template 48 | struct OrganizedImage3D { 49 | const pcl::PointCloud& cloud; 50 | //NOTE: pcl::PointCloud from OpenNI uses meter as unit, 51 | //while ahc::PlaneFitter assumes mm as unit!!! 52 | const double unitScaleFactor; 53 | 54 | OrganizedImage3D(const pcl::PointCloud& c) : cloud(c), unitScaleFactor(1000) {} 55 | int width() const { return cloud.width; } 56 | int height() const { return cloud.height; } 57 | bool get(const int row, const int col, double& x, double& y, double& z) const { 58 | const PointT& pt=cloud.at(col,row); 59 | x=pt.x*unitScaleFactor; y=pt.y*unitScaleFactor; z=pt.z*unitScaleFactor; 60 | return pcl_isnan(z)==0; //return false if current depth is NaN 61 | } 62 | }; 63 | typedef OrganizedImage3D RGBDImage; 64 | typedef ahc::PlaneFitter PlaneFitter; 65 | 66 | class MainLoop 67 | { 68 | protected: 69 | PlaneFitter pf; 70 | cv::Mat rgb, seg; 71 | bool done; 72 | 73 | public: 74 | MainLoop () : done(false) {} 75 | 76 | //process a new frame of point cloud 77 | void onNewCloud (const pcl::PointCloud::ConstPtr &cloud) 78 | { 79 | //fill RGB 80 | if(rgb.empty() || rgb.rows!=cloud->height || rgb.cols!=cloud->width) { 81 | rgb.create(cloud->height, cloud->width, CV_8UC3); 82 | seg.create(cloud->height, cloud->width, CV_8UC3); 83 | } 84 | for(int i=0; i<(int)cloud->height; ++i) { 85 | for(int j=0; j<(int)cloud->width; ++j) { 86 | const pcl::PointXYZRGBA& p=cloud->at(j,i); 87 | if(!pcl_isnan(p.z)) { 88 | rgb.at(i,j)=cv::Vec3b(p.b,p.g,p.r); 89 | } else { 90 | rgb.at(i,j)=cv::Vec3b(255,255,255);//whiten invalid area 91 | } 92 | } 93 | } 94 | 95 | //run PlaneFitter on the current frame of point cloud 96 | RGBDImage rgbd(*cloud); 97 | Timer timer(1000); 98 | timer.tic(); 99 | 100 | #if 0 101 | pf.run(&rgbd, 0, &seg); 102 | #else 103 | std::vector< std::vector > membership; 104 | pf.run(&rgbd, &membership, 0); 105 | { 106 | static cv::Mat my_color_map; 107 | my_color_map.reserve(256); 108 | static const unsigned char default_colors[10][3] = 109 | { 110 | { 255, 0, 0 }, 111 | { 255, 255, 0 }, 112 | { 100, 20, 50 }, 113 | { 0, 30, 255 }, 114 | { 10, 255, 60 }, 115 | { 80, 10, 100 }, 116 | { 0, 255, 200 }, 117 | { 10, 60, 60 }, 118 | { 255, 0, 128 }, 119 | { 60, 128, 128 } 120 | }; 121 | if (my_color_map.empty()) { 122 | for (int i = 0; i < 10; ++i) { 123 | my_color_map.push_back(cv::Vec3b(default_colors[i])); 124 | } 125 | for (int i = 10; i < 256; ++i) { 126 | my_color_map.push_back(cv::Vec3b(rand() % 255, rand() % 255, rand() % 255)); 127 | } 128 | my_color_map.at(255) = cv::Vec3b(0, 0, 0); 129 | } 130 | seg = cv::Mat::zeros(pf.height, pf.width, CV_8UC3); 131 | for (int i = 0; i < membership.size(); ++i) { 132 | const std::vector& mi = membership[i]; 133 | for (int k = 0; k < mi.size(); ++k) { 134 | const int pixid = mi[k]; 135 | const int py = pixid / pf.width; 136 | const int px = pixid - py*pf.width; 137 | seg.at(py, px) = my_color_map.at(i); 138 | } 139 | } 140 | //cv::applyColorMap(tmp, seg, cv::COLORMAP_JET); 141 | } 142 | #endif 143 | double process_ms=timer.toc(); 144 | 145 | //blend segmentation with rgb 146 | cv::cvtColor(seg,seg,CV_RGB2BGR); 147 | seg=(rgb+seg)/2.0; 148 | 149 | //show frame rate 150 | std::stringstream stext; 151 | stext<<"Frame Rate: "<<(1000.0/process_ms)<<"Hz"; 152 | cv::putText(seg, stext.str(), cv::Point(15,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0,0,0,1)); 153 | 154 | cv::imshow("rgb", rgb); 155 | cv::imshow("seg", seg); 156 | } 157 | 158 | //start the main loop 159 | void run () 160 | { 161 | #ifdef USE_OPENNI1 162 | pcl::Grabber* grabber = new pcl::OpenNIGrabber(); 163 | #else 164 | pcl::Grabber* grabber = new pcl::io::OpenNI2Grabber(); 165 | #endif 166 | 167 | boost::function::ConstPtr&)> f = 168 | boost::bind (&MainLoop::onNewCloud, this, _1); 169 | 170 | grabber->registerCallback(f); 171 | 172 | //grabbing loop 173 | grabber->start(); 174 | 175 | cv::namedWindow("rgb"); 176 | cv::namedWindow("seg"); 177 | cv::namedWindow("control", cv::WINDOW_NORMAL); 178 | const int baseX = 80, baseY = 10; 179 | cv::moveWindow("rgb", baseX, baseY); 180 | cv::moveWindow("control", baseX + 10 + 640, baseY); 181 | cv::moveWindow("seg", baseX, baseY + 10 + 480); 182 | 183 | // related to T_mse 184 | int depthSigma = (int)(pf.params.depthSigma * 1e7); 185 | int mergeMSETol = (int)pf.params.stdTol_merge; 186 | int initMSETol = (int)pf.params.stdTol_init; 187 | cv::createTrackbar("depthSigma", "control", &depthSigma, 5 * depthSigma); 188 | cv::createTrackbar("epsilon", "control", &mergeMSETol, 2 * mergeMSETol); 189 | cv::createTrackbar("eps_init", "control", &initMSETol, 2 * initMSETol); 190 | 191 | // related to T_ang 192 | int z_near = (int)pf.params.z_near; 193 | int z_far = (int)pf.params.z_far; 194 | int angle_near = (int)15; 195 | int angle_far = (int)90; 196 | int simThMergeDeg = 60; 197 | int simThRefDeg = 30; 198 | cv::createTrackbar("z_near", "control", &z_near, 2 * z_near); 199 | cv::createTrackbar("z_far", "control", &z_far, 2 * z_far); 200 | cv::createTrackbar("angle_near", "control", &angle_near, 90); 201 | cv::createTrackbar("angle_far", "control", &angle_far, 90); 202 | cv::createTrackbar("simThMergeDeg", "control", &simThMergeDeg, 90); 203 | cv::createTrackbar("simThRefDeg", "control", &simThRefDeg, 90); 204 | 205 | // related to T_dz 206 | int depthAlpha = (int)(pf.params.depthAlpha * 100); 207 | int depthChangeTol = (int)(pf.params.depthChangeTol * 100); 208 | cv::createTrackbar("depthAlpha", "control", &depthAlpha, 2 * depthAlpha); 209 | cv::createTrackbar("depthChangeTol", "control", &depthChangeTol, 2 * depthChangeTol); 210 | 211 | // other 212 | int minSupport = (int)pf.minSupport; 213 | int doRefine = (int)pf.doRefine; 214 | cv::createTrackbar("T_{NUM}","control", &minSupport, pf.minSupport*5); 215 | cv::createTrackbar("Refine On","control", &doRefine, 1); 216 | cv::createTrackbar("windowHeight","control", &(pf.windowHeight), 2*pf.windowHeight); 217 | cv::createTrackbar("windowWidth","control", &(pf.windowWidth), 2*pf.windowWidth); 218 | int erodeType = (int)pf.erodeType; 219 | cv::createTrackbar("erodeType", "control", &erodeType, 2); 220 | 221 | //GUI loop 222 | while (!done) 223 | { 224 | static bool reported = false; 225 | pf.params.depthSigma = depthSigma*1e-7; 226 | pf.params.stdTol_init = (double)initMSETol; 227 | pf.params.stdTol_merge = (double)mergeMSETol; 228 | 229 | pf.params.z_near = (double)z_near; 230 | pf.params.z_far = (double)z_far; 231 | pf.params.angle_near = (double)MACRO_DEG2RAD(angle_near); 232 | pf.params.angle_far = (double)MACRO_DEG2RAD(angle_far); 233 | pf.params.similarityTh_merge = std::cos(MACRO_DEG2RAD(simThMergeDeg)); 234 | pf.params.similarityTh_refine = std::cos(MACRO_DEG2RAD(simThRefDeg)); 235 | 236 | pf.params.depthAlpha = (double)depthAlpha*0.01; 237 | pf.params.depthChangeTol = (double)depthChangeTol*0.01; 238 | 239 | pf.minSupport=minSupport; 240 | pf.doRefine=doRefine!=0; 241 | pf.erodeType = (ahc::ErodeType)erodeType; 242 | 243 | onKey(cv::waitKey(1000)); //update parameter once per second 244 | } 245 | 246 | grabber->stop(); 247 | } 248 | 249 | //handle keyboard commands 250 | void onKey(const unsigned char key) 251 | { 252 | static bool stop=false; 253 | switch(key) { 254 | case 'q': 255 | this->done=true; 256 | break; 257 | } 258 | } 259 | }; 260 | 261 | int main () 262 | { 263 | MainLoop loop; 264 | loop.run(); 265 | return 0; 266 | } -------------------------------------------------------------------------------- /cpp/plane_fitter_pcd.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma warning(disable: 4996) 28 | #pragma warning(disable: 4819) 29 | #define _CRT_SECURE_NO_WARNINGS 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include "opencv2/opencv.hpp" 39 | #include 40 | 41 | #include "AHCPlaneFitter.hpp" 42 | using ahc::utils::Timer; 43 | 44 | // pcl::PointCloud interface for our ahc::PlaneFitter 45 | template 46 | struct OrganizedImage3D { 47 | const pcl::PointCloud& cloud; 48 | //note: ahc::PlaneFitter assumes mm as unit!!! 49 | const double unitScaleFactor; 50 | 51 | OrganizedImage3D(const pcl::PointCloud& c) : cloud(c), unitScaleFactor(1) {} 52 | OrganizedImage3D(const OrganizedImage3D& other) : cloud(other.cloud), unitScaleFactor(other.unitScaleFactor) {} 53 | 54 | inline int width() const { return cloud.width; } 55 | inline int height() const { return cloud.height; } 56 | inline bool get(const int row, const int col, double& x, double& y, double& z) const { 57 | const PointT& pt=cloud.at(col,row); 58 | x=pt.x*unitScaleFactor; y=pt.y*unitScaleFactor; z=pt.z*unitScaleFactor; //TODO: will this slowdown the speed? 59 | return pcl_isnan(z)==0; //return false if current depth is NaN 60 | } 61 | }; 62 | typedef OrganizedImage3D ImageXYZ; 63 | typedef ahc::PlaneFitter< ImageXYZ > PlaneFitter; 64 | typedef pcl::PointCloud CloudXYZRGB; 65 | 66 | namespace global { 67 | std::map ini; 68 | PlaneFitter pf; 69 | bool showWindow = true; 70 | 71 | #ifdef _WIN32 72 | const char filesep = '\\'; 73 | #else 74 | const char filesep = '/'; 75 | #endif 76 | 77 | // similar to matlab's fileparts 78 | // if in=parent/child/file.txt 79 | // then path=parent/child 80 | // name=file, ext=txt 81 | void fileparts(const std::string& str, std::string* pPath=0, 82 | std::string* pName=0, std::string* pExt=0) 83 | { 84 | std::string::size_type last_sep = str.find_last_of(filesep); 85 | std::string::size_type last_dot = str.find_last_of('.'); 86 | if (last_dot "D:/test/" 123 | std::string getFileDir(const std::string &fileName) 124 | { 125 | std::string path; 126 | fileparts(fileName, &path); 127 | return path; 128 | } 129 | 130 | //"D:/parent/test.txt" -> "test" 131 | //"D:/parent/test" -> "test" 132 | std::string getNameNoExtension(const std::string &fileName) 133 | { 134 | std::string name; 135 | fileparts(fileName, 0, &name); 136 | return name; 137 | } 138 | 139 | void iniLoad(std::string iniFileName) { 140 | std::ifstream in(iniFileName); 141 | if(!in.is_open()) { 142 | std::cout<<"[iniLoad] "<"< 163 | T iniGet(std::string key, T default_value) { 164 | std::map::const_iterator itr=ini.find(key); 165 | if(itr!=ini.end()) { 166 | std::stringstream ss; 167 | ss<second; 168 | T ret; 169 | ss>>ret; 170 | return ret; 171 | } 172 | return default_value; 173 | } 174 | 175 | template<> std::string iniGet(std::string key, std::string default_value) { 176 | std::map::const_iterator itr=ini.find(key); 177 | if(itr!=ini.end()) { 178 | return itr->second; 179 | } 180 | return default_value; 181 | } 182 | }//global 183 | 184 | void processOneFrame(pcl::PointCloud& cloud, const std::string& outputFilePrefix) 185 | { 186 | using global::pf; 187 | cv::Mat seg(cloud.height, cloud.width, CV_8UC3); 188 | 189 | //run PlaneFitter on the current frame of point cloud 190 | ImageXYZ Ixyz(cloud); 191 | Timer timer(1000); 192 | timer.tic(); 193 | pf.run(&Ixyz, 0, &seg); 194 | double process_ms=timer.toc(); 195 | std::cout<(r,c);; 209 | pix.x=pxyz.x; 210 | pix.y=pxyz.y; 211 | pix.z=pxyz.z; 212 | pix.r=prgb(2); 213 | pix.g=prgb(1); 214 | pix.b=prgb(0); 215 | } 216 | } 217 | pcl::io::savePCDFileBinary(outputFilePrefix+".seg.pcd", xyzrgb); 218 | 219 | if(global::showWindow) { 220 | //show frame rate 221 | std::stringstream stext; 222 | stext<<"Frame Rate: "<<(1000.0/process_ms)<<"Hz"; 223 | cv::putText(seg, stext.str(), cv::Point(15,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1)); 224 | 225 | cv::imshow("seg", seg); 226 | cv::waitKey(10); 227 | } 228 | } 229 | 230 | int process() { 231 | const double unitScaleFactor = global::iniGet("unitScaleFactor", 1.0f); 232 | const std::string outputDir = global::iniGet("outputDir", "."); 233 | {//create outputDir 234 | #ifdef _WIN32 235 | std::string cmd="mkdir "+outputDir + " 2> NUL"; 236 | #else 237 | std::string cmd="mkdir -p "+outputDir; 238 | #endif 239 | system(cmd.c_str()); 240 | std::cout << "create:" << outputDir << std::endl; 241 | } 242 | 243 | using global::pf; 244 | //setup fitter 245 | pf.minSupport = global::iniGet("minSupport", 3000); 246 | pf.windowWidth = global::iniGet("windowWidth", 10); 247 | pf.windowHeight = global::iniGet("windowHeight", 10); 248 | pf.doRefine = global::iniGet("doRefine", 1) != 0; 249 | 250 | pf.params.initType = (ahc::InitType)global::iniGet("initType", (int)pf.params.initType); 251 | 252 | //T_mse 253 | pf.params.stdTol_merge = global::iniGet("stdTol_merge", pf.params.stdTol_merge); 254 | pf.params.stdTol_init = global::iniGet("stdTol_init", pf.params.stdTol_init); 255 | pf.params.depthSigma = global::iniGet("depthSigma", pf.params.depthSigma); 256 | 257 | //T_dz 258 | pf.params.depthAlpha = global::iniGet("depthAlpha", pf.params.depthAlpha); 259 | pf.params.depthChangeTol = global::iniGet("depthChangeTol", pf.params.depthChangeTol); 260 | 261 | //T_ang 262 | pf.params.z_near = global::iniGet("z_near", pf.params.z_near); 263 | pf.params.z_far = global::iniGet("z_far", pf.params.z_far); 264 | pf.params.angle_near = MACRO_DEG2RAD(global::iniGet("angleDegree_near", MACRO_RAD2DEG(pf.params.angle_near))); 265 | pf.params.angle_far = MACRO_DEG2RAD(global::iniGet("angleDegree_far", MACRO_RAD2DEG(pf.params.angle_far))); 266 | pf.params.similarityTh_merge = std::cos(MACRO_DEG2RAD(global::iniGet("similarityDegreeTh_merge", MACRO_RAD2DEG(pf.params.similarityTh_merge)))); 267 | pf.params.similarityTh_refine = std::cos(MACRO_DEG2RAD(global::iniGet("similarityDegreeTh_refine", MACRO_RAD2DEG(pf.params.similarityTh_refine)))); 268 | 269 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 270 | pf.saveDir = outputDir; 271 | {//create debug result folder 272 | #ifdef _WIN32 273 | std::string cmd = "mkdir " + pf.saveDir + "\\output 2> NUL"; 274 | #else 275 | std::string cmd = "mkdir -p " + pf.saveDir + "\\output"; 276 | #endif 277 | system(cmd.c_str()); 278 | std::cout << "create:" << (pf.saveDir + "\\output") << std::endl; 279 | } 280 | #endif 281 | 282 | bool is_debug = global::iniGet("debug", 0); 283 | int loop_cnt = global::iniGet("loop", 0); //0: no loop; -1: infinite loop; >0: n loops 284 | 285 | std::vector fnamelist; 286 | std::string filelist = global::iniGet("list","list.txt"); 287 | 288 | if (filelist.find(".pcd") != std::string::npos) { 289 | fnamelist.push_back(filelist); //support list=test.pcd to only input a single file 290 | loop_cnt = global::iniGet("loop", -1); //by default turn on infinite loop (-1) mode 291 | } else { 292 | std::string inputDir = global::getFileDir(filelist); 293 | 294 | std::ifstream is(filelist.c_str()); 295 | if (!is.is_open()) { 296 | std::cout << "could not open list=" << filelist << std::endl; 297 | return -1; 298 | } 299 | 300 | while (is) { 301 | std::string fname; 302 | std::getline(is, fname); 303 | if (fname.empty()) continue; 304 | fname = inputDir + global::filesep + fname; 305 | fnamelist.push_back(fname); 306 | } 307 | } 308 | 309 | using global::showWindow; 310 | showWindow = global::iniGet("showWindow", true); 311 | if (showWindow) 312 | cv::namedWindow("seg"); 313 | 314 | int idx = 0; 315 | while(true) 316 | { 317 | if (idx >= fnamelist.size()) { 318 | if (loop_cnt != 0) { 319 | idx = 0; 320 | if (loop_cnt > 0) --loop_cnt; 321 | } 322 | else break; 323 | } 324 | 325 | const std::string& fname = fnamelist[idx]; 326 | pcl::PointCloud cloud; 327 | if(pcl::io::loadPCDFile(fname, cloud) <0) { 328 | std::cout<<"fail to load: "<( 331 | cloud, cloud, 332 | Eigen::Affine3f(Eigen::UniformScaling( 333 | (float)unitScaleFactor))); 334 | std::cout<=0) 106 | * \return cos of the normal deviation threshold at depth z 107 | * 108 | * \details This is simply a linear mapping from depth to thresholding angle 109 | * and the threshold will be used to reject edge when initialize the graph; 110 | * this function corresponds to T_{ANG} in our paper 111 | */ 112 | inline double T_ang(const Phase phase, const double z=0) const { 113 | switch(phase) { 114 | case P_INIT: 115 | {//linear maping z->thresholding angle, clipping z also 116 | double clipped_z = z; 117 | clipped_z=std::max(clipped_z,z_near); 118 | clipped_z=std::min(clipped_z,z_far); 119 | const double factor = (angle_far-angle_near)/(z_far-z_near); 120 | return std::cos(factor*clipped_z+angle_near-factor*z_near); 121 | } 122 | case P_MERGING: 123 | { 124 | return similarityTh_merge; 125 | } 126 | case P_REFINE: 127 | default: 128 | { 129 | return similarityTh_refine; 130 | } 131 | } 132 | } 133 | 134 | /** 135 | * \brief Dynamic threshold to test whether the two adjacent pixels are discontinuous in depth 136 | * 137 | * \param [in] z depth of the current pixel 138 | * \return the max depth change allowed at depth z to make the points connected in a single block 139 | * 140 | * \details This is modified from pcl's segmentation code as well as suggested in 2013.iros.Holzer 141 | * essentially returns factor*z+tolerance 142 | * (TODO: maybe change this to 3D-point distance threshold) 143 | */ 144 | inline double T_dz(const double z) const { 145 | return depthAlpha * fabs(z) + depthChangeTol; 146 | } 147 | };//ParamSet 148 | 149 | }//ahc -------------------------------------------------------------------------------- /include/AHCPlaneFitter.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #define _USE_MATH_DEFINES 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | //#define DEBUG_CLUSTER 41 | //#define DEBUG_CALC 42 | //#define DEBUG_INIT 43 | //#define EVAL_SPEED 44 | 45 | #include "AHCTypes.hpp" 46 | #include "AHCPlaneSeg.hpp" 47 | #include "AHCParamSet.hpp" 48 | #include "AHCUtils.hpp" 49 | 50 | namespace ahc { 51 | using ahc::utils::Timer; 52 | using ahc::utils::pseudocolor; 53 | 54 | /** 55 | * \brief An example of Image3D struct as an adaptor for any kind of point cloud to be used by our ahc::PlaneFitter 56 | * 57 | * \details A valid Image3D struct should implements the following three member functions: 58 | * 1. int width() 59 | * return the #pixels for each row of the point cloud 60 | * 2. int height() 61 | * return the #pixels for each column of the point cloud 62 | * 3. bool get(const int i, const int j, double &x, double &y, double &z) const 63 | * access the xyz coordinate of the point at i-th-row j-th-column, return true if success and false otherwise (due to NaN depth or any other reasons) 64 | */ 65 | struct NullImage3D { 66 | int width() { return 0; } 67 | int height() { return 0; } 68 | //get point at row i, column j 69 | bool get(const int i, const int j, double &x, double &y, double &z) const { return false; } 70 | }; 71 | 72 | //three types of erode operation for segmentation refinement 73 | enum ErodeType { 74 | ERODE_NONE=0, //no erode 75 | ERODE_SEG_BORDER=1, //erode only borders between two segments 76 | ERODE_ALL_BORDER=2 //erode all borders, either between two segments or between segment and "black" 77 | }; 78 | 79 | /** 80 | * \brief ahc::PlaneFitter implements the Agglomerative Hierarchical Clustering based fast plane extraction 81 | * 82 | * \details note: default parameters assume point's unit is mm 83 | */ 84 | template 85 | struct PlaneFitter { 86 | /************************************************************************/ 87 | /* Internal Classes */ 88 | /************************************************************************/ 89 | //for sorting PlaneSeg by size-decreasing order 90 | struct PlaneSegSizeCmp { 91 | bool operator()(const PlaneSeg::shared_ptr& a, 92 | const PlaneSeg::shared_ptr& b) const { 93 | return b->N < a->N; 94 | } 95 | }; 96 | 97 | //for maintaining the Min MSE heap of PlaneSeg 98 | struct PlaneSegMinMSECmp { 99 | bool operator()(const PlaneSeg::shared_ptr& a, 100 | const PlaneSeg::shared_ptr& b) const { 101 | return b->mse < a->mse; 102 | } 103 | }; 104 | typedef std::priority_queue, 106 | PlaneSegMinMSECmp> PlaneSegMinMSEQueue; 107 | 108 | /************************************************************************/ 109 | /* Public Class Members */ 110 | /************************************************************************/ 111 | //input 112 | const Image3D *points; //dim=, no ownership 113 | int width, height; //witdth=#cols, height=#rows (size of the input point cloud) 114 | 115 | int maxStep; //max number of steps for merging clusters 116 | int minSupport; //min number of supporting point 117 | int windowWidth; //make sure width is divisible by windowWidth 118 | int windowHeight; //similarly for height and windowHeight 119 | bool doRefine; //perform refinement of details or not 120 | ErodeType erodeType; 121 | 122 | ParamSet params; //sets of parameters controlling dynamic thresholds T_mse, T_ang, T_dz 123 | 124 | //output 125 | ahc::shared_ptr ds;//with ownership, this disjoint set maintains membership of initial window/blocks during AHC merging 126 | std::vector extractedPlanes;//a set of extracted planes 127 | cv::Mat membershipImg;//segmentation map of the input pointcloud, membershipImg(i,j) records which plane (plid, i.e. plane id) this pixel/point (i,j) belongs to 128 | 129 | //intermediate 130 | std::map rid2plid; //extractedPlanes[rid2plid[rootid]].rid==rootid, i.e. rid2plid[rid] gives the idx of a plane in extractedPlanes 131 | std::vector blkMap; //(i,j) block belong to extractedPlanes[blkMap[i*Nh+j]] 132 | //std::vector> blkMembership; //blkMembership[i] contains all block id for extractedPlanes[i] 133 | bool dirtyBlkMbship; 134 | std::vector colors; 135 | std::vector> rfQueue;//for region grow/floodfill, p.first=pixidx, p.second=plid 136 | bool drawCoarseBorder; 137 | //std::vector blkStats; 138 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 139 | std::string saveDir; 140 | #endif 141 | #ifdef DEBUG_CALC 142 | std::vector numNodes; 143 | std::vector numEdges; 144 | std::vector mseNodeDegree; 145 | int maxIndvidualNodeDegree; 146 | #endif 147 | 148 | /************************************************************************/ 149 | /* Public Class Functions */ 150 | /************************************************************************/ 151 | PlaneFitter() : points(0), width(0), height(0), 152 | maxStep(100000), minSupport(3000), 153 | windowWidth(10), windowHeight(10), 154 | doRefine(true), erodeType(ERODE_ALL_BORDER), 155 | dirtyBlkMbship(true), drawCoarseBorder(false) 156 | { 157 | static const unsigned char default_colors[10][3] = 158 | { 159 | {255, 0, 0}, 160 | {255, 255, 0}, 161 | {100, 20, 50}, 162 | {0, 30, 255}, 163 | {10, 255, 60}, 164 | {80, 10, 100}, 165 | {0, 255, 200}, 166 | {10, 60, 60}, 167 | {255, 0, 128}, 168 | {60, 128, 128} 169 | }; 170 | for(int i=0; i<10; ++i) { 171 | colors.push_back(cv::Vec3b(default_colors[i])); 172 | } 173 | } 174 | 175 | ~PlaneFitter() {} 176 | 177 | /** 178 | * \brief clear/reset for next run 179 | */ 180 | void clear() { 181 | this->points=0; 182 | this->extractedPlanes.clear(); 183 | ds.reset(); 184 | rid2plid.clear(); 185 | blkMap.clear(); 186 | rfQueue.clear(); 187 | //blkStats.clear(); 188 | dirtyBlkMbship=true; 189 | } 190 | 191 | /** 192 | * \brief run AHC plane fitting on one frame of point cloud pointsIn 193 | * 194 | * \param [in] pointsIn a frame of point cloud 195 | * \param [out] pMembership pointer to segmentation membership vector, each pMembership->at(i) is a vector of pixel indices that belong to the i-th extracted plane 196 | * \param [out] pSeg a 3-channel RGB image as another form of output of segmentation 197 | * \param [in] pIdxMap usually not needed (reserved for KinectSLAM to input pixel index map) 198 | * \param [in] verbose print out cluster steps and #planes or not 199 | * \return when compiled without EVAL_SPEED: 0 if pointsIn==0 and 1 otherwise; when compiled with EVAL_SPEED: total running time for this frame 200 | * 201 | * \details this function corresponds to Algorithm 1 in our paper 202 | */ 203 | double run(const Image3D* pointsIn, 204 | std::vector>* pMembership=0, 205 | cv::Mat* pSeg=0, 206 | const std::vector * const pIdxMap=0, bool verbose=true) 207 | { 208 | if(!pointsIn) return 0; 209 | #ifdef EVAL_SPEED 210 | Timer timer(1000), timer2(1000); 211 | timer.tic(); timer2.tic(); 212 | #endif 213 | clear(); 214 | this->points = pointsIn; 215 | this->height = points->height(); 216 | this->width = points->width(); 217 | this->ds.reset(new DisjointSet((height/windowHeight)*(width/windowWidth))); 218 | 219 | PlaneSegMinMSEQueue minQ; 220 | this->initGraph(minQ); 221 | #ifdef EVAL_SPEED 222 | timer.toctic("init time"); 223 | #endif 224 | int step=this->ahCluster(minQ); 225 | #ifdef EVAL_SPEED 226 | timer.toctic("cluster time"); 227 | #endif 228 | if(doRefine) { 229 | this->refineDetails(pMembership, pIdxMap, pSeg); 230 | #ifdef EVAL_SPEED 231 | timer.toctic("refine time"); 232 | #endif 233 | } else { 234 | if(pMembership) { 235 | this->findMembership(*pMembership, pIdxMap); 236 | } 237 | if(pSeg) { 238 | this->plotSegmentImage(pSeg, minSupport); 239 | } 240 | #ifdef EVAL_SPEED 241 | timer.toctic("return time"); 242 | #endif 243 | } 244 | if(verbose) { 245 | std::cout<<"#step="<> *pMembership, //pMembership->size()==nPlanes 292 | const std::vector * const pIdxMap, //if pIdxMap!=0 pMembership->at(i).at(j)=pIdxMap(pixIdx) 293 | cv::Mat* pSeg) 294 | { 295 | if(pMembership==0 && pSeg==0) return; 296 | std::vector isValidExtractedPlane; //some planes might be eroded completely 297 | this->findBlockMembership(isValidExtractedPlane); 298 | 299 | //save the border regions 300 | std::vector border; 301 | if(drawCoarseBorder && pSeg) { 302 | border.resize(rfQueue.size()); 303 | for(int i=0; i<(int)this->rfQueue.size(); ++i) { 304 | border[i]=rfQueue[i].first; 305 | } 306 | } 307 | 308 | this->floodFill(); 309 | 310 | //try to merge one last time 311 | std::vector oldExtractedPlanes; 312 | this->extractedPlanes.swap(oldExtractedPlanes); 313 | PlaneSegMinMSEQueue minQ; 314 | for(int i=0; i<(int)oldExtractedPlanes.size(); ++i) { 315 | if(isValidExtractedPlane[i]) 316 | minQ.push(oldExtractedPlanes[i]); 317 | } 318 | this->ahCluster(minQ, false); 319 | 320 | //find plane idx maping from oldExtractedPlanes to final extractedPlanes 321 | std::vector plidmap(oldExtractedPlanes.size(),-1); 322 | int nFinalPlanes=0; 323 | for(int i=0; i<(int)oldExtractedPlanes.size(); ++i) { 324 | const PlaneSeg& op=*oldExtractedPlanes[i]; 325 | if(!isValidExtractedPlane[i]) { 326 | plidmap[i]=-1;//this plane was eroded 327 | continue; 328 | } 329 | int np_rid; 330 | if((np_rid=ds->Find(op.rid))==op.rid) {//op is not changed 331 | if(plidmap[i]<0) //if not updated, otherwise was updated before 332 | plidmap[i]=nFinalPlanes++; 333 | } else {//op is merged to np 334 | const int npid=this->rid2plid[np_rid]; 335 | if(plidmap[npid]<0) //if np's idmap not updated 336 | plidmap[i]=plidmap[npid]=nFinalPlanes++; 337 | else 338 | plidmap[i]=plidmap[npid]; 339 | } 340 | } 341 | assert(nFinalPlanes==(int)this->extractedPlanes.size()); 342 | 343 | //scan membershipImg 344 | if(nFinalPlanes>colors.size()) { 345 | std::vector tmpColors=pseudocolor(nFinalPlanes-(int)colors.size()); 346 | colors.insert(colors.end(), tmpColors.begin(), tmpColors.end()); 347 | } 348 | if(pMembership) { 349 | pMembership->resize(nFinalPlanes, std::vector()); 350 | for(int i=0; iat(i).reserve( 352 | (int)(this->extractedPlanes[i]->N*1.2f)); 353 | } 354 | } 355 | 356 | static const cv::Vec3b blackColor(0,0,0); 357 | const int nPixels=this->width*this->height; 358 | for(int i=0; i(i); 360 | if(plid>=0 && plidmap[plid]>=0) { 361 | plid=plidmap[plid]; 362 | if(pSeg) pSeg->at(i)=this->colors[plid]; 363 | if(pMembership) pMembership->at(plid).push_back( 364 | pIdxMap?pIdxMap->at(i):i); 365 | } else { 366 | if(pSeg) pSeg->at(i)=blackColor; 367 | } 368 | } 369 | 370 | static const cv::Vec3b whiteColor(255,255,255); 371 | for(int k=0; pSeg && drawCoarseBorder && k<(int)border.size(); ++k) { 372 | pSeg->at(border[k])=whiteColor; 373 | } 374 | //TODO: refine the plane equation as well after!! 375 | } 376 | 377 | /** 378 | * \brief find out all valid 4-connect neighbours pixels of pixel (i,j) 379 | * 380 | * \param [in] i row index of the center pixel 381 | * \param [in] j column index of the center pixel 382 | * \param [in] H height of the image 383 | * \param [in] W weight of the image 384 | * \param [out] nbs pixel id of all valid neighbours 385 | * \return number of valid neighbours 386 | * 387 | * \details invalid 4-connect neighbours means out of image boundary 388 | */ 389 | static inline int getValid4Neighbor( 390 | const int i, const int j, 391 | const int H, const int W, 392 | int nbs[4]) 393 | { 394 | const int id=i*W+j; 395 | int cnt=0; 396 | if(j>0) nbs[cnt++]=(id-1); //left 397 | if(j0) nbs[cnt++]=(id-W); //up 399 | if(i=0 && pixY>=0 && pixXwidth && pixYheight); 412 | const int Nw = this->width/this->windowWidth; 413 | const int Nh = this->height/this->windowHeight; 414 | const int by = pixY/this->windowHeight; 415 | const int bx = pixX/this->windowWidth; 416 | return (by distMap(this->height*this->width, 427 | std::numeric_limits::max()); 428 | 429 | for(int k=0; k<(int)this->rfQueue.size(); ++k) { 430 | const int sIdx=rfQueue[k].first; 431 | const int seedy=sIdx/this->width; 432 | const int seedx=sIdx-seedy*this->width; 433 | const int plid=rfQueue[k].second; 434 | const PlaneSeg& pl = *extractedPlanes[plid]; 435 | 436 | int nbs[4]={-1}; 437 | const int Nnbs=this->getValid4Neighbor(seedy,seedx,this->height,this->width,nbs); 438 | for(int itr=0; itr(cIdx); 441 | if(trail<=-6) continue; //visited from 4 neighbors already, skip 442 | if(trail>=0 && trail==plid) continue; //if visited by the same plane, skip 443 | const int cy=cIdx/this->width; 444 | const int cx=cIdx-cy*this->width; 445 | const int blkid=this->getBlockIdx(cx,cy); 446 | if(blkid>=0 && this->blkMap[blkid]>=0) continue; //not in "black" block 447 | 448 | double pt[3]={0}; 449 | float cdist=-1; 450 | if(this->points->get(cy,cx,pt[0],pt[1],pt[2]) && 451 | std::pow(cdist=(float)std::abs(pl.signedDist(pt)),2)<9*pl.mse+1e-5) //point-plane distance within 3*std 452 | { 453 | if(trail>=0) { 454 | PlaneSeg& n_pl=*extractedPlanes[trail]; 455 | if(pl.normalSimilarity(n_pl)>=params.T_ang(ParamSet::P_REFINE, pl.center[2])) {//potential for merging 456 | n_pl.connect(extractedPlanes[plid].get()); 457 | } 458 | } 459 | float& old_dist=distMap[cIdx]; 460 | if(cdistrfQueue.push_back(std::pair(cIdx,plid)); 464 | } else if(trail<0) { 465 | trail-=1; 466 | } 467 | } else { 468 | if(trail<0) trail-=1; 469 | } 470 | } 471 | }//for rfQueue 472 | } 473 | 474 | /** 475 | * \brief erode each segment at initial block/window level 476 | * 477 | * \param [in] isValidExtractedPlane coarsely extracted plane i is completely eroded if isValidExtractedPlane(i)==false 478 | * 479 | * \details this function implements line 5~13 of Algorithm 4 in our paper, called by refineDetails; FIXME: after this ds is not updated, i.e. is dirtied 480 | */ 481 | void findBlockMembership(std::vector& isValidExtractedPlane) { 482 | rid2plid.clear(); 483 | for(int plid=0; plid<(int)extractedPlanes.size(); ++plid) { 484 | rid2plid.insert(std::pair(extractedPlanes[plid]->rid,plid)); 485 | } 486 | 487 | const int Nh = this->height/this->windowHeight; 488 | const int Nw = this->width/this->windowWidth; 489 | const int NptsPerBlk = this->windowHeight*this->windowWidth; 490 | 491 | membershipImg.create(height, width, CV_32SC1); 492 | membershipImg.setTo(-1); 493 | this->blkMap.resize(Nh*Nw); 494 | 495 | isValidExtractedPlane.resize(this->extractedPlanes.size(),false); 496 | for(int i=0,blkid=0; iFind(blkid); 499 | const int setSize = ds->getSetSize(setid)*NptsPerBlk; 500 | if(setSize>=minSupport) {//cluster large enough 501 | int nbs[4]={-1}; 502 | const int nNbs=this->getValid4Neighbor(i,j,Nh,Nw,nbs); 503 | bool nbClsAllTheSame=true; 504 | for(int k=0; kerodeType!=ERODE_NONE; ++k) { 505 | if( ds->Find(nbs[k])!=setid && 506 | (this->erodeType==ERODE_ALL_BORDER || 507 | ds->getSetSize(nbs[k])*NptsPerBlk>=minSupport) ) 508 | { 509 | nbClsAllTheSame=false; break; 510 | } 511 | } 512 | const int plid=this->rid2plid[setid]; 513 | if(nbClsAllTheSame) { 514 | this->blkMap[blkid]=plid; 515 | const int by=blkid/Nw; 516 | const int bx=blkid-by*Nw; 517 | membershipImg(cv::Range(by*windowHeight,(by+1)*windowHeight), 518 | cv::Range(bx*windowWidth, (bx+1)*windowWidth)).setTo(plid); 519 | isValidExtractedPlane[plid]=true; 520 | } else {//erode border region 521 | this->blkMap[blkid]=-1; 522 | //this->extractedPlanes[plid]->stats.pop(this->blkStats[blkid]); 523 | } 524 | } else {//too small cluster, i.e. "black" cluster 525 | this->blkMap[blkid]=-1; 526 | }//if setSize>=blkMinSupport 527 | 528 | //save seed points for floodFill 529 | if(this->blkMap[blkid]<0) {//current block is not valid 530 | if(i>0) { 531 | const int u_blkid=blkid-Nw; 532 | if(this->blkMap[u_blkid]>=0) {//up blk is in border 533 | const int u_plid=this->blkMap[u_blkid]; 534 | const int spixidx=(i*this->windowHeight-1)*this->width+j*this->windowWidth; 535 | for(int k=1; kwindowWidth; ++k) { 536 | this->rfQueue.push_back(std::pair(spixidx+k,u_plid)); 537 | } 538 | } 539 | } 540 | if(j>0) { 541 | const int l_blkid=blkid-1; 542 | if(this->blkMap[l_blkid]>=0) {//left blk is in border 543 | const int l_plid=this->blkMap[l_blkid]; 544 | const int spixidx=(i*this->windowHeight)*this->width+j*this->windowWidth-1; 545 | for(int k=0; kwindowHeight-1; ++k) { 546 | this->rfQueue.push_back(std::pair(spixidx+k*this->width,l_plid)); 547 | } 548 | } 549 | } 550 | } else {//current block is still valid 551 | const int plid=this->blkMap[blkid]; 552 | if(i>0) { 553 | const int u_blkid=blkid-Nw; 554 | if(this->blkMap[u_blkid]!=plid) {//up blk is in border 555 | const int spixidx=(i*this->windowHeight)*this->width+j*this->windowWidth; 556 | for(int k=0; kwindowWidth-1; ++k) { 557 | this->rfQueue.push_back(std::pair(spixidx+k,plid)); 558 | } 559 | } 560 | } 561 | if(j>0) { 562 | const int l_blkid=blkid-1; 563 | if(this->blkMap[l_blkid]!=plid) {//left blk is in border 564 | const int spixidx=(i*this->windowHeight)*this->width+j*this->windowWidth; 565 | for(int k=1; kwindowHeight; ++k) { 566 | this->rfQueue.push_back(std::pair(spixidx+k*this->width,plid)); 567 | } 568 | } 569 | } 570 | }//save seed points for floodFill 571 | } 572 | }//for blkik 573 | 574 | ////update plane equation 575 | //for(int i=0; i<(int)this->extractedPlanes.size(); ++i) { 576 | // if(isValidExtractedPlane[i]) { 577 | // if(this->extractedPlanes[i]->stats.N>=this->minSupport) 578 | // this->extractedPlanes[i]->update(); 579 | // } else { 580 | // this->extractedPlanes[i]->nouse=true; 581 | // } 582 | //} 583 | } 584 | 585 | //called by findMembership and/or plotSegmentImage when doRefine==false 586 | void findBlockMembership() { 587 | if(!this->dirtyBlkMbship) return; 588 | this->dirtyBlkMbship=false; 589 | 590 | rid2plid.clear(); 591 | for(int plid=0; plid<(int)extractedPlanes.size(); ++plid) { 592 | rid2plid.insert(std::pair(extractedPlanes[plid]->rid,plid)); 593 | } 594 | 595 | const int Nh = this->height/this->windowHeight; 596 | const int Nw = this->width/this->windowWidth; 597 | const int NptsPerBlk = this->windowHeight*this->windowWidth; 598 | 599 | membershipImg.create(height, width, CV_32SC1); 600 | membershipImg.setTo(-1); 601 | this->blkMap.resize(Nh*Nw); 602 | 603 | for(int i=0,blkid=0; iFind(blkid); 606 | const int setSize = ds->getSetSize(setid)*NptsPerBlk; 607 | if(setSize>=minSupport) {//cluster large enough 608 | const int plid=this->rid2plid[setid]; 609 | this->blkMap[blkid]=plid; 610 | const int by=blkid/Nw; 611 | const int bx=blkid-by*Nw; 612 | membershipImg(cv::Range(by*windowHeight,(by+1)*windowHeight), 613 | cv::Range(bx*windowWidth, (bx+1)*windowWidth)).setTo(plid); 614 | } else {//too small cluster, i.e. "black" cluster 615 | this->blkMap[blkid]=-1; 616 | }//if setSize>=blkMinSupport 617 | } 618 | }//for blkik 619 | } 620 | 621 | //called by run when doRefine==false 622 | void findMembership(std::vector< std::vector >& ret, 623 | const std::vector* pIdxMap) 624 | { 625 | const int Nh = this->height/this->windowHeight; 626 | const int Nw = this->width/this->windowWidth; 627 | this->findBlockMembership(); 628 | const int cnt = (int)extractedPlanes.size(); 629 | ret.resize(cnt); 630 | for(int i=0; iN); 631 | for(int i=0,blkid=0; iblkMap[blkid]; 634 | if(plid<0) continue; 635 | for(int y=i*windowHeight; y<(i+1)*windowHeight; ++y) { 636 | for(int x=j*windowWidth; x<(j+1)*windowWidth; ++x) { 637 | const int pixIdx=x+y*width; 638 | ret[plid].push_back(pIdxMap?pIdxMap->at(pixIdx):pixIdx); 639 | } 640 | } 641 | } 642 | } 643 | } 644 | 645 | //called by run when doRefine==false 646 | void plotSegmentImage(cv::Mat* pSeg, const double supportTh) 647 | { 648 | if(pSeg==0) return; 649 | const int Nh = this->height/this->windowHeight; 650 | const int Nw = this->width/this->windowWidth; 651 | std::vector ret; 652 | int cnt=0; 653 | 654 | std::vector* pBlkid2plid; 655 | if(supportTh==this->minSupport) { 656 | this->findBlockMembership(); 657 | pBlkid2plid=&(this->blkMap); 658 | cnt=(int)this->extractedPlanes.size(); 659 | } else { //mainly for DEBUG_CLUSTER since then supportTh!=minSupport 660 | std::map map; //map setid->cnt 661 | ret.resize(Nh*Nw); 662 | for(int i=0,blkid=0; iFind(blkid); 665 | const int setSize = ds->getSetSize(setid)*windowHeight*windowWidth; 666 | if(setSize>=supportTh) { 667 | std::map::iterator fitr=map.find(setid); 668 | if(fitr==map.end()) {//found a new set id 669 | map.insert(std::pair(setid,cnt)); 670 | ret[blkid]=cnt; 671 | ++cnt; 672 | } else {//found a existing set id 673 | ret[blkid]=fitr->second; 674 | } 675 | } else {//too small cluster, ignore 676 | ret[blkid]=-1; 677 | } 678 | } 679 | } 680 | pBlkid2plid=&ret; 681 | } 682 | std::vector& blkid2plid=*pBlkid2plid; 683 | 684 | if(cnt>colors.size()) { 685 | std::vector tmpColors=pseudocolor(cnt-(int)colors.size()); 686 | colors.insert(colors.end(), tmpColors.begin(), tmpColors.end()); 687 | } 688 | cv::Mat& seg=*pSeg; 689 | for(int i=0,blkid=0; i=0) { 693 | seg(cv::Range(i*windowHeight,(i+1)*windowHeight), 694 | cv::Range(j*windowWidth, (j+1)*windowWidth)).setTo(colors[plid]); 695 | } else { 696 | seg(cv::Range(i*windowHeight,(i+1)*windowHeight), 697 | cv::Range(j*windowWidth, (j+1)*windowWidth)).setTo(cv::Vec3b(0,0,0)); 698 | } 699 | } 700 | } 701 | } 702 | 703 | #ifdef DEBUG_CLUSTER 704 | void floodFillColor(const int seedIdx, cv::Mat& seg, const cv::Vec3b& clr) { 705 | static const int step[8][2]={ 706 | {1,0},{1,1},{0,1},{-1,1}, 707 | {-1,0},{-1,-1},{0,-1},{1,-1} 708 | }; 709 | const int Nh = this->height/this->windowHeight; 710 | const int Nw = this->width/this->windowWidth; 711 | 712 | std::vector visited(Nh*Nw, false); 713 | std::vector idxStack; 714 | idxStack.reserve(Nh*Nw/10); 715 | idxStack.push_back(seedIdx); 716 | visited[seedIdx]=true; 717 | const int sy=seedIdx/Nw; 718 | const int sx=seedIdx-sy*Nw; 719 | seg(cv::Range(sy*windowHeight,(sy+1)*windowHeight), 720 | cv::Range(sx*windowWidth, (sx+1)*windowWidth)).setTo(clr); 721 | 722 | const int clsId=ds->Find(seedIdx); 723 | while(!idxStack.empty()) { 724 | const int sIdx=idxStack.back(); 725 | idxStack.pop_back(); 726 | const int seedy=sIdx/Nw; 727 | const int seedx=sIdx-seedy*Nw; 728 | for(int i=0; i<8; ++i) { 729 | const int cx=seedx+step[i][0]; 730 | const int cy=seedy+step[i][1]; 731 | if(0<=cx && cxFind(cIdx)) {//if same plane, move 736 | idxStack.push_back(cIdx); 737 | seg(cv::Range(cy*windowHeight,(cy+1)*windowHeight), 738 | cv::Range(cx*windowWidth, (cx+1)*windowWidth)).setTo(clr); 739 | } 740 | } 741 | } 742 | }//while 743 | } 744 | #endif 745 | 746 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 747 | cv::Mat dInit; 748 | cv::Mat dSeg; 749 | cv::Mat dGraph; 750 | #endif 751 | 752 | /** 753 | * \brief initialize a graph from pointsIn 754 | * 755 | * \param [in/out] minQ a min MSE queue of PlaneSegs 756 | * 757 | * \details this function implements Algorithm 2 in our paper 758 | */ 759 | void initGraph(PlaneSegMinMSEQueue& minQ) { 760 | const int Nh = this->height/this->windowHeight; 761 | const int Nw = this->width/this->windowWidth; 762 | 763 | //1. init nodes 764 | std::vector G(Nh*Nw, static_cast(0)); 765 | //this->blkStats.resize(Nh*Nw); 766 | 767 | #ifdef DEBUG_INIT 768 | dInit.create(this->height, this->width, CV_8UC3); 769 | dInit.setTo(cv::Vec3b(0,0,0)); 770 | #endif 771 | for(int i=0; ipoints, (i*Nw+j), 775 | i*this->windowHeight, j*this->windowWidth, 776 | this->width, this->height, 777 | this->windowWidth, this->windowHeight, 778 | this->params) ); 779 | if(p->msecenter[2]) 780 | && !p->nouse) 781 | { 782 | G[i*Nw+j]=p.get(); 783 | minQ.push(p); 784 | //this->blkStats[i*Nw+j]=p->stats; 785 | #ifdef DEBUG_INIT 786 | //const uchar cl=uchar(p->mse*255/dynThresh); 787 | //const cv::Vec3b clr(cl,cl,cl); 788 | dInit(cv::Range(i*windowHeight,(i+1)*windowHeight), 789 | cv::Range(j*windowWidth, (j+1)*windowWidth)).setTo(p->getColor(true)); 790 | 791 | const int cx=j*windowWidth+0.5*(windowWidth-1); 792 | const int cy=i*windowHeight+0.5*(windowHeight-1); 793 | static const cv::Scalar blackColor(0,0,0,1); 794 | cv::circle(dInit, cv::Point(cx,cy), 1, blackColor, 2); 795 | #endif 796 | } else { 797 | G[i*Nw+j]=0; 798 | #ifdef DEBUG_INIT 799 | const int cx=j*windowWidth+0.5*(windowWidth-1); 800 | const int cy=i*windowHeight+0.5*(windowHeight-1); 801 | static const cv::Scalar blackColor(0,0,0,1); 802 | static const cv::Vec3b whiteColor(255,255,255); 803 | dInit(cv::Range(i*windowHeight,(i+1)*windowHeight), 804 | cv::Range(j*windowWidth, (j+1)*windowWidth)).setTo(whiteColor); 805 | 806 | switch(p->type) { 807 | case PlaneSeg::TYPE_NORMAL: //draw a big dot 808 | { 809 | static const cv::Scalar yellow(255,0,0,1); 810 | cv::circle(dInit, cv::Point(cx,cy), 3, yellow, 4); 811 | break; 812 | } 813 | case PlaneSeg::TYPE_MISSING_DATA: //draw an 'o' 814 | { 815 | static const cv::Scalar black(0,0,0,1); 816 | cv::circle(dInit, cv::Point(cx,cy), 3, black, 2); 817 | break; 818 | } 819 | case PlaneSeg::TYPE_DEPTH_DISCONTINUE: //draw an 'x' 820 | { 821 | static const cv::Scalar red(255,0,0,1); 822 | static const int len=4; 823 | cv::line(dInit, cv::Point(cx-len, cy-len), cv::Point(cx+len,cy+len), red, 2); 824 | cv::line(dInit, cv::Point(cx+len, cy-len), cv::Point(cx-len,cy+len), red, 2); 825 | break; 826 | } 827 | } 828 | #endif 829 | } 830 | } 831 | } 832 | #ifdef DEBUG_INIT 833 | //cv::applyColorMap(dInit, dInit, cv::COLORMAP_COOL); 834 | #endif 835 | #ifdef DEBUG_CALC 836 | int nEdge=0; 837 | this->numEdges.clear(); 838 | this->numNodes.clear(); 839 | #endif 840 | 841 | //2. init edges 842 | //first pass, connect neighbors from row direction 843 | for(int i=0; icenter[2]); 851 | if((jnormalSimilarity(*G[cidx+1])>=similarityTh) || 852 | (j==Nw-1 && G[cidx]->normalSimilarity(*G[cidx-1])>=similarityTh)) { 853 | G[cidx]->connect(G[cidx-1]); 854 | if(jconnect(G[cidx+1]); 855 | #ifdef DEBUG_INIT 856 | const int cx=j*windowWidth+0.5*(windowWidth-1); 857 | const int cy=i*windowHeight+0.5*(windowHeight-1); 858 | const int rx=(j+1)*windowWidth+0.5*(windowWidth-1); 859 | const int lx=(j-1)*windowWidth+0.5*(windowWidth-1); 860 | static const cv::Scalar blackColor(0,0,0,1); 861 | cv::line(dInit, cv::Point(cx,cy), cv::Point(lx,cy),blackColor); 862 | if(jcenter[2]); 881 | if((inormalSimilarity(*G[cidx+Nw])>=similarityTh) || 882 | (i==Nh-1 && G[cidx]->normalSimilarity(*G[cidx-Nw])>=similarityTh)) { 883 | G[cidx]->connect(G[cidx-Nw]); 884 | if(iconnect(G[cidx+Nw]); 885 | #ifdef DEBUG_INIT 886 | const int cx=j*windowWidth+0.5*(windowWidth-1); 887 | const int cy=i*windowHeight+0.5*(windowHeight-1); 888 | const int uy=(i-1)*windowHeight+0.5*(windowHeight-1); 889 | const int dy=(i+1)*windowHeight+0.5*(windowHeight-1); 890 | static const cv::Scalar blackColor(0,0,0,1); 891 | cv::line(dInit, cv::Point(cx,cy), cv::Point(cx,uy),blackColor); 892 | if(inumNodes.push_back(minQ.size()); 914 | this->numEdges.push_back(nEdge); 915 | this->maxIndvidualNodeDegree=4; 916 | this->mseNodeDegree.clear(); 917 | #endif 918 | } 919 | 920 | /** 921 | * \brief main clustering step 922 | * 923 | * \param [in] minQ a min MSE queue of PlaneSegs 924 | * \param [in] debug whether to collect some statistics when compiled with DEBUG_CALC 925 | * \return number of cluster steps 926 | * 927 | * \details this function implements the Algorithm 3 in our paper 928 | */ 929 | int ahCluster(PlaneSegMinMSEQueue& minQ, bool debug=true) { 930 | #if !defined(DEBUG_INIT) && defined(DEBUG_CLUSTER) 931 | dInit.create(this->height, this->width, CV_8UC3); 932 | #endif 933 | #ifdef DEBUG_CLUSTER 934 | const int Nw = this->width/this->windowWidth; 935 | int dSegCnt=0; 936 | //dSeg.create(this->height, this->width, CV_8UC3); 937 | dInit.copyTo(dSeg); 938 | dGraph.create(this->height, this->width, CV_8UC3); 939 | { 940 | //this->plotSegmentImage(&dSeg, 0); 941 | std::stringstream ss; 942 | ss<nouse) { 954 | assert(p->nbs.size()<=0); 955 | continue; 956 | } 957 | #ifdef DEBUG_CALC 958 | this->maxIndvidualNodeDegree=std::max(this->maxIndvidualNodeDegree,(int)p->nbs.size()); 959 | this->mseNodeDegree.push_back((int)p->nbs.size()); 960 | #endif 961 | #ifdef DEBUG_CLUSTER 962 | int cx, cy; 963 | { 964 | dSeg.copyTo(dGraph); 965 | const int blkid=p->rid; 966 | this->floodFillColor(blkid, dGraph, p->getColor(false)); 967 | const int i=blkid/Nw; 968 | const int j=blkid-i*Nw; 969 | cx=j*windowWidth+0.5*(windowWidth-1); 970 | cy=i*windowHeight+0.5*(windowHeight-1); 971 | static const cv::Scalar blackColor(0,0,255,1); 972 | cv::circle(dGraph, cv::Point(cx,cy),3,blackColor,2); 973 | } 974 | #endif 975 | PlaneSeg::shared_ptr cand_merge; 976 | PlaneSeg::Ptr cand_nb(0); 977 | PlaneSeg::NbSet::iterator itr=p->nbs.begin(); 978 | for(; itr!=p->nbs.end();itr++) {//test merge with all nbs, pick the one with min mse 979 | PlaneSeg::Ptr nb=(*itr); 980 | #ifdef DEBUG_CLUSTER 981 | { 982 | const int n_blkid=nb->rid; 983 | this->floodFillColor(n_blkid, dGraph, nb->getColor(false)); 984 | } 985 | #endif 986 | //TODO: should we use dynamic similarityTh here? 987 | //const double similarityTh=ahc::depthDependNormalDeviationTh(p->center[2],500,4000,M_PI*15/180.0,M_PI/2); 988 | if(p->normalSimilarity(*nb) < params.T_ang(ParamSet::P_MERGING, p->center[2])) continue; 989 | PlaneSeg::shared_ptr merge(new PlaneSeg(*p, *nb)); 990 | if(cand_merge==0 || cand_merge->mse>merge->mse || 991 | (cand_merge->mse==merge->mse && cand_merge->NN)) 992 | { 993 | cand_merge=merge; 994 | cand_nb=nb; 995 | } 996 | }//for nbs 997 | #ifdef DEBUG_CLUSTER 998 | itr=p->nbs.begin(); 999 | for(; debug && itr!=p->nbs.end();itr++) { 1000 | PlaneSeg::Ptr nb=(*itr); 1001 | const int n_blkid=nb->rid; 1002 | const int i=n_blkid/Nw; 1003 | const int j=n_blkid-i*Nw; 1004 | const int mx=j*windowWidth+0.5*(windowWidth-1); 1005 | const int my=i*windowHeight+0.5*(windowHeight-1); 1006 | static const cv::Scalar blackColor(0,0,255,1); 1007 | cv::circle(dGraph, cv::Point(mx,my),3,blackColor,1); 1008 | cv::line(dGraph, cv::Point(cx,cy), cv::Point(mx,my), blackColor,1); 1009 | }//for nbs 1010 | #endif 1011 | //TODO: maybe a better merge condition? such as adaptive threshold on MSE like Falzenszwalb's method 1012 | if(cand_merge!=0 && cand_merge->msecenter[2])) 1014 | {//merge and add back to minQ 1015 | #ifdef DEBUG_CLUSTER 1016 | { 1017 | const int n_blkid=cand_nb->rid; 1018 | const int i=n_blkid/Nw; 1019 | const int j=n_blkid-i*Nw; 1020 | const int mx=j*windowWidth+0.5*(windowWidth-1); 1021 | const int my=i*windowHeight+0.5*(windowHeight-1); 1022 | static const cv::Scalar blackColor(0,0,255,1); 1023 | cv::circle(dGraph, cv::Point(mx,my),2,blackColor,2); 1024 | cv::line(dGraph, cv::Point(cx,cy), cv::Point(mx,my), blackColor,2); 1025 | std::stringstream ss; 1026 | ss<nbs.size()*2; 1033 | const int nEdge_nb=(int)cand_nb->nbs.size()*2; 1034 | #endif 1035 | minQ.push(cand_merge); 1036 | cand_merge->mergeNbsFrom(*p, *cand_nb, *this->ds); 1037 | #ifdef DEBUG_CALC 1038 | if(debug) {//don't do this when merging one last time 1039 | this->numNodes.push_back(this->numNodes.back()-1); 1040 | this->numEdges.push_back(this->numEdges.back() + cand_merge->nbs.size()*2 1041 | - nEdge_p - nEdge_nb + 2); 1042 | } 1043 | #endif 1044 | #ifdef DEBUG_CLUSTER 1045 | { 1046 | floodFillColor(cand_merge->rid, dSeg, cand_merge->getColor(false)); 1047 | std::stringstream ss; 1048 | ss<N>=this->minSupport) { 1056 | this->extractedPlanes.push_back(p); 1057 | #ifdef DEBUG_CLUSTER 1058 | const int blkid=p->rid; 1059 | const int i=blkid/Nw; 1060 | const int j=blkid-i*Nw; 1061 | const int ex=j*windowWidth+0.5*(windowWidth-1); 1062 | const int ey=i*windowHeight+0.5*(windowHeight-1); 1063 | static const cv::Scalar blackColor(0,0,0,1); 1064 | const int len=3; 1065 | { 1066 | cv::line(dGraph, cv::Point(ex-len,ey), cv::Point(ex+len,ey), blackColor, 2); 1067 | cv::line(dGraph, cv::Point(ex,ey-len), cv::Point(ex,ey+len), blackColor, 2); 1068 | std::stringstream ss; 1069 | ss<rid, dSeg, p->getColor(false)); 1075 | cv::line(dSeg, cv::Point(ex-len,ey), cv::Point(ex+len,ey), blackColor, 2); 1076 | cv::line(dSeg, cv::Point(ex,ey-len), cv::Point(ex,ey+len), blackColor, 2); 1077 | std::stringstream ss; 1078 | ss<rid, dGraph, cv::Vec3b(0,0,0)); 1089 | std::stringstream ss; 1090 | ss<rid, dSeg, cv::Vec3b(0,0,0)); 1096 | std::stringstream ss; 1097 | ss<numNodes.push_back(this->numNodes.back()-1); 1107 | this->numEdges.push_back(this->numEdges.back()-(int)p->nbs.size()*2); 1108 | } 1109 | #endif 1110 | p->disconnectAllNbs(); 1111 | } 1112 | ++step; 1113 | }//end while minQ 1114 | while(!minQ.empty()) {//just check if any remaining PlaneSeg if maxstep reached 1115 | const PlaneSeg::shared_ptr p=minQ.top(); 1116 | minQ.pop(); 1117 | if(p->N>=this->minSupport) { 1118 | this->extractedPlanes.push_back(p); 1119 | } 1120 | p->disconnectAllNbs(); 1121 | } 1122 | #ifdef DEBUG_CLUSTER 1123 | { 1124 | std::stringstream ss; 1125 | ss<extractedPlanes.begin(), 1132 | this->extractedPlanes.end(), 1133 | sizecmp); 1134 | return step; 1135 | } 1136 | };//end of PlaneFitter 1137 | }//end of namespace ahc -------------------------------------------------------------------------------- /include/AHCPlaneSeg.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #include //PlaneSeg::NbSet 30 | #include //mseseq 31 | #include //quiet_NaN 32 | 33 | #include "AHCTypes.hpp" //shared_ptr 34 | #include "eig33sym.hpp" //PlaneSeg::Stats::compute 35 | #include "AHCParamSet.hpp" //depthDisContinuous 36 | #include "DisjointSet.hpp" //PlaneSeg::mergeNbsFrom 37 | 38 | namespace ahc { 39 | 40 | //return true if d0 and d1 is discontinuous 41 | inline static bool depthDisContinuous(const double d0, const double d1, const ParamSet& params) 42 | { 43 | return fabs(d0-d1) > params.T_dz(d0); 44 | } 45 | 46 | /** 47 | * \brief PlaneSeg is a struct representing a Plane Segment as a node of a graph 48 | * 49 | * \details It is usually dynamically allocated and garbage collected by boost::shared_ptr 50 | */ 51 | struct PlaneSeg { 52 | typedef PlaneSeg* Ptr; 53 | typedef ahc::shared_ptr shared_ptr; 54 | 55 | /** 56 | * \brief An internal struct holding this PlaneSeg's member points' 1st and 2nd order statistics 57 | * 58 | * \details It is usually dynamically allocated and garbage collected by boost::shared_ptr 59 | */ 60 | struct Stats { 61 | double sx, sy, sz, //sum of x/y/z 62 | sxx, syy, szz, //sum of xx/yy/zz 63 | sxy, syz, sxz; //sum of xy/yz/xz 64 | int N; //#points in this PlaneSeg 65 | 66 | Stats() : sx(0), sy(0), sz(0), 67 | sxx(0), syy(0), szz(0), 68 | sxy(0), syz(0), sxz(0), N(0) {} 69 | 70 | //merge from two other Stats 71 | Stats(const Stats& a, const Stats& b) : 72 | sx(a.sx+b.sx), sy(a.sy+b.sy), sz(a.sz+b.sz), 73 | sxx(a.sxx+b.sxx), syy(a.syy+b.syy), szz(a.szz+b.szz), 74 | sxy(a.sxy+b.sxy), syz(a.syz+b.syz), sxz(a.sxz+b.sxz), N(a.N+b.N) {} 75 | 76 | inline void clear() { 77 | sx=sy=sz=sxx=syy=szz=sxy=syz=sxz=0; 78 | N=0; 79 | } 80 | 81 | //push a new point (x,y,z) into this Stats 82 | inline void push(const double x, const double y, const double z) { 83 | sx+=x; sy+=y; sz+=z; 84 | sxx+=x*x; syy+=y*y; szz+=z*z; 85 | sxy+=x*y; syz+=y*z; sxz+=x*z; 86 | ++N; 87 | } 88 | 89 | //push a new Stats into this Stats 90 | inline void push(const Stats& other) { 91 | sx+=other.sx; sy+=other.sy; sz+=other.sz; 92 | sxx+=other.sxx; syy+=other.syy; szz+=other.szz; 93 | sxy+=other.sxy; syz+=other.syz; sxz+=other.sxz; 94 | N+=other.N; 95 | } 96 | 97 | //caller is responsible to ensure (x,y,z) was collected in this stats 98 | inline void pop(const double x, const double y, const double z) { 99 | sx-=x; sy-=y; sz-=z; 100 | sxx-=x*x; syy-=y*y; szz-=z*z; 101 | sxy-=x*y; syz-=y*z; sxz-=x*z; 102 | --N; 103 | 104 | assert(N>=0); 105 | } 106 | 107 | //caller is responsible to ensure {other} were collected in this stats 108 | inline void pop(const Stats& other) { 109 | sx-=other.sx; sy-=other.sy; sz-=other.sz; 110 | sxx-=other.sxx; syy-=other.syy; szz-=other.szz; 111 | sxy-=other.sxy; syz-=other.syz; sxz-=other.sxz; 112 | N-=other.N; 113 | 114 | assert(N>=0); 115 | } 116 | 117 | /** 118 | * \brief PCA-based plane fitting 119 | * 120 | * \param [out] center center of mass of the PlaneSeg 121 | * \param [out] normal unit normal vector of the PlaneSeg (ensure normal.z>=0) 122 | * \param [out] mse mean-square-error of the plane fitting 123 | * \param [out] curvature defined as in pcl 124 | */ 125 | inline void compute(double center[3], double normal[3], 126 | double& mse, double& curvature) const 127 | { 128 | assert(N>=4); 129 | 130 | const double sc=((double)1.0)/this->N;//this->ids.size(); 131 | //calc plane equation: center, normal and mse 132 | center[0]=sx*sc; 133 | center[1]=sy*sc; 134 | center[2]=sz*sc; 135 | double K[3][3] = { 136 | {sxx-sx*sx*sc,sxy-sx*sy*sc,sxz-sx*sz*sc}, 137 | { 0,syy-sy*sy*sc,syz-sy*sz*sc}, 138 | { 0, 0,szz-sz*sz*sc} 139 | }; 140 | K[1][0]=K[0][1]; K[2][0]=K[0][2]; K[2][1]=K[1][2]; 141 | double sv[3]={0,0,0}; 142 | double V[3][3]={0}; 143 | LA::eig33sym(K, sv, V); //!!! first eval is the least one 144 | //LA.svd33(K, sv, V); 145 | if(V[0][0]*center[0]+V[1][0]*center[1]+V[2][0]*center[2]<=0) {//enforce dot(normal,center)<00 so normal always points towards camera 146 | normal[0]=V[0][0]; 147 | normal[1]=V[1][0]; 148 | normal[2]=V[2][0]; 149 | } else { 150 | normal[0]=-V[0][0]; 151 | normal[1]=-V[1][0]; 152 | normal[2]=-V[2][0]; 153 | } 154 | mse = sv[0]*sc; 155 | curvature=sv[0]/(sv[0]+sv[1]+sv[2]); 156 | } 157 | } stats; //member points' 1st & 2nd order statistics 158 | 159 | int rid; //root block id 160 | double mse; //mean square error 161 | double center[3]; //q: plane center (center of mass) 162 | double normal[3]; //n: plane equation n'p=q 163 | int N; //#member points, same as stats.N 164 | double curvature; 165 | bool nouse; //this PlaneSeg will be marked as nouse after merged with others to produce a new PlaneSeg node in the graph 166 | 167 | #ifdef DEBUG_INIT 168 | enum Type { 169 | TYPE_NORMAL=0, //default value 170 | TYPE_MISSING_DATA=1, 171 | TYPE_DEPTH_DISCONTINUE=2 172 | } type; 173 | #endif 174 | 175 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 176 | cv::Vec3b clr; 177 | cv::Vec3b normalClr; 178 | cv::Vec3b& getColor(bool useNormal=true) { 179 | if(useNormal) return normalClr; 180 | return clr; 181 | } 182 | #endif 183 | 184 | #ifdef DEBUG_CALC 185 | std::vector mseseq; 186 | #endif 187 | 188 | typedef std::set NbSet; //no ownership of its content 189 | NbSet nbs; //neighbors, i.e. adjacency list for a graph structure 190 | 191 | inline void update() { 192 | this->stats.compute(this->center, this->normal, this->mse, this->curvature); 193 | } 194 | 195 | PlaneSeg(const int init_block_id, const double mse, const double center[3], const double normal[3], const double curvature, const Stats& stats) 196 | { 197 | this->stats = stats; 198 | this->rid = init_block_id; 199 | this->mse = mse; 200 | std::copy(center, center + 3, this->center); 201 | std::copy(normal, normal + 3, this->normal); 202 | this->N = stats.N; 203 | this->curvature = curvature; 204 | if (this->N >= 4) { 205 | nouse = false; 206 | } else { 207 | nouse = true; 208 | } 209 | } 210 | 211 | /** 212 | * \brief construct a PlaneSeg during graph initialization 213 | * 214 | * \param [in] points organized point cloud adapter, see NullImage3D 215 | * \param [in] root_block_id initial window/block's id 216 | * \param [in] seed_row row index of the upper left pixel of the initial window/block 217 | * \param [in] seed_col row index of the upper left pixel of the initial window/block 218 | * \param [in] imgWidth width of the organized point cloud 219 | * \param [in] imgHeight height of the organized point cloud 220 | * \param [in] winWidth width of the initial window/block 221 | * \param [in] winHeight height of the initial window/block 222 | * \param [in] depthChangeFactor parameter to determine depth discontinuity 223 | * 224 | * \details if exist depth discontinuity in this initial PlaneSeg, nouse will be set true and N 0. 225 | */ 226 | template 227 | PlaneSeg(const Image3D& points, const int root_block_id, 228 | const int seed_row, const int seed_col, 229 | const int imgWidth, const int imgHeight, 230 | const int winWidth, const int winHeight, 231 | const ParamSet& params) 232 | { 233 | //assert(0<=seed_row && seed_row0 && winH>0); 234 | this->rid = root_block_id; 235 | 236 | bool windowValid=true; 237 | int nanCnt=0, nanCntTh=winHeight*winWidth/2; 238 | //calc stats 239 | for(int i=seed_row, icnt=0; icnttype=TYPE_MISSING_DATA; 249 | #endif 250 | windowValid=false; break; 251 | } 252 | double xn=0,yn=0,zn=10000; 253 | if(j+1type=TYPE_DEPTH_DISCONTINUE; 257 | #endif 258 | windowValid=false; break; 259 | } 260 | if(i+1type=TYPE_DEPTH_DISCONTINUE; 264 | #endif 265 | windowValid=false; break; 266 | } 267 | this->stats.push(x,y,z); 268 | } 269 | if(!windowValid) break; 270 | } 271 | if(windowValid) {//if nan or depth-discontinuity shows, this obj will be rejected 272 | this->nouse=false; 273 | this->N=this->stats.N; 274 | #ifdef DEBUG_INIT 275 | this->type=TYPE_NORMAL; 276 | #endif 277 | } else { 278 | this->N=0; 279 | this->stats.clear(); 280 | this->nouse=true; 281 | } 282 | 283 | if(this->N<4) { 284 | this->mse=this->curvature=std::numeric_limits::quiet_NaN(); 285 | } else { 286 | this->stats.compute(this->center, this->normal, this->mse, this->curvature); 287 | #ifdef DEBUG_CALC 288 | this->mseseq.push_back(cv::Vec2d(this->N,this->mse)); 289 | #endif 290 | //nbs information to be maintained outside the class 291 | //typically when initializing the graph structure 292 | } 293 | #if defined(DEBUG_INIT) || defined(DEBUG_CLUSTER) 294 | const uchar clx=uchar((this->normal[0]+1.0)*0.5*255.0); 295 | const uchar cly=uchar((this->normal[1]+1.0)*0.5*255.0); 296 | const uchar clz=uchar((this->normal[2]+1.0)*0.5*255.0); 297 | this->normalClr=cv::Vec3b(clx,cly,clz); 298 | this->clr=cv::Vec3b(rand()%255,rand()%255,rand()%255); 299 | #endif 300 | //std::cout<curvature<type=TYPE_NORMAL; 313 | #endif 314 | this->nouse=false; 315 | this->rid = pa.N>=pb.N ? pa.rid : pb.rid; 316 | this->N=this->stats.N; 317 | 318 | //ds.union(pa.rid, pb.rid) will be called later 319 | //in mergeNbsFrom(pa,pb) function, since 320 | //this object might not be accepted into the graph structure 321 | 322 | this->stats.compute(this->center, this->normal, this->mse, this->curvature); 323 | 324 | #if defined(DEBUG_CLUSTER) 325 | const uchar clx=uchar((this->normal[0]+1.0)*0.5*255.0); 326 | const uchar cly=uchar((this->normal[1]+1.0)*0.5*255.0); 327 | const uchar clz=uchar((this->normal[2]+1.0)*0.5*255.0); 328 | this->normalClr=cv::Vec3b(clx,cly,clz); 329 | this->clr=cv::Vec3b(rand()%255,rand()%255,rand()%255); 330 | #endif 331 | //nbs information to be maintained later if this node is accepted 332 | } 333 | 334 | /** 335 | * \brief similarity of two plane normals 336 | * 337 | * \param [in] p another PlaneSeg 338 | * \return abs(dot(this->normal, p->normal)) 339 | * 340 | * \details 1 means identical, 0 means perpendicular 341 | */ 342 | inline double normalSimilarity(const PlaneSeg& p) const { 343 | return std::abs(normal[0]*p.normal[0]+ 344 | normal[1]*p.normal[1]+ 345 | normal[2]*p.normal[2]); 346 | } 347 | 348 | /** 349 | * \brief signed distance between this plane and the point pt[3] 350 | */ 351 | inline double signedDist(const double pt[3]) const { 352 | return normal[0]*(pt[0]-center[0])+ 353 | normal[1]*(pt[1]-center[1])+ 354 | normal[2]*(pt[2]-center[2]); 355 | } 356 | 357 | /** 358 | * \brief connect this PlaneSeg to another PlaneSeg p in the graph 359 | * 360 | * \param [in] p the other PlaneSeg 361 | */ 362 | inline void connect(PlaneSeg::Ptr p) { 363 | if(p) { 364 | this->nbs.insert(p); 365 | p->nbs.insert(this); 366 | } 367 | } 368 | 369 | /** 370 | * \brief disconnect this PlaneSeg with all its neighbors 371 | * 372 | * \details after this call, this->nbs.nbs should not contain this, and this->nbs should be empty i.e. after this call this PlaneSeg node should be isolated in the graph 373 | */ 374 | inline void disconnectAllNbs() { 375 | NbSet::iterator itr = this->nbs.begin(); 376 | for(; itr!=this->nbs.end(); ++itr) { 377 | PlaneSeg::Ptr nb = (*itr); 378 | if(!nb->nbs.erase(this)) { 379 | std::cout<<"[PlaneSeg warn] this->nbs.nbs" 380 | " should have contained this!"<nbs.clear(); 384 | } 385 | 386 | /** 387 | * \brief finish merging PlaneSeg pa and pb to this 388 | * 389 | * \param [in] pa a parent PlaneSeg of this 390 | * \param [in] pb another parent PlaneSeg of this 391 | * \param [in] ds the disjoint set of initial window/block membership to be updated 392 | * 393 | * \details Only call this if this obj is accepted to be added to the graph of PlaneSeg pa and pb should not exist after this function is called, i.e. after this call this PlaneSeg node will be representing a merged node of pa and pb, and pa/pb will be isolated (and thus Garbage Collected) in the graph 394 | */ 395 | inline void mergeNbsFrom(PlaneSeg& pa, PlaneSeg& pb, DisjointSet& ds) { 396 | //now we are sure that merging pa and pb is accepted 397 | this->rid = ds.Union(pa.rid, pb.rid); 398 | 399 | //the new neighbors should be pa.nbs+pb.nbs-pa-pb 400 | this->nbs.insert(pa.nbs.begin(), pa.nbs.end()); 401 | this->nbs.insert(pb.nbs.begin(), pb.nbs.end()); 402 | this->nbs.erase(&pa); 403 | this->nbs.erase(&pb); 404 | 405 | //pa and pb should be GC later after the following two steps 406 | pa.disconnectAllNbs(); 407 | pb.disconnectAllNbs(); 408 | 409 | //complete the neighborhood from the other side 410 | NbSet::iterator itr = this->nbs.begin(); 411 | for(; itr!=this->nbs.end(); ++itr) { 412 | PlaneSeg::Ptr nb = (*itr); 413 | nb->nbs.insert(this); 414 | } 415 | 416 | pa.nouse=pb.nouse=true; 417 | #ifdef DEBUG_CALC 418 | if(pa.N>=pb.N) { 419 | this->mseseq.swap(pa.mseseq); 420 | } else { 421 | this->mseseq.swap(pb.mseseq); 422 | } 423 | this->mseseq.push_back(cv::Vec2d(this->N,this->mse)); 424 | #endif 425 | } 426 | };//PlaneSeg 427 | 428 | }//ahc -------------------------------------------------------------------------------- /include/AHCTypes.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #ifndef USE_BOOST_SHARED_PTR 30 | #include 31 | #else 32 | #include 33 | #endif 34 | 35 | namespace ahc { 36 | #ifndef USE_BOOST_SHARED_PTR 37 | using std::shared_ptr; 38 | #else 39 | using boost::shared_ptr; 40 | #endif 41 | } -------------------------------------------------------------------------------- /include/AHCUtils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include "opencv2/opencv.hpp" 31 | 32 | namespace ahc { 33 | namespace utils { 34 | 35 | /** 36 | * \brief Generate pseudo-colors 37 | * 38 | * \param [in] ncolors number of colors to be generated (will be reset to 10 if ncolors<=0) 39 | * \return a vector of cv::Vec3b 40 | */ 41 | inline std::vector pseudocolor(int ncolors) { 42 | srand((unsigned int)time(0)); 43 | if(ncolors<=0) ncolors=10; 44 | std::vector ret(ncolors); 45 | for(int i=0; iscale = scale; 69 | startTick=0; 70 | } 71 | 72 | /** 73 | start record time, similar to matlab function "tic"; 74 | 75 | @return the start tick 76 | */ 77 | inline double tic() { 78 | return startTick = (double)cv::getTickCount(); 79 | } 80 | 81 | /** 82 | return duration from last tic, in (second * scale), similar to matlab function "toc" 83 | 84 | @return duration from last tic, in (second * scale) 85 | */ 86 | inline double toc() { 87 | return ((double)cv::getTickCount()-startTick)/cv::getTickFrequency() * scale; 88 | } 89 | inline double toc(std::string tag) { 90 | double time=toc(); 91 | std::cout< 30 | 31 | class DisjointSet 32 | { 33 | private: 34 | std::vector parent_; 35 | std::vector size_; 36 | 37 | public: 38 | DisjointSet(const int n) 39 | { 40 | parent_.reserve(n); 41 | size_.reserve(n); 42 | 43 | for (int i=0; i 39 | #include 40 | #endif 41 | 42 | #include 43 | #include 44 | 45 | namespace LA { 46 | //s[0]<=s[1]<=s[2], V[:][i] correspond to s[i] 47 | inline static bool eig33sym(double K[3][3], double s[3], double V[3][3]) 48 | { 49 | #ifdef USE_DSYEVH3 50 | double tmpV[3][3]; 51 | if(dsyevh3(K, tmpV, s)!=0) return false; 52 | 53 | int order[]={0,1,2}; 54 | for(int i=0; i<3; ++i) { 55 | for(int j=i+1; j<3; ++j) { 56 | if(s[i]>s[j]) { 57 | double tmp=s[i]; 58 | s[i]=s[j]; 59 | s[j]=tmp; 60 | int tmpor=order[i]; 61 | order[i]=order[j]; 62 | order[j]=tmpor; 63 | } 64 | } 65 | } 66 | V[0][0]=tmpV[0][order[0]]; V[0][1]=tmpV[0][order[1]]; V[0][2]=tmpV[0][order[2]]; 67 | V[1][0]=tmpV[1][order[0]]; V[1][1]=tmpV[1][order[1]]; V[1][2]=tmpV[1][order[2]]; 68 | V[2][0]=tmpV[2][order[0]]; V[2][1]=tmpV[2][order[1]]; V[2][2]=tmpV[2][order[2]]; 69 | #else 70 | //below we did not specify row major since it does not matter, K==K' 71 | Eigen::SelfAdjointEigenSolver es( 72 | Eigen::Map(K[0], 3, 3) ); 73 | Eigen::Map(s,3,1)=es.eigenvalues(); 74 | //below we need to specify row major since V!=V' 75 | Eigen::Map>(V[0],3,3)=es.eigenvectors(); 76 | #endif 77 | return true; 78 | } 79 | }//end of namespace LA -------------------------------------------------------------------------------- /matlab/Kinect.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | classdef Kinect < handle 28 | % a matlab wrapper for Kinect access 29 | % h=Kinect; 30 | % h=Kinect(recordedONIFileName); 31 | % depends on Kinect_Matlab toolbox at: 32 | % http://www.mathworks.com/matlabcentral/fileexchange/30242-kinect-matlab 33 | 34 | properties(Access=private) 35 | hKinect; 36 | hCapture; 37 | end 38 | 39 | methods 40 | function h=Kinect(varargin) 41 | fdir=selfInstall; 42 | SAMPLE_XML_PATH=fullfile(fdir,'Config/SamplesConfig.xml'); 43 | 44 | if nargin==0 45 | h.hKinect=mxNiCreateContext(SAMPLE_XML_PATH); 46 | else 47 | fprintf('[Kinect] open from file: %s\n',varargin{1}); 48 | h.hKinect=mxNiCreateContext(SAMPLE_XML_PATH,varargin{1}); 49 | end 50 | 51 | h.hCapture=[]; 52 | end 53 | 54 | function delete(h) 55 | %dtor, close context handle 56 | assert(isa(h,'Kinect')); 57 | h.recordStop(); 58 | mxNiDeleteContext(h.hKinect); 59 | end 60 | 61 | function [xyz,rgb,depth]=grab(h) 62 | %grab a new kinect frame 63 | %xyz: point clouds with unit of mm 64 | %rgb: color image 65 | %depth: depth image 66 | assert(isa(h,'Kinect')); 67 | mxNiUpdateContext(h.hKinect); 68 | 69 | if nargout>0 70 | xyz=mxNiDepthRealWorld(h.hKinect); 71 | z=xyz(:,:,3); 72 | z(z==0)=nan; 73 | z(z>5000)=nan; 74 | xyz(:,:,3)=z; 75 | end 76 | 77 | if nargout>1 78 | rgb=mxNiPhoto(h.hKinect); 79 | rgb=permute(rgb,[3 2 1]); 80 | end 81 | 82 | if nargout>2 83 | depth=mxNiDepth(h.hKinect); 84 | depth=permute(depth,[2 1]); 85 | end 86 | end 87 | 88 | function recordStart(h, fileName) 89 | %start recording the Kinect frames as a file 90 | assert(isa(h,'Kinect')); 91 | if ~isempty(h.hCapture) 92 | disp('[Kinect.recordStart warn] forgot to call recordStop to close previous record?'); 93 | h.recordStop(); 94 | end 95 | 96 | h.hCapture = mxNiStartCapture(h.hKinect, fileName); 97 | end 98 | 99 | function recordStop(h) 100 | %stop recording if exist 101 | assert(isa(h,'Kinect')); 102 | if ~isempty(h.hCapture) 103 | mxNiStopCapture(h.hCapture); 104 | end 105 | end 106 | end 107 | 108 | methods(Static) 109 | function [xyz,rgb,depth]=live(viewRGB, viewXYZ, viewDepth) 110 | % start a live view of Kinect by calling 111 | % Kinect.live(viewRGB, viewXYZ, viewDepth) 112 | 113 | if ~viewRGB && ~viewXYZ && ~viewDepth 114 | return; 115 | end 116 | 117 | hd = Kinect; 118 | [xyz,rgb,depth]=hd.grab; 119 | 120 | hfRGB=[]; 121 | hfXYZ=[]; 122 | hfD=[]; 123 | if viewRGB 124 | figure; 125 | hfRGB=imshow(rgb); 126 | end 127 | if viewXYZ 128 | figure; 129 | [x,y,z]=splitXYZ(xyz); 130 | hfXYZ=plot3(x,y,z,'.'); 131 | axis equal; 132 | grid on; 133 | end 134 | if viewDepth 135 | figure; 136 | hfD=imshow(depth); colormap('jet'); 137 | end 138 | 139 | function ret=rgbNotDone 140 | ret = viewRGB && ~isempty(hfRGB) && ishghandle(hfRGB); 141 | end 142 | function ret=xyzNotDone 143 | ret = viewXYZ && ~isempty(hfXYZ) && ishghandle(hfXYZ); 144 | end 145 | function ret=depthNotDone 146 | ret = viewDepth && ~isempty(hfD) && ishghandle(hfD); 147 | end 148 | function ret=notDone 149 | ret= rgbNotDone... 150 | || xyzNotDone... 151 | || depthNotDone; 152 | end 153 | 154 | while notDone 155 | [xyz,rgb,depth]=hd.grab; 156 | if rgbNotDone 157 | set(hfRGB,'CDATA',rgb); 158 | end 159 | if xyzNotDone 160 | [x,y,z]=splitXYZ(xyz); 161 | set(hfXYZ,'XDATA',x); 162 | set(hfXYZ,'YDATA',y); 163 | set(hfXYZ,'ZDATA',z); 164 | end 165 | if depthNotDone 166 | set(hfD,'CDATA',depth); 167 | end 168 | drawnow; 169 | end 170 | end 171 | end 172 | end 173 | 174 | function ret=getFileFolder 175 | fname = mfilename('fullpath'); 176 | ret = fileparts(fname); 177 | end 178 | 179 | function fdir=selfInstall 180 | fdir=getFileFolder; 181 | mexDir = fullfile(fdir,'Mex'); 182 | addpath(fdir); 183 | addpath(mexDir); 184 | end 185 | 186 | function [x,y,z]=splitXYZ(xyz) 187 | x=xyz(:,:,1); 188 | y=xyz(:,:,2); 189 | z=xyz(:,:,3); 190 | x=x(:); 191 | y=y(:); 192 | z=z(:); 193 | end -------------------------------------------------------------------------------- /matlab/createSegImg.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function seg=createSegImg(mbs,w,h) 28 | % given mbs output from fitAHCPlane, create a RGB segmentation image of size wxh 29 | seg=ones(h,w); 30 | for i=1:length(mbs) 31 | mb=mbs{i}; 32 | seg(mb)=i+1; 33 | end 34 | seg=uint8(255*ind2rgb(seg,myPseudoColor(length(mbs)+1))); 35 | end -------------------------------------------------------------------------------- /matlab/fitAHCPlane.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function mbs=fitAHCPlane(xyz) 28 | % fast plane extraction using AHC 29 | % xyz: organized point cloud, as a 3-channel image, xyz(i,j,1) is the x coordinate of the (i,j) point, xyz(i,j,2) is the y coordinate, xyz(i,j,3) is the z coordinate 30 | % mbs: N cells, each cell mbs{i} is a Kix1 vector storing pixel indices of the points belonging to the i-th extracted plane 31 | tic; 32 | mbs=mxFitAHCPlane(xyz); 33 | toc; 34 | end -------------------------------------------------------------------------------- /matlab/fitPlane.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function [center,majorAxis,majorAxisVar]=fitPlane(Vxyz) 28 | % fit plane from Vxyz using PCA 29 | % Vxyz<3xn>: points expressed in world coordinate system 30 | % center<3x1>: center of mass 31 | % majorAxis<3x3>: from pca usually, majorAxis(:,1) is the x-direction in 32 | % world coordinate system, majorAxis(:,2) is y-direction, majorAxis(:,3) is 33 | % z-direction 34 | 35 | center=mean(Vxyz,2); 36 | Vnew = bsxfun(@minus, Vxyz, center); 37 | K=Vnew*Vnew'; 38 | [zyxVar,zyxDir]=mxEig33Sym(K); 39 | majorAxis=zyxDir(:,[3,2,1]); 40 | viewDir = center/norm(center); %assume viewpoint is at (0,0,0)' 41 | if dot(viewDir,majorAxis(:,3))>0 42 | majorAxis(:,3)=-majorAxis(:,3); %ensure <0 43 | end 44 | if det(majorAxis)<0 %ensure rotation matrix 45 | majorAxis(:,2)=-majorAxis(:,2); 46 | end 47 | if nargout>2 48 | majorAxisVar=zyxVar([3,2,1])/size(Vxyz,2); 49 | end 50 | end -------------------------------------------------------------------------------- /matlab/frame.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ai4ce/peac/cd078773aa2ab3ba5d6315345004e382f3bfdb7e/matlab/frame.mat -------------------------------------------------------------------------------- /matlab/getDefaultAHCFitterparams.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function params=getDefaultAHCFitterparams 28 | params.depthSigma=1.6e-6; 29 | params.stdTol_merge=8; 30 | params.stdTol_init=5; 31 | params.similarityTh_merge=cosd(60); 32 | params.similarityTh_refine=cosd(30); 33 | params.angle_far=deg2rad(90); 34 | params.angle_near=deg2rad(15); 35 | params.z_near=500; 36 | params.z_far=4000; 37 | params.depthAlpha=0.04; 38 | params.depthChangeTol=0.02; 39 | 40 | params.windowWidth=10; 41 | params.windowHeight=10; 42 | params.maxStep=100000; 43 | params.minSupport=800; 44 | params.doRefine=1; 45 | params.erodeType=2; 46 | params.initType=0; 47 | end -------------------------------------------------------------------------------- /matlab/kinect_ahc.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function [xyz,rgb,depth,mbs]=kinect_ahc(varargin) 28 | % call kinect_ahc('rgb'); to see our fitAHCPlane run in real-time 29 | % depends on Kinect.m 30 | 31 | viewRGB=false; 32 | viewXYZ=false; 33 | viewDepth=false; 34 | for i=1:nargin 35 | if strcmpi(varargin{i},'rgb') 36 | viewRGB=true; 37 | elseif strcmpi(varargin{i},'xyz') 38 | viewXYZ=true; 39 | elseif strcmpi(varargin{i},'depth') 40 | viewDepth=true; 41 | end 42 | end 43 | if ~viewRGB && ~viewXYZ && ~viewDepth 44 | xyz=[]; rgb=[]; depth=[]; mbs=[]; 45 | return; 46 | end 47 | 48 | hd = Kinect; 49 | [xyz,rgb,depth]=hd.grab; 50 | 51 | ahcParams = getDefaultAHCFitterparams; 52 | ahcParams.windowWidth=8; 53 | ahcParams.windowHeight=8; 54 | ahcParams.mergeMSETolerance=10; 55 | % ahcParams.initMSETolerance=5; 56 | % ahcParams.depthSigmaFactor=9e-7; 57 | setAHCPlaneFitterParams(ahcParams); 58 | [h,w]=size(depth); 59 | 60 | hfRGB=[]; 61 | hfXYZ=[]; 62 | hfD=[]; 63 | if viewRGB 64 | figure; 65 | hfRGB=imshow(rgb); 66 | end 67 | if viewXYZ 68 | figure; 69 | [x,y,z]=splitXYZ(xyz); 70 | hfXYZ=plot3(x,y,z,'.'); 71 | axis equal; 72 | grid on; 73 | end 74 | if viewDepth 75 | figure; 76 | hfD=imshow(depth); colormap('jet'); 77 | end 78 | 79 | function ret=rgbNotDone 80 | ret = viewRGB && ~isempty(hfRGB) && ishghandle(hfRGB); 81 | end 82 | function ret=xyzNotDone 83 | ret = viewXYZ && ~isempty(hfXYZ) && ishghandle(hfXYZ); 84 | end 85 | function ret=depthNotDone 86 | ret = viewDepth && ~isempty(hfD) && ishghandle(hfD); 87 | end 88 | function ret=notDone 89 | ret= rgbNotDone... 90 | || xyzNotDone... 91 | || depthNotDone; 92 | end 93 | 94 | while notDone 95 | [xyz,rgb,depth]=hd.grab; 96 | mbs=fitAHCPlane(xyz); 97 | seg=createSegImg(mbs,w,h); 98 | rgb=(rgb+seg)/2; 99 | if rgbNotDone 100 | set(hfRGB,'CDATA',rgb); 101 | end 102 | if xyzNotDone 103 | [x,y,z]=splitXYZ(xyz); 104 | set(hfXYZ,'XDATA',x); 105 | set(hfXYZ,'YDATA',y); 106 | set(hfXYZ,'ZDATA',z); 107 | end 108 | if depthNotDone 109 | set(hfD,'CDATA',depth); 110 | end 111 | drawnow; 112 | end 113 | end -------------------------------------------------------------------------------- /matlab/kinect_record.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function kinect_record(fname) 28 | if nargin<1 29 | fname=uiputfile; 30 | end 31 | 32 | msgbox('Click image to start record and click it again to stop and quit!'); 33 | 34 | hd = Kinect; 35 | [~,rgb,depth]=hd.grab; 36 | 37 | startRecord=false; 38 | function myfunc(~,~) 39 | if startRecord %if started 40 | fprintf('Record End, saved as %s\n',fname); 41 | hd.recordStop(); 42 | close(hfRGB); 43 | else %not started yet 44 | startRecord=true; %let's start 45 | fprintf('Record Start...\n'); 46 | hd.recordStart(fname); 47 | end 48 | end 49 | 50 | hfRGB=figure; 51 | rgb=uint8((double(rgb)+double(mono2rgb(depth)))/2); 52 | hRGB=imshow(rgb); 53 | set(hRGB,'ButtonDownFcn',@myfunc); 54 | 55 | function ret=rgbNotDone 56 | ret = ~isempty(hRGB) && ishghandle(hRGB); 57 | end 58 | function ret=notDone 59 | ret= rgbNotDone; 60 | end 61 | 62 | while notDone 63 | [~,rgb,depth]=hd.grab; 64 | rgb=uint8((double(rgb)+double(mono2rgb(depth)))/2); 65 | if rgbNotDone 66 | set(hRGB,'CDATA',rgb); 67 | end 68 | drawnow; 69 | end 70 | end -------------------------------------------------------------------------------- /matlab/mex/makefile.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function makefile(OpenCV_Lib, OpenCV_Include, Eigen_Include) 28 | % compile the mex functions 29 | % OpenCV_Lib: directory to opencv lib files 30 | % OpenCV_Include: directory to opencv include files 31 | % Eigen_Include: directory to boost include files 32 | 33 | AHC_Include=fullfile(fileparts(mfilename('fullpath')),'..','..','include'); 34 | 35 | if nargin<1 36 | %note: my matlab is 64bit, so must link to 64bit libraries 37 | OpenCV_Lib='D:\lib\opencv_249\build\x64\vc10\lib'; 38 | OpenCV_Include='D:\lib\opencv_249\build\include\'; 39 | Eigen_Include='D:\lib\eigen\'; 40 | end 41 | 42 | if ~exist(OpenCV_Lib,'dir') 43 | OpenCV_Lib = uigetdir([],'Please select a valid OpenCV_Lib dir'); 44 | end 45 | if ~exist(OpenCV_Include,'dir') 46 | OpenCV_Include = uigetdir([],'Please select a valid OpenCV_Include dir'); 47 | end 48 | if ~exist(Eigen_Include,'dir') 49 | Eigen_Include = uigetdir([],'Please select a valid Eigen_Include dir'); 50 | end 51 | 52 | fprintf('OpenCV_Lib=%s\n',OpenCV_Lib); 53 | fprintf('OpenCV_Include=%s\n',OpenCV_Include); 54 | fprintf('Eigen_Include=%s\n',Eigen_Include); 55 | 56 | mex('-v', '-O', ['-L' OpenCV_Lib],... 57 | '-lopencv_core249',... 58 | ['-I' AHC_Include],... 59 | ['-I' OpenCV_Include],... 60 | ['-I' Eigen_Include],... 61 | 'mxFitAHCPlane.cpp'); 62 | 63 | mex('-v', '-O',... 64 | ['-I' AHC_Include],... 65 | ['-I' Eigen_Include],... 66 | 'mxEig33Sym.cpp'); 67 | end -------------------------------------------------------------------------------- /matlab/mex/mxEig33Sym.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #include "mex.h" 28 | #include "matrix.h" 29 | 30 | #include "stdlib.h" 31 | 32 | #include "eig33sym.hpp" 33 | 34 | void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { 35 | if (nlhs==0) return; 36 | if (nlhs>2) { 37 | mexErrMsgIdAndTxt("MATLAB:mxEig33Sym:argout", 38 | "Please call [s,V]=mxEig33Sym(K) with at most two output arguments!"); 39 | } 40 | if (nrhs!=1) { 41 | mexErrMsgIdAndTxt("MATLAB:mxEig33Sym:argin", 42 | "Please call mxEig33Sym(K) with one and only one input argument!"); 43 | } 44 | if (!mxIsDouble(prhs[0])) { 45 | mexErrMsgIdAndTxt("MATLAB:mxEig33Sym:argin", 46 | "Please call mxEig33Sym(K) with double-typed K!"); 47 | } 48 | if (mxGetM(prhs[0])!=3 || mxGetN(prhs[0])!=3) { 49 | mexErrMsgIdAndTxt("MATLAB:mxEig33Sym:argin", 50 | "Please call mxEig33Sym(K) with K of size 3x3!"); 51 | } 52 | 53 | double K[3][3]={0}, V[3][3]={0}; 54 | const double *pin=mxGetPr(prhs[0]); 55 | memcpy((void*)K, pin, sizeof(double)*9); //since K is symmetric, col-major doesn't matter 56 | 57 | plhs[0] = mxCreateDoubleMatrix(3,1,mxREAL); 58 | double *s = mxGetPr(plhs[0]); 59 | if (!LA::eig33sym(K,s,V)) { 60 | mexPrintf("[mxEig33Sym error] LA::eig33sys failed!\n"); 61 | return; 62 | } 63 | 64 | if (nlhs>1) { 65 | plhs[1] = mxCreateDoubleMatrix(3,3,mxREAL); 66 | double *v = mxGetPr(plhs[1]); 67 | for(int c=0, cnt=0; c<3; ++c) 68 | for(int r=0; r<3; ++r, ++cnt) 69 | *(v+cnt) = V[r][c]; 70 | } 71 | } -------------------------------------------------------------------------------- /matlab/mex/mxEig33Sym.m: -------------------------------------------------------------------------------- 1 | % [s,V]=mxEig33Sym(K) 2 | % calculate the eigen-decomposition of a 3x3 real symmetric matrix K 3 | % s: ascending eigen-values, s(1)<=s(2)<=s(3) 4 | % V: eigen-vectors, V(:,i) correspond to s(i) -------------------------------------------------------------------------------- /matlab/mex/mxEig33Sym.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ai4ce/peac/cd078773aa2ab3ba5d6315345004e382f3bfdb7e/matlab/mex/mxEig33Sym.mexw64 -------------------------------------------------------------------------------- /matlab/mex/mxFitAHCPlane.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | // Rights Reserved. 4 | // 5 | // Permission to use, copy and modify this software and its 6 | // documentation without fee for educational, research and non-profit 7 | // purposes, is hereby granted, provided that the above copyright 8 | // notice, this paragraph, and the following three paragraphs appear 9 | // in all copies. 10 | // 11 | // To request permission to incorporate this software into commercial 12 | // products contact: Director; Mitsubishi Electric Research 13 | // Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | // 15 | // IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | // INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | // LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | // DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | // SUCH DAMAGES. 20 | // 21 | // MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | // "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | // SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | // 27 | #include "mex.h" 28 | #include "matrix.h" 29 | 30 | #define _USE_MATH_DEFINES 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | //opencv 37 | #include "opencv2/opencv.hpp" 38 | 39 | #include "AHCPlaneFitter.hpp" 40 | 41 | struct MatlabImage3D { 42 | const double* src; //pointer to a matlab matrix of xyz ( WxHx3 ) 43 | const int w; 44 | const int h; 45 | const int yStride; 46 | const int zStride; 47 | 48 | MatlabImage3D(const double* pSrc, const int width, const int height) 49 | : src(pSrc), w(width), h(height), yStride(w*h), zStride(2*w*h) 50 | {} 51 | 52 | inline int width() const { return w; } 53 | inline int height() const { return h; } 54 | 55 | bool get(const int row, const int col, double &x, double &y, double &z) const { 56 | const int pixIdx = col * h + row; //note matlab data is in col-major 57 | 58 | z=src[pixIdx+zStride]; 59 | if(mxIsNaN(z)) return false; 60 | x=src[pixIdx]; 61 | y=src[pixIdx+yStride]; 62 | 63 | return true; 64 | } 65 | };//MatlabImage3D 66 | 67 | typedef ahc::PlaneFitter MatlabKinectPlaneFitter; 68 | MatlabKinectPlaneFitter gFitter; 69 | 70 | void reportParams() { 71 | mexPrintf("[mxFitAHCPlane] parameters:\n"); 72 | #define reportVar(var,flag,type) mexPrintf("%25s = " #flag "\n", #var, (type)gFitter.var) 73 | reportVar(params.depthSigma,%g,double); 74 | reportVar(params.stdTol_merge,%f,double); 75 | reportVar(params.stdTol_init,%f,double); 76 | reportVar(params.similarityTh_merge,%f,double); 77 | reportVar(params.similarityTh_refine,%f,double); 78 | reportVar(params.angle_far,%f,double); 79 | reportVar(params.angle_near,%f,double); 80 | reportVar(params.z_near,%f,double); 81 | reportVar(params.z_far,%f,double); 82 | reportVar(params.depthAlpha,%f,double); 83 | reportVar(params.depthChangeTol,%f,double); 84 | reportVar(params.initType,%d,int); 85 | 86 | reportVar(windowWidth,%d,int); 87 | reportVar(windowHeight,%d,int); 88 | reportVar(maxStep,%d,int); 89 | reportVar(minSupport,%d,int); 90 | reportVar(doRefine,%d,int); 91 | reportVar(erodeType,%d,int); 92 | #undef reportVar 93 | } 94 | 95 | enum FitterParams { 96 | FP_depthSigma=0, 97 | FP_stdTol_merge, 98 | FP_stdTol_init, 99 | FP_similarityTh_merge, 100 | FP_similarityTh_refine, 101 | FP_angle_far, 102 | FP_angle_near, 103 | FP_z_near, 104 | FP_z_far, 105 | FP_windowWidth, 106 | FP_windowHeight, 107 | FP_maxStep, 108 | FP_minSupport, 109 | FP_doRefine, 110 | FP_depthAlpha, 111 | FP_depthChangeTol, 112 | FP_erodeType, 113 | FP_initType, 114 | FP_drawCoarseBorder, 115 | FP_TOTAL 116 | }; 117 | void setParams(const int *param_keys, const double *param_vals, const int nParams) { 118 | for(int i=0; i& ids, const int w, const int h) { 152 | for(int i=0; i<(int)ids.size(); ++i) { 153 | int &id = ids[i]; 154 | const int c=id%w; 155 | const int r=id/w; 156 | id = c*h+r+1; //matlab use 1-based col-major index 157 | } 158 | } 159 | 160 | /* The matlab mex function */ 161 | // ret=mxFitAHCPlane(xyz): run plane fitting 162 | // mxFitAHCPlane(param_keys, param_vals): reset parameters 163 | // mxFitAHCPlane(): report current parameters 164 | void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { 165 | if (nrhs==0) { 166 | reportParams(); 167 | return; 168 | } 169 | if (nrhs==2) { 170 | if (!mxIsClass(prhs[0],"int32") || !mxIsDouble(prhs[1])) { 171 | mexErrMsgIdAndTxt("MATLAB:mxFitAHCPlane:argin", 172 | "When calling mxFitAHCPlane(param_keys, param_vals)," 173 | "param_keys must be of type int32 and " 174 | "param_vals must be of type double!"); 175 | } 176 | const int* keys = (const int*)mxGetData(prhs[0]); 177 | const double* vals = mxGetPr(prhs[1]); 178 | const int nParams_key = (int)mxGetNumberOfElements(prhs[0]); 179 | const int nParams_val = (int)mxGetNumberOfElements(prhs[1]); 180 | if(nParams_key!=nParams_val) { 181 | mexErrMsgIdAndTxt("MATLAB:mxFitAHCPlane:argin", 182 | "When calling mxFitAHCPlane(param_keys, param_vals)," 183 | "the two arguments should have same length!"); 184 | } 185 | setParams(keys, vals, nParams_key); 186 | return; 187 | } 188 | if (nrhs>1) { 189 | mexErrMsgIdAndTxt("MATLAB:mxFitAHCPlane:argin", 190 | "Too much input parameters!"); 191 | } 192 | 193 | const int nDims = mxGetNumberOfDimensions(prhs[0]); 194 | const mwSize* dims = mxGetDimensions(prhs[0]); 195 | const int h=dims[0]; 196 | const int w=dims[1]; 197 | 198 | if (nDims!=3 || dims[2]!=3) { 199 | mexErrMsgIdAndTxt("MATLAB:mxFitAHCPlane:argin", 200 | "When calling ret=mxFitAHCPlane(xyz), xyz should be of size WxHx3!"); 201 | } 202 | if (nlhs==0) { 203 | mexPrintf("[mxFitAHCPlane warn] no output parameter, skip."); 204 | return; 205 | } 206 | 207 | MatlabImage3D mi3d((double*)mxGetPr(prhs[0]), w, h); 208 | std::vector> ret; 209 | gFitter.run(&mi3d, &ret); 210 | 211 | mxArray *output = mxCreateCellMatrix((mwSize)ret.size(),1); 212 | plhs[0]=output; 213 | for(int i=0; i<(int)ret.size(); ++i) { 214 | std::vector& plid=ret[i]; 215 | rowMajorIdx2colMajorIdx(plid, w, h); 216 | mxArray *out=mxCreateNumericMatrix((int)plid.size(), 1, mxINT32_CLASS, mxREAL); 217 | mxSetCell(output, i, out); 218 | int* p = (int*)mxGetData(out); 219 | memcpy(p, &plid[0], sizeof(int)*plid.size()); 220 | } 221 | }//mexFunction -------------------------------------------------------------------------------- /matlab/mex/mxFitAHCPlane.m: -------------------------------------------------------------------------------- 1 | % ret=mxFitAHCPlane(xyz): run plane fitting 2 | % mxFitAHCPlane(param_keys, param_vals): reset parameters 3 | % mxFitAHCPlane(): report current parameters 4 | -------------------------------------------------------------------------------- /matlab/mex/mxFitAHCPlane.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ai4ce/peac/cd078773aa2ab3ba5d6315345004e382f3bfdb7e/matlab/mex/mxFitAHCPlane.mexw64 -------------------------------------------------------------------------------- /matlab/myPseudoColor.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function map=myPseudoColor(ncolors) 28 | map=[0,0,0; 29 | 255, 0, 0; 30 | 255, 255, 0; 31 | 100, 20, 50; 32 | 0, 30, 255; 33 | 10, 255, 60; 34 | 80, 10, 100; 35 | 0, 255, 200; 36 | 10, 60, 60; 37 | 255, 0, 128; 38 | 60, 128, 128]/255.0; 39 | if ncolors>length(map) 40 | nMore=ncolors-length(map); 41 | map=[map;rand(nMore,3)]; 42 | elseif ncolors: points expressed in world coordinate system 30 | % majorAxis<3x3>: from pca usually, majorAxis(:,1) is the x-direction in 31 | % world coordinate system, majorAxis(:,2) is y-direction, majorAxis(:,3) is 32 | % z-direction 33 | 34 | Vxy=majorAxis'*Vxyz; 35 | Vxy=Vxy(1:2,:); 36 | end -------------------------------------------------------------------------------- /matlab/reportAHCPlaneFitterParams.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function reportAHCPlaneFitterParams 28 | % report the current parameters of the AHC plane fitter 29 | mxFitAHCPlane; 30 | end -------------------------------------------------------------------------------- /matlab/setAHCPlaneFitterParams.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function setAHCPlaneFitterParams(params) 28 | % set the AHC plane fitter parameters, params must be constructed exactly as the same format as getDefaultAHCFitterparams 29 | vals=double(struct2array(params)); 30 | keys=int32((1:length(vals))-1); 31 | mxFitAHCPlane(keys, vals); 32 | end -------------------------------------------------------------------------------- /matlab/splitXYZ.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function [x,y,z]=splitXYZ(xyz) 28 | % split the 3-channel point cloud representation into 3 vectors of coordinates 29 | x=xyz(:,:,1); 30 | y=xyz(:,:,2); 31 | z=xyz(:,:,3); 32 | x=x(:); 33 | y=y(:); 34 | z=z(:); 35 | end -------------------------------------------------------------------------------- /matlab/viewSeg.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function viewSeg(mbs,w,h) 28 | seg=createSegImg(mbs,w,h); 29 | figure,imshow(seg); 30 | [x,y]=meshgrid(1:w,1:h); 31 | for i=1:length(mbs) 32 | mb=mbs{i}; 33 | mx=mean(x(mb)); 34 | my=mean(y(mb)); 35 | text(mx,my,num2str(i),'color','w'); 36 | end 37 | end -------------------------------------------------------------------------------- /matlab/xyz2pts.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright 2014 Mitsubishi Electric Research Laboratories All 3 | % Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its 6 | % documentation without fee for educational, research and non-profit 7 | % purposes, is hereby granted, provided that the above copyright 8 | % notice, this paragraph, and the following three paragraphs appear 9 | % in all copies. 10 | % 11 | % To request permission to incorporate this software into commercial 12 | % products contact: Director; Mitsubishi Electric Research 13 | % Laboratories (MERL); 201 Broadway; Cambridge, MA 02139. 14 | % 15 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, 16 | % INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING 17 | % LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 18 | % DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF 19 | % SUCH DAMAGES. 20 | % 21 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT 22 | % LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | % FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN 24 | % "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, 25 | % SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 26 | % 27 | function pts=xyz2pts(xyz) 28 | [x,y,z]=splitXYZ(xyz); 29 | pts=[x(:),y(:),z(:)]'; 30 | end --------------------------------------------------------------------------------