From: Leopold Palomo-Avellaneda Date: Mon, 6 Oct 2014 07:24:27 +0000 (+0200) Subject: Imported Upstream version 1.7.2 X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~14 X-Git-Url: https://dgit.raspbian.org/?a=commitdiff_plain;h=8ee0cd905c894d8294ada94bf0adeacd1ea55b84;p=pcl.git Imported Upstream version 1.7.2 --- diff --git a/.travis.sh b/.travis.sh new file mode 100755 index 00000000..cba0e050 --- /dev/null +++ b/.travis.sh @@ -0,0 +1,135 @@ +#!/bin/sh + +PCL_DIR=`pwd` +BUILD_DIR=$PCL_DIR/build +DOC_DIR=$BUILD_DIR/doc/doxygen/html + +TUTORIALS_DIR=$BUILD_DIR/doc/tutorials/html +ADVANCED_DIR=$BUILD_DIR/doc/advanced/html + +CMAKE_C_FLAGS="-Wall -Wextra -Wabi -O2" +CMAKE_CXX_FLAGS="-Wall -Wextra -Wabi -O2" + +function build () +{ + case $CC in + clang ) build_clang;; + gcc ) build_gcc;; + esac +} + +function build_clang () +{ + # A complete build + # Configure + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DBUILD_global_tests=OFF \ + $PCL_DIR + # Build + make -j2 +} + +function build_gcc () +{ + # A reduced build, only pcl_common + # Configure + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DBUILD_2d=OFF \ + -DBUILD_features=OFF \ + -DBUILD_filters=OFF \ + -DBUILD_geometry=OFF \ + -DBUILD_global_tests=OFF \ + -DBUILD_io=OFF \ + -DBUILD_kdtree=OFF \ + -DBUILD_keypoints=OFF \ + -DBUILD_ml=OFF \ + -DBUILD_octree=OFF \ + -DBUILD_outofcore=OFF \ + -DBUILD_people=OFF \ + -DBUILD_recognition=OFF \ + -DBUILD_registration=OFF \ + -DBUILD_sample_consensus=OFF \ + -DBUILD_search=OFF \ + -DBUILD_segmentation=OFF \ + -DBUILD_stereo=OFF \ + -DBUILD_surface=OFF \ + -DBUILD_tools=OFF \ + -DBUILD_tracking=OFF \ + -DBUILD_visualization=OFF \ + $PCL_DIR + # Build + make -j2 +} + +function test () +{ + # Configure + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS -DPCL_ONLY_CORE_POINT_TYPES=ON -DBUILD_global_tests=ON -DPCL_NO_PRECOMPILE=ON $PCL_DIR + # Build and run tests + make pcl_filters -j3 + make test_filters + make pcl_registration -j3 + make test_registration + make test_registration_api + make tests -j3 +} + +function doc () +{ + # Do not generate documentation for pull requests + if [[ $TRAVIS_PULL_REQUEST != 'false' ]]; then exit; fi + # Install doxygen and sphinx + sudo apt-get install doxygen doxygen-latex graphviz python-pip + sudo pip install sphinx sphinxcontrib-doxylink + # Configure + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake -DDOXYGEN_USE_SHORT_NAMES=OFF \ + -DSPHINX_HTML_FILE_SUFFIX=php \ + -DWITH_DOCS=1 \ + -DWITH_TUTORIALS=1 \ + $PCL_DIR + + git config --global user.email "documentation@pointclouds.org" + git config --global user.name "PointCloudLibrary (via TravisCI)" + + if [ -z "$id_rsa_{1..23}" ]; then echo 'No $id_rsa_{1..23} found !' ; exit 1; fi + + echo -n $id_rsa_{1..23} >> ~/.ssh/travis_rsa_64 + base64 --decode --ignore-garbage ~/.ssh/travis_rsa_64 > ~/.ssh/id_rsa + + chmod 600 ~/.ssh/id_rsa + + echo -e "Host github.com\n\tStrictHostKeyChecking no\n" >> ~/.ssh/config + + cd $DOC_DIR + git clone git@github.com:PointCloudLibrary/documentation.git . + + # Generate documentation and tutorials + cd $BUILD_DIR + make doc tutorials advanced + + # Upload to GitHub if generation succeeded + if [[ $? == 0 ]]; then + # Copy generated tutorials to the doc directory + cp -r $TUTORIALS_DIR/* $DOC_DIR/tutorials + cp -r $ADVANCED_DIR/* $DOC_DIR/advanced + # Commit and push + cd $DOC_DIR + git add --all + git commit --amend -m "Documentation for commit $TRAVIS_COMMIT" + git push --force + else + exit 2 + fi +} + +case $TASK in + build ) build;; + test ) test;; + doc ) doc;; +esac diff --git a/.travis.yml b/.travis.yml index 5abd127d..f2562b5e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,11 +2,45 @@ language: cpp compiler: - gcc - clang +env: + matrix: + - TASK="build" + global: + - secure: XQw5SBf/7b1SHFR+kKklBWhWVgNvm4vIi+wwyajFSbDLOPpsAqtnDKeA2DV9ciaQJ3CVAvBoyxYgzAvpbsb5k95jadbvu9aSlo/AQnAbz+8DhkJL25DwJAn8G4s4zD1MFi7P4fxJHZsv/l9UcdW4BzjEhh0VidWCO4hP6I9BAQc= + - secure: dRKTSeQI2Jad+/K9XCkNZxuu8exPi2wGzf6D0ogd1Nb2ZIUsOtnHSME4DO+xv7F5ZYrythHTrfezQl5hhcK+cr7A12okxlvmF/gVFuGCBPkUbyWPOrxx/Ic5pqdVnmrMFG1hFmr1KmOxCVx0F48JfGNd4ZgtUBAmnIomRp8sXRI= + - secure: ah6/Y0D8bBFfAU38RdWsLJ/0Gp5uN5KEHVOnyhEUx1wDaBcDl9+aIE9Xyah44ei/fqQg1MXBfgMnaF7oHpDs4dAKITYP4wV8WDX1DCl1dalIrWMTSFYRknc3Y6hT+HadMlkcV9CCLEhZ7gyyNkm+idbekt/WbQE6Jls/vBhdZxI= + - secure: V8XIEPjagHSFInXjogs8ypsC/gL5dq4VENYZbC9q8KYrNT9y1DLnMgNh9pGad25OjOqftBFwsgx6QYo357+MwpynfF+KcYNybjuR/vVGXpLcm0Uytnp6bE7oQbQ1s806TlXHb0Xk/bO2cLRYCCMCkhUOm+Dreu1uhKpoI/z0VMs= + - secure: vPRheiXkGnzBHRaVFsA3VtwK6DtWG48VIdbOxboaR6jb9jT9yNz5EQrapiLOFFkhGsZ1mCgeunR58BGUPiSl8gEvOheBBUgZW6x8kbHDpxSUc2H70bLRAKxP4t8e3ZDFg2RjGnvXkGhzUTu9oWnbfWxGAILJAOpNoT0MqDgP+Z0= + - secure: bT9kXekVN89zJeS4IEobLOeuAmRJqsgTjZky/mGqmOY1H8oIgcHC+41twB+nxkaU2mQskOvG45UwnAAW/8X6WdTbwKYeFWI5mXpCiXoiGL/dsYiig3GzYYv32ZJQL9B8tkEW6Xc0GY6v4K91Xw+HoKLi/tjxh3NjQxpfktwwCBQ= + - secure: kp5CTFyo/E5+v382ypD1mIP7RIVOq5A1NDur1NrYI9or+8+zGcK5A2/Ex6Bb6rac5l7cVcqx6Xel+clxH2eQP2D8GqkaxdZs/u9L7MdFqsfn/QYOIshYlowRoLX86W0KOhsTdLLv2mOS6DnWhIuhgDfrczD3hFQdP9PMTBpd65A= + - secure: YXJJRdv5OD/Qo1vNqPnFfx//SotxecFQyXjzEKQ8Zxu+EfVKVs95DUiUX+hiDlPrVGKhI3kTvza+lkaIv9fkjWiwSja5LsUbW/+2cwkhoHmOzVtdcpascfP7fcXc0kWTuvg1FLGNBv26SEym1FfNzp0Sm+ANZsCc4fQVXjDhQSY= + - secure: wyWnJoZydXcFW359Vid6YfG9l0WTFqIpuEhgFV8gtSbkrSU0JrrUrKuN5ld/cPb+sRa/I8FAftSDsH4CW67impwLmfNfGf4wZTOBvBKyDGjlI+TLEo4ew/yq+MYIkkEleCWOj5eUkAHPHUUhJzI8SvX+ZuxiJEXapoV0xymbrig= + - secure: vCvNI8egL2DRCicbWTuLOyXiW8478tedYvRneDMtCDB2lYToG32sYY0fS9KIciNohgObC8dbVraScgobL5cL4Ir4qrTUfBvGJ71OOMVujNEvkTts4YyI5zhA1PeEJ9+xcbQkE0/f78g7HQnd2LjZLXSxxrFVyJnjGaq1siNNRik= + - secure: iUjKlT1bJq3pZp3tS0yOmycSZwmGSbrfTe9fWs0P78o3WRYlzBx4IWa+RZb1l0l6LpCUo00wdgeKKhvzClHFza31fumZJKkFeDf/jxAM2TtSbcIxxXwqpi4yJJZs7SkYrU/lugv0NI6UHtl5wAkIp8sjtCCHVYJjlYNHaLhL75o= + - secure: DWrK3JsbzV5iUK3Xj6MpVWwlZv85GxdLl8c73ONcynZJ1tzUU3bfdvOtMxJrAnmH9QANw5RGORgbOyiDnvydCCpmL9RjswFqfkQX0mRTV8UYDPlJ1bBZJkX8I97yw8W1UyK1Q76kfr5bZjhikpsS/81Ll6z6Kj9FOte6KFfbznk= + - secure: B32G/fehtPQdu1bF+dbCJ/Eo22MiszxZj7UOwNggRFifoxTmRdffZyuqapW0PLCTbBEvlS6UfajGOmVvZx5QH8Q/gtutJqXDTMgWpOai66JUwnpn6AX6NnhFGc+s2cpp3V2+5I26OrFkaXTcS7flz32XJdKZmPgvjY1qoppmyzo= + - secure: u0cyLpY3LVF1gA8Sj4Q7X4Xv4bBT7m8IBDnm/AS412H0dM47dcFu8uxVFgWRu3WrM8T87Uc7+ftHkxKJezXgDPEVUixAnwLg2nEVEf6v3HC/HW+X9m98q1mELKXTLxc/f57rAIhhYPkjQh+leg6JmrhG1t0X7kh7d3CY0wwdSL0= + - secure: KlIjQVEWlr+H/PAKIAcKS6WO1EFBdXXvXLisEnferB1RWgGwuLRgtIZzI+BJdt/7BMSyWvRExqt6xNyfqpxKXl1kdJawc3rpeYufklHQgmB2qGBSrpMtNr0S3gPFePBdnETbdPHwA63QCrpRKcrHqJxmeIzAmstH736iRXuubFs= + - secure: ldwk79cqSgaEUZLt5rbNLRcNaVf/bx2zjBEDbvzAF9JCqVx/L5zCRTr5raHsxzBOzz25Qs3nFW81e1WLwxMAWshvI7EM/YG8GXqECvJGWpHBwcZI1SVP3zMhpH/jJE8vbMFaM2NOmhMT03z91vt0NlvR4DJMY0KV351awGSL2Do= + - secure: SyLCwwc+jjkdmNUkRdGRDR2lNrPCu1ZvPstvVWQke5uw3BlnVKXWK7V3yq9g11ZsCl/7aldBexjf7pqeXbJ5Wl5goiI3E+/Ooujd/EWkMn9K3YlG57p8Zdw/A/fUMJgAH3qrM//ihdO0KDJD8eCGLlm4qV0SnlWFPQ+Dy8BsAc8= + - secure: DaqjsZS60Y070Gw2lUNltzDltiYKB8IlYqsp0SyOjZAKlDGgp97+9YGEaIGcKF1qw3VGswg+7rrjWQ59iiwstTqgvx1mTTik0Qc0RCc8GtvIm+PS9TOhWxQginmhZmET9QKnGB7uj6K63qN8V8MZakZWIJxgUXx8jGiCTD22/eQ= + - secure: x4x3vHY6Wf4kxjAT8dbWRl8n4PxTGv8RtzfGIZYXvJgbnY2qW+cJ8Edp64V1LegvQbBQmKqAP9YSLHwZsuL3LxfVqt/seRs+DJMDVUd9jVYmym0rPqemJLezapalEg6qfLuoeNkDPWvIVccQCDEBPfaUdD0ZXYo44LS5jIV0+T8= + - secure: Pm2hyxdSLnY3ltrAva0FwNWWEQQcnf1JK2Fhjc3sWFpStMF7Obk93u4G5M6f2f28ZY6HFaMRYC1qEz/+yMIjsusIv8j0E6hgB/EnoM0dlxCc0aryH3X2IOYBVjRMjOFPmhYbNBoMmZWLluHWSyVSqr9k9nxowMfM3mi4fah11aQ= + - secure: WTZ238yAEfXRyll1n8yau3FUW9HTvq6scKIl9AmNZrnzTr9dktupWrBVV6CtvaufT1mSmDigZ7VGC6T71HkyRIyb2qfVTrnjnxE96Wtcci6PfkuQc2L2puuZYo8dXaBRoOgJKGHFo/uKVKWnp7t55dp3lBJJmclHhon+K2hMSJw= + - secure: LNsNoBvqY/jYDoBjWCE5cM+f1H8xOwSBc/tbWZo6E/jPRjUOLzXSicMMUMrlVto+bFzSUT8OVajV3XmoRx+Qntzv6bDSAGjdycvHd2YZQPn8BYrsFtR4So7SsJkF9FlxzbiOXaiSRpwGn7TP/DO7Neubrr4IS2ef4nWowGrnCE8= + - secure: PZivWbaCWFA2BFFY8n3UMxdEWjz7rBh568u9LF5LH3HgWADnfiwWzNriACqX9fhe7tSmDru5Bk978s+xPPAY9v24cfiDEX5a5MQ/XVr2rP48n3vlUDWERDhIodJ73F9F9GGZXToGdNz0MBUAHgiv7Lb0GYUfmOYzUJjWghngLBw= +matrix: + include: + - compiler: clang + env: TASK="test" + - env: TASK="doc" before_install: - sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl -y - sudo add-apt-repository ppa:apokluda/boost1.53 -y + - sudo add-apt-repository ppa:yade-users/external -y + - sudo add-apt-repository ppa:libreoffice/ppa -y - sudo apt-get update -d - - sudo apt-get install cmake libvtk5-qt4-dev libflann-dev libeigen3-dev libopenni-dev libqhull-dev libboost-filesystem1.53-dev libboost-iostreams1.53-dev libboost-thread1.53-dev -script: - - mkdir build && cd build - - cmake -DBUILD_global_tests=ON -DCMAKE_C_FLAGS="-Wall -Wextra -Wabi -O2" -DCMAKE_CXX_FLAGS="-Wall -Wextra -Wabi -O2" -DPCL_ONLY_CORE_POINT_TYPES=ON .. && make -j2 && make test +install: + - sudo apt-get install libvtk5-qt4-dev libflann-dev libeigen3-dev libopenni-dev libqhull-dev libboost-filesystem1.53-dev libboost-iostreams1.53-dev libboost-thread1.53-dev libboost-chrono1.53-dev libusb-1.0-0-dev libgtest-dev +script: + - bash .travis.sh diff --git a/CHANGES.md b/CHANGES.md index 68f33564..adc8e4ef 100644 --- a/CHANGES.md +++ b/CHANGES.md @@ -1,6 +1,285 @@ # ChangeList +## *= 1.7.2 (10.09.2014) =* + +* Added support for VTK6 + [[#363]](https://github.com/PointCloudLibrary/pcl/pull/363) +* Removed Google Test from the source tree and added it as a system dependency + [[#731]](https://github.com/PointCloudLibrary/pcl/pull/731) +* Added support for QHull 2012 on non-Debian platforms + [[#852]](https://github.com/PointCloudLibrary/pcl/pull/852) + +### `libpcl_common:` + +* Added `BearingAngleImage` class + [[#198]](https://github.com/PointCloudLibrary/pcl/pull/198) +* Added `pcl::CPPFSignature` point type + [[#296]](https://github.com/PointCloudLibrary/pcl/pull/296) +* Added `getRGBAVector4i()`, `getBGRVector3cMap()`, and `getBGRAVector4cMap()` + to all point types containing RGB/RGBA fields + [[#450]](https://github.com/PointCloudLibrary/pcl/pull/450) +* Added a family of "has field" functions to check presence of a particular + field in a point type both at compile- and run-time + [[#462]](https://github.com/PointCloudLibrary/pcl/pull/462) +* Added a function to copy data between points of different types + [[#465]](https://github.com/PointCloudLibrary/pcl/pull/465) +* Added test macros for equality/nearness checks + [[#499]](https://github.com/PointCloudLibrary/pcl/pull/499) +* Added `descriptorSize()` to all point types with descriptors + [[#531]](https://github.com/PointCloudLibrary/pcl/pull/531) +* Added possibility to copy a cloud inside another one while interpolating + borders + [[#567]](https://github.com/PointCloudLibrary/pcl/pull/567) +* Added a function to determine the point of intersection of three non-parallel + planes + [[#571]](https://github.com/PointCloudLibrary/pcl/pull/571) +* Fixed a bug in HSV to RGB color conversion + [[#581]](https://github.com/PointCloudLibrary/pcl/pull/581) +* Added a new `CentroidPoint` class + [[#586]](https://github.com/PointCloudLibrary/pcl/pull/586) +* Templated intersection computation functions on scalar type + [[#646]](https://github.com/PointCloudLibrary/pcl/pull/646) +* Templated functions in 'eigen.h' on scalar type + [[#660]](https://github.com/PointCloudLibrary/pcl/pull/660) +* Added functions to transform points, vectors, lines, etc. + [[#660]](https://github.com/PointCloudLibrary/pcl/pull/660) + +### `libpcl_features:` + +* Added a simple implementation of CPPF using normalised HSV values in the + feature vector + [[#296]](https://github.com/PointCloudLibrary/pcl/pull/296) +* Added `MomentOfInertiaEstimation` and `ROPSEstimation` features + [[#319]](https://github.com/PointCloudLibrary/pcl/pull/319) +* Fixed a problem in `OURCVFHEstimation::computeRFAndShapeDistribution()` + [[#738]](https://github.com/PointCloudLibrary/pcl/pull/738) +* Fixed undefined behavior in `OURCVFHEstimation::computeFeature()` + [[#811]](https://github.com/PointCloudLibrary/pcl/pull/811) +* Fixed memory corruption error in OUR-CVFH + [[#875]](https://github.com/PointCloudLibrary/pcl/pull/875) + +### `libpcl_filters:` + +* Added a function to set the minimum number of points required for a voxel to + be used in `VoxelGrid` + [[#434]](https://github.com/PointCloudLibrary/pcl/pull/434) +* Added `GridMinimum` filter + [[#520]](https://github.com/PointCloudLibrary/pcl/pull/520) +* Added a morphological filter that operates on Z dimension + [[#533]](https://github.com/PointCloudLibrary/pcl/pull/533) +* Added progressive morphological filter to extract ground returns + [[#574]](https://github.com/PointCloudLibrary/pcl/pull/574) +* Added a filter to remove locally maximal points in the z dimension + [[#577]](https://github.com/PointCloudLibrary/pcl/pull/577) +* Added an approximate version of the progressive morphological filter + [[#665]](https://github.com/PointCloudLibrary/pcl/pull/665) +* Added `ModelOutlierRemoval` class that filters points in a cloud based on the + distance between model and point + [[#702]](https://github.com/PointCloudLibrary/pcl/pull/702) + +### `libpcl_io:` + +* Added experimental version of an OpenNI 2.x grabber + [[#276]](https://github.com/PointCloudLibrary/pcl/pull/276) + [[#843]](https://github.com/PointCloudLibrary/pcl/pull/843) +* Added support for IFS file format + [[#354]](https://github.com/PointCloudLibrary/pcl/pull/354) + [[#356]](https://github.com/PointCloudLibrary/pcl/pull/356) +* Added possibility to load `PCLPointCloud2` from OBJ files + [[#363]](https://github.com/PointCloudLibrary/pcl/pull/363) +* Fixed loading and saving of PLY files + [[#510]](https://github.com/PointCloudLibrary/pcl/pull/510) + [[#579]](https://github.com/PointCloudLibrary/pcl/pull/579) +* Fixed race conditions in `PCDGrabber` + [[#582]](https://github.com/PointCloudLibrary/pcl/pull/582) +* Fixed multi openni grabber buffer corruption + [[#845]](https://github.com/PointCloudLibrary/pcl/pull/845) +* Fixed incompatibility with Boost 1.56 in `LZFImageWriter` + [[#867]](https://github.com/PointCloudLibrary/pcl/pull/867) +* Fixed a bug in `PLYReader` which lead to deformation of point clouds when + displayed in `CloudViewer` or `PCLVisualizer` + [[#879]](https://github.com/PointCloudLibrary/pcl/pull/879) + +### `libpcl_kdtree:` + +* Fixed double memory free bug in `KdTreeFLANN` + [[#618]](https://github.com/PointCloudLibrary/pcl/pull/618) + +### `libpcl_keypoints:` + +* Added a method `Keypoint::getKeypointsIndices ()` + [[#318]](https://github.com/PointCloudLibrary/pcl/pull/318) +* Added keypoints based on Trajkovic and Hedley operator (2D and 3D versions) + [[#409]](https://github.com/PointCloudLibrary/pcl/pull/409) + +### `libpcl_octree:` + +* Fixed a bug in `OctreePointCloudAdjacency::computeNeighbors()` + [[#455]](https://github.com/PointCloudLibrary/pcl/pull/455) +* Accelerated `OctreePointCloudAdjacency` building by disabling dynamic key + resizing + [[#332]](https://github.com/PointCloudLibrary/pcl/pull/332) +* Fixed a bug with infinite points in `OctreePointCloudAdjacency` + [[#723]](https://github.com/PointCloudLibrary/pcl/pull/723) + +### `libpcl_people:` + +* Added a possibility to define a transformation matrix for people tracker + [[#606]](https://github.com/PointCloudLibrary/pcl/pull/606) + +### `libpcl_recognition:` + +* Allow PCL to be built against a system-wide installed metslib + [[#299]](https://github.com/PointCloudLibrary/pcl/pull/299) +* Fixed a bug in `ObjRecRANSAC::addModel()` + [[#269]](https://github.com/PointCloudLibrary/pcl/pull/269) +* Added `LINEMOD::loadTemplates()` (useful for object recognition systems that + store templates for different objects in different files) + [[#358]](https://github.com/PointCloudLibrary/pcl/pull/358) + +### `libpcl_registration:` + +* Fixed `SampleConsensusInitialAlignment::hasConverged()` + [[#339]](https://github.com/PointCloudLibrary/pcl/pull/339) +* Added `JointIterativeClosestPoint` + [[#344]](https://github.com/PointCloudLibrary/pcl/pull/344) +* Made correspondence rejectors to actually work with ICP + [[#419]](https://github.com/PointCloudLibrary/pcl/pull/419) +* Added `GeneralizedIterativeClosestPoint6D` that integrates Lab color space + information into the GICP algorithm + [[#491]](https://github.com/PointCloudLibrary/pcl/pull/491) +* Fixed bugs and optimized `SampleConsensusPrerejective` + [[#741]](https://github.com/PointCloudLibrary/pcl/pull/741) +* Fixed a bug in `TransformationEstimationSVDScale` + [[#885]](https://github.com/PointCloudLibrary/pcl/pull/885) + +### `libpcl_sample_consensus:` + +* Unified `SampleConsensusModelNormalParallelPlane` with + `SampleConsensusModelNormalPlane` to avoid code duplication + [[#696]](https://github.com/PointCloudLibrary/pcl/pull/696) + +### `libpcl_search:` + +* `search::KdTree` can now be used with different KdTree implementations + [[#81]](https://github.com/PointCloudLibrary/pcl/pull/81) +* Added a new interface to FLANN's multiple randomized trees for + high-dimensional (feature) searches + [[#435]](https://github.com/PointCloudLibrary/pcl/pull/435) +* Fixed a bug in the `Ptr` typdef in `KdTree` + [[#820]](https://github.com/PointCloudLibrary/pcl/pull/820) + +### `libpcl_segmentation:` + +* Added `GrabCut` segmentation and a show-case application for 2D + [[#330]](https://github.com/PointCloudLibrary/pcl/pull/330) +* Updated `RegionGrowingRGB::assembleRegion()` to speed up the algorithm + [[#538]](https://github.com/PointCloudLibrary/pcl/pull/538) +* Fixed a bug with missing point infinity test in `RegionGrowing` + [[#617]](https://github.com/PointCloudLibrary/pcl/pull/617) +* Fixed alignment issue in `SupervoxelClustering` + [[#625]](https://github.com/PointCloudLibrary/pcl/pull/625) +* Added a curvature parameter to `Region3D` class + [[#653]](https://github.com/PointCloudLibrary/pcl/pull/653) +* Fixed a minor bug in `OrganizedConnectedComponentSegmentation` + [[#802]](https://github.com/PointCloudLibrary/pcl/pull/802) + +### `libpcl_surface:` + +* Fixed a bug in `EarClipping` where computation failed if all vertices have + the same x or y component + [[#130]](https://github.com/PointCloudLibrary/pcl/pull/130) +* Added support for unequal focal lengths along different axes in texture + mapping + [[#352]](https://github.com/PointCloudLibrary/pcl/pull/352) +* Speeded up bilateral upsampling + [[#689]](https://github.com/PointCloudLibrary/pcl/pull/689) +* Reduced space usage in `MovingLeastSquares` + [[#785]](https://github.com/PointCloudLibrary/pcl/pull/785) + +### `libpcl_tracking:` + +* Fixed Hue distance calculation in tracking `HSVColorCoherence` + [[#390]](https://github.com/PointCloudLibrary/pcl/pull/390) +* Added pyramidal KLT tracking + [[#587]](https://github.com/PointCloudLibrary/pcl/pull/587) + +### `libpcl_visualization:` + +* Added a new color handler `PointCloudColorHandlerRGBAField` that takes into + account alpha channel + [[#306]](https://github.com/PointCloudLibrary/pcl/pull/306) +* Fixed `PCLVisualizer` crashes on OS X + [[#384]](https://github.com/PointCloudLibrary/pcl/pull/384) +* Added possibility to display texture on polygon meshes + [[#400]](https://github.com/PointCloudLibrary/pcl/pull/400) +* Added ability to add and remove several coordinate systems + [[#401]](https://github.com/PointCloudLibrary/pcl/pull/401) +* Added `ImageViewer::markPoints()` + [[#439]](https://github.com/PointCloudLibrary/pcl/pull/439) +* Added `setWindowPosition()` and `setWindowName()` to `PCLPlotter` + [[#457]](https://github.com/PointCloudLibrary/pcl/pull/457) +* Changed camera parameters display to be more user-friendly + [[#544]](https://github.com/PointCloudLibrary/pcl/pull/544) +* Added `PCLVisualizer::updateCoordinateSystemPose()` + [[#569]](https://github.com/PointCloudLibrary/pcl/pull/569) +* Fixed display of non-triangular meshes in `PCLVisualizer` + [[#686]](https://github.com/PointCloudLibrary/pcl/pull/686) +* Added a capability to save and restore camera view in `PCLVisualizer` + [[#703]](https://github.com/PointCloudLibrary/pcl/pull/703) +* Added `PCLVisualizer::getShapeActorMap()` function + [[#725]](https://github.com/PointCloudLibrary/pcl/pull/725) +* Fixed undefined behavior when drawing axis in `PCLVisualizer` + [[#762]](https://github.com/PointCloudLibrary/pcl/pull/762) +* Fixed HSV to RGB conversion in `PointCloudColorHandlerHSVField` + [[#772]](https://github.com/PointCloudLibrary/pcl/pull/772) +* Fixed non-working key presses in visualization GUIs on Mac OS X systems + [[#795]](https://github.com/PointCloudLibrary/pcl/pull/795) +* Fixed a bug in `PCLVisualizer::addCube()` + [[#846]](https://github.com/PointCloudLibrary/pcl/pull/846) +* Fixed a bug in cone visualization and added possibility to set cone length + [[#881]](https://github.com/PointCloudLibrary/pcl/pull/881) + +### `PCL Tools:` + +* Added a simple tool to compute Hausdorff distance between two point clouds + [[#519]](https://github.com/PointCloudLibrary/pcl/pull/519) +* Updated `pcl_viewer` to use RGB color handler as default + [[#556]](https://github.com/PointCloudLibrary/pcl/pull/556) +* Added a morphological tool `pcl_morph` to apply dilate/erode/open/close + operations on the Z dimension + [[#572]](https://github.com/PointCloudLibrary/pcl/pull/572) +* Added a tool `pcl_generate` to generate random clouds + [[#599]](https://github.com/PointCloudLibrary/pcl/pull/599) +* Added a tool `pcl_grid_min` to find grid minimums + [[#603]](https://github.com/PointCloudLibrary/pcl/pull/603) +* Added a tool `pcl_local_max` to filter out local maxima + [[#604]](https://github.com/PointCloudLibrary/pcl/pull/604) +* Added optional depth image input to `pcl_png2pcd` converter + [[#680]](https://github.com/PointCloudLibrary/pcl/pull/680) +* Fixed memory size calculation in `pcl_openni_pcd_recorder` + [[#676]](https://github.com/PointCloudLibrary/pcl/pull/676) +* Added device ID parameter to `pcl_openni_pcd_recorder` + [[#673]](https://github.com/PointCloudLibrary/pcl/pull/673) +* Added automatic camera reset on startup in `pcl_viewer` + [[#693]](https://github.com/PointCloudLibrary/pcl/pull/693) +* Added a capability to save and restore camera view in `pcl_viewer` + [[#703]](https://github.com/PointCloudLibrary/pcl/pull/703) +* Updated `pcl_pcd2png` tool to be able to paint pixels corresponding to + infinite points with black. Added Glasbey lookup table to paint labels with + a fixed set of highly distinctive colors. + [[#767]](https://github.com/PointCloudLibrary/pcl/pull/767) +* Added `pcl_obj2pcd` tool + [[#816]](https://github.com/PointCloudLibrary/pcl/pull/816) + +### `PCL Apps:` + +* Fixed disappearing cloud from selection in Cloud Composer + [[#814]](https://github.com/PointCloudLibrary/pcl/pull/814) + + ## *= 1.7.1 (07.10.2013) =* + * New pcl::io::savePNGFile() functions and pcd2png tool (deprecates organized_pcd_to_png). * Support for Intel Perceptual Computing SDK cameras. * New Dual quaternion transformation estimation algorithm. diff --git a/CMakeLists.txt b/CMakeLists.txt index f2196da1..f0a56003 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,10 @@ ### ---[ PCL global CMake cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +if(POLICY CMP0048) + cmake_policy(SET CMP0048 OLD) # do not use VERSION option in project() command +endif() + set(CMAKE_CONFIGURATION_TYPES "Debug;Release" CACHE STRING "possible configurations" FORCE) # In case the user does not setup CMAKE_BUILD_TYPE, assume it's RelWithDebInfo @@ -12,7 +16,10 @@ project(PCL) string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) ### ---[ Find universal dependencies -set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODULE_PATH}) +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODULE_PATH}) + +# ---[ Include pkgconfig +include (FindPkgConfig) # ---[ Release/Debug specific flags if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") @@ -25,6 +32,12 @@ if(WIN32 AND NOT MINGW) if(NOT DEFINED CMAKE_RELEASE_POSTFIX) set(CMAKE_RELEASE_POSTFIX "_release") endif() + if(NOT DEFINED CMAKE_RELWITHDEBINFO_POSTFIX) + set(CMAKE_RELWITHDEBINFO_POSTFIX "_release") + endif() + if(NOT DEFINED CMAKE_MINSIZEREL_POSTFIX) + set(CMAKE_MINSIZEREL_POSTFIX "_release") + endif() endif() # ---[ special maintainer mode @@ -58,9 +71,9 @@ if (ANDROID) message ("PCL shared libs on Android must be: ${PCL_SHARED_LIBS}") endif() -include(${PCL_SOURCE_DIR}/cmake/pcl_verbosity.cmake) -include(${PCL_SOURCE_DIR}/cmake/pcl_targets.cmake) -include(${PCL_SOURCE_DIR}/cmake/pcl_options.cmake) +include("${PCL_SOURCE_DIR}/cmake/pcl_verbosity.cmake") +include("${PCL_SOURCE_DIR}/cmake/pcl_targets.cmake") +include("${PCL_SOURCE_DIR}/cmake/pcl_options.cmake") # Enable verbose timing display? if(CMAKE_TIMING_VERBOSE AND UNIX) @@ -69,7 +82,7 @@ if(CMAKE_TIMING_VERBOSE AND UNIX) endif(CMAKE_TIMING_VERBOSE AND UNIX) # check for SSE flags -include(${PCL_SOURCE_DIR}/cmake/pcl_find_sse.cmake) +include("${PCL_SOURCE_DIR}/cmake/pcl_find_sse.cmake") if (PCL_ENABLE_SSE) PCL_CHECK_FOR_SSE() endif (PCL_ENABLE_SSE) @@ -145,33 +158,29 @@ if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") SET(CMAKE_C_FLAGS "-Qunused-arguments") endif() if("${CMAKE_CXX_FLAGS}" STREQUAL "") - SET(CMAKE_CXX_FLAGS "-Qunused-arguments -Wno-invalid-offsetof ${SSE_FLAGS}") # Unfortunately older Clang versions do not have this: -Wno-unnamed-type-template-args + SET(CMAKE_CXX_FLAGS "-ftemplate-depth=1024 -Qunused-arguments -Wno-invalid-offsetof ${SSE_FLAGS}") # Unfortunately older Clang versions do not have this: -Wno-unnamed-type-template-args + if(APPLE AND WITH_CUDA AND CUDA_FOUND) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libstdc++") + endif() endif() SET(CLANG_LIBRARIES "stdc++") endif() -# ---[ Project folders -option(USE_PROJECT_FOLDERS "Use folders to organize PCL projects in an IDE." OFF) -mark_as_advanced(USE_PROJECT_FOLDERS) -if(USE_PROJECT_FOLDERS) - set_property(GLOBAL PROPERTY USE_FOLDERS ON) -endif(USE_PROJECT_FOLDERS) - -include(${PCL_SOURCE_DIR}/cmake/pcl_utils.cmake) -set(PCL_VERSION 1.7.1 CACHE STRING "PCL version") +include("${PCL_SOURCE_DIR}/cmake/pcl_utils.cmake") +set(PCL_VERSION 1.7.2 CACHE STRING "PCL version") DISSECT_VERSION() GET_OS_INFO() SET_INSTALL_DIRS() if(WIN32) - set(PCL_RESOURCES_DIR ${PCL_SOURCE_DIR}/resources) - set(PCL_POINTCLOUDS_DIR ${PCL_RESOURCES_DIR}/pointclouds) + set(PCL_RESOURCES_DIR "${PCL_SOURCE_DIR}/resources") + set(PCL_POINTCLOUDS_DIR "${PCL_RESOURCES_DIR}/pointclouds") endif(WIN32) -set(PCL_OUTPUT_LIB_DIR ${PCL_BINARY_DIR}/${LIB_INSTALL_DIR}) -set(PCL_OUTPUT_BIN_DIR ${PCL_BINARY_DIR}/${BIN_INSTALL_DIR}) -make_directory(${PCL_OUTPUT_LIB_DIR}) -make_directory(${PCL_OUTPUT_BIN_DIR}) +set(PCL_OUTPUT_LIB_DIR "${PCL_BINARY_DIR}/${LIB_INSTALL_DIR}") +set(PCL_OUTPUT_BIN_DIR "${PCL_BINARY_DIR}/${BIN_INSTALL_DIR}") +make_directory("${PCL_OUTPUT_LIB_DIR}") +make_directory("${PCL_OUTPUT_BIN_DIR}") if(WIN32) foreach(config ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER ${config} CONFIG) @@ -207,7 +216,7 @@ ENDIF("${is_system_dir}" STREQUAL "-1") ### ---[ Find universal dependencies # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it -if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1)) +if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")) find_package(OpenMP) endif() if(OPENMP_FOUND) @@ -227,7 +236,7 @@ else(OPENMP_FOUND) message (STATUS "Not found OpenMP") endif() # Boost (required) -include(${PCL_SOURCE_DIR}/cmake/pcl_find_boost.cmake) +include("${PCL_SOURCE_DIR}/cmake/pcl_find_boost.cmake") # Eigen (required) find_package(Eigen REQUIRED) include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) @@ -241,121 +250,177 @@ find_package(FLANN 1.7.0 REQUIRED) include_directories(${FLANN_INCLUDE_DIRS}) # libusb-1.0 -find_package(libusb-1.0) -if(LIBUSB_1_FOUND) - include_directories(${LIBUSB_1_INCLUDE_DIR}) -endif(LIBUSB_1_FOUND) +option(WITH_LIBUSB "Build USB RGBD-Camera drivers" TRUE) +if(WITH_LIBUSB) + find_package(libusb-1.0) + if(LIBUSB_1_FOUND) + include_directories("${LIBUSB_1_INCLUDE_DIR}") + endif(LIBUSB_1_FOUND) +endif(WITH_LIBUSB) # OpenNI -find_package(OpenNI) -if (OPENNI_FOUND) - set(HAVE_OPENNI ON) - include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS}) -endif() +option(WITH_OPENNI "OpenNI driver support" TRUE) +if(WITH_OPENNI) + find_package(OpenNI) + if (OPENNI_FOUND) + set(HAVE_OPENNI ON) + include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS}) + endif(OPENNI_FOUND) +endif(WITH_OPENNI) + +# OpenNI 2 +option(WITH_OPENNI2 "OpenNI 2 driver support" TRUE) +if(WITH_OPENNI2) + find_package(OpenNI2) + if (OPENNI2_FOUND) + set(HAVE_OPENNI2 ON) + include_directories(SYSTEM ${OPENNI2_INCLUDE_DIRS}) + endif(OPENNI2_FOUND) +endif(WITH_OPENNI2) # Fotonic (FZ_API) -find_package(FZAPI) -if (FZAPI_FOUND) - set(HAVE_FZAPI ON) - include_directories(SYSTEM ${FZAPI_INCLUDE_DIR}) -endif() +option(WITH_FZAPI "Build Fotonic Camera support" TRUE) +if(WITH_FZAPI) + find_package(FZAPI) + if (FZAPI_FOUND) + set(HAVE_FZAPI ON) + include_directories(SYSTEM "${FZAPI_INCLUDE_DIR}") + endif(FZAPI_FOUND) +endif(WITH_FZAPI) # Intel Perceptional Computing Interface (PXCAPI) -find_package(PXCAPI) -if (PXCAPI_FOUND) - set(HAVE_PXCAPI ON) - include_directories(SYSTEM ${PXCAPI_INCLUDE_DIRS}) +option(WITH_PXCAPI "Build PXC Device support" TRUE) +if(WITH_PXCAPI) + find_package(PXCAPI) + if (PXCAPI_FOUND) + set(HAVE_PXCAPI ON) + include_directories(SYSTEM ${PXCAPI_INCLUDE_DIRS}) + endif(PXCAPI_FOUND) +endif(WITH_PXCAPI) + +# metslib +if (PKG_CONFIG_FOUND) + pkg_check_modules(METSLIB metslib) + if (METSLIB_FOUND) + set (HAVE_METSLIB ON) + include_directories(${METSLIB_INCLUDE_DIRS}) + else() + include_directories("${PCL_SOURCE_DIR}/recognition/include/pcl/recognition/3rdparty/") + endif() +else() + include_directories(${PCL_SOURCE_DIR}/recognition/include/pcl/recognition/3rdparty/) endif() # LibPNG -find_package(PNG) -if (PNG_FOUND) - set (HAVE_PNG ON) - include_directories(${PNG_INCLUDE_DIR}) -endif(PNG_FOUND) +option(WITH_PNG "PNG file support" TRUE) +if(WITH_PNG) + find_package(PNG) + if (PNG_FOUND) + set (HAVE_PNG ON) + include_directories("${PNG_INCLUDE_DIR}") + endif(PNG_FOUND) +endif(WITH_PNG) # Qhull -if(NOT PCL_SHARED_LIBS OR WIN32) - set(QHULL_USE_STATIC ON) -endif(NOT PCL_SHARED_LIBS OR WIN32) -find_package(Qhull) - -# Find Qt5 -include(cmake/pcl_find_qt5.cmake) - -# Find QT4 -if(NOT QT5_FOUND) - find_package(Qt4) - if (QT4_FOUND) - include(${QT_USE_FILE}) - endif (QT4_FOUND) -endif() +option(WITH_QHULL "Include convex-hull operations" TRUE) +if(WITH_QHULL) + if(NOT PCL_SHARED_LIBS OR WIN32) + set(QHULL_USE_STATIC ON) + endif(NOT PCL_SHARED_LIBS OR WIN32) + find_package(Qhull) +endif(WITH_QHULL) + +option(WITH_QT "Build QT Front-End" TRUE) +if(WITH_QT) + # Find Qt4 + find_package(Qt4) + if (QT4_FOUND) + include("${QT_USE_FILE}") + endif (QT4_FOUND) + + # Find QT5 + if(NOT QT4_FOUND) + include(cmake/pcl_find_qt5.cmake) + endif(NOT QT4_FOUND) +endif(WITH_QT) # Find VTK -find_package(VTK) -if(VTK_FOUND) - if (PCL_SHARED_LIBS OR - (NOT (PCL_SHARED_LIBS) AND NOT (VTK_BUILD_SHARED_LIBS))) - set(VTK_FOUND TRUE) - find_package (QVTK) - message(STATUS "VTK found (include: ${VTK_INCLUDE_DIRS}, lib: ${VTK_LIBRARY_DIRS})") - link_directories(${VTK_LIBRARY_DIRS}) - set(HAVE_VTK ON) - else () - set(VTK_FOUND OFF) - set(HAVE_VTK OFF) - message ("Warning: You are to build PCL in STATIC but VTK is SHARED!") - message ("Warning: VTK disabled!") - endif () -endif(VTK_FOUND) -# Find MPI -if (WITH_MPI) # this script searches for MPI 10 sec under windows, annoying especially if you do this often - find_package(MPI) - if(MPI_CXX_FOUND) - include_directories(SYSTEM ${MPI_INCLUDE_PATH}) - endif(MPI_CXX_FOUND) -endif() -#Find Doxygen and html help compiler if any -find_package(Doxygen) -if(DOXYGEN_FOUND) - find_package(HTMLHelp) -endif(DOXYGEN_FOUND) +option(WITH_VTK "Build VTK-Visualizations" TRUE) +if(WITH_VTK AND NOT ANDROID) + find_package(VTK) + if(VTK_FOUND) + message(STATUS "VTK_MAJOR_VERSION ${VTK_MAJOR_VERSION}") + if (PCL_SHARED_LIBS OR + (NOT (PCL_SHARED_LIBS) AND NOT (VTK_BUILD_SHARED_LIBS))) + set(VTK_FOUND TRUE) + find_package (QVTK) + if (${VTK_MAJOR_VERSION} VERSION_LESS "6.0") + message(STATUS "VTK found (include: ${VTK_INCLUDE_DIRS}, lib: ${VTK_LIBRARY_DIRS})") + link_directories(${VTK_LIBRARY_DIRS}) + else(${VTK_MAJOR_VERSION} VERSION_LESS "6.0") + include (${VTK_USE_FILE}) + message(STATUS "VTK found (include: ${VTK_INCLUDE_DIRS}, lib: ${VTK_LIBRARIES}") + endif (${VTK_MAJOR_VERSION} VERSION_LESS "6.0") + if (APPLE) + option (VTK_USE_COCOA "Use Cocoa for VTK render windows" ON) + MARK_AS_ADVANCED (VTK_USE_COCOA) + endif (APPLE) + set(HAVE_VTK ON) + else () + set(VTK_FOUND OFF) + set(HAVE_VTK OFF) + message ("Warning: You are to build PCL in STATIC but VTK is SHARED!") + message ("Warning: VTK disabled!") + endif () + endif(VTK_FOUND) +else(WITH_VTK AND NOT ANDROID) + set(VTK_FOUND OFF) + set(HAVE_VTK OFF) +endif(WITH_VTK AND NOT ANDROID) + + #Find PCAP -find_package(Pcap) +option(WITH_PCAP "pcap file capabilities in Velodyne HDL driver" TRUE) +if(WITH_PCAP) + find_package(Pcap) +endif(WITH_PCAP) + +# OpenGL and GLUT +include("${PCL_SOURCE_DIR}/cmake/pcl_find_gl.cmake") ### ---[ Create the config.h file set(pcl_config_h_in "${CMAKE_CURRENT_SOURCE_DIR}/pcl_config.h.in") set(pcl_config_h "${CMAKE_CURRENT_BINARY_DIR}/include/pcl/pcl_config.h") -configure_file(${pcl_config_h_in} ${pcl_config_h}) -PCL_ADD_INCLUDES(common "" ${pcl_config_h}) -include_directories(${CMAKE_CURRENT_BINARY_DIR}/include) +configure_file("${pcl_config_h_in}" "${pcl_config_h}") +PCL_ADD_INCLUDES(common "" "${pcl_config_h}") +include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") ### ---[ Set up for tests enable_testing() ### ---[ Set up for examples -#include(${PCL_SOURCE_DIR}/cmake/pcl_examples.cmake) +#include("${PCL_SOURCE_DIR}/cmake/pcl_examples.cmake") ### ---[ Add the libraries subdirectories -include(${PCL_SOURCE_DIR}/cmake/pcl_targets.cmake) +include("${PCL_SOURCE_DIR}/cmake/pcl_targets.cmake") -collect_subproject_directory_names(${PCL_SOURCE_DIR} "CMakeLists.txt" PCL_MODULES_NAMES PCL_MODULES_DIRS doc) +collect_subproject_directory_names("${PCL_SOURCE_DIR}" "CMakeLists.txt" PCL_MODULES_NAMES PCL_MODULES_DIRS doc) set(PCL_MODULES_NAMES_UNSORTED ${PCL_MODULES_NAMES}) topological_sort(PCL_MODULES_NAMES PCL_ _DEPENDS) sort_relative(PCL_MODULES_NAMES_UNSORTED PCL_MODULES_NAMES PCL_MODULES_DIRS) foreach(subdir ${PCL_MODULES_DIRS}) - add_subdirectory(${PCL_SOURCE_DIR}/${subdir}) + add_subdirectory("${PCL_SOURCE_DIR}/${subdir}") endforeach(subdir) ### ---[ Documentation add_subdirectory(doc) ### ---[ Configure PCLConfig.cmake -include(${PCL_SOURCE_DIR}/cmake/pcl_pclconfig.cmake) +include("${PCL_SOURCE_DIR}/cmake/pcl_pclconfig.cmake") ### ---[ Package creation -include(${PCL_SOURCE_DIR}/cmake/pcl_all_in_one_installer.cmake) -include(${PCL_SOURCE_DIR}/cmake/pcl_cpack.cmake) +include("${PCL_SOURCE_DIR}/cmake/pcl_all_in_one_installer.cmake") +include("${PCL_SOURCE_DIR}/cmake/pcl_cpack.cmake") if(CPACK_GENERATOR) message(STATUS "Found CPack generators: ${CPACK_GENERATOR}") @@ -364,7 +429,7 @@ if(CPACK_GENERATOR) include(CPack) endif(CPACK_GENERATOR) ### ---[ Make a pretty picture of the dependency graph -include(${PCL_SOURCE_DIR}/cmake/dep_graph.cmake) +include("${PCL_SOURCE_DIR}/cmake/dep_graph.cmake") MAKE_DEP_GRAPH() ### ---[ Finish up diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..46cba519 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,147 @@ +# Contributing to PCL + +Please take a moment to review this document in order to make the contribution +process easy and effective for everyone involved. + +Following these guidelines helps to communicate that you respect the time of +the developers managing and developing this open source project. In return, +they should reciprocate that respect in addressing your issue or assessing +patches and features. + + +## Using the issue tracker + +The [issue tracker](https://github.com/PointCloudLibrary/pcl/issues) is +the preferred channel for submitting [pull requests](#pull-requests) and +[bug reports](#bugs), but please respect the following +restrictions: + +* Please **do not** use the issue tracker for personal support requests (use + [mailing list](http://www.pcl-users.org/)). + +* Please **do not** derail or troll issues. Keep the discussion on topic and + respect the opinions of others. + + + +## Pull requests + +Good pull requests - patches, improvements, new features - are a fantastic +help. They should remain focused in scope and avoid containing unrelated +commits. + +**Please ask first** before embarking on any significant pull request (e.g. +implementing features, refactoring code), otherwise you risk spending a lot of +time working on something that the project's developers might not want to merge +into the project. Please read the [tutorial on writing a new PCL class](http://pointclouds.org/documentation/tutorials/writing_new_classes.php#writing-new-classes) if you want to contribute a +brand new feature. + +If you are new to Git, GitHub, or contributing to an open-source project, you +may want to consult the [step-by-step guide on preparing and submitting a pull request](https://github.com/PointCloudLibrary/pcl/wiki/A-step-by-step-guide-on-preparing-and-submitting-a-pull-request). + + + +### Checklist + +Please use the following checklist to make sure that your contribution is well +prepared for merging into PCL: + +1. Source code adheres to the coding conventions described in [PCL Style Guide](http://pointclouds.org/documentation/advanced/pcl_style_guide.php). + But if you modify existing code, do not change/fix style in the lines that + are not related to your contribution. + +2. Commit history is tidy (no merge commits, commits are [squashed](http://davidwalsh.name/squash-commits-git) + into logical units). + +3. Each contributed file has a [license](#license) text on top. + + + +## Bug reports + +A bug is a _demonstrable problem_ that is caused by the code in the repository. +Good bug reports are extremely helpful - thank you! + +Guidelines for bug reports: + +1. **Check if the issue has been reported** — use GitHub issue search and + mailing list archive search. + +2. **Check if the issue has been fixed** — try to reproduce it using the + latest `master` branch in the repository. + +3. **Isolate the problem** — ideally create a reduced test + case. + +A good bug report shouldn't leave others needing to chase you up for more +information. Please try to be as detailed as possible in your report. What is +your environment? What steps will reproduce the issue? What would you expect to +be the outcome? All these details will help people to fix any potential bugs. + +Example: + +> Short and descriptive example bug report title +> +> A summary of the issue and the OS environment in which it occurs. If +> suitable, include the steps required to reproduce the bug. +> +> 1. This is the first step +> 2. This is the second step +> 3. Further steps, etc. +> +> Any other information you want to share that is relevant to the issue being +> reported. This might include the lines of code that you have identified as +> causing the bug, and potential solutions (and your opinions on their +> merits). + + + +## License + +PCL is 100% [BSD licensed](LICENSE.txt), and by submitting a patch, you agree to +allow Open Perception, Inc. to license your work under the terms of the BSD +License. The corpus of the license should be inserted as a C++ comment on top +of each `.h` and `.cpp` file: + +```cpp +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +``` + +Please note that if the academic institution or company you are affiliated with +does not allow to give up the rights, you may insert an additional copyright +line. diff --git a/PCLConfig.cmake.in b/PCLConfig.cmake.in index 5076f626..65ea6881 100644 --- a/PCLConfig.cmake.in +++ b/PCLConfig.cmake.in @@ -80,8 +80,9 @@ macro(find_boost) endif(PCL_ALL_IN_ONE_INSTALLER) # use static Boost in Windows if(WIN32) - set(Boost_USE_STATIC_LIBS ON) - set(Boost_USE_STATIC ON) + set(Boost_USE_STATIC_LIBS @Boost_USE_STATIC_LIBS@) + set(Boost_USE_STATIC @Boost_USE_STATIC@) + set(Boost_USE_MULTITHREAD @Boost_USE_MULTITHREAD@) endif(WIN32) if(${CMAKE_VERSION} VERSION_LESS 2.8.5) SET(Boost_ADDITIONAL_VERSIONS "1.43" "1.43.0" "1.44" "1.44.0" "1.45" "1.45.0" "1.46.1" "1.46.0" "1.46" "1.47" "1.47.0") @@ -139,14 +140,13 @@ macro(find_qhull) # Most likely we are on windows so prefer static libraries over shared ones (Mourad's recommend) find_library(QHULL_LIBRARY - NAMES qhullstatic qhull qhull${QHULL_MAJOR_VERSION} + NAMES "@QHULL_LIBRARY_NAME@" HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" PATHS "$ENV{PROGRAMFILES}/qhull" "$ENV{PROGRAMW6432}/qhull" PATH_SUFFIXES project build bin lib) find_library(QHULL_LIBRARY_DEBUG - NAMES qhullstatic_d qhull_d qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION} - qhull qhull${QHULL_MAJOR_VERSION} + NAMES "@QHULL_LIBRARY_DEBUG_NAME@" HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" PATHS "$ENV{PROGRAMFILES}/qhull" "$ENV{PROGRAMW6432}/qhull" PATH_SUFFIXES project build bin lib) @@ -210,6 +210,43 @@ macro(find_openni) endif(OPENNI_FOUND) endmacro(find_openni) +#remove this as soon as openni2-dev is shipped with FindOpenni2.cmake +macro(find_openni2) + if(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON")) + get_filename_component(OPENNI2_LIBRARY_HINT "OPENNI_LIBRARY-NOTFOUND" PATH) + endif(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON")) + + set(OPENNI2_SUFFIX) + if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + set(OPENNI2_SUFFIX 64) + endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + + if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_OPENNI2 openni2-dev) + endif(PKG_CONFIG_FOUND) + + find_path(OPENNI2_INCLUDE_DIRS OpenNI.h + HINTS /usr/include/openni2 /usr/include/ni2 + PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" + PATH_SUFFIXES openni openni2 include Include) + + find_library(OPENNI2_LIBRARY + NAMES OpenNI2 # No suffix needed on Win64 + HINTS /usr/lib + PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" + PATH_SUFFIXES lib Lib Lib64) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) + + if(OPENNI2_FOUND) + get_filename_component(OPENNI_LIBRARY_PATH ${OPENNI2_LIBRARY} PATH) + set(OPENNI2_LIBRARY_DIRS ${OPENNI2_LIBRARY_PATH}) + set(OPENNI2_LIBRARIES "${OPENNI2_LIBRARY}") + set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) + endif(OPENNI2_FOUND) +endmacro(find_openni2) + #remove this as soon as flann is shipped with FindFlann.cmake macro(find_flann) if(PCL_ALL_IN_ONE_INSTALLER) @@ -260,15 +297,17 @@ macro(find_flann) endmacro(find_flann) macro(find_VTK) - if(PCL_ALL_IN_ONE_INSTALLER) + if(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID) set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-5.8") - elseif(NOT VTK_DIR) + elseif(NOT VTK_DIR AND NOT ANDROID) set(VTK_DIR "@VTK_DIR@") - endif(PCL_ALL_IN_ONE_INSTALLER) - find_package(VTK ${QUIET_}) - if (VTK_FOUND AND NOT ANDROID) - set(VTK_LIBRARIES vtkCommon vtkRendering vtkHybrid vtkCharts) - endif(VTK_FOUND AND NOT ANDROID) + endif(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID) + if(NOT ANDROID) + find_package(VTK ${QUIET_}) + if (VTK_FOUND) + set(VTK_LIBRARIES "@VTK_LIBRARIES@") + endif(VTK_FOUND) + endif() endmacro(find_VTK) macro(find_libusb) @@ -401,6 +440,8 @@ macro(find_external_library _component _lib _is_optional) find_qhull() elseif("${_lib}" STREQUAL "openni") find_openni() + elseif("${_lib}" STREQUAL "openni2") + find_openni2() elseif("${_lib}" STREQUAL "vtk") find_VTK() elseif("${_lib}" STREQUAL "libusb-1.0") @@ -423,9 +464,9 @@ macro(find_external_library _component _lib _is_optional) if(${LIB}_LIBRARIES) list(APPEND PCL_${COMPONENT}_LIBRARIES "${${LIB}_LIBRARIES}") endif(${LIB}_LIBRARIES) - if(${LIB}_DEFINITIONS) + if(${LIB}_DEFINITIONS AND NOT ${LIB} STREQUAL "VTK") list(APPEND PCL_${COMPONENT}_DEFINITIONS ${${LIB}_DEFINITIONS}) - endif(${LIB}_DEFINITIONS) + endif(${LIB}_DEFINITIONS AND NOT ${LIB} STREQUAL "VTK") else(${LIB}_FOUND) if("${_is_optional}" STREQUAL "OPTIONAL") add_definitions("-DDISABLE_${LIB}") @@ -503,6 +544,14 @@ if(EXISTS "${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}/pcl if(EXISTS "${PCL_ROOT}/3rdParty") set(PCL_ALL_IN_ONE_INSTALLER ON) endif(EXISTS "${PCL_ROOT}/3rdParty") +elseif(EXISTS "${PCL_ROOT}/include/pcl/pcl_config.h") + # Found a non-standard (likely ANDROID) PCL installation + # pcl_message("Found a PCL installation") + set(PCL_INCLUDE_DIRS "${PCL_ROOT}/include") + set(PCL_LIBRARY_DIRS "${PCL_ROOT}/lib") + if(EXISTS "${PCL_ROOT}/3rdParty") + set(PCL_ALL_IN_ONE_INSTALLER ON) + endif(EXISTS "${PCL_ROOT}/3rdParty") elseif(EXISTS "${PCL_DIR}/include/pcl/pcl_config.h") # Found PCLConfig.cmake in a build tree of PCL # pcl_message("PCL found into a build tree.") @@ -528,7 +577,7 @@ list(LENGTH pcl_all_components PCL_NB_COMPONENTS) @PCLCONFIG_OPTIONAL_DEPENDENCIES@ -set(pcl_header_only_components geometry modeler in_hand_scanner) +set(pcl_header_only_components geometry modeler in_hand_scanner point_cloud_editor cloud_composer optronic_viewer) include(FindPackageHandleStandardArgs) @@ -603,6 +652,12 @@ foreach(component ${PCL_TO_FIND_COMPONENTS}) PATH) endif(PCL_${COMPONENT}_LIBRARY_DEBUG) + # Restrict this to Windows users + if(NOT PCL_${COMPONENT}_LIBRARY AND WIN32) + # might be debug only + set(PCL_${COMPONENT}_LIBRARY ${PCL_${COMPONENT}_LIBRARY_DEBUG}) + endif(NOT PCL_${COMPONENT}_LIBRARY AND WIN32) + find_package_handle_standard_args(PCL_${COMPONENT} DEFAULT_MSG PCL_${COMPONENT}_LIBRARY PCL_${COMPONENT}_INCLUDE_DIR) else(_is_header_only EQUAL -1) @@ -649,15 +704,6 @@ if(NOT "${PCL_LIBRARY_DIRS}" STREQUAL "") list(REMOVE_DUPLICATES PCL_LIBRARY_DIRS) endif(NOT "${PCL_LIBRARY_DIRS}" STREQUAL "") -# We need to export march=native for tutorials or user code -if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Clang") -list (APPEND PCL_DEFINITIONS "@SSE_FLAGS@") -endif() - -if(NOT MSVC) -list (APPEND PCL_DEFINITIONS "-Wno-invalid-offsetof") -endif(NOT MSVC) - if(NOT "${PCL_DEFINITIONS}" STREQUAL "") list(REMOVE_DUPLICATES PCL_DEFINITIONS) endif(NOT "${PCL_DEFINITIONS}" STREQUAL "") @@ -665,7 +711,7 @@ endif(NOT "${PCL_DEFINITIONS}" STREQUAL "") pcl_remove_duplicate_libraries(PCL_LIBRARIES PCL_DEDUP_LIBRARIES) set(PCL_LIBRARIES ${PCL_DEDUP_LIBRARIES}) # Add 3rd party libraries, as user code might include our .HPP implementations -list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES}) +list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES}) find_package_handle_standard_args(PCL DEFAULT_MSG PCL_LIBRARIES PCL_INCLUDE_DIRS) mark_as_advanced(PCL_LIBRARIES PCL_INCLUDE_DIRS PCL_LIBRARY_DIRS) diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index cf80bbc9..93b11e97 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -9,7 +9,7 @@ if(NOT VTK_FOUND) else(NOT VTK_FOUND) set(DEFAULT TRUE) set(REASON) - include (${VTK_USE_FILE}) + include("${VTK_USE_FILE}") endif(NOT VTK_FOUND) # OpenNI found? @@ -22,140 +22,143 @@ else(NOT OPENNI_FOUND) endif(NOT OPENNI_FOUND) set(DEFAULT FALSE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS openni vtk) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni vtk) if(build) - include_directories (${CMAKE_CURRENT_BINARY_DIR}) - include_directories (${CMAKE_CURRENT_SOURCE_DIR}/include) + include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") - PCL_ADD_EXECUTABLE(pcl_test_search_speed ${SUBSYS_NAME} src/test_search.cpp) + PCL_ADD_EXECUTABLE(pcl_test_search_speed "${SUBSYS_NAME}" src/test_search.cpp) target_link_libraries(pcl_test_search_speed pcl_common pcl_io pcl_search pcl_kdtree pcl_visualization) - PCL_ADD_EXECUTABLE(pcl_nn_classification_example ${SUBSYS_NAME} src/nn_classification_example.cpp) + PCL_ADD_EXECUTABLE(pcl_nn_classification_example "${SUBSYS_NAME}" src/nn_classification_example.cpp) target_link_libraries(pcl_nn_classification_example pcl_common pcl_io pcl_features pcl_kdtree) - PCL_ADD_EXECUTABLE(pcl_pyramid_surface_matching ${SUBSYS_NAME} src/pyramid_surface_matching.cpp) + PCL_ADD_EXECUTABLE(pcl_pyramid_surface_matching "${SUBSYS_NAME}" src/pyramid_surface_matching.cpp) target_link_libraries(pcl_pyramid_surface_matching pcl_common pcl_io pcl_features pcl_registration pcl_filters) - PCL_ADD_EXECUTABLE(pcl_statistical_multiscale_interest_region_extraction_example ${SUBSYS_NAME} src/statistical_multiscale_interest_region_extraction_example.cpp) + PCL_ADD_EXECUTABLE(pcl_statistical_multiscale_interest_region_extraction_example "${SUBSYS_NAME}" src/statistical_multiscale_interest_region_extraction_example.cpp) target_link_libraries(pcl_statistical_multiscale_interest_region_extraction_example pcl_common pcl_io pcl_features pcl_filters) if(LIBUSB_1_FOUND) - PCL_ADD_EXECUTABLE(pcl_dinast_grabber ${SUBSYS_NAME} src/dinast_grabber_example.cpp) + PCL_ADD_EXECUTABLE(pcl_dinast_grabber "${SUBSYS_NAME}" src/dinast_grabber_example.cpp) target_link_libraries(pcl_dinast_grabber pcl_common pcl_visualization pcl_io) endif(LIBUSB_1_FOUND) if (VTK_FOUND) - PCL_ADD_EXECUTABLE(pcl_ppf_object_recognition ${SUBSYS_NAME} src/ppf_object_recognition.cpp) + PCL_ADD_EXECUTABLE(pcl_ppf_object_recognition "${SUBSYS_NAME}" src/ppf_object_recognition.cpp) target_link_libraries(pcl_ppf_object_recognition pcl_common pcl_io pcl_filters pcl_features pcl_registration pcl_visualization pcl_sample_consensus pcl_segmentation) - PCL_ADD_EXECUTABLE(pcl_multiscale_feature_persistence_example ${SUBSYS_NAME} src/multiscale_feature_persistence_example.cpp) + PCL_ADD_EXECUTABLE(pcl_multiscale_feature_persistence_example "${SUBSYS_NAME}" src/multiscale_feature_persistence_example.cpp) target_link_libraries(pcl_multiscale_feature_persistence_example pcl_common pcl_io pcl_filters pcl_features pcl_visualization) - PCL_ADD_EXECUTABLE(pcl_surfel_smoothing_test ${SUBSYS_NAME} src/surfel_smoothing_test.cpp) + PCL_ADD_EXECUTABLE(pcl_surfel_smoothing_test "${SUBSYS_NAME}" src/surfel_smoothing_test.cpp) target_link_libraries(pcl_surfel_smoothing_test pcl_common pcl_io pcl_surface pcl_filters pcl_features pcl_visualization) - PCL_ADD_EXECUTABLE(pcl_feature_matching ${SUBSYS_NAME} src/feature_matching.cpp) + PCL_ADD_EXECUTABLE(pcl_feature_matching "${SUBSYS_NAME}" src/feature_matching.cpp) target_link_libraries(pcl_feature_matching pcl_common pcl_io pcl_registration pcl_keypoints pcl_sample_consensus pcl_visualization pcl_search pcl_features pcl_kdtree pcl_surface pcl_segmentation) - PCL_ADD_EXECUTABLE(pcl_convolve ${SUBSYS_NAME} src/convolve.cpp) + PCL_ADD_EXECUTABLE(pcl_convolve "${SUBSYS_NAME}" src/convolve.cpp) target_link_libraries(pcl_convolve pcl_common pcl_io pcl_visualization) - PCL_ADD_EXECUTABLE(pcl_pcd_organized_multi_plane_segmentation ${SUBSYS_NAME} src/pcd_organized_multi_plane_segmentation.cpp) + PCL_ADD_EXECUTABLE(pcl_pcd_organized_multi_plane_segmentation "${SUBSYS_NAME}" src/pcd_organized_multi_plane_segmentation.cpp) target_link_libraries(pcl_pcd_organized_multi_plane_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features) if (QHULL_FOUND) - PCL_ADD_EXECUTABLE(pcl_pcd_select_object_plane ${SUBSYS_NAME} src/pcd_select_object_plane.cpp) + PCL_ADD_EXECUTABLE(pcl_pcd_select_object_plane "${SUBSYS_NAME}" src/pcd_select_object_plane.cpp) target_link_libraries(pcl_pcd_select_object_plane pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features pcl_surface) endif() -# PCL_ADD_EXECUTABLE(pcl_convolve ${SUBSYS_NAME} src/convolve.cpp) +# PCL_ADD_EXECUTABLE(pcl_convolve "${SUBSYS_NAME}" src/convolve.cpp) # target_link_libraries(pcl_convolve pcl_common pcl_io pcl_visualization) + if (QT4_FOUND AND VTK_USE_QVTK) + + # Manual registration demo + QT4_WRAP_UI(manual_registration_ui src/manual_registration/manual_registration.ui) + QT4_WRAP_CPP(manual_registration_moc include/pcl/apps/manual_registration.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_manual_registration "${SUBSYS_NAME}" ${manual_registration_ui} ${manual_registration_moc} src/manual_registration/manual_registration.cpp) + target_link_libraries(pcl_manual_registration pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface ${QVTK_LIBRARY} ${QT_LIBRARIES}) + + QT4_WRAP_UI(pcd_video_player_ui src/pcd_video_player/pcd_video_player.ui) + QT4_WRAP_CPP(pcd_video_player_moc include/pcl/apps/pcd_video_player.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_pcd_video_player "${SUBSYS_NAME}" ${pcd_video_player_ui} ${pcd_video_player_moc} src/pcd_video_player/pcd_video_player.cpp) + target_link_libraries(pcl_pcd_video_player pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface ${QVTK_LIBRARY} ${QT_LIBRARIES}) + + endif (QT4_FOUND AND VTK_USE_QVTK) + if (OPENNI_FOUND AND BUILD_OPENNI) -# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_grab_frame ${SUBSYS_NAME} src/openni_grab_frame.cpp) +# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_grab_frame "${SUBSYS_NAME}" src/openni_grab_frame.cpp) # target_link_libraries(pcl_openni_grab_frame pcl_common pcl_io pcl_visualization) -# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_grab_images ${SUBSYS_NAME} src/openni_grab_images.cpp) +# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_grab_images "${SUBSYS_NAME}" src/openni_grab_images.cpp) # target_link_libraries(pcl_openni_grab_images pcl_common pcl_io pcl_visualization) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_fast_mesh ${SUBSYS_NAME} src/openni_fast_mesh.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_fast_mesh "${SUBSYS_NAME}" src/openni_fast_mesh.cpp) target_link_libraries(pcl_openni_fast_mesh pcl_common pcl_io pcl_visualization pcl_surface) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_voxel_grid ${SUBSYS_NAME} src/openni_voxel_grid.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_voxel_grid "${SUBSYS_NAME}" src/openni_voxel_grid.cpp) target_link_libraries(pcl_openni_voxel_grid pcl_common pcl_io pcl_filters pcl_visualization) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_octree_compression ${SUBSYS_NAME} src/openni_octree_compression.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_octree_compression "${SUBSYS_NAME}" src/openni_octree_compression.cpp) target_link_libraries(pcl_openni_octree_compression pcl_common pcl_io pcl_filters pcl_visualization pcl_octree) if(HAVE_PNG) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_organized_compression ${SUBSYS_NAME} src/openni_organized_compression.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_organized_compression "${SUBSYS_NAME}" src/openni_organized_compression.cpp) target_link_libraries(pcl_openni_organized_compression pcl_common pcl_io pcl_filters pcl_visualization pcl_octree) endif(HAVE_PNG) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_shift_to_depth_conversion ${SUBSYS_NAME} src/openni_shift_to_depth_conversion.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_shift_to_depth_conversion "${SUBSYS_NAME}" src/openni_shift_to_depth_conversion.cpp) target_link_libraries(pcl_openni_shift_to_depth_conversion pcl_common pcl_visualization) - PCL_ADD_EXECUTABLE(pcl_openni_mobile_server ${SUBSYS_NAME} src/openni_mobile_server.cpp) + PCL_ADD_EXECUTABLE(pcl_openni_mobile_server "${SUBSYS_NAME}" src/openni_mobile_server.cpp) target_link_libraries(pcl_openni_mobile_server pcl_common pcl_io pcl_filters pcl_visualization) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_planar_segmentation ${SUBSYS_NAME} src/openni_planar_segmentation.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_planar_segmentation "${SUBSYS_NAME}" src/openni_planar_segmentation.cpp) target_link_libraries(pcl_openni_planar_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_organized_multi_plane_segmentation ${SUBSYS_NAME} src/openni_organized_multi_plane_segmentation.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_organized_multi_plane_segmentation "${SUBSYS_NAME}" src/openni_organized_multi_plane_segmentation.cpp) target_link_libraries(pcl_openni_organized_multi_plane_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_ii_normal_estimation ${SUBSYS_NAME} src/openni_ii_normal_estimation.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_ii_normal_estimation "${SUBSYS_NAME}" src/openni_ii_normal_estimation.cpp) target_link_libraries(pcl_openni_ii_normal_estimation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_feature_persistence ${SUBSYS_NAME} src/openni_feature_persistence.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_feature_persistence "${SUBSYS_NAME}" src/openni_feature_persistence.cpp) target_link_libraries(pcl_openni_feature_persistence pcl_common pcl_io pcl_filters pcl_visualization pcl_features) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_mls_smoothing ${SUBSYS_NAME} src/openni_mls_smoothing.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_mls_smoothing "${SUBSYS_NAME}" src/openni_mls_smoothing.cpp) target_link_libraries(pcl_openni_mls_smoothing pcl_common pcl_io pcl_surface pcl_visualization) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_change_viewer ${SUBSYS_NAME} src/openni_change_viewer.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_change_viewer "${SUBSYS_NAME}" src/openni_change_viewer.cpp) target_link_libraries(pcl_openni_change_viewer pcl_common pcl_io pcl_kdtree pcl_octree pcl_visualization pcl_filters) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_uniform_sampling ${SUBSYS_NAME} src/openni_uniform_sampling.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_uniform_sampling "${SUBSYS_NAME}" src/openni_uniform_sampling.cpp) target_link_libraries(pcl_openni_uniform_sampling pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface pcl_keypoints) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_boundary_estimation ${SUBSYS_NAME} src/openni_boundary_estimation.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_boundary_estimation "${SUBSYS_NAME}" src/openni_boundary_estimation.cpp) target_link_libraries(pcl_openni_boundary_estimation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface) if (QT4_FOUND AND VTK_USE_QVTK) # OpenNI Passthrough application demo QT4_WRAP_UI(openni_passthrough_ui src/openni_passthrough.ui) QT4_WRAP_CPP(openni_passthrough_moc include/pcl/apps/openni_passthrough.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - PCL_ADD_EXECUTABLE(pcl_openni_passthrough ${SUBSYS_NAME} ${openni_passthrough_ui} ${openni_passthrough_moc} src/openni_passthrough.cpp) - target_link_libraries(pcl_openni_passthrough pcl_common pcl_io pcl_filters pcl_visualization QVTK ${QT_LIBRARIES}) + PCL_ADD_EXECUTABLE(pcl_openni_passthrough "${SUBSYS_NAME}" ${openni_passthrough_ui} ${openni_passthrough_moc} src/openni_passthrough.cpp) + target_link_libraries(pcl_openni_passthrough pcl_common pcl_io pcl_filters pcl_visualization ${QVTK_LIBRARY} ${QT_LIBRARIES}) # OpenNI Organized Connected Component application demo QT4_WRAP_UI(organized_segmentation_demo_ui src/organized_segmentation_demo.ui) QT4_WRAP_CPP(organized_segmentation_demo_moc include/pcl/apps/organized_segmentation_demo.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_organized_segmentation_demo ${SUBSYS_NAME} ${organized_segmentation_demo_ui} ${organized_segmentation_demo_moc} src/organized_segmentation_demo.cpp) - target_link_libraries(pcl_organized_segmentation_demo pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) - - # Manual registration demo - QT4_WRAP_UI(manual_registration_ui src/manual_registration/manual_registration.ui) - QT4_WRAP_CPP(manual_registration_moc include/pcl/apps/manual_registration.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_manual_registration ${SUBSYS_NAME} ${manual_registration_ui} ${manual_registration_moc} src/manual_registration/manual_registration.cpp) - target_link_libraries(pcl_manual_registration pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) - - QT4_WRAP_UI(pcd_video_player_ui src/pcd_video_player/pcd_video_player.ui) - QT4_WRAP_CPP(pcd_video_player_moc include/pcl/apps/pcd_video_player.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_pcd_video_player ${SUBSYS_NAME} ${pcd_video_player_ui} ${pcd_video_player_moc} src/pcd_video_player/pcd_video_player.cpp) - target_link_libraries(pcl_pcd_video_player pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_organized_segmentation_demo "${SUBSYS_NAME}" ${organized_segmentation_demo_ui} ${organized_segmentation_demo_moc} src/organized_segmentation_demo.cpp) + target_link_libraries(pcl_organized_segmentation_demo pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface ${QVTK_LIBRARY} ${QT_LIBRARIES}) # Database processing (integration) demo # QT4_WRAP_UI(db_proc_ui src/db_proc/db_proc.ui) # QT4_WRAP_CPP(db_proc_moc include/pcl/apps/db_proc.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED) -# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_db_proc ${SUBSYS_NAME} ${db_proc_ui} ${db_proc_moc} src/db_proc/db_proc.cpp) -# target_link_libraries(pcl_db_proc pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) +# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_db_proc "${SUBSYS_NAME}" ${db_proc_ui} ${db_proc_moc} src/db_proc/db_proc.cpp) +# target_link_libraries(pcl_db_proc pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface ${QVTK_LIBRARY} ${QT_LIBRARIES}) endif () @@ -165,58 +168,67 @@ if(build) set(srcs src/render_views_tesselated_sphere.cpp) if (QHULL_FOUND) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_3d_convex_hull ${SUBSYS_NAME} src/openni_3d_convex_hull.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_3d_convex_hull "${SUBSYS_NAME}" src/openni_3d_convex_hull.cpp) target_link_libraries(pcl_openni_3d_convex_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_3d_concave_hull ${SUBSYS_NAME} src/openni_3d_concave_hull.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_3d_concave_hull "${SUBSYS_NAME}" src/openni_3d_concave_hull.cpp) target_link_libraries(pcl_openni_3d_concave_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_tracking ${SUBSYS_NAME} src/openni_tracking.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_tracking "${SUBSYS_NAME}" src/openni_tracking.cpp) target_link_libraries(pcl_openni_tracking pcl_common pcl_io pcl_surface pcl_visualization pcl_filters pcl_features pcl_segmentation pcl_tracking pcl_search) - set(incs include/pcl/${SUBSYS_NAME}/dominant_plane_segmentation.h ${incs}) - set(impl_incs include/pcl/${SUBSYS_NAME}/impl/dominant_plane_segmentation.hpp) + set(incs "include/pcl/${SUBSYS_NAME}/dominant_plane_segmentation.h" ${incs}) + set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/dominant_plane_segmentation.hpp") set(srcs src/dominant_plane_segmentation.cpp ${srcs}) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_planar_convex_hull ${SUBSYS_NAME} src/openni_planar_convex_hull.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_planar_convex_hull "${SUBSYS_NAME}" src/openni_planar_convex_hull.cpp) target_link_libraries(pcl_openni_planar_convex_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_linemod "${SUBSYS_NAME}" src/ni_linemod.cpp) + target_link_libraries(pcl_ni_linemod pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface pcl_search) + endif() # QHULL_FOUND # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) - set(LIB_NAME pcl_${SUBSYS_NAME}) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${impl_incs} ${incs}) - target_link_libraries(${LIB_NAME} pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search) + set(LIB_NAME "pcl_${SUBSYS_NAME}") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${impl_incs} ${incs}) + target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" "" "" "") + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "") - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_agast ${SUBSYS_NAME} src/ni_agast.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_agast "${SUBSYS_NAME}" src/ni_agast.cpp) target_link_libraries(pcl_ni_agast pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_keypoints pcl_surface pcl_search) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_linemod ${SUBSYS_NAME} src/ni_linemod.cpp) - target_link_libraries(pcl_ni_linemod pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface pcl_search) - - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_susan ${SUBSYS_NAME} src/ni_susan.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_susan "${SUBSYS_NAME}" src/ni_susan.cpp) target_link_libraries(pcl_ni_susan pcl_common pcl_visualization pcl_features pcl_keypoints pcl_search) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_trajkovic ${SUBSYS_NAME} src/ni_trajkovic.cpp) + target_link_libraries(pcl_ni_trajkovic pcl_common pcl_visualization pcl_features pcl_keypoints pcl_search) + + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_klt "${SUBSYS_NAME}" src/openni_klt.cpp) + target_link_libraries(pcl_openni_klt pcl_common pcl_io pcl_visualization pcl_tracking) endif() # OPENNI_FOUND + BUILD_OPENNI endif() # VTK_FOUND - add_subdirectory(modeler) - add_subdirectory(cloud_composer) - if(OPENNI_FOUND) - add_subdirectory(in_hand_scanner) - endif(OPENNI_FOUND) - add_subdirectory(point_cloud_editor) - if(FZAPI_FOUND) - add_subdirectory(optronic_viewer) - endif(FZAPI_FOUND) + # OpenGL and GLUT + if(OPENGL_FOUND AND GLUT_FOUND) + include_directories("${OPENGL_INCLUDE_DIR}") + include_directories("${GLUT_INCLUDE_DIR}") + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_grabcut_2d "${SUBSYS_NAME}" src/grabcut_2d.cpp) + target_link_libraries (pcl_grabcut_2d pcl_common pcl_io pcl_segmentation pcl_search ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES}) + endif(OPENGL_FOUND AND GLUT_FOUND) + + collect_subproject_directory_names("${CMAKE_CURRENT_SOURCE_DIR}" "CMakeLists.txt" PCL_APPS_MODULES_NAMES PCL_APPS_MODULES_DIRS ${SUBSYS_NAME}) + set(PCL_APPS_MODULES_NAMES_UNSORTED ${PCL_APPS_MODULES_NAMES}) + topological_sort(PCL_APPS_MODULES_NAMES PCL_APPS_ _DEPENDS) + sort_relative(PCL_APPS_MODULES_NAMES_UNSORTED PCL_APPS_MODULES_NAMES PCL_APPS_MODULES_DIRS) + foreach(subdir ${PCL_APPS_MODULES_DIRS}) + add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}") + endforeach(subdir) endif(build) - - diff --git a/apps/cloud_composer/CMakeLists.txt b/apps/cloud_composer/CMakeLists.txt index 009fd98a..d4449752 100644 --- a/apps/cloud_composer/CMakeLists.txt +++ b/apps/cloud_composer/CMakeLists.txt @@ -3,69 +3,69 @@ # string(REGEX REPLACE "-Wconversion(.+)" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") #endif() -set(SUBSYS_NAME cloud_composer) -set(SUBSYS_DESC "Cloud Composer - Application for Manipulating Point Clouds") -set(SUBSYS_DEPS common io visualization filters apps) +SET(SUBSUBSYS_NAME cloud_composer) +SET(SUBSUBSYS_DESC "Cloud Composer - Application for Manipulating Point Clouds") +SET(SUBSUBSYS_DEPS common io visualization filters apps) # Find VTK if(NOT VTK_FOUND) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "VTK was not found.") else(NOT VTK_FOUND) set(DEFAULT TRUE) set(REASON) - include (${VTK_USE_FILE}) + include("${VTK_USE_FILE}") endif(NOT VTK_FOUND) # QT4 Found? if(NOT QT4_FOUND) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "Qt4 was not found.") -else(NOT QT4_FOUND) +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") set(DEFAULT TRUE) set(REASON) endif(NOT QT4_FOUND) # QVTK? if(NOT VTK_USE_QVTK) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "Cloud composer requires QVTK") -else(NOT VTK_USE_QVTK) +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") set(DEFAULT TRUE) set(REASON) endif(NOT VTK_USE_QVTK) #Default to not building for now -set(DEFAULT FALSE) +if ("${DEFAULT}" STREQUAL "TRUE") + set(DEFAULT FALSE) +endif ("${DEFAULT}" STREQUAL "TRUE") -PCL_SUBSYS_OPTION(build app_${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -PCL_SUBSYS_DEPEND(build app_${SUBSYS_NAME} ${SUBSYS_DEPS}) +PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC(${SUBSUBSYS_NAME}) if(build) - include_directories (${CMAKE_CURRENT_BINARY_DIR}) - include_directories (${CMAKE_CURRENT_SOURCE_DIR}/include) + include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") - if (VTK_FOUND AND QT4_FOUND AND VTK_USE_QVTK) #Sources & Headers for main application - set(incs include/pcl/apps/${SUBSYS_NAME}/qt.h - include/pcl/apps/${SUBSYS_NAME}/cloud_composer.h - include/pcl/apps/${SUBSYS_NAME}/project_model.h - include/pcl/apps/${SUBSYS_NAME}/cloud_viewer.h - include/pcl/apps/${SUBSYS_NAME}/cloud_view.h - include/pcl/apps/${SUBSYS_NAME}/cloud_browser.h - include/pcl/apps/${SUBSYS_NAME}/item_inspector.h - include/pcl/apps/${SUBSYS_NAME}/tool_interface/abstract_tool.h - include/pcl/apps/${SUBSYS_NAME}/tool_interface/tool_factory.h - include/pcl/apps/${SUBSYS_NAME}/commands.h - include/pcl/apps/${SUBSYS_NAME}/work_queue.h - include/pcl/apps/${SUBSYS_NAME}/toolbox_model.h - include/pcl/apps/${SUBSYS_NAME}/properties_model.h - include/pcl/apps/${SUBSYS_NAME}/signal_multiplexer.h - include/pcl/apps/${SUBSYS_NAME}/merge_selection.h - include/pcl/apps/${SUBSYS_NAME}/transform_clouds.h) + set(incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/qt.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_composer.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/project_model.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_viewer.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_view.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_browser.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/item_inspector.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tool_interface/abstract_tool.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tool_interface/tool_factory.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/commands.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/work_queue.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/toolbox_model.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/properties_model.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/signal_multiplexer.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/merge_selection.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/transform_clouds.h") set(srcs src/main.cpp src/cloud_composer.cpp @@ -83,15 +83,15 @@ if(build) src/tool_interface/abstract_tool.cpp src/transform_clouds.cpp) - set(impl_incs include/pcl/apps/${SUBSYS_NAME}/impl/cloud_item.hpp - include/pcl/apps/${SUBSYS_NAME}/impl/merge_selection.hpp - include/pcl/apps/${SUBSYS_NAME}/impl/transform_clouds.hpp) + set(impl_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/cloud_item.hpp" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/merge_selection.hpp" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/transform_clouds.hpp") #Sources and headers for item types - set(item_incs include/pcl/apps/${SUBSYS_NAME}/items/cloud_composer_item.h - include/pcl/apps/${SUBSYS_NAME}/items/cloud_item.h - include/pcl/apps/${SUBSYS_NAME}/items/normals_item.h - include/pcl/apps/${SUBSYS_NAME}/items/fpfh_item.h) + set(item_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/items/cloud_composer_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/items/cloud_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/items/normals_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/items/fpfh_item.h") set(item_srcs src/items/cloud_composer_item.cpp src/items/cloud_item.cpp @@ -99,10 +99,10 @@ if(build) src/items/fpfh_item.cpp) #Sources and headers for point selectors - set (selector_incs include/pcl/apps/${SUBSYS_NAME}/point_selectors/interactor_style_switch.h - include/pcl/apps/${SUBSYS_NAME}/point_selectors/rectangular_frustum_selector.h - include/pcl/apps/${SUBSYS_NAME}/point_selectors/selected_trackball_interactor_style.h - include/pcl/apps/${SUBSYS_NAME}/point_selectors/click_trackball_interactor_style.h) + set (selector_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/point_selectors/interactor_style_switch.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/point_selectors/rectangular_frustum_selector.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/point_selectors/selected_trackball_interactor_style.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/point_selectors/click_trackball_interactor_style.h") set (selector_srcs src/point_selectors/interactor_style_switch.cpp src/point_selectors/selection_event.cpp @@ -118,80 +118,81 @@ if(build) QT4_WRAP_CPP(cloud_composer_moc ${incs} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) QT4_ADD_RESOURCES(resource_srcs ${resources}) - set(EXE_NAME pcl_${SUBSYS_NAME}) - PCL_ADD_EXECUTABLE(${EXE_NAME} ${SUBSYS_NAME} ${cloud_composer_ui} ${cloud_composer_moc} ${srcs} ${resource_srcs} ${item_srcs} ${selector_srcs} ${impl_incs}) - target_link_libraries(${EXE_NAME} pcl_common pcl_io pcl_visualization pcl_filters QVTK ${QT_LIBRARIES}) + set(EXE_NAME "pcl_${SUBSUBSYS_NAME}") + PCL_ADD_EXECUTABLE("${EXE_NAME}" "${SUBSUBSYS_NAME}" ${cloud_composer_ui} ${cloud_composer_moc} ${srcs} ${resource_srcs} ${item_srcs} ${selector_srcs} ${impl_incs}) + target_link_libraries("${EXE_NAME}" pcl_common pcl_io pcl_visualization pcl_filters ${QVTK_LIBRARY} ${QT_LIBRARIES}) # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs} ${item_incs} ${selector_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${incs} ${item_incs} ${selector_incs}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}/impl" ${impl_incs}) - PCL_MAKE_PKGCONFIG(${EXE_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" "" "" "") + PCL_MAKE_PKGCONFIG("${EXE_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "") - #TOOL BUILDING SCRIPTS + #TOOL buildING SCRIPTS #Create subdirectory for plugin libs - set (CLOUD_COMPOSER_PLUGIN_DIR ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cloud_composer_plugins) - make_directory (${CLOUD_COMPOSER_PLUGIN_DIR}) + set (CLOUD_COMPOSER_PLUGIN_DIR "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cloud_composer_plugins") + make_directory("${CLOUD_COMPOSER_PLUGIN_DIR}") - set(INTERFACE_HEADERS include/pcl/apps/${SUBSYS_NAME}/tool_interface/abstract_tool.h) + set(INTERFACE_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tool_interface/abstract_tool.h") set(INTERFACE_SOURCES src/tool_interface/abstract_tool.cpp) QT4_WRAP_CPP(INTERFACE_HEADERS_MOC ${INTERFACE_HEADERS} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - PCL_ADD_LIBRARY(pcl_cc_tool_interface ${SUBSYS_NAME} ${INTERFACE_SOURCES} ${INTERFACE_HEADERS_MOC}) + PCL_ADD_LIBRARY(pcl_cc_tool_interface "${SUBSUBSYS_NAME}" ${INTERFACE_SOURCES} ${INTERFACE_HEADERS_MOC}) target_link_libraries(pcl_cc_tool_interface pcl_common ${QT_LIBRARIES}) + + IF(APPLE) + SET_TARGET_PROPERTIES(pcl_cc_tool_interface PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") + ENDIF() include(ComposerTool.cmake REQUIRED) #FPFH Tool set (FPFH_DEPS pcl_features pcl_kdtree pcl_filters) set (FPFH_SOURCES tools/fpfh_estimation.cpp) - set (FPFH_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/fpfh_estimation.h) + set (FPFH_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/fpfh_estimation.h") define_composer_tool (fpfh_estimation "${FPFH_SOURCES}" "${FPFH_HEADERS}" "${FPFH_DEPS}") #Normals Tool set (NORMALS_DEPS pcl_features pcl_kdtree) set (NORMALS_SOURCES tools/normal_estimation.cpp) - set (NORMALS_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/normal_estimation.h) + set (NORMALS_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/normal_estimation.h") define_composer_tool (normal_estimation "${NORMALS_SOURCES}" "${NORMALS_HEADERS}" "${NORMALS_DEPS}") #Euclidean Clustering Tool set (EC_DEPS pcl_segmentation pcl_kdtree) set (EC_SOURCES tools/euclidean_clustering.cpp) - set (EC_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/euclidean_clustering.h) + set (EC_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/euclidean_clustering.h") define_composer_tool (euclidean_clustering "${EC_SOURCES}" "${EC_HEADERS}" "${EC_DEPS}") #Statistical Outlier Removal Tool set (SOR_DEPS pcl_filters) set (SOR_SOURCES tools/statistical_outlier_removal.cpp) - set (SOR_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/statistical_outlier_removal.h) + set (SOR_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/statistical_outlier_removal.h") define_composer_tool (statistical_outlier_removal "${SOR_SOURCES}" "${SOR_HEADERS}" "${SOR_DEPS}") #Vox Grid Downsample Tool set (VOXDS_DEPS pcl_filters) set (VOXDS_SOURCES tools/voxel_grid_downsample.cpp) - set (VOXDS_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/voxel_grid_downsample.h) + set (VOXDS_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/voxel_grid_downsample.h") define_composer_tool (voxel_grid_downsample "${VOXDS_SOURCES}" "${VOXDS_HEADERS}" "${VOXDS_DEPS}") #Organized Segmentation set (OSEG_DEPS pcl_segmentation pcl_kdtree) - set (OSEG_SOURCES tools/organized_segmentation.cpp include/pcl/apps/${SUBSYS_NAME}/tools/impl/organized_segmentation.hpp) - set (OSEG_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/organized_segmentation.h) + set (OSEG_SOURCES tools/organized_segmentation.cpp "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/impl/organized_segmentation.hpp") + set (OSEG_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/organized_segmentation.h") define_composer_tool (organized_segmentation "${OSEG_SOURCES}" "${OSEG_HEADERS}" "${OSEG_DEPS}") #Sanitize Cloud Tool set (SAN_DEPS pcl_filters) set (SAN_SOURCES tools/sanitize_cloud.cpp) - set (SAN_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/sanitize_cloud.h) + set (SAN_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/sanitize_cloud.h") define_composer_tool (sanitize_cloud "${SAN_SOURCES}" "${SAN_HEADERS}" "${SAN_DEPS}") #Supervoxels set (VSP_DEPS pcl_octree pcl_segmentation) - set (VSP_SOURCES tools/supervoxels.cpp include/pcl/apps/${SUBSYS_NAME}/tools/impl/supervoxels.hpp) - set (VSP_HEADERS include/pcl/apps/${SUBSYS_NAME}/tools/supervoxels.h) + set (VSP_SOURCES tools/supervoxels.cpp "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/impl/supervoxels.hpp") + set (VSP_HEADERS "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/supervoxels.h") define_composer_tool (supervoxels "${VSP_SOURCES}" "${VSP_HEADERS}" "${VSP_DEPS}") - - endif () #VTK_FOUND AND QT4_FOUND AND VTK_USE_QVTK - endif(build) diff --git a/apps/cloud_composer/ComposerTool.cmake b/apps/cloud_composer/ComposerTool.cmake index d55df6cb..ee37b627 100644 --- a/apps/cloud_composer/ComposerTool.cmake +++ b/apps/cloud_composer/ComposerTool.cmake @@ -21,5 +21,9 @@ function(define_composer_tool TOOL_NAME TOOL_SOURCES TOOL_HEADERS DEPS) add_dependencies(${TOOL_TARGET} pcl_cc_tool_interface ${DEPS}) target_link_libraries(${TOOL_TARGET} pcl_cc_tool_interface pcl_common pcl_io ${DEPS} ${QT_LIBRARIES}) + + IF(APPLE) + SET_TARGET_PROPERTIES(${TOOL_TARGET} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") + ENDIF() endfunction(define_composer_tool TOOL_NAME TOOL_SOURCES TOOL_HEADERS DEPS) diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h index 10d4c527..0fefa8e7 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_view.h @@ -152,3 +152,4 @@ namespace pcl Q_DECLARE_METATYPE (pcl::cloud_composer::CloudView); #endif + diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/project_model.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/project_model.h index ca436417..4b5950e2 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/project_model.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/project_model.h @@ -148,7 +148,7 @@ namespace pcl /** \brief Slot Called whenever the item selection_model_ changes */ void - itemSelectionChanged ( const QItemSelection & selected, const QItemSelection & deselected ); + itemSelectionChanged ( const QItemSelection &, const QItemSelection &); /** \brief Creates a new cloud from the selected items and points */ void diff --git a/apps/cloud_composer/src/cloud_view.cpp b/apps/cloud_composer/src/cloud_view.cpp index 4ad242b5..8e435451 100644 --- a/apps/cloud_composer/src/cloud_view.cpp +++ b/apps/cloud_composer/src/cloud_view.cpp @@ -195,7 +195,7 @@ pcl::cloud_composer::CloudView::selectedItemChanged (const QItemSelection & sele } void -pcl::cloud_composer::CloudView::dataChanged (const QModelIndex & topLeft, const QModelIndex & bottomRight) +pcl::cloud_composer::CloudView::dataChanged (const QModelIndex &, const QModelIndex &) { @@ -283,7 +283,7 @@ pcl::cloud_composer::CloudView::setInteractorStyle (interactor_styles::INTERACTO } void -pcl::cloud_composer::CloudView::selectionCompleted (vtkObject* caller, unsigned long event_id, void* client_data, void* call_data) +pcl::cloud_composer::CloudView::selectionCompleted (vtkObject*, unsigned long, void*, void* call_data) { boost::shared_ptr selected (static_cast (call_data)); @@ -297,7 +297,7 @@ pcl::cloud_composer::CloudView::selectionCompleted (vtkObject* caller, unsigned void -pcl::cloud_composer::CloudView::manipulationCompleted (vtkObject* caller, unsigned long event_id, void* client_data, void* call_data) +pcl::cloud_composer::CloudView::manipulationCompleted (vtkObject*, unsigned long, void*, void* call_data) { boost::shared_ptr manip_event (static_cast (call_data)); @@ -307,4 +307,5 @@ pcl::cloud_composer::CloudView::manipulationCompleted (vtkObject* caller, unsign model_->manipulateClouds (manip_event); } -} \ No newline at end of file +} + diff --git a/apps/cloud_composer/src/commands.cpp b/apps/cloud_composer/src/commands.cpp index 61fa24ff..ecd6d015 100644 --- a/apps/cloud_composer/src/commands.cpp +++ b/apps/cloud_composer/src/commands.cpp @@ -475,12 +475,11 @@ pcl::cloud_composer::DeleteItemCommand::DeleteItemCommand (QList output; @@ -592,4 +591,5 @@ pcl::cloud_composer::MergeCloudCommand::redo () qCritical () << "Removal of items failed in MergeCloudCommand::redo"; } -} \ No newline at end of file +} + diff --git a/apps/cloud_composer/src/item_inspector.cpp b/apps/cloud_composer/src/item_inspector.cpp index 9e769b0b..57b412d2 100644 --- a/apps/cloud_composer/src/item_inspector.cpp +++ b/apps/cloud_composer/src/item_inspector.cpp @@ -47,7 +47,7 @@ pcl::cloud_composer::ItemInspector::setModel (ProjectModel* new_model) } void -pcl::cloud_composer::ItemInspector::selectionChanged (const QModelIndex ¤t, const QModelIndex &) +pcl::cloud_composer::ItemInspector::selectionChanged (const QModelIndex &, const QModelIndex &) { //If we have a model loaded, save its tree state // if (current_item_properties_model_) diff --git a/apps/cloud_composer/src/items/cloud_composer_item.cpp b/apps/cloud_composer/src/items/cloud_composer_item.cpp index db74523d..e647a4c7 100644 --- a/apps/cloud_composer/src/items/cloud_composer_item.cpp +++ b/apps/cloud_composer/src/items/cloud_composer_item.cpp @@ -60,13 +60,13 @@ pcl::cloud_composer::CloudComposerItem::addChild (CloudComposerItem *item_arg) } void -pcl::cloud_composer::CloudComposerItem::paintView (boost::shared_ptr vis) const +pcl::cloud_composer::CloudComposerItem::paintView (boost::shared_ptr) const { qDebug () << "Paint View in Cloud Composer Item - doing nothing"; } void -pcl::cloud_composer::CloudComposerItem::removeFromView (boost::shared_ptr vis) const +pcl::cloud_composer::CloudComposerItem::removeFromView (boost::shared_ptr) const { qDebug () << "Remove from View in Cloud Composer Item - doing nothing"; } diff --git a/apps/cloud_composer/src/merge_selection.cpp b/apps/cloud_composer/src/merge_selection.cpp index c27c474d..3638380c 100644 --- a/apps/cloud_composer/src/merge_selection.cpp +++ b/apps/cloud_composer/src/merge_selection.cpp @@ -55,6 +55,10 @@ pcl::cloud_composer::MergeSelection::performAction (ConstItemList input_data, Po pcl::ExtractIndices filter; pcl::PCLPointCloud2::Ptr merged_cloud (new pcl::PCLPointCloud2); + //To Save the pose of the first original item + Eigen::Vector4f source_origin; + Eigen::Quaternionf source_orientation; + bool pose_found = false; foreach (const CloudItem* input_cloud_item, selected_item_index_map_.keys ()) { //If this cloud hasn't been completely selected @@ -72,8 +76,13 @@ pcl::cloud_composer::MergeSelection::performAction (ConstItemList input_data, Po filter.filter (*selected_points); qDebug () << "Original minus indices is "<width; - Eigen::Vector4f source_origin = input_cloud_item->data (ItemDataRole::ORIGIN).value (); - Eigen::Quaternionf source_orientation = input_cloud_item->data (ItemDataRole::ORIENTATION).value (); + + if (!pose_found) + { + source_origin = input_cloud_item->data (ItemDataRole::ORIGIN).value (); + source_orientation = input_cloud_item->data (ItemDataRole::ORIENTATION).value (); + pose_found = true; + } CloudItem* new_cloud_item = new CloudItem (input_cloud_item->text () , original_minus_indices , source_origin @@ -95,11 +104,13 @@ pcl::cloud_composer::MergeSelection::performAction (ConstItemList input_data, Po concatenatePointCloud (*merged_cloud, *input_cloud, *temp_cloud); merged_cloud = temp_cloud; } - + CloudItem* cloud_item = new CloudItem ("Cloud from Selection" - , merged_cloud); + , merged_cloud + , source_origin + , source_orientation); output.append (cloud_item); return output; -} \ No newline at end of file +} diff --git a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp index 52909b2d..ad86a060 100644 --- a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp +++ b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp @@ -54,18 +54,24 @@ pcl::cloud_composer::RectangularFrustumSelector::OnLeftButtonUp () vtkMapper* mapper = act->actor->GetMapper (); vtkDataSet* data = mapper->GetInput (); vtkPolyData* poly_data = vtkPolyData::SafeDownCast (data); +#if VTK_MAJOR_VERSION > 5 + id_filter->SetInputData (poly_data); +#else id_filter->SetInput (poly_data); +#endif //extract_geometry->SetInput (poly_data); vtkSmartPointer selected = vtkSmartPointer::New (); glyph_filter->SetOutput (selected); glyph_filter->Update (); +#if VTK_MAJOR_VERSION < 6 selected->SetSource (0); +#endif if (selected->GetNumberOfPoints() > 0) { qDebug () << "Selected " << selected->GetNumberOfPoints () << " points."; id_selected_data_map.insert ( QString::fromStdString ((*it).first), selected); - #if VTK_MAJOR_VERSION <= 5 + #if VTK_MAJOR_VERSION < 6 append->AddInput (selected); #else // VTK 6 append->AddInputData (selected); @@ -77,9 +83,12 @@ pcl::cloud_composer::RectangularFrustumSelector::OnLeftButtonUp () append->Update (); vtkSmartPointer all_points = append->GetOutput (); qDebug () << "Allpoints = " <GetNumberOfPoints (); - - selected_mapper->SetInput (all_points); +#if VTK_MAJOR_VERSION < 6 + selected_mapper->SetInput (all_points); +#else + selected_mapper->SetInputData (all_points); +#endif selected_mapper->ScalarVisibilityOff (); vtkIdTypeArray* ids = vtkIdTypeArray::SafeDownCast (all_points->GetPointData ()->GetArray ("OriginalIds")); diff --git a/apps/cloud_composer/src/project_model.cpp b/apps/cloud_composer/src/project_model.cpp index 0ea004cd..744ab2ab 100644 --- a/apps/cloud_composer/src/project_model.cpp +++ b/apps/cloud_composer/src/project_model.cpp @@ -380,7 +380,7 @@ pcl::cloud_composer::ProjectModel::saveSelectedCloudToFile () pcl::PCLPointCloud2::ConstPtr cloud = cloud_to_save->data (ItemDataRole::CLOUD_BLOB).value (); Eigen::Vector4f origin = cloud_to_save->data (ItemDataRole::ORIGIN).value (); Eigen::Quaternionf orientation = cloud_to_save->data (ItemDataRole::ORIENTATION).value (); - int result = pcl::io::savePCDFile (filename.toStdString (), *cloud, origin, orientation ); + pcl::io::savePCDFile (filename.toStdString (), *cloud, origin, orientation ); } @@ -539,7 +539,6 @@ void pcl::cloud_composer::ProjectModel::selectAllItems (QStandardItem* item) { - int num_rows; if (!item) item = this->invisibleRootItem (); else @@ -564,7 +563,6 @@ pcl::cloud_composer::ProjectModel::emitAllStateSignals () emit newCloudFromSelectionAvailable (onlyCloudItemsSelected ()); //Find out which style is active, emit the signal - QMap::iterator itr = selected_style_map_.begin(); foreach (interactor_styles::INTERACTOR_STYLES style, selected_style_map_.keys()) { if (selected_style_map_.value (style)) @@ -578,7 +576,7 @@ pcl::cloud_composer::ProjectModel::emitAllStateSignals () } void -pcl::cloud_composer::ProjectModel::itemSelectionChanged ( const QItemSelection & selected, const QItemSelection & deselected ) +pcl::cloud_composer::ProjectModel::itemSelectionChanged ( const QItemSelection &, const QItemSelection &) { //qDebug () << "Item selection changed!"; //Set all point selected cloud items back to green text, since if they are selected they get changed to white diff --git a/apps/cloud_composer/src/properties_model.cpp b/apps/cloud_composer/src/properties_model.cpp index 83ad52f4..3688caa1 100644 --- a/apps/cloud_composer/src/properties_model.cpp +++ b/apps/cloud_composer/src/properties_model.cpp @@ -128,9 +128,10 @@ pcl::cloud_composer::PropertiesModel::copyProperties (const PropertiesModel* to_ void -pcl::cloud_composer::PropertiesModel::propertyChanged (QStandardItem* property_item) +pcl::cloud_composer::PropertiesModel::propertyChanged (QStandardItem*) { //qDebug () << "Property Changed in properties model"; parent_item_->propertyChanged (); -} \ No newline at end of file +} + diff --git a/apps/cloud_composer/src/toolbox_model.cpp b/apps/cloud_composer/src/toolbox_model.cpp index 558f33d5..d451f073 100644 --- a/apps/cloud_composer/src/toolbox_model.cpp +++ b/apps/cloud_composer/src/toolbox_model.cpp @@ -73,7 +73,7 @@ pcl::cloud_composer::ToolBoxModel::addToolGroup (QString tool_group_name) } void -pcl::cloud_composer::ToolBoxModel::activeProjectChanged(ProjectModel* new_model, ProjectModel* previous_model) +pcl::cloud_composer::ToolBoxModel::activeProjectChanged(ProjectModel* new_model, ProjectModel*) { //Disconnect old project model signal for selection change if (project_model_) @@ -132,7 +132,7 @@ pcl::cloud_composer::ToolBoxModel::toolAction () } void -pcl::cloud_composer::ToolBoxModel::selectedItemChanged ( const QItemSelection & selected, const QItemSelection & deselected ) +pcl::cloud_composer::ToolBoxModel::selectedItemChanged ( const QItemSelection & selected, const QItemSelection &) { updateEnabledTools (selected); } @@ -247,4 +247,4 @@ pcl::cloud_composer::ToolBoxModel::updateEnabledTools (const QItemSelection curr -} \ No newline at end of file +} diff --git a/apps/cloud_composer/tools/sanitize_cloud.cpp b/apps/cloud_composer/tools/sanitize_cloud.cpp index b03c0ef8..edfde181 100644 --- a/apps/cloud_composer/tools/sanitize_cloud.cpp +++ b/apps/cloud_composer/tools/sanitize_cloud.cpp @@ -20,7 +20,7 @@ pcl::cloud_composer::SanitizeCloudTool::~SanitizeCloudTool () } QList -pcl::cloud_composer::SanitizeCloudTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) +pcl::cloud_composer::SanitizeCloudTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList output; const CloudComposerItem* input_item; @@ -80,4 +80,5 @@ pcl::cloud_composer::SanitizeCloudToolFactory::createToolParameterModel (QObject parameter_model->addProperty ("Keep Organized", false, Qt::ItemIsEditable | Qt::ItemIsEnabled); return parameter_model; -} \ No newline at end of file +} + diff --git a/apps/cloud_composer/tools/voxel_grid_downsample.cpp b/apps/cloud_composer/tools/voxel_grid_downsample.cpp index 08e59207..59ef0987 100644 --- a/apps/cloud_composer/tools/voxel_grid_downsample.cpp +++ b/apps/cloud_composer/tools/voxel_grid_downsample.cpp @@ -22,7 +22,7 @@ pcl::cloud_composer::VoxelGridDownsampleTool::~VoxelGridDownsampleTool () } QList -pcl::cloud_composer::VoxelGridDownsampleTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) +pcl::cloud_composer::VoxelGridDownsampleTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList output; const CloudComposerItem* input_item; diff --git a/apps/in_hand_scanner/CMakeLists.txt b/apps/in_hand_scanner/CMakeLists.txt index ab1cbaee..a67dd5e2 100644 --- a/apps/in_hand_scanner/CMakeLists.txt +++ b/apps/in_hand_scanner/CMakeLists.txt @@ -1,24 +1,62 @@ -set(SUBSYS_NAME in_hand_scanner) -set(SUBSYS_DESC "In-hand scanner for small objects") -set(SUBSYS_DEPS common features io kdtree apps) -set(SUBSYS_LIBS pcl_common pcl_features pcl_io pcl_kdtree) +set(SUBSUBSYS_NAME in_hand_scanner) +set(SUBSUBSYS_DESC "In-hand scanner for small objects") +set(SUBSUBSYS_DEPS common features io kdtree apps) +set(SUBSUBSYS_LIBS pcl_common pcl_features pcl_io pcl_kdtree) + +################################################################################ +# Qt +if(NOT QT4_FOUND) + set(DEFAULT AUTO_OFF) + set(REASON "Qt4 is required for the in_hand_scanner app!") +else() + set(DEFAULT TRUE) + set(REASON) +endif() + +# OpenGL +if(NOT OPENGL_FOUND AND NOT OPENGL_GLU_FOUND) + set(DEFAULT AUTO_OFF) + set(REASON "OpenGL & GLU are required for the in_hand_scanner app!") +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT TRUE) + set(REASON) +endif() + +#OpenNI +if(NOT OPENNI_FOUND OR NOT BUILD_OPENNI) + set(DEFAULT AUTO_OFF) + set(REASON "OpenNI was not found or was disabled by the user.") +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT TRUE) + set(REASON) +endif() + +# Default to not building for now +if (${DEFAULT} STREQUAL "TRUE") + set(DEFAULT FALSE) +endif() + +pcl_subsubsys_option(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") +pcl_subsubsys_depend(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS Qt4 OpenGL OpenGL_GLU openni) + +pcl_add_doc("${SUBSUBSYS_NAME}") ################################################################################ set(INCS - include/pcl/apps/${SUBSYS_NAME}/boost.h - include/pcl/apps/${SUBSYS_NAME}/common_types.h - include/pcl/apps/${SUBSYS_NAME}/eigen.h - include/pcl/apps/${SUBSYS_NAME}/icp.h - include/pcl/apps/${SUBSYS_NAME}/input_data_processing.h - include/pcl/apps/${SUBSYS_NAME}/integration.h - include/pcl/apps/${SUBSYS_NAME}/mesh_processing.h - include/pcl/apps/${SUBSYS_NAME}/utils.h - include/pcl/apps/${SUBSYS_NAME}/visibility_confidence.h + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/common_types.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/icp.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/input_data_processing.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/integration.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/mesh_processing.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/visibility_confidence.h" ) set(IMPL_INCS - include/pcl/apps/${SUBSYS_NAME}/impl/common_types.hpp + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/common_types.hpp" ) set(SRCS @@ -35,19 +73,19 @@ set(SRCS ) # Qt -set(MOC_IN_HAND_SCANNER_INC include/pcl/apps/${SUBSYS_NAME}/in_hand_scanner.h) -set(MOC_OPENGL_VIEWER_INC include/pcl/apps/${SUBSYS_NAME}/opengl_viewer.h) -set(MOC_OFFLINE_INTEGRATION_INC include/pcl/apps/${SUBSYS_NAME}/offline_integration.h) +set(MOC_IN_HAND_SCANNER_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/in_hand_scanner.h") +set(MOC_OPENGL_VIEWER_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/opengl_viewer.h") +set(MOC_OFFLINE_INTEGRATION_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/offline_integration.h") -set(MOC_MAIN_WINDOW_INC include/pcl/apps/${SUBSYS_NAME}/main_window.h) -set(MOC_HELP_WINDOW_INC include/pcl/apps/${SUBSYS_NAME}/help_window.h) +set(MOC_MAIN_WINDOW_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h") +set(MOC_HELP_WINDOW_INC "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/help_window.h") set(UI_MAIN_WINDOW src/main_window.ui) set(UI_HELP_WINDOW src/help_window.ui) # Offline integration set(OI_INCS - include/pcl/apps/${SUBSYS_NAME}/integration.h - include/pcl/apps/${SUBSYS_NAME}/visibility_confidence.h + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/integration.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/visibility_confidence.h" ) set(OI_SRCS @@ -60,72 +98,43 @@ set(OI_SRCS ################################################################################ -# Default to not building for now -set(DEFAULT FALSE) - -pcl_subsys_option(BUILD app_${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -pcl_subsys_depend(BUILD app_${SUBSYS_NAME} ${SUBSYS_DEPS}) -pcl_add_doc(${SUBSYS_NAME}) - -set(ADDITIONAL_LIBS "") -if(BUILD) - # Qt - if(NOT QT4_FOUND) - message(WARNING "Qt4 is needed for the in_hand_scanner app! It will not be built!") - set(BUILD FALSE) - endif() - - # OpenGL - find_package(OpenGL) - if(OPENGL_FOUND AND OPENGL_GLU_FOUND) - list(APPEND ADDITIONAL_LIBS ${OPENGL_LIBRARIES}) - else() - message(WARNING "OpenGL & GLU are needed for the in_hand_scanner app! It will not be built!") - set(BUILD FALSE) - endif() -endif() - -################################################################################ - -if(BUILD) +if(build) # Qt # http://qtnode.net/wiki/Qt4_with_cmake # http://qt-project.org/quarterly/view/using_cmake_to_build_qt_projects set(QT_USE_QTOPENGL TRUE) - include(${QT_USE_FILE}) - qt4_wrap_cpp(MOC_IN_HAND_SCANNER_SRC ${MOC_IN_HAND_SCANNER_INC}) - qt4_wrap_cpp(MOC_OPENGL_VIEWER_SRC ${MOC_OPENGL_VIEWER_INC} OPTIONS -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - qt4_wrap_cpp(MOC_OFFLINE_INTEGRATION_SRC ${MOC_OFFLINE_INTEGRATION_INC} OPTIONS -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) + include("${QT_USE_FILE}") + qt4_wrap_cpp(MOC_IN_HAND_SCANNER_SRC "${MOC_IN_HAND_SCANNER_INC}") + qt4_wrap_cpp(MOC_OPENGL_VIEWER_SRC "${MOC_OPENGL_VIEWER_INC}" OPTIONS -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) + qt4_wrap_cpp(MOC_OFFLINE_INTEGRATION_SRC "${MOC_OFFLINE_INTEGRATION_INC}" OPTIONS -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) - qt4_wrap_cpp(MOC_MAIN_WINDOW_SRC ${MOC_MAIN_WINDOW_INC}) - qt4_wrap_cpp(MOC_HELP_WINDOW_SRC ${MOC_HELP_WINDOW_INC}) - qt4_wrap_ui(UI_MAIN_WINDOW_INC ${UI_MAIN_WINDOW}) - qt4_wrap_ui(UI_HELP_WINDOW_INC ${UI_HELP_WINDOW}) + qt4_wrap_cpp(MOC_MAIN_WINDOW_SRC "${MOC_MAIN_WINDOW_INC}") + qt4_wrap_cpp(MOC_HELP_WINDOW_SRC "${MOC_HELP_WINDOW_INC}") + qt4_wrap_ui(UI_MAIN_WINDOW_INC "${UI_MAIN_WINDOW}") + qt4_wrap_ui(UI_HELP_WINDOW_INC "${UI_HELP_WINDOW}") list(APPEND ADDITIONAL_LIBS ${QT_LIBRARIES}) - include_directories(${CMAKE_CURRENT_BINARY_DIR}) # For the ui files + include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") # In-hand scanner - list(APPEND INCS ${MOC_IN_HAND_SCANNER_INC} ${MOC_OPENGL_VIEWER_INC} ${MOC_MAIN_WINDOW_INC} ${MOC_HELP_WINDOW_INC} ${UI_MAIN_WINDOW_INC} ${UI_HELP_WINDOW_INC}) - list(APPEND SRCS ${MOC_IN_HAND_SCANNER_SRC} ${MOC_OPENGL_VIEWER_SRC} ${MOC_MAIN_WINDOW_SRC} ${MOC_HELP_WINDOW_SRC}) - - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + list(APPEND INCS "${MOC_IN_HAND_SCANNER_INC}" "${MOC_OPENGL_VIEWER_INC}" "${MOC_MAIN_WINDOW_INC}" "${MOC_HELP_WINDOW_INC}" "${UI_MAIN_WINDOW_INC}" "${UI_HELP_WINDOW_INC}") + list(APPEND SRCS "${MOC_IN_HAND_SCANNER_SRC}" "${MOC_OPENGL_VIEWER_SRC}" "${MOC_MAIN_WINDOW_SRC}" "${MOC_HELP_WINDOW_SRC}") - set(EXE_NAME pcl_${SUBSYS_NAME}) - pcl_add_executable_opt_bundle(${EXE_NAME} ${SUBSYS_NAME} ${SRCS} ${INCS} ${IMPL_INCS}) - target_link_libraries(${EXE_NAME} ${SUBSYS_LIBS} ${ADDITIONAL_LIBS}) + set(EXE_NAME "pcl_${SUBSUBSYS_NAME}") + pcl_add_executable_opt_bundle("${EXE_NAME}" "${SUBSUBSYS_NAME}" ${SRCS} ${INCS} ${IMPL_INCS}) + target_link_libraries("${EXE_NAME}" ${SUBSUBSYS_LIBS} ${OPENGL_LIBRARIES} ${QT_LIBRARIES}) - pcl_add_includes(${SUBSYS_NAME} ${SUBSYS_NAME} ${INCS}) - pcl_add_includes(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${IMPL_INCS}) + pcl_add_includes("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${INCS}) + pcl_add_includes("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}/impl" ${IMPL_INCS}) - pcl_make_pkgconfig(${EXE_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" "" "" "") + pcl_make_pkgconfig("${EXE_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" "" "" "" "" "") # Offline integration - list(APPEND OI_INCS ${MOC_OPENGL_VIEWER_INC} ${MOC_OFFLINE_INTEGRATION_INC}) - list(APPEND OI_SRCS ${MOC_OPENGL_VIEWER_SRC} ${MOC_OFFLINE_INTEGRATION_SRC}) + list(APPEND OI_INCS "${MOC_OPENGL_VIEWER_INC}" "${MOC_OFFLINE_INTEGRATION_INC}") + list(APPEND OI_SRCS "${MOC_OPENGL_VIEWER_SRC}" "${MOC_OFFLINE_INTEGRATION_SRC}") - pcl_add_executable_opt_bundle(pcl_offline_integration ${SUBSYS_NAME} ${OI_SRCS} ${OI_INCS}) - target_link_libraries(pcl_offline_integration ${SUBSYS_LIBS} ${ADDITIONAL_LIBS}) + pcl_add_executable_opt_bundle(pcl_offline_integration "${SUBSUBSYS_NAME}" ${OI_SRCS} ${OI_INCS}) + target_link_libraries(pcl_offline_integration ${SUBSUBSYS_LIBS} ${OPENGL_LIBRARIES} ${QT_LIBRARIES}) endif() diff --git a/apps/in_hand_scanner/src/opengl_viewer.cpp b/apps/in_hand_scanner/src/opengl_viewer.cpp index e51fbb8f..784e03f0 100644 --- a/apps/in_hand_scanner/src/opengl_viewer.cpp +++ b/apps/in_hand_scanner/src/opengl_viewer.cpp @@ -44,12 +44,13 @@ #include #include -#ifdef __APPLE__ -# include -# include +#include +#ifdef OPENGL_IS_A_FRAMEWORK +# include +# include #else -# include -# include +# include +# include #endif #include diff --git a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp index b5ed5bea..7863af4b 100644 --- a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp +++ b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp @@ -87,7 +87,7 @@ pcl::apps::DominantPlaneSegmentation::compute_table_plane () if (int (cloud_filtered_->points.size ()) < k_) { - PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.", + PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.", cloud_filtered_->points.size ()); return; } @@ -217,7 +217,7 @@ pcl::apps::DominantPlaneSegmentation::compute_fast (std::vectorpoints.size ()) < k_) { - PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.", + PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.", cloud_filtered_->points.size ()); return; } @@ -588,7 +588,7 @@ pcl::apps::DominantPlaneSegmentation::compute (std::vector if (int (cloud_filtered_->points.size ()) < k_) { - PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.", + PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.", cloud_filtered_->points.size ()); return; } @@ -599,14 +599,14 @@ pcl::apps::DominantPlaneSegmentation::compute (std::vector grid_.setInputCloud (cloud_filtered_); grid_.filter (*cloud_downsampled_); - PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering (%f -> %f): %zu out of %zu\n", + PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering (%f -> %f): %lu out of %lu\n", min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ()); // ---[ Estimate the point normals n3d_.setInputCloud (cloud_downsampled_); n3d_.compute (*cloud_normals_); - PCL_INFO ("[DominantPlaneSegmentation] %zu normals estimated. \n", cloud_normals_->points.size ()); + PCL_INFO ("[DominantPlaneSegmentation] %lu normals estimated. \n", cloud_normals_->points.size ()); // ---[ Perform segmentation seg_.setInputCloud (cloud_downsampled_); @@ -681,7 +681,7 @@ pcl::apps::DominantPlaneSegmentation::compute (std::vector cluster_.setIndices (boost::make_shared (cloud_object_indices)); cluster_.extract (clusters2); - PCL_INFO ("[DominantPlaneSegmentation::compute()] Number of clusters found matching the given constraints: %zu.\n", + PCL_INFO ("[DominantPlaneSegmentation::compute()] Number of clusters found matching the given constraints: %lu.\n", clusters2.size ()); clusters.resize (clusters2.size ()); @@ -751,7 +751,7 @@ pcl::apps::DominantPlaneSegmentation::compute_full (std::vectorpoints.size ()) < k_) { - PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.", + PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.", cloud_filtered_->points.size ()); return; } @@ -762,14 +762,14 @@ pcl::apps::DominantPlaneSegmentation::compute_full (std::vector %f): %zu out of %zu\n", + PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %lu out of %lu\n", min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ()); // ---[ Estimate the point normals n3d_.setInputCloud (cloud_downsampled_); n3d_.compute (*cloud_normals_); - PCL_INFO ("[DominantPlaneSegmentation] %zu normals estimated. \n", cloud_normals_->points.size ()); + PCL_INFO ("[DominantPlaneSegmentation] %lu normals estimated. \n", cloud_normals_->points.size ()); // ---[ Perform segmentation seg_.setInputCloud (cloud_downsampled_); @@ -842,7 +842,7 @@ pcl::apps::DominantPlaneSegmentation::compute_full (std::vector (cloud_object_indices)); cluster_.extract (clusters2); - PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %zu.\n", + PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %lu.\n", clusters2.size ()); clusters.resize (clusters2.size ()); diff --git a/apps/modeler/CMakeLists.txt b/apps/modeler/CMakeLists.txt old mode 100755 new mode 100644 index 7fe3f7f0..7181cd46 --- a/apps/modeler/CMakeLists.txt +++ b/apps/modeler/CMakeLists.txt @@ -1,70 +1,68 @@ -set(SUBSYS_NAME modeler) -set(SUBSYS_DESC "PCLModeler: PCL based reconstruction platform") -set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps) +set(SUBSUBSYS_NAME modeler) +set(SUBSUBSYS_DESC "PCLModeler: PCL based reconstruction platform") +set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps) set(REASON "") # Find VTK and QVTK if(VTK_FOUND AND VTK_USE_QVTK) set(DEFAULT TRUE) set(REASON) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") elseif(NOT VTK_FOUND) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "VTK was not found.") elseif(NOT VTK_USE_QVTK) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "VTK was not built with Qt support.") endif(VTK_FOUND AND VTK_USE_QVTK) -include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - # Default to not building for now -set(DEFAULT FALSE) +if (${DEFAULT} STREQUAL "TRUE") + set(DEFAULT FALSE) +endif() -PCL_SUBSYS_OPTION(build app_${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -PCL_SUBSYS_DEPEND(build app_${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS vtk) +PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSUBSYS_NAME}") if(build) - - include_directories (${CMAKE_CURRENT_BINARY_DIR}) - include_directories (${CMAKE_CURRENT_SOURCE_DIR}/include) + include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") # Set Qt files and resources here set(uis main_window.ui) - set(moc_incs include/pcl/apps/${SUBSYS_NAME}/main_window.h - include/pcl/apps/${SUBSYS_NAME}/scene_tree.h - include/pcl/apps/${SUBSYS_NAME}/parameter_dialog.h - include/pcl/apps/${SUBSYS_NAME}/thread_controller.h - include/pcl/apps/${SUBSYS_NAME}/abstract_worker.h - include/pcl/apps/${SUBSYS_NAME}/cloud_mesh_item_updater.h) + set(moc_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/scene_tree.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/parameter_dialog.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/thread_controller.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/abstract_worker.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_mesh_item_updater.h") set(resources resources/resources.qrc) set(incs ${moc_incs} - include/pcl/apps/${SUBSYS_NAME}/qt.h + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/qt.h" - include/pcl/apps/${SUBSYS_NAME}/dock_widget.h - include/pcl/apps/${SUBSYS_NAME}/abstract_item.h - include/pcl/apps/${SUBSYS_NAME}/render_window.h - include/pcl/apps/${SUBSYS_NAME}/render_window_item.h + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/dock_widget.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/abstract_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/render_window.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/render_window_item.h" - include/pcl/apps/${SUBSYS_NAME}/parameter.h + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/parameter.h" - include/pcl/apps/${SUBSYS_NAME}/cloud_mesh.h - include/pcl/apps/${SUBSYS_NAME}/cloud_mesh_item.h - include/pcl/apps/${SUBSYS_NAME}/channel_actor_item.h - include/pcl/apps/${SUBSYS_NAME}/points_actor_item.h - include/pcl/apps/${SUBSYS_NAME}/normals_actor_item.h - include/pcl/apps/${SUBSYS_NAME}/surface_actor_item.h + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_mesh.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_mesh_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/channel_actor_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/points_actor_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/normals_actor_item.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/surface_actor_item.h" - include/pcl/apps/${SUBSYS_NAME}/icp_registration_worker.h - include/pcl/apps/${SUBSYS_NAME}/voxel_grid_downsample_worker.h - include/pcl/apps/${SUBSYS_NAME}/statistical_outlier_removal_worker.h - include/pcl/apps/${SUBSYS_NAME}/normal_estimation_worker.h - include/pcl/apps/${SUBSYS_NAME}/poisson_worker.h) + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/icp_registration_worker.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/voxel_grid_downsample_worker.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/statistical_outlier_removal_worker.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/normal_estimation_worker.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/poisson_worker.h") set(srcs src/main.cpp @@ -95,8 +93,8 @@ if(build) src/normal_estimation_worker.cpp src/poisson_worker.cpp) - set(impl_incs include/pcl/apps/${SUBSYS_NAME}/impl/parameter.hpp - include/pcl/apps/${SUBSYS_NAME}/impl/scene_tree.hpp) + set(impl_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/parameter.hpp" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/impl/scene_tree.hpp") # Qt stuff QT4_WRAP_UI(ui_srcs ${uis}) @@ -109,23 +107,22 @@ if(build) SET_SOURCE_FILES_PROPERTIES(${srcs} PROPERTIES OBJECT_DEPENDS "${ui_srcs}") # Generate executable - set(EXE_NAME pcl_${SUBSYS_NAME}) - PCL_ADD_EXECUTABLE(${EXE_NAME} ${SUBSYS_NAME} ${ui_srcs} ${moc_srcs} ${resource_srcs} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${EXE_NAME} pcl_common pcl_io pcl_kdtree pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search QVTK ${QT_LIBRARIES}) + set(EXE_NAME "pcl_${SUBSUBSYS_NAME}") + PCL_ADD_EXECUTABLE("${EXE_NAME}" "${SUBSUBSYS_NAME}" ${ui_srcs} ${moc_srcs} ${resource_srcs} ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${EXE_NAME}" pcl_common pcl_io pcl_kdtree pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search ${QVTK_LIBRARY} ${QT_LIBRARIES}) # Put the ui in the windows project file - IF (${CMAKE_BUILD_TOOL} MATCHES "msdev") - SET (srcs ${srcs} ${uis}) - ENDIF (${CMAKE_BUILD_TOOL} MATCHES "msdev") - IF (${CMAKE_BUILD_TOOL} MATCHES "devenv") - SET (srcs ${srcs} ${uis}) - ENDIF (${CMAKE_BUILD_TOOL} MATCHES "devenv") + IF("${CMAKE_BUILD_TOOL}" MATCHES "msdev") + LIST(APPEND srcs ${uis}) + ELSEIF("${CMAKE_BUILD_TOOL}" MATCHES "devenv") + LIST(APPEND srcs ${uis}) + ENDIF("${CMAKE_BUILD_TOOL}" MATCHES "msdev") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}/impl" ${impl_incs}) - PCL_MAKE_PKGCONFIG(${EXE_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" "" "" "") + PCL_MAKE_PKGCONFIG("${EXE_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" "" "" "" "" "") add_subdirectory(tools) diff --git a/apps/modeler/src/icp_registration_worker.cpp b/apps/modeler/src/icp_registration_worker.cpp index 48dc47af..f3e8ad29 100755 --- a/apps/modeler/src/icp_registration_worker.cpp +++ b/apps/modeler/src/icp_registration_worker.cpp @@ -138,7 +138,7 @@ pcl::modeler::ICPRegistrationWorker::processImpl(CloudMeshItem* cloud_mesh_item) // Set the euclidean distance difference epsilon (criterion 3) icp.setEuclideanFitnessEpsilon (*euclidean_fitness_epsilon_); - icp.setInputCloud(cloud_mesh_item->getCloudMesh()->getCloud()); + icp.setInputSource(cloud_mesh_item->getCloudMesh()->getCloud()); icp.setInputTarget(cloud_); pcl::PointCloud result; icp.align(result); diff --git a/apps/modeler/src/normals_actor_item.cpp b/apps/modeler/src/normals_actor_item.cpp index bb443f5b..fc8bd541 100755 --- a/apps/modeler/src/normals_actor_item.cpp +++ b/apps/modeler/src/normals_actor_item.cpp @@ -137,7 +137,11 @@ pcl::modeler::NormalsActorItem::initImpl() createNormalLines(); vtkSmartPointer mapper = vtkSmartPointer::New(); +#if VTK_MAJOR_VERSION < 6 mapper->SetInput(poly_data_); +#else + mapper->SetInputData (poly_data_); +#endif vtkSmartPointer scalars; cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_); @@ -166,8 +170,6 @@ pcl::modeler::NormalsActorItem::updateImpl() scalars->GetRange(minmax); actor_->GetMapper()->SetScalarRange(minmax); - poly_data_->Update(); - return; } diff --git a/apps/modeler/src/points_actor_item.cpp b/apps/modeler/src/points_actor_item.cpp index 2c2474ac..fbd94669 100755 --- a/apps/modeler/src/points_actor_item.cpp +++ b/apps/modeler/src/points_actor_item.cpp @@ -64,13 +64,12 @@ void pcl::modeler::PointsActorItem::initImpl() { poly_data_->SetPoints(cloud_mesh_->getVtkPoints()); - poly_data_->Update(); vtkSmartPointer vertex_glyph_filter = vtkSmartPointer::New(); -#if VTK_MAJOR_VERSION <= 5 +#if VTK_MAJOR_VERSION < 6 vertex_glyph_filter->AddInput(poly_data_); #else - vertex_glyph_filter->AddInputData(polydata); + vertex_glyph_filter->AddInputData (poly_data_); #endif vertex_glyph_filter->Update(); @@ -109,8 +108,6 @@ pcl::modeler::PointsActorItem::updateImpl() scalars->GetRange(minmax); actor_->GetMapper()->SetScalarRange(minmax); - poly_data_->Update(); - return; } diff --git a/apps/modeler/src/surface_actor_item.cpp b/apps/modeler/src/surface_actor_item.cpp index cf7b0a5d..6a8a9252 100755 --- a/apps/modeler/src/surface_actor_item.cpp +++ b/apps/modeler/src/surface_actor_item.cpp @@ -69,10 +69,13 @@ pcl::modeler::SurfaceActorItem::initImpl() vtkSmartPointer scalars; cloud_mesh_->getColorScalarsFromField(scalars, color_scheme_); poly_data_->GetPointData ()->SetScalars (scalars); - poly_data_->Update(); vtkSmartPointer mapper = vtkSmartPointer::New(); +#if VTK_MAJOR_VERSION < 6 mapper->SetInput(poly_data_); +#else + mapper->SetInputData (poly_data_); +#endif double minmax[2]; scalars->GetRange(minmax); @@ -108,8 +111,6 @@ pcl::modeler::SurfaceActorItem::updateImpl() scalars->GetRange(minmax); actor_->GetMapper()->SetScalarRange(minmax); - poly_data_->Update(); - return; } diff --git a/apps/optronic_viewer/CMakeLists.txt b/apps/optronic_viewer/CMakeLists.txt index d0dd4432..24f9b5db 100644 --- a/apps/optronic_viewer/CMakeLists.txt +++ b/apps/optronic_viewer/CMakeLists.txt @@ -1,57 +1,72 @@ -set(SUBSYS_NAME optronic_viewer) -set(SUBSYS_DESC "PCL Optronic Viewer") -set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps) -set(DEFAULT OFF) -set(REASON "") +set(SUBSUBSYS_NAME optronic_viewer) +set(SUBSUBSYS_DESC "PCL Optronic Viewer") +set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps) # Find VTK and QVTK if(VTK_FOUND AND VTK_USE_QVTK) set(DEFAULT TRUE) set(REASON) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") elseif(NOT VTK_FOUND) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "VTK was not found.") elseif(NOT VTK_USE_QVTK) - set(DEFAULT FALSE) + set(DEFAULT AUTO_OFF) set(REASON "VTK was not built with Qt support.") endif(VTK_FOUND AND VTK_USE_QVTK) -include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) +# Find QT +if(NOT QT_USE_FILE) + set(DEFAULT AUTO_OFF) + set(REASON "Qt was not found.") +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT TRUE) + set(REASON) +endif(NOT QT_USE_FILE) + +# FZAPI +if(FZAPI_FOUND) + set(DEFAULT TRUE) + set(REASON) +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT AUTO_OFF) + set(REASON "FZAPI was not found.") +endif() # Default to not building for now -set(DEFAULT FALSE) +if (${DEFAULT} STREQUAL "TRUE") + set(DEFAULT FALSE) +endif() -PCL_SUBSYS_OPTION(build app_${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -PCL_SUBSYS_DEPEND(build app_${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS vtk) +PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS vtk) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSUBSYS_NAME}") if(build) - include_directories (${CMAKE_CURRENT_BINARY_DIR}) - include_directories (${CMAKE_CURRENT_SOURCE_DIR}/include) + include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") # Set Qt files and resources here # set(uis main_window.ui) - set(moc_incs include/pcl/apps/${SUBSYS_NAME}/main_window.h - include/pcl/apps/${SUBSYS_NAME}/filter_window.h - include/pcl/apps/${SUBSYS_NAME}/openni_grabber.h) -# include/pcl/apps/${SUBSYS_NAME}/scene_tree.h -# include/pcl/apps/${SUBSYS_NAME}/parameter_dialog.h -# include/pcl/apps/${SUBSYS_NAME}/thread_controller.h -# include/pcl/apps/${SUBSYS_NAME}/abstract_worker.h -# include/pcl/apps/${SUBSYS_NAME}/cloud_mesh_item_updater.h) + set(moc_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/filter_window.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/openni_grabber.h") +# "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/scene_tree.h" +# "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/parameter_dialog.h" +# "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/thread_controller.h" +# "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/abstract_worker.h" +# "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_mesh_item_updater.h") # set(resources resources/resources.qrc) # set(incs ${moc_incs}) - set(incs include/pcl/apps/${SUBSYS_NAME}/qt.h - include/pcl/apps/${SUBSYS_NAME}/openni_grabber.h - include/pcl/apps/${SUBSYS_NAME}/cloud_filter.h - include/pcl/apps/${SUBSYS_NAME}/main_window.h - include/pcl/apps/${SUBSYS_NAME}/filter_window.h) + set(incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/qt.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/openni_grabber.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/cloud_filter.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/filter_window.h") set(srcs src/main.cpp src/cloud_filter.cpp @@ -72,23 +87,22 @@ if(build) # SET_SOURCE_FILES_PROPERTIES(${srcs} PROPERTIES OBJECT_DEPENDS "${ui_srcs}") # Generate executable - set(EXE_NAME pcl_${SUBSYS_NAME}) -# PCL_ADD_EXECUTABLE(${EXE_NAME} ${SUBSYS_NAME} ${ui_srcs} ${moc_srcs} ${resource_srcs} ${srcs} ${incs} ${impl_incs}) - PCL_ADD_EXECUTABLE(${EXE_NAME} ${SUBSYS_NAME} ${moc_srcs} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${EXE_NAME} pcl_common pcl_io pcl_kdtree pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search QVTK ${QT_LIBRARIES}) + set(EXE_NAME "pcl_${SUBSUBSYS_NAME}") +# PCL_ADD_EXECUTABLE("${EXE_NAME}" "${SUBSUBSYS_NAME}" ${ui_srcs} ${moc_srcs} ${resource_srcs} ${srcs} ${incs} ${impl_incs}) + PCL_ADD_EXECUTABLE("${EXE_NAME}" "${SUBSUBSYS_NAME}" ${moc_srcs} ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${EXE_NAME}" pcl_common pcl_io pcl_kdtree pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search QVTK ${QT_LIBRARIES}) # Put the ui in the windows project file - IF (${CMAKE_BUILD_TOOL} MATCHES "msdev") - SET (srcs ${srcs} ${uis}) - ENDIF (${CMAKE_BUILD_TOOL} MATCHES "msdev") - IF (${CMAKE_BUILD_TOOL} MATCHES "devenv") - SET (srcs ${srcs} ${uis}) - ENDIF (${CMAKE_BUILD_TOOL} MATCHES "devenv") + IF("${CMAKE_BUILD_TOOL}" MATCHES "msdev") + LIST(APPEND srcs ${uis}) + ELSEIF("${CMAKE_BUILD_TOOL}" MATCHES "devenv") + LIST(APPEND srcs ${uis}) + ENDIF("${CMAKE_BUILD_TOOL}" MATCHES "msdev") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSUBSYS_NAME}/impl" ${impl_incs}) - PCL_MAKE_PKGCONFIG(${EXE_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" "" "" "") + PCL_MAKE_PKGCONFIG("${EXE_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" "" "" "" "" "") endif(build) diff --git a/apps/point_cloud_editor/CMakeLists.txt b/apps/point_cloud_editor/CMakeLists.txt index 5670d762..7c027567 100644 --- a/apps/point_cloud_editor/CMakeLists.txt +++ b/apps/point_cloud_editor/CMakeLists.txt @@ -1,121 +1,119 @@ -SET(SUBSYS_NAME point_cloud_editor) -SET(SUBSYS_DESC "Point Cloud Editor - Simple editor for 3D point clouds") -SET(SUBSYS_DEPS common filters io apps) +SET(SUBSUBSYS_NAME point_cloud_editor) +SET(SUBSUBSYS_DESC "Point Cloud Editor - Simple editor for 3D point clouds") +SET(SUBSUBSYS_DEPS common filters io apps) -SET(MOC_INCS include/pcl/apps/${SUBSYS_NAME}/cloudEditorWidget.h - include/pcl/apps/${SUBSYS_NAME}/mainWindow.h - include/pcl/apps/${SUBSYS_NAME}/denoiseParameterForm.h - include/pcl/apps/${SUBSYS_NAME}/statisticsDialog.h +SET(MOC_INCS "include/pcl/apps/${SUBSUBSYS_NAME}/cloudEditorWidget.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/mainWindow.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/denoiseParameterForm.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/statisticsDialog.h" ) SET(RSRC resources/pceditor_resources.qrc) SET(INCS ${MOC_INCS} - include/pcl/apps/${SUBSYS_NAME}/cloud.h - include/pcl/apps/${SUBSYS_NAME}/cloudTransformTool.h - include/pcl/apps/${SUBSYS_NAME}/command.h - include/pcl/apps/${SUBSYS_NAME}/commandQueue.h - include/pcl/apps/${SUBSYS_NAME}/common.h - include/pcl/apps/${SUBSYS_NAME}/copyBuffer.h - include/pcl/apps/${SUBSYS_NAME}/copyCommand.h - include/pcl/apps/${SUBSYS_NAME}/cutCommand.h - include/pcl/apps/${SUBSYS_NAME}/deleteCommand.h - include/pcl/apps/${SUBSYS_NAME}/denoiseCommand.h - include/pcl/apps/${SUBSYS_NAME}/localTypes.h - include/pcl/apps/${SUBSYS_NAME}/pasteCommand.h - include/pcl/apps/${SUBSYS_NAME}/select1DTool.h - include/pcl/apps/${SUBSYS_NAME}/select2DTool.h - include/pcl/apps/${SUBSYS_NAME}/selection.h - include/pcl/apps/${SUBSYS_NAME}/selectionTransformTool.h - include/pcl/apps/${SUBSYS_NAME}/statistics.h - include/pcl/apps/${SUBSYS_NAME}/toolInterface.h - include/pcl/apps/${SUBSYS_NAME}/trackball.h - include/pcl/apps/${SUBSYS_NAME}/transformCommand.h + "include/pcl/apps/${SUBSUBSYS_NAME}/cloud.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/cloudTransformTool.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/command.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/commandQueue.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/common.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/copyBuffer.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/copyCommand.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/cutCommand.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/deleteCommand.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/denoiseCommand.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/localTypes.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/pasteCommand.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/select1DTool.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/select2DTool.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/selection.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/selectionTransformTool.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/statistics.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/toolInterface.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/trackball.h" + "include/pcl/apps/${SUBSUBSYS_NAME}/transformCommand.h" ) -SET(SRCS src/main.cpp - src/mainWindow.cpp - src/commandQueue.cpp - src/selection.cpp - src/copyBuffer.cpp - src/deleteCommand.cpp - src/cutCommand.cpp - src/pasteCommand.cpp - src/cloud.cpp - src/cloudEditorWidget.cpp - src/cloudTransformTool.cpp - src/select1DTool.cpp - src/select2DTool.cpp - src/selectionTransformTool.cpp - src/transformCommand.cpp - src/common.cpp - src/denoiseCommand.cpp - src/statistics.cpp - src/statisticsDialog.cpp - src/trackball.cpp - src/denoiseParameterForm.cpp +SET(SRCS src/main.cpp + src/mainWindow.cpp + src/commandQueue.cpp + src/selection.cpp + src/copyBuffer.cpp + src/deleteCommand.cpp + src/cutCommand.cpp + src/pasteCommand.cpp + src/cloud.cpp + src/cloudEditorWidget.cpp + src/cloudTransformTool.cpp + src/select1DTool.cpp + src/select2DTool.cpp + src/selectionTransformTool.cpp + src/transformCommand.cpp + src/common.cpp + src/denoiseCommand.cpp + src/statistics.cpp + src/statisticsDialog.cpp + src/trackball.cpp + src/denoiseParameterForm.cpp ) IF(NOT QT4_FOUND) - SET(DEFAULT FALSE) + SET(DEFAULT AUTO_OFF) SET(REASON "Qt4 was not found.") ELSE(NOT QT4_FOUND) SET(DEFAULT TRUE) SET(REASON) ENDIF(NOT QT4_FOUND) + # Find OpenGL -find_package(OpenGL) IF(NOT OPENGL_FOUND) - SET(DEFAULT FALSE) + SET(DEFAULT AUTO_OFF) SET(REASON "OpenGL was not found.") -ELSE(NOT OPENGL_FOUND) +ELSEIF(NOT ${DEFAULT} STREQUAL "AUTO_OFF") SET(DEFAULT TRUE) SET(REASON) ENDIF(NOT OPENGL_FOUND) # Default to not building for now -set(DEFAULT FALSE) -SET(REASON "") +if (${DEFAULT} STREQUAL "TRUE") + set(DEFAULT FALSE) +endif() -PCL_SUBSYS_OPTION(BUILD app_${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -PCL_SUBSYS_DEPEND(BUILD app_${SUBSYS_NAME} ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_SUBSUBSYS_OPTION(BUILD "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSUBSYS_DEPEND(BUILD "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${SUBSYS_DEPS}) +PCL_ADD_DOC(${SUBSUBSYS_NAME}) IF(BUILD) - - INCLUDE(${QT_USE_FILE}) + INCLUDE("${QT_USE_FILE}") SET(QT_USE_QTOPENGL TRUE) #QT4_WRAP_CPP(MOC_SRCS ${MOC_INCS}) QT4_WRAP_CPP(MOC_SRCS ${MOC_INCS} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED) QT4_ADD_RESOURCES(RESOURCES_SRCS ${RSRC}) - INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR} - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${QT_QT_INCLUDE_DIR} - ${QT_QTOPENGL_INCLUDE_DIR} - ) - - SET(EXE_NAME pcl_${SUBSYS_NAME}) - PCL_ADD_EXECUTABLE(${EXE_NAME} - ${SUBSYS_NAME} - ${SRCS} - ${RESOURCES_SRCS} - ${MOC_SRCS} - ${INCS} - ) + INCLUDE_DIRECTORIES("${CMAKE_CURRENT_BINARY_DIR}" + "${CMAKE_CURRENT_SOURCE_DIR}/include" + "${QT_QTOPENGL_INCLUDE_DIR}" + ) - TARGET_LINK_LIBRARIES(${EXE_NAME} - ${QT_LIBRARIES} - ${QT_QTOPENGL_LIBRARY} - ${OPENGL_LIBRARIES} - ${BOOST_LIBRARIES} - pcl_common - pcl_io - pcl_filters - ) + SET(EXE_NAME "pcl_${SUBSUBSYS_NAME}") + PCL_ADD_EXECUTABLE("${EXE_NAME}" + "${SUBSUBSYS_NAME}" + ${SRCS} + ${RESOURCES_SRCS} + ${MOC_SRCS} + ${INCS} + ) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${INCS}) - PCL_MAKE_PKGCONFIG(${EXE_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" "" "" "") + TARGET_LINK_LIBRARIES("${EXE_NAME}" + ${QT_LIBRARIES} + ${QT_QTOPENGL_LIBRARY} + ${OPENGL_LIBRARIES} + ${BOOST_LIBRARIES} + pcl_common + pcl_io + pcl_filters + ) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}" ${INCS}) + PCL_MAKE_PKGCONFIG("${EXE_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" "" "" "" "" "") ENDIF(BUILD) diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h index 882171c7..7c79e248 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h @@ -93,13 +93,6 @@ class CutCommand : public Command assert(false); return (*this); } - /// The copy buffer which backs up the points removed from the cloud. - CopyBuffer cut_cloud_buffer_; - - /// a selection which backs up the index of the points cut in the - /// original cloud. - Selection cut_selection_; - /// A shared pointer pointing to the selection object. SelectionPtr selection_ptr_; @@ -109,6 +102,13 @@ class CutCommand : public Command /// a pointer pointing to the copy buffer. CopyBufferPtr copy_buffer_ptr_; + /// a selection which backs up the index of the points cut in the + /// original cloud. + Selection cut_selection_; + + /// The copy buffer which backs up the points removed from the cloud. + CopyBuffer cut_cloud_buffer_; + }; #endif // CUT_COMMAND_H_ diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/deleteCommand.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/deleteCommand.h index d41fc0d7..e2a94a58 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/deleteCommand.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/deleteCommand.h @@ -89,18 +89,18 @@ class DeleteCommand : public Command assert(false); return (*this); } - /// a copy buffer which backs up the points deleted from the cloud. - CopyBuffer deleted_cloud_buffer_; + /// a pointer pointing to the cloud + CloudPtr cloud_ptr_; + + /// A shared pointer pointing to the selection object. + SelectionPtr selection_ptr_; /// a selection which backs up the index of the deleted points in the /// original cloud. Selection deleted_selection_; - /// A shared pointer pointing to the selection object. - SelectionPtr selection_ptr_; - - /// a pointer pointing to the cloud - CloudPtr cloud_ptr_; + /// a copy buffer which backs up the points deleted from the cloud. + CopyBuffer deleted_cloud_buffer_; }; #endif // DELETE_COMMAND_H_ diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/pasteCommand.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/pasteCommand.h index d43e6c04..5a87532c 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/pasteCommand.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/pasteCommand.h @@ -92,10 +92,8 @@ class PasteCommand : public Command assert(false); return (*this); } - /// The size of the cloud before new points are pasted. This value is used - /// to mark the point where points were added to the cloud. In order to - /// support undo, one only has to resize the cloud using this value. - unsigned int prev_cloud_size_; + /// a pointer pointing to the copy buffer. + ConstCopyBufferPtr copy_buffer_ptr_; /// A shared pointer pointing to the selection object. SelectionPtr selection_ptr_; @@ -103,7 +101,9 @@ class PasteCommand : public Command /// a pointer pointing to the cloud CloudPtr cloud_ptr_; - /// a pointer pointing to the copy buffer. - ConstCopyBufferPtr copy_buffer_ptr_; + /// The size of the cloud before new points are pasted. This value is used + /// to mark the point where points were added to the cloud. In order to + /// support undo, one only has to resize the cloud using this value. + unsigned int prev_cloud_size_; }; #endif // PASTE_COMMAND_H_ diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h index 80a14539..12e65612 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/transformCommand.h @@ -92,6 +92,14 @@ class TransformCommand : public Command void applyTransform(ConstSelectionPtr sel_ptr); + /// pointers to constructor params + ConstSelectionPtr selection_ptr_; + + /// a pointer poiting to the cloud + CloudPtr cloud_ptr_; + + float translate_x_, translate_y_, translate_z_; + /// An internal selection object used to perform undo SelectionPtr internal_selection_ptr_; @@ -99,14 +107,6 @@ class TransformCommand : public Command /// of the selected points float transform_matrix_[MATRIX_SIZE]; - float translate_x_, translate_y_, translate_z_; - - /// pointers to constructor params - ConstSelectionPtr selection_ptr_; - - /// a pointer poiting to the cloud - CloudPtr cloud_ptr_; - /// The transform matrix of the cloud used by this command float cloud_matrix_[MATRIX_SIZE]; /// The inverted transform matrix of the cloud used by this command diff --git a/apps/point_cloud_editor/src/cloudEditorWidget.cpp b/apps/point_cloud_editor/src/cloudEditorWidget.cpp index 7a6e6330..c6a90a7e 100644 --- a/apps/point_cloud_editor/src/cloudEditorWidget.cpp +++ b/apps/point_cloud_editor/src/cloudEditorWidget.cpp @@ -42,7 +42,15 @@ #include #include #include -#include + +#include + +#ifdef OPENGL_IS_A_FRAMEWORK +# include +#else +# include +#endif + #include #include #include diff --git a/apps/point_cloud_editor/src/statisticsDialog.cpp b/apps/point_cloud_editor/src/statisticsDialog.cpp index 74fdfe82..5d5edfe3 100644 --- a/apps/point_cloud_editor/src/statisticsDialog.cpp +++ b/apps/point_cloud_editor/src/statisticsDialog.cpp @@ -40,7 +40,7 @@ #include -StatisticsDialog::StatisticsDialog(QWidget *parent) +StatisticsDialog::StatisticsDialog(QWidget *) { button_box_ = new QDialogButtonBox; button_box_->addButton(tr("Hide"), QDialogButtonBox::AcceptRole); diff --git a/apps/src/convolve.cpp b/apps/src/convolve.cpp index 1ee96235..050a206b 100644 --- a/apps/src/convolve.cpp +++ b/apps/src/convolve.cpp @@ -64,7 +64,7 @@ main (int argc, char ** argv) { int viewport_source, viewport_convolved = 0; int direction = -1; - int nb_threads = 0; + int nb_threads = 1; char border_policy = 'Z'; double threshold = 0.001; pcl::filters::Convolution convolution; @@ -116,7 +116,20 @@ main (int argc, char ** argv) { if (nb_threads <= 0) nb_threads = 1; +#ifndef _OPENMP + if (nb_threads > 1) + { + pcl::console::print_info ("OpenMP not activated. Number of threads: 1\n"); + nb_threads = 1; + } +#endif + } +#ifdef _OPENMP + else + { + nb_threads = omp_get_num_procs(); } +#endif convolution.setNumberOfThreads (nb_threads); // borders policy if any @@ -188,6 +201,12 @@ main (int argc, char ** argv) } } convolved_label << pcl::getTime () - t0 << "s"; +#ifdef _OPENMP + convolved_label << "\ncpu cores: " << omp_get_num_procs() << " "; +#else + convolved_label << "\n"; +#endif + convolved_label << "threads: " << nb_threads; // Display boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("Convolution")); // viewport stuff diff --git a/apps/src/grabcut_2d.cpp b/apps/src/grabcut_2d.cpp new file mode 100644 index 00000000..67bbe82e --- /dev/null +++ b/apps/src/grabcut_2d.cpp @@ -0,0 +1,565 @@ +#include +#include +#include +#include +#include +#include +#include + +#ifdef GLUT_IS_A_FRAMEWORK +#include +#else +#include +#if defined (FREEGLUT) +#include +#elif defined (GLUI_OPENGLUT) +#include +#endif +#endif + +class GrabCutHelper : public pcl::GrabCut +{ + using pcl::GrabCut::n_links_; + using pcl::GrabCut::graph_; + using pcl::GrabCut::indices_; + using pcl::GrabCut::hard_segmentation_; + using pcl::GrabCut::width_; + using pcl::GrabCut::height_; + using pcl::GrabCut::graph_nodes_; + using pcl::GrabCut::L_; + using pcl::GrabCut::K_; + using pcl::GrabCut::GMM_component_; + using pcl::GrabCut::input_; + + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + GrabCutHelper (uint32_t K = 5, float lambda = 50.f) + : pcl::GrabCut (K, lambda) + {} + + ~GrabCutHelper () + { } + + void + setInputCloud (const pcl::PointCloud::ConstPtr& cloud); + void + setBackgroundPointsIndices (const pcl::PointIndices::ConstPtr& point_indices); + void + setBackgroundPointsIndices (int x1, int y1, int x2, int y2); + void + setTrimap(int x1, int y1, int x2, int y2, const pcl::segmentation::grabcut::TrimapValue& t); + void + refine (); + int + refineOnce (); + void + fitGMMs (); + void + display (int display_type); + void + overlayAlpha (); + + private: + void + buildImages (); + + // Clouds of various variables that can be displayed for debugging. + pcl::PointCloud::Ptr n_links_image_; + pcl::segmentation::grabcut::Image::Ptr t_links_image_; + pcl::segmentation::grabcut::Image::Ptr gmm_image_; + pcl::PointCloud::Ptr alpha_image_; + + int image_height_1_; + int image_width_1_; +}; + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::setInputCloud (const pcl::PointCloud::ConstPtr& cloud) +{ + pcl::GrabCut::setInputCloud (cloud); + // Reset clouds + n_links_image_.reset (new pcl::PointCloud (cloud->width, cloud->height, 0)); + t_links_image_.reset (new pcl::segmentation::grabcut::Image (cloud->width, cloud->height)); + gmm_image_.reset (new pcl::segmentation::grabcut::Image (cloud->width, cloud->height)); + alpha_image_.reset (new pcl::PointCloud (cloud->width, cloud->height, 0)); + image_height_1_ = cloud->height-1; + image_width_1_ = cloud->width-1; +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::setBackgroundPointsIndices (const pcl::PointIndices::ConstPtr& point_indices) +{ + pcl::GrabCut::setBackgroundPointsIndices (point_indices); + buildImages (); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::setBackgroundPointsIndices (int x1, int y1, int x2, int y2) +{ + pcl::PointIndices::Ptr point_indices (new pcl::PointIndices); + point_indices->indices.reserve (input_->size ()); + if (x1 > x2) std::swap (x1, x2); + if (y1 > y2) std::swap (y1, y2); + for (int y = std::max (y1, 0); y <= std::min (static_cast (input_->height -1), y2); ++y) + for (int x = std::max (x1, 0); x <= std::min (static_cast (input_->width -1), x2); ++x) + point_indices->indices.push_back (y * input_->width + x); + setBackgroundPointsIndices (point_indices); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::setTrimap(int x1, int y1, int x2, int y2, const pcl::segmentation::grabcut::TrimapValue& t) +{ + using namespace pcl::segmentation::grabcut; + if (x1 > x2) std::swap (x1, x2); + if (y1 > y2) std::swap (y1, y2); + for (int y = std::max (y1, 0); y <= std::min (static_cast (image_height_1_), y2); ++y) + for (int x = std::max (x1, 0); x <= std::min (static_cast (image_width_1_), x2); ++x) + { + std::size_t idx = y * input_->width + x; + trimap_[idx] = TrimapUnknown; + // Immediately set the segmentation as well so that the display will update. + if (t == TrimapForeground) + hard_segmentation_[idx] = SegmentationForeground; + else if (t == TrimapBackground) + hard_segmentation_[idx] = SegmentationBackground; + } + + // Build debugging images + buildImages(); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::refine () +{ +// boost::lock_guard lock (refine_mutex); + pcl::GrabCut::refine (); + buildImages (); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +int +GrabCutHelper::refineOnce () +{ + // boost::lock_guard lock (refine_once_mutex); + int result = pcl::GrabCut::refineOnce (); + buildImages (); + return (result); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::fitGMMs () +{ +// boost::lock_guard lock (fit_gmms_mutex); + pcl::GrabCut::fitGMMs (); + buildImages (); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::buildImages () +{ + using namespace pcl::segmentation::grabcut; + memset (&n_links_image_->points[0], 0, sizeof (float) * n_links_image_->size ()); + for (int y = 0; y < static_cast (image_->height); ++y) + { + for (int x = 0; x < static_cast (image_->width); ++x) + { + std::size_t index = y * image_->width + x; + + if (x > 0 && y < image_height_1_) + { + (*n_links_image_)(x,y) += n_links_[index].weights[0]; + (*n_links_image_)(x-1,y+1) += n_links_[index].weights[0]; + } + + if (y < image_height_1_) + { + (*n_links_image_)(x,y) += n_links_[index].weights[1]; + (*n_links_image_)(x,y+1) += n_links_[index].weights[1]; + } + + if (x < image_width_1_ && y < image_height_1_) + { + (*n_links_image_)(x,y) += n_links_[index].weights[2]; + (*n_links_image_)(x+1,y+1) += n_links_[index].weights[2]; + } + + if (x < image_width_1_) + { + (*n_links_image_)(x,y) += n_links_[index].weights[3]; + (*n_links_image_)(x+1,y) += n_links_[index].weights[3]; + } + + // TLinks cloud + pcl::segmentation::grabcut::Color &tlink_point = t_links_image_->points[index]; + pcl::segmentation::grabcut::Color &gmm_point = gmm_image_->points[index]; + float &alpha_point = alpha_image_->points[index]; + double red = pow (graph_.getSourceEdgeCapacity (index)/L_, 0.25); // red + double green = pow (graph_.getTargetEdgeCapacity (index)/L_, 0.25); // green + tlink_point.r = static_cast (red); + tlink_point.g = static_cast (green); + gmm_point.b = tlink_point.b = 0; + // GMM cloud and Alpha cloud + if (hard_segmentation_[index] == SegmentationForeground) + { + //assert (static_cast(GMM_component_[index]+1)/static_cast (K_) < 1.f); + gmm_point.r = static_cast(GMM_component_[index]+1)/static_cast (K_); + alpha_point = 0; + } + else + { + gmm_point.g = static_cast(GMM_component_[index]+1)/static_cast (K_); + alpha_point = 0.75; + } + } + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::display (int display_type) +{ + switch (display_type) + { + case 0: + glDrawPixels (image_->width, image_->height, GL_RGB, GL_FLOAT, &(image_->points[0])); + break; + + case 1: + glDrawPixels (gmm_image_->width, gmm_image_->height, GL_RGB, GL_FLOAT, &(gmm_image_->points[0])); + break; + + case 2: + glDrawPixels (n_links_image_->width, n_links_image_->height, GL_LUMINANCE, GL_FLOAT, &(n_links_image_->points[0])); + break; + + case 3: + glDrawPixels (t_links_image_->width, t_links_image_->height, GL_RGB, GL_FLOAT, &(t_links_image_->points[0])); + break; + + default: + // Do nothing + break; + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +GrabCutHelper::overlayAlpha () +{ + glDrawPixels (alpha_image_->width, alpha_image_->height, GL_ALPHA, GL_FLOAT, &(alpha_image_->points[0])); +} + +/* GUI interface */ +int display_type = 0; +bool show_mask = false; +bool initialized = false; +// 2D stuff +int xstart, ystart, xend, yend; +bool box = false; +bool left = false, right = false; +bool refining_ = false; +uint32_t width, height; +GrabCutHelper grabcut; +pcl::segmentation::grabcut::Image::Ptr display_image; + +///////////////////////////////////////////////////////////////////////////////////////////// +void +display () +{ + glClear(GL_COLOR_BUFFER_BIT); + + if (display_type == -1) + glDrawPixels (display_image->width, display_image->height, GL_RGB, GL_FLOAT, &(display_image->points[0])); + else + grabcut.display (display_type); + + if (show_mask) + { + grabcut.overlayAlpha (); + } + + if (box) + { + glColor4f( 1, 1, 1, 1 ); + glBegin( GL_LINE_LOOP ); + glVertex2d( xstart, ystart ); + glVertex2d( xstart, yend ); + glVertex2d( xend, yend ); + glVertex2d( xend, ystart ); + glEnd(); + } + + glFlush(); + glutSwapBuffers(); +} + +///////////////////////////////////////////////////////////////////////// +void +idle_callback () +{ + int changed = 0; + + if (refining_) + { + changed = grabcut.refineOnce (); + glutPostRedisplay (); + } + + if (!changed) + { + refining_ = false; + glutIdleFunc (NULL); + } +} + +///////////////////////////////////////////////////////////////////////// +void +motion_callback (int x, int y) +{ + y = height - y; + + if (box == true) + { + xend = x; yend = y; + glutPostRedisplay (); + } + + if (initialized) + { + if (left) + grabcut.setTrimap (x-2,y-2,x+2,y+2,pcl::segmentation::grabcut::TrimapForeground); + + if (right) + grabcut.setTrimap (x-2,y-2,x+2,y+2,pcl::segmentation::grabcut::TrimapForeground); + + glutPostRedisplay (); + } +} + +void +mouse_callback (int button, int state, int x, int y) +{ + y = height - y; + + switch (button) + { + case GLUT_LEFT_BUTTON: + if (state==GLUT_DOWN) + { + left = true; + + if (!initialized) + { + xstart = x; ystart = y; + box = true; + } + } + + if (state==GLUT_UP) + { + left = false; + + if (initialized) + { + grabcut.refineOnce (); + glutPostRedisplay (); + } + else + { + xend = x; yend = y; + grabcut.setBackgroundPointsIndices (xstart, ystart, xend, yend); + box = false; + initialized = true; + show_mask = true; + glutPostRedisplay (); + } + } + break; + + case GLUT_RIGHT_BUTTON: + if (state==GLUT_DOWN) + { + right = true; + } + if (state==GLUT_UP) + { + right = false; + + if (initialized) + { + grabcut.refineOnce (); + glutPostRedisplay (); + } + } + break; + + default: + break; + } +} + +///////////////////////////////////////////////////////////////////////// +void +keyboard_callback (unsigned char key, int, int) +{ + switch (key) + { + case ' ': // space bar show/hide alpha mask + show_mask = !show_mask; + break; + case '0': case 'i': case 'I':// choose the RGB image + display_type = 0; + break; + case '1': case 'g': case 'G':// choose GMM index mask + display_type = 1; + break; + case '2': case 'n': case 'N': // choose N-Link mask + display_type = 2; + break; + case '3': case 't': case 'T': // choose T-Link mask + display_type = 3; + break; + case 'r': // run GrabCut refinement + refining_ = true; + glutIdleFunc (idle_callback); + break; + case 'o': // run one step of GrabCut refinement + grabcut.refineOnce (); + glutPostRedisplay (); + break; + case 'l': // rerun the Orchard-Bowman GMM clustering + grabcut.fitGMMs (); + glutPostRedisplay (); + break; + // case 's': case 'S': + // save (); + // break; + case 'q': case 'Q': +#if defined (FREEGLUT) || defined (GLUI_OPENGLUT) + glutLeaveMainLoop (); +#else + exit (0); +#endif + break; + case 27: + refining_ = false; + glutIdleFunc(NULL); + default: + break; + } + glutPostRedisplay (); +} + +/////////////////////////////////////////////////////////////////////////////////// +int main (int argc, char** argv) +{ + // Parse the command line arguments for .pcd files + std::vector parsed_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); + if (parsed_file_indices.empty ()) + { + pcl::console::print_error ("Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n"); + pcl::console::print_info ("Ideally, need an input file, and two output PCD files, e.g., object.pcd, background.pcd\n"); + return (-1); + } + + std::string object_file = "object.pcd", background_file = "background.pcd"; + if (parsed_file_indices.size () >= 3) + background_file = argv[parsed_file_indices[2]]; + if (parsed_file_indices.size () >= 2) + object_file = argv[parsed_file_indices[1]]; + + pcl::PCDReader reader; + // Test the header + pcl::PCLPointCloud2 dummy; + reader.readHeader (argv[parsed_file_indices[0]], dummy); + pcl::PointCloud::Ptr scene (new pcl::PointCloud); + if (pcl::getFieldIndex (dummy, "rgba") != -1) + { + if (pcl::io::loadPCDFile (argv[parsed_file_indices[0]], *scene) < 0) + { + pcl::console::print_error (stderr, "[error]\n"); + return (-2); + } + } + else + if (pcl::getFieldIndex (dummy, "rgb") != -1) + { + if (pcl::io::loadPCDFile (argv[parsed_file_indices[0]], *scene) < 0) + { + pcl::console::print_error (stderr, "[error]\n"); + return (-2); + } + } + else + { + pcl::console::print_error (stderr, "[No RGB data found!]\n"); + return (-1); + } + + if (scene->isOrganized ()) + { + pcl::console::print_highlight ("Enabling 2D image viewer mode.\n"); + } + + + width = scene->width; + height = scene->height; + + display_type = -1; + + display_image.reset (new pcl::segmentation::grabcut::Image (scene->width, scene->height)); + pcl::PointCloud::Ptr tmp (new pcl::PointCloud (scene->width, scene->height)); + + if (scene->isOrganized ()) + { + pcl::uint32_t height_1 = scene->height -1; + for (std::size_t i = 0; i < scene->height; ++i) + { + for (std::size_t j = 0; j < scene->width; ++j) + { + const pcl::PointXYZRGB &p = (*scene) (j,i); + std::size_t reverse_index = (height_1-i) * scene->width + j; + display_image->points[reverse_index].r = static_cast (p.r) / 255.0; + display_image->points[reverse_index].g = static_cast (p.g) / 255.0; + display_image->points[reverse_index].b = static_cast (p.b) / 255.0; + tmp->points[reverse_index] = p; + } + } + } + + grabcut.setInputCloud (tmp); + + glutInit (&argc,argv); + glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB); + + glutInitWindowSize (width, height); + glutInitWindowPosition (100,100); + + glutCreateWindow ("GrabCut"); + + glOrtho (0,width,0,height,-1,1); + + //set the background color to black (RGBA) + glClearColor(0.0,0.0,0.0,0.0); + glEnable(GL_TEXTURE_2D); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); + + glutDisplayFunc (display); + glutMouseFunc (mouse_callback); + glutMotionFunc (motion_callback); + glutKeyboardFunc (keyboard_callback); + + glutMainLoop (); + + return (0); +} diff --git a/apps/src/ni_linemod.cpp b/apps/src/ni_linemod.cpp index a61c2fdc..9e436a2b 100644 --- a/apps/src/ni_linemod.cpp +++ b/apps/src/ni_linemod.cpp @@ -323,7 +323,7 @@ class NILinemod vector label_indices; vector boundary_indices; mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); - PCL_DEBUG ("Number of planar regions detected: %zu for a cloud of %zu points and %zu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ()); + PCL_DEBUG ("Number of planar regions detected: %lu for a cloud of %lu points and %lu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ()); double max_dist = numeric_limits::max (); // Compute the distances from all the planar regions to the picked point, and select the closest region @@ -428,7 +428,7 @@ class NILinemod PlanarRegion refined_region; pcl::approximatePolygon (region, refined_region, 0.01, false, true); - PCL_INFO ("Planar region: %zu points initial, %zu points after refinement.\n", region.getContour ().size (), refined_region.getContour ().size ()); + PCL_INFO ("Planar region: %lu points initial, %lu points after refinement.\n", region.getContour ().size (), refined_region.getContour ().size ()); cloud_viewer_.addPolygon (refined_region, 0.0, 0.0, 1.0, "refined_region"); cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "refined_region"); diff --git a/apps/src/ni_trajkovic.cpp b/apps/src/ni_trajkovic.cpp new file mode 100644 index 00000000..e497a272 --- /dev/null +++ b/apps/src/ni_trajkovic.cpp @@ -0,0 +1,249 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, O R PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#define SHOW_FPS 1 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace pcl; +typedef PointXYZRGBA PointT; +typedef PointXYZI KeyPointT; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +class TrajkovicDemo +{ + public: + typedef PointCloud Cloud; + typedef Cloud::Ptr CloudPtr; + typedef Cloud::ConstPtr CloudConstPtr; + + TrajkovicDemo (Grabber& grabber, bool enable_3d) + : cloud_viewer_ ("TRAJKOVIC 3D Keypoints -- PointCloud") + , grabber_ (grabber) + , image_viewer_ ("TRAJKOVIC 3D Keypoints -- Image") + , enable_3d_ (enable_3d) + { + } + + ///////////////////////////////////////////////////////////////////////// + void + cloud_callback_3d (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + + // Compute TRAJKOVIC keypoints 3D + TrajkovicKeypoint3D trajkovic; + trajkovic.setInputCloud (cloud); + trajkovic.setNumberOfThreads (6); + keypoints_.reset (new PointCloud); + trajkovic.compute (*keypoints_); + keypoints_indices_ = trajkovic.getKeypointsIndices (); + } + + ///////////////////////////////////////////////////////////////////////// + void + cloud_callback_2d (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + + // Compute TRAJKOVIC keypoints 2D + TrajkovicKeypoint2D trajkovic; + trajkovic.setInputCloud (cloud); + trajkovic.setNumberOfThreads (6); + keypoints_.reset (new PointCloud); + trajkovic.compute (*keypoints_); + keypoints_indices_ = trajkovic.getKeypointsIndices (); + } + + ///////////////////////////////////////////////////////////////////////// + void + init () + { + boost::function cloud_cb; + if (enable_3d_) + cloud_cb = boost::bind (&TrajkovicDemo::cloud_callback_3d, this, _1); + else + cloud_cb = boost::bind (&TrajkovicDemo::cloud_callback_2d, this, _1); + + cloud_connection = grabber_.registerCallback (cloud_cb); + } + + ///////////////////////////////////////////////////////////////////////// + std::string + getStrBool (bool state) + { + std::ostringstream ss; + ss << state; + return (ss.str ()); + } + + ///////////////////////////////////////////////////////////////////////// + void + run () + { + grabber_.start (); + + bool image_init = false, cloud_init = false; + bool keypts = true; + + while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ()) + { + PointCloud::Ptr keypoints; + CloudConstPtr cloud; + if (cloud_mutex_.try_lock ()) + { + cloud_.swap (cloud); + keypoints_.swap (keypoints); + + cloud_mutex_.unlock (); + } + + if (cloud) + { + int w (cloud->width); + if (!cloud_init) + { + cloud_viewer_.setPosition (0, 0); + cloud_viewer_.setSize (cloud->width, cloud->height); + cloud_init = !cloud_init; + } + + if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud")) + { + cloud_viewer_.addPointCloud (cloud, "OpenNICloud"); + cloud_viewer_.resetCameraViewpoint ("OpenNICloud"); + } + + if (!image_init) + { + image_viewer_.setPosition (cloud->width, 0); + image_viewer_.setSize (cloud->width, cloud->height); + image_init = !image_init; + } + + image_viewer_.addRGBImage (cloud); + + if (keypoints && !keypoints->empty ()) + { + image_viewer_.removeLayer (getStrBool (keypts)); + std::vector uv; + uv.reserve (keypoints_indices_->indices.size () * 2); + for (std::vector::const_iterator id = keypoints_indices_->indices.begin (); + id != keypoints_indices_->indices.end (); + ++id) + { + int u (*id % w); + int v (*id / w); + image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 5, getStrBool (!keypts), 0.5); + } + keypts = !keypts; + + visualization::PointCloudColorHandlerCustom blue (keypoints, 0, 0, 255); + if (!cloud_viewer_.updatePointCloud (keypoints, blue, "keypoints")) + cloud_viewer_.addPointCloud (keypoints, blue, "keypoints"); + cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints"); + cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints"); + } + } + + cloud_viewer_.spinOnce (); + image_viewer_.spinOnce (); + boost::this_thread::sleep (boost::posix_time::microseconds (100)); + } + + grabber_.stop (); + cloud_connection.disconnect (); + } + + visualization::PCLVisualizer cloud_viewer_; + Grabber& grabber_; + boost::mutex cloud_mutex_; + CloudConstPtr cloud_; + + visualization::ImageViewer image_viewer_; + + PointCloud::Ptr keypoints_; + pcl::PointIndicesConstPtr keypoints_indices_; + bool enable_3d_; + private: + boost::signals2::connection cloud_connection; +}; + +/* ---[ */ +int +main (int argc, char** argv) +{ + if (pcl::console::find_switch (argc, argv, "-h")) + { + pcl::console::print_info ("Syntax is: %s [-device device_id_string] [-2d]\n", argv[0]); + return (0); + } + + std::string device_id ("#1"); + bool enable_3d = true; + if (pcl::console::find_switch (argc, argv, "-2d")) + enable_3d = false; + + if (pcl::console::find_argument (argc, argv, "-device")) + pcl::console::parse (argc, argv, "-device", device_id); + + pcl::console::print_info ("Extracting Trajkovic %s keypoints from device %s.\n", + enable_3d ? "3D" : "2D", device_id.c_str ()); + + OpenNIGrabber grabber (device_id); + + TrajkovicDemo openni_viewer (grabber, enable_3d); + + openni_viewer.init (); + openni_viewer.run (); + + return (0); +} +/* ]--- */ diff --git a/apps/src/openni_klt.cpp b/apps/src/openni_klt.cpp new file mode 100644 index 00000000..2f1a9f00 --- /dev/null +++ b/apps/src/openni_klt.cpp @@ -0,0 +1,408 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014-, Open Perception, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#define MEASURE_FUNCTION_TIME +#include //fps calculations +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SHOW_FPS 1 +#if SHOW_FPS +#define FPS_CALC(_WHAT_) \ +do \ +{ \ + static unsigned count = 0;\ + static double last = pcl::getTime ();\ + double now = pcl::getTime (); \ + ++count; \ + if (now - last >= 1.0) \ + { \ + std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \ + count = 0; \ + last = now; \ + } \ +}while(false) +#else +#define FPS_CALC(_WHAT_) \ +do \ +{ \ +}while(false) +#endif + +void +printHelp (int, char **argv) +{ + using pcl::console::print_error; + using pcl::console::print_info; + + print_error ("Syntax is: %s [(( | ) [-depthmode ] [-imagemode ] [-xyz] | -l []| -h | --help)]\n", argv [0]); + print_info ("%s -h | --help : shows this help\n", argv [0]); + print_info ("%s -xyz : use only XYZ values and ignore RGB components (this flag is required for use with ASUS Xtion Pro) \n", argv [0]); + print_info ("%s -l : list all available devices\n", argv [0]); + print_info ("%s -l :list all available modes for specified device\n", argv [0]); + print_info ("\t\t may be \"#1\", \"#2\", ... for the first, second etc device in the list\n"); +#ifndef _WIN32 + print_info ("\t\t bus@address for the device connected to a specific usb-bus / address combination\n"); + print_info ("\t\t \n"); +#endif + print_info ("\n\nexamples:\n"); + print_info ("%s \"#1\"\n", argv [0]); + print_info ("\t\t uses the first device.\n"); + print_info ("%s \"./temp/test.oni\"\n", argv [0]); + print_info ("\t\t uses the oni-player device to play back oni file given by path.\n"); + print_info ("%s -l\n", argv [0]); + print_info ("\t\t list all available devices.\n"); + print_info ("%s -l \"#2\"\n", argv [0]); + print_info ("\t\t list all available modes for the second device.\n"); + #ifndef _WIN32 + print_info ("%s A00361800903049A\n", argv [0]); + print_info ("\t\t uses the device with the serial number \'A00361800903049A\'.\n"); + print_info ("%s 1@16\n", argv [0]); + print_info ("\t\t uses the device on address 16 at USB bus 1.\n"); + #endif +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OpenNIViewer +{ + public: + typedef pcl::PointCloud Cloud; + typedef typename Cloud::ConstPtr CloudConstPtr; + + OpenNIViewer (pcl::Grabber& grabber) + : image_viewer_ () + , grabber_ (grabber) + , rgb_data_ (0), rgb_data_size_ (0) + { } + + void + detect_keypoints (const CloudConstPtr& cloud) + { + pcl::HarrisKeypoint2D harris; + harris.setInputCloud (cloud); + harris.setNumberOfThreads (6); + harris.setNonMaxSupression (true); + harris.setRadiusSearch (0.01); + harris.setMethod (pcl::HarrisKeypoint2D::TOMASI); + harris.setThreshold (0.05); + harris.setWindowWidth (5); + harris.setWindowHeight (5); + pcl::PointCloud::Ptr response (new pcl::PointCloud); + harris.compute (*response); + points_ = harris.getKeypointsIndices (); + } + + void + cloud_callback (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + // Compute Tomasi keypoints + tracker_->setInputCloud (cloud_); + // if (!points_) + // { + if (!points_ || (counter_ % 10 == 0)) + { + detect_keypoints (cloud_); + tracker_->setPointsToTrack (points_); + } + + // } + tracker_->compute (); + ++counter_; + } + + void + image_callback (const boost::shared_ptr& image) + { + FPS_CALC ("image callback"); + boost::mutex::scoped_lock lock (image_mutex_); + image_ = image; + + if (image->getEncoding () != openni_wrapper::Image::RGB) + { + if (rgb_data_size_ < image->getWidth () * image->getHeight ()) + { + if (rgb_data_) + delete [] rgb_data_; + rgb_data_size_ = image->getWidth () * image->getHeight (); + rgb_data_ = new unsigned char [rgb_data_size_ * 3]; + } + image_->fillRGB (image_->getWidth (), image_->getHeight (), rgb_data_); + } + } + + void + keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*) + { + static pcl::PCDWriter writer; + static std::ostringstream frame; + if (event.keyUp ()) + { + if ((event.getKeyCode () == 's') || (event.getKeyCode () == 'S')) + { + boost::mutex::scoped_lock lock (cloud_mutex_); + frame.str ("frame-"); + frame << boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ()) << ".pcd"; + writer.writeBinaryCompressed (frame.str (), *cloud_); + PCL_INFO ("Written cloud %s.\n", frame.str ().c_str ()); + } + } + } + + void + mouse_callback (const pcl::visualization::MouseEvent& mouse_event, void*) + { + if (mouse_event.getType() == pcl::visualization::MouseEvent::MouseButtonPress && mouse_event.getButton() == pcl::visualization::MouseEvent::LeftButton) + { + cout << "left button pressed @ " << mouse_event.getX () << " , " << mouse_event.getY () << endl; + } + } + + /** + * @brief starts the main loop + */ + void + run () + { + boost::function cloud_cb = boost::bind (&OpenNIViewer::cloud_callback, this, _1); + boost::signals2::connection cloud_connection = grabber_.registerCallback (cloud_cb); + + boost::signals2::connection image_connection; + if (grabber_.providesCallback&)>()) + { + image_viewer_.reset (new pcl::visualization::ImageViewer ("Pyramidal KLT Tracker")); + boost::function&) > image_cb = boost::bind (&OpenNIViewer::image_callback, this, _1); + image_connection = grabber_.registerCallback (image_cb); + } + + tracker_.reset (new pcl::tracking::PyramidalKLTTracker); + + bool image_init = false; + + grabber_.start (); + + while (!image_viewer_->wasStopped ()) + { + boost::shared_ptr image; + CloudConstPtr cloud; + + // See if we can get a cloud + if (cloud_mutex_.try_lock ()) + { + cloud_.swap (cloud); + cloud_mutex_.unlock (); + } + + // See if we can get an image + if (image_mutex_.try_lock ()) + { + image_.swap (image); + image_mutex_.unlock (); + } + + if (image) + { + if (!image_init && cloud && cloud->width != 0) + { + image_viewer_->setPosition (0, 0); + image_viewer_->setSize (cloud->width, cloud->height); + image_init = !image_init; + } + + if (image->getEncoding() == openni_wrapper::Image::RGB) + image_viewer_->addRGBImage (image->getMetaData ().Data (), image->getWidth (), image->getHeight ()); + else + image_viewer_->addRGBImage (rgb_data_, image->getWidth (), image->getHeight ()); + image_viewer_->spinOnce (); + } + + if (tracker_->getInitialized () && cloud_) + { + if (points_mutex_.try_lock ()) + { + keypoints_ = tracker_->getTrackedPoints (); + points_status_ = tracker_->getPointsToTrackStatus (); + points_mutex_.unlock (); + } + + std::vector markers; + markers.reserve (keypoints_->size () * 2); + for (std::size_t i = 0; i < keypoints_->size (); ++i) + { + if (points_status_->indices[i] < 0) + continue; + const pcl::PointUV &uv = keypoints_->points[i]; + markers.push_back (uv.u); + markers.push_back (uv.v); + } + image_viewer_->removeLayer ("tracked"); + image_viewer_->markPoints (markers, pcl::visualization::blue_color, + pcl::visualization::red_color, 5, "tracked", 1.0); + + } + } + + grabber_.stop (); + + cloud_connection.disconnect (); + image_connection.disconnect (); + if (rgb_data_) + delete[] rgb_data_; + } + + boost::shared_ptr image_viewer_; + + pcl::Grabber& grabber_; + boost::mutex cloud_mutex_; + boost::mutex image_mutex_; + boost::mutex points_mutex_; + + CloudConstPtr cloud_; + boost::shared_ptr image_; + unsigned char* rgb_data_; + unsigned rgb_data_size_; + boost::shared_ptr > tracker_; + pcl::PointCloud::ConstPtr keypoints_; + pcl::PointIndicesConstPtr points_; + pcl::PointIndicesConstPtr points_status_; + int counter_; +}; + +// Create the PCLVisualizer object +boost::shared_ptr img; + +/* ---[ */ +int +main (int argc, char** argv) +{ + std::string device_id(""); + pcl::OpenNIGrabber::Mode depth_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode; + pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode; + bool xyz = false; + + if (argc >= 2) + { + device_id = argv[1]; + if (device_id == "--help" || device_id == "-h") + { + printHelp(argc, argv); + return 0; + } + else if (device_id == "-l") + { + if (argc >= 3) + { + pcl::OpenNIGrabber grabber(argv[2]); + boost::shared_ptr device = grabber.getDevice(); + cout << "Supported depth modes for device: " << device->getVendorName() << " , " << device->getProductName() << endl; + std::vector > modes = grabber.getAvailableDepthModes(); + for (std::vector >::const_iterator it = modes.begin(); it != modes.end(); ++it) + { + cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << endl; + } + + if (device->hasImageStream ()) + { + cout << endl << "Supported image modes for device: " << device->getVendorName() << " , " << device->getProductName() << endl; + modes = grabber.getAvailableImageModes(); + for (std::vector >::const_iterator it = modes.begin(); it != modes.end(); ++it) + { + cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << endl; + } + } + } + else + { + openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance(); + if (driver.getNumberDevices() > 0) + { + for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) + { + cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName(deviceIdx) << ", product: " << driver.getProductName(deviceIdx) + << ", connected: " << driver.getBus(deviceIdx) << " @ " << driver.getAddress(deviceIdx) << ", serial number: \'" << driver.getSerialNumber(deviceIdx) << "\'" << endl; + } + + } + else + cout << "No devices connected." << endl; + + cout <<"Virtual Devices available: ONI player" << endl; + } + return 0; + } + } + else + { + openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance(); + if (driver.getNumberDevices() > 0) + cout << "Device Id not set, using first device." << endl; + } + + unsigned mode; + if (pcl::console::parse(argc, argv, "-depthmode", mode) != -1) + depth_mode = pcl::OpenNIGrabber::Mode (mode); + + if (pcl::console::parse(argc, argv, "-imagemode", mode) != -1) + image_mode = pcl::OpenNIGrabber::Mode (mode); + + if (pcl::console::find_argument (argc, argv, "-xyz") != -1) + xyz = true; + + pcl::OpenNIGrabber grabber (device_id, depth_mode, image_mode); + + if (xyz || !grabber.providesCallback ()) + { + OpenNIViewer openni_viewer (grabber); + openni_viewer.run (); + } + else + { + OpenNIViewer openni_viewer (grabber); + openni_viewer.run (); + } + + return (0); +} +/* ]--- */ diff --git a/apps/src/openni_organized_multi_plane_segmentation.cpp b/apps/src/openni_organized_multi_plane_segmentation.cpp index 3fbef21c..65e70496 100644 --- a/apps/src/openni_organized_multi_plane_segmentation.cpp +++ b/apps/src/openni_organized_multi_plane_segmentation.cpp @@ -73,7 +73,7 @@ class OpenNIOrganizedMultiPlaneSegmentation viewer->addPointCloud (cloud, single_color, "cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud"); - viewer->addCoordinateSystem (1.0); + viewer->addCoordinateSystem (1.0, "global"); viewer->initCameraParameters (); return (viewer); } @@ -95,7 +95,7 @@ class OpenNIOrganizedMultiPlaneSegmentation char name[1024]; for (size_t i = 0; i < prev_models_size; i++) { - sprintf (name, "normal_%zu", i); + sprintf (name, "normal_%lu", i); viewer->removeShape (name); sprintf (name, "plane_%02zu", i); @@ -173,7 +173,7 @@ class OpenNIOrganizedMultiPlaneSegmentation pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]), centroid[1] + (0.5f * model[1]), centroid[2] + (0.5f * model[2])); - sprintf (name, "normal_%zu", i); + sprintf (name, "normal_%lu", i); viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name); contour->points = regions[i].getContour (); diff --git a/apps/src/pcd_organized_multi_plane_segmentation.cpp b/apps/src/pcd_organized_multi_plane_segmentation.cpp index 1ae0aa9f..954a8653 100644 --- a/apps/src/pcd_organized_multi_plane_segmentation.cpp +++ b/apps/src/pcd_organized_multi_plane_segmentation.cpp @@ -68,7 +68,7 @@ class PCDOrganizedMultiPlaneSegmentation viewer.setBackgroundColor (0, 0, 0); //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud"); - viewer.addCoordinateSystem (1.0); + viewer.addCoordinateSystem (1.0, "global"); viewer.initCameraParameters (); viewer.registerKeyboardCallback(&PCDOrganizedMultiPlaneSegmentation::keyboard_callback, *this, 0); } diff --git a/apps/src/pcd_select_object_plane.cpp b/apps/src/pcd_select_object_plane.cpp index 377b7030..e7c527cf 100644 --- a/apps/src/pcd_select_object_plane.cpp +++ b/apps/src/pcd_select_object_plane.cpp @@ -227,7 +227,7 @@ class ObjectSelection ec.setIndices (points_above_plane); ec.extract (euclidean_label_indices); - print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%zu", euclidean_label_indices.size ()); print_info (" clusters]\n"); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n"); } // For each cluster found @@ -270,7 +270,7 @@ class ObjectSelection // Estimate normals PointCloud::Ptr normal_cloud (new PointCloud); estimateNormals (cloud_, *normal_cloud); - print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%zu", normal_cloud->size ()); print_info (" points]\n"); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n"); OrganizedMultiPlaneSegmentation mps; mps.setMinInliers (1000); @@ -313,7 +313,7 @@ class ObjectSelection print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++); TicToc tt; tt.tic (); seg.segment (*inliers, coefficients); - print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%zu", inliers->indices.size ()); print_info (" points]\n"); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n"); // No datasets could be found anymore if (inliers->indices.empty ()) @@ -335,7 +335,7 @@ class ObjectSelection cloud_segmented.swap (cloud_remaining); } } - print_highlight ("Number of planar regions detected: %zu for a cloud of %zu points\n", regions.size (), cloud_->size ()); + print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ()); double max_dist = numeric_limits::max (); // Compute the distances from all the planar regions to the picked point, and select the closest region @@ -358,7 +358,7 @@ class ObjectSelection if (cloud_->isOrganized ()) { approximatePolygon (regions[idx], region, 0.01f, false, true); - print_highlight ("Planar region: %zu points initial, %zu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ()); + print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ()); } else { @@ -384,7 +384,7 @@ class ObjectSelection PointCloud plane_hull; chull.reconstruct (plane_hull); region.setContour (plane_hull); - print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%zu", plane_hull.size ()); print_info (" points]\n"); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n"); } } @@ -566,7 +566,7 @@ class ObjectSelection cloud_viewer_->addPointCloud (cloud_, "scene"); cloud_viewer_->resetCameraViewpoint ("scene"); - cloud_viewer_->addCoordinateSystem (0.1, 0, 0, 0); + cloud_viewer_->addCoordinateSystem (0.1, 0, 0, 0, "global"); } ///////////////////////////////////////////////////////////////////////// @@ -584,7 +584,7 @@ class ObjectSelection return (false); } print_info ("[done, "); print_value ("%g", tt.toc ()); - print_info (" ms : "); print_value ("%zu", cloud_->size ()); print_info (" points]\n"); + print_info (" ms : "); print_value ("%lu", cloud_->size ()); print_info (" points]\n"); if (cloud_->isOrganized ()) search_.reset (new search::OrganizedNeighbor); diff --git a/apps/src/render_views_tesselated_sphere.cpp b/apps/src/render_views_tesselated_sphere.cpp index 513fb526..48e7dfe8 100644 --- a/apps/src/render_views_tesselated_sphere.cpp +++ b/apps/src/render_views_tesselated_sphere.cpp @@ -61,7 +61,11 @@ pcl::apps::RenderViewsTesselatedSphere::generateViews() { vtkSmartPointer trans_filter_center = vtkSmartPointer::New (); trans_filter_center->SetTransform (trans_center); +#if VTK_MAJOR_VERSION < 6 trans_filter_center->SetInput (polydata_); +#else + trans_filter_center->SetInputData (polydata_); +#endif trans_filter_center->Update (); vtkSmartPointer mapper = vtkSmartPointer::New (); @@ -116,7 +120,6 @@ pcl::apps::RenderViewsTesselatedSphere::generateViews() { // Get camera positions vtkPolyData *sphere = subdivide->GetOutput (); - sphere->Update (); std::vector cam_positions; if (!use_vertices_) diff --git a/cmake/Modules/FindEigen.cmake b/cmake/Modules/FindEigen.cmake index 9cb7086e..5819a5c7 100644 --- a/cmake/Modules/FindEigen.cmake +++ b/cmake/Modules/FindEigen.cmake @@ -5,6 +5,7 @@ # EIGEN_FOUND - True if Eigen was found. # EIGEN_INCLUDE_DIRS - Directories containing the Eigen include files. # EIGEN_DEFINITIONS - Compiler flags for Eigen. +# EIGEN_VERSION - Package version find_package(PkgConfig QUIET) pkg_check_modules(PC_EIGEN eigen3) @@ -13,6 +14,10 @@ set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) if(CMAKE_SYSTEM_NAME STREQUAL Linux) set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} /usr /usr/local) endif(CMAKE_SYSTEM_NAME STREQUAL Linux) +if(APPLE) + list(APPEND CMAKE_INCLUDE_PATH /opt/local) + set(CMAKE_FIND_FRAMEWORK NEVER) +endif() find_path(EIGEN_INCLUDE_DIR Eigen/Core HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" @@ -20,8 +25,20 @@ find_path(EIGEN_INCLUDE_DIR Eigen/Core "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" PATH_SUFFIXES eigen3 include/eigen3 include) +if(EIGEN_INCLUDE_DIR) + file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen_world_version_match "${_eigen_version_header}") + set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen_major_version_match "${_eigen_version_header}") + set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_version_match "${_eigen_version_header}") + set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") + set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) +endif(EIGEN_INCLUDE_DIR) set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) +set(CMAKE_FIND_FRAMEWORK) include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) @@ -29,6 +46,5 @@ find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) mark_as_advanced(EIGEN_INCLUDE_DIR) if(EIGEN_FOUND) - message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS}, version: ${EIGEN_VERSION})") endif(EIGEN_FOUND) - diff --git a/cmake/Modules/FindFZAPI.cmake b/cmake/Modules/FindFZAPI.cmake index 0cad5e0f..0f7507ae 100644 --- a/cmake/Modules/FindFZAPI.cmake +++ b/cmake/Modules/FindFZAPI.cmake @@ -6,17 +6,13 @@ # FZAPI_INCLUDE_DIRS - Directories containing the FZAPI include files. # FZAPI_LIBRARIES - Libraries needed to use FZAPI. -#MESSAGE("Searching for Fotonic in: ${FZ_API_DIR}") - if(FZ_API_DIR) # Find include dirs - #MESSAGE("Searching Fotonic includes: ${FZ_API_DIR}") find_path(FZAPI_INCLUDE_DIR fz_api.h PATHS "${FZ_API_DIR}" NO_DEFAULT_PATH DOC "Fotonic include directories") # Find libraries - #MESSAGE("Searching Fotonic libs: ${FZ_API_DIR}/Release") find_library(FZAPI_LIBS fotonic_fz_api HINTS "${FZ_API_DIR}/Release" NO_DEFAULT_PATH DOC "Fotonic libraries") @@ -24,9 +20,6 @@ else() set(FZ_API_DIR "default value" CACHE FILEPATH "directory of Fotonic API") endif() -#MESSAGE("FZAPI_INCLUDE_DIR: ${FZAPI_INCLUDE_DIR}") -#MESSAGE("FZAPI_LIBS: ${FZAPI_LIBS}") - include(FindPackageHandleStandardArgs) find_package_handle_standard_args(FZAPI DEFAULT_MSG FZAPI_LIBS FZAPI_INCLUDE_DIR) diff --git a/cmake/Modules/FindGLEW.cmake b/cmake/Modules/FindGLEW.cmake index c45585c3..f6c6e2a1 100644 --- a/cmake/Modules/FindGLEW.cmake +++ b/cmake/Modules/FindGLEW.cmake @@ -42,11 +42,17 @@ ELSE (WIN32) IF (APPLE) # These values for Apple could probably do with improvement. + if (${CMAKE_SYSTEM_VERSION} VERSION_LESS "13.0.0") FIND_PATH( GLEW_INCLUDE_DIR glew.h /System/Library/Frameworks/GLEW.framework/Versions/A/Headers ${OPENGL_LIBRARY_DIR} - ) + ) SET(GLEW_GLEW_LIBRARY "-framework GLEW" CACHE STRING "GLEW library for OSX") + else (${CMAKE_SYSTEM_VERSION} VERSION_LESS "13.0.0") + find_package(PkgConfig) + pkg_check_modules(glew GLEW) + SET(GLEW_GLEW_LIBRARY ${GLEW_LIBRARIES} CACHE STRING "GLEW library for OSX") + endif (${CMAKE_SYSTEM_VERSION} VERSION_LESS "13.0.0") SET(GLEW_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX") ELSE (APPLE) diff --git a/cmake/Modules/FindGtest.cmake b/cmake/Modules/FindGtest.cmake new file mode 100644 index 00000000..7c393737 --- /dev/null +++ b/cmake/Modules/FindGtest.cmake @@ -0,0 +1,40 @@ +############################################################################### +# Find GTest +# +# This sets the following variables: +# GTEST_FOUND - True if GTest was found. +# GTEST_INCLUDE_DIRS - Directories containing the GTest include files. +# GTEST_SRC - Directories containing the GTest source files. + +if(CMAKE_SYSTEM_NAME STREQUAL Linux) + set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} /usr /usr/local) +endif(CMAKE_SYSTEM_NAME STREQUAL Linux) +if(APPLE) + list(APPEND CMAKE_INCLUDE_PATH /opt/local) + set(CMAKE_FIND_FRAMEWORK NEVER) +endif() + +find_path(GTEST_INCLUDE_DIR gtest/gtest.h + HINTS "${GTEST_ROOT}" "$ENV{GTEST_ROOT}" + PATHS "$ENV{PROGRAMFILES}/gtest" "$ENV{PROGRAMW6432}/gtest" + PATHS "$ENV{PROGRAMFILES}/gtest-1.7.0" "$ENV{PROGRAMW6432}/gtest-1.7.0" + PATH_SUFFIXES gtest include/gtest include) + +find_path(GTEST_SRC_DIR src/gtest-all.cc + HINTS "${GTEST_ROOT}" "$ENV{GTEST_ROOT}" + PATHS "$ENV{PROGRAMFILES}/gtest" "$ENV{PROGRAMW6432}/gtest" + PATHS "$ENV{PROGRAMFILES}/gtest-1.7.0" "$ENV{PROGRAMW6432}/gtest-1.7.0" + PATH /usr/src/gtest + PATH_SUFFIXES gtest usr/src/gtest) + +set(GTEST_INCLUDE_DIRS ${GTEST_INCLUDE_DIR}) +set(CMAKE_FIND_FRAMEWORK) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Gtest DEFAULT_MSG GTEST_INCLUDE_DIR GTEST_SRC_DIR) + +mark_as_advanced(GTEST_INCLUDE_DIR GTEST_SRC_DIR) + +if(GTEST_FOUND) + message(STATUS "GTest found (include: ${GTEST_INCLUDE_DIRS}, src: ${GTEST_SRC_DIR})") +endif(GTEST_FOUND) diff --git a/cmake/Modules/FindMPI.cmake b/cmake/Modules/FindMPI.cmake deleted file mode 100644 index bfc80eeb..00000000 --- a/cmake/Modules/FindMPI.cmake +++ /dev/null @@ -1,617 +0,0 @@ -# - Find a Message Passing Interface (MPI) implementation -# The Message Passing Interface (MPI) is a library used to write -# high-performance distributed-memory parallel applications, and -# is typically deployed on a cluster. MPI is a standard interface -# (defined by the MPI forum) for which many implementations are -# available. All of them have somewhat different include paths, -# libraries to link against, etc., and this module tries to smooth -# out those differences. -# -# === Variables === -# -# This module will set the following variables per language in your project, -# where is one of C, CXX, or Fortran: -# MPI__FOUND TRUE if FindMPI found MPI flags for -# MPI__COMPILER MPI Compiler wrapper for -# MPI__COMPILE_FLAGS Compilation flags for MPI programs -# MPI__INCLUDE_PATH Include path(s) for MPI header -# MPI__LINK_FLAGS Linking flags for MPI programs -# MPI__LIBRARIES All libraries to link MPI programs against -# Additionally, FindMPI sets the following variables for running MPI -# programs from the command line: -# MPIEXEC Executable for running MPI programs -# MPIEXEC_NUMPROC_FLAG Flag to pass to MPIEXEC before giving -# it the number of processors to run on -# MPIEXEC_PREFLAGS Flags to pass to MPIEXEC directly -# before the executable to run. -# MPIEXEC_POSTFLAGS Flags to pass to MPIEXEC after other flags -# === Usage === -# -# To use this module, simply call FindMPI from a CMakeLists.txt file, or -# run find_package(MPI), then run CMake. If you are happy with the auto- -# detected configuration for your language, then you're done. If not, you -# have two options: -# 1. Set MPI__COMPILER to the MPI wrapper (mpicc, etc.) of your -# choice and reconfigure. FindMPI will attempt to determine all the -# necessary variables using THAT compiler's compile and link flags. -# 2. If this fails, or if your MPI implementation does not come with -# a compiler wrapper, then set both MPI__LIBRARIES and -# MPI__INCLUDE_PATH. You may also set any other variables -# listed above, but these two are required. This will circumvent -# autodetection entirely. -# When configuration is successful, MPI__COMPILER will be set to the -# compiler wrapper for , if it was found. MPI__FOUND and other -# variables above will be set if any MPI implementation was found for , -# regardless of whether a compiler was found. -# -# When using MPIEXEC to execute MPI applications, you should typically use -# all of the MPIEXEC flags as follows: -# ${MPIEXEC} ${MPIEXEC_NUMPROC_FLAG} PROCS -# ${MPIEXEC_PREFLAGS} EXECUTABLE ${MPIEXEC_POSTFLAGS} ARGS -# where PROCS is the number of processors on which to execute the program, -# EXECUTABLE is the MPI program, and ARGS are the arguments to pass to the -# MPI program. -# -# === Backward Compatibility === -# -# For backward compatibility with older versions of FindMPI, these -# variables are set, but deprecated: -# MPI_FOUND MPI_COMPILER MPI_LIBRARY -# MPI_COMPILE_FLAGS MPI_INCLUDE_PATH MPI_EXTRA_LIBRARY -# MPI_LINK_FLAGS MPI_LIBRARIES -# In new projects, please use the MPI__XXX equivalents. - -#============================================================================= -# Copyright 2001-2011 Kitware, Inc. -# Copyright 2010-2011 Todd Gamblin tgamblin@llnl.gov -# Copyright 2001-2009 Dave Partyka -# -# Distributed under the OSI-approved BSD License (the "License"); -# see accompanying file Copyright.txt for details. -# -# This software is distributed WITHOUT ANY WARRANTY; without even the -# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -# See the License for more information. -#============================================================================= -# (To distribute this file outside of CMake, substitute the full -# License text for the above reference.) - -# include this to handle the QUIETLY and REQUIRED arguments -include(FindPackageHandleStandardArgs) -include(GetPrerequisites) - -# -# This part detects MPI compilers, attempting to wade through the mess of compiler names in -# a sensible way. -# -# The compilers are detected in this order: -# -# 1. Try to find the most generic availble MPI compiler, as this is usually set up by -# cluster admins. e.g., if plain old mpicc is available, we'll use it and assume it's -# the right compiler. -# -# 2. If a generic mpicc is NOT found, then we attempt to find one that matches -# CMAKE__COMPILER_ID. e.g. if you are using XL compilers, we'll try to find mpixlc -# and company, but not mpiicc. This hopefully prevents toolchain mismatches. -# -# If you want to force a particular MPI compiler other than what we autodetect (e.g. if you -# want to compile regular stuff with GNU and parallel stuff with Intel), you can always set -# your favorite MPI__COMPILER explicitly and this stuff will be ignored. -# - -# Start out with the generic MPI compiler names, as these are most commonly used. -set(_MPI_C_COMPILER_NAMES mpicc mpcc mpicc_r mpcc_r) -set(_MPI_CXX_COMPILER_NAMES mpicxx mpiCC mpcxx mpCC mpic++ mpc++ - mpicxx_r mpiCC_r mpcxx_r mpCC_r mpic++_r mpc++_r) -set(_MPI_Fortran_COMPILER_NAMES mpif95 mpif95_r mpf95 mpf95_r - mpif90 mpif90_r mpf90 mpf90_r - mpif77 mpif77_r mpf77 mpf77_r) - -# GNU compiler names -set(_MPI_GNU_C_COMPILER_NAMES mpigcc mpgcc mpigcc_r mpgcc_r) -set(_MPI_GNU_CXX_COMPILER_NAMES mpig++ mpg++ mpig++_r mpg++_r) -set(_MPI_GNU_Fortran_COMPILER_NAMES mpigfortran mpgfortran mpigfortran_r mpgfortran_r - mpig77 mpig77_r mpg77 mpg77_r) - -# Intel MPI compiler names -set(_MPI_Intel_C_COMPILER_NAMES mpiicc) -set(_MPI_Intel_CXX_COMPILER_NAMES mpiicpc mpiicxx mpiic++ mpiiCC) -set(_MPI_Intel_Fortran_COMPILER_NAMES mpiifort mpiif95 mpiif90 mpiif77) - -# PGI compiler names -set(_MPI_PGI_C_COMPILER_NAMES mpipgcc mppgcc) -set(_MPI_PGI_CXX_COMPILER_NAMES mpipgCC mppgCC) -set(_MPI_PGI_Fortran_COMPILER_NAMES mpipgf95 mpipgf90 mppgf95 mppgf90 mpipgf77 mppgf77) - -# XLC MPI Compiler names -set(_MPI_XL_C_COMPILER_NAMES mpxlc mpxlc_r mpixlc mpixlc_r) -set(_MPI_XL_CXX_COMPILER_NAMES mpixlcxx mpixlC mpixlc++ mpxlcxx mpxlc++ mpixlc++ mpxlCC - mpixlcxx_r mpixlC_r mpixlc++_r mpxlcxx_r mpxlc++_r mpixlc++_r mpxlCC_r) -set(_MPI_XL_Fortran_COMPILER_NAMES mpixlf95 mpixlf95_r mpxlf95 mpxlf95_r - mpixlf90 mpixlf90_r mpxlf90 mpxlf90_r - mpixlf77 mpixlf77_r mpxlf77 mpxlf77_r - mpixlf mpixlf_r mpxlf mpxlf_r) - -# append vendor-specific compilers to the list if we either don't know the compiler id, -# or if we know it matches the regular compiler. -foreach (lang C CXX Fortran) - foreach (id GNU Intel PGI XL) - if (NOT CMAKE_${lang}_COMPILER_ID OR "${CMAKE_${lang}_COMPILER_ID}" STREQUAL "${id}") - list(APPEND _MPI_${lang}_COMPILER_NAMES ${_MPI_${id}_${lang}_COMPILER_NAMES}) - endif() - unset(_MPI_${id}_${lang}_COMPILER_NAMES) # clean up the namespace here - endforeach() -endforeach() - - -# Names to try for MPI exec -set(_MPI_EXEC_NAMES mpiexec mpirun lamexec srun) - -# Grab the path to MPI from the registry if we're on windows. -set(_MPI_PREFIX_PATH) -if(WIN32) - list(APPEND _MPI_PREFIX_PATH "[HKEY_LOCAL_MACHINE\\SOFTWARE\\MPICH\\SMPD;binary]/..") - list(APPEND _MPI_PREFIX_PATH "[HKEY_LOCAL_MACHINE\\SOFTWARE\\MPICH2;Path]") - list(APPEND _MPI_PREFIX_PATH "$ENV{ProgramW6432}/MPICH2/") -endif() - -# Build a list of prefixes to search for MPI. -foreach(SystemPrefixDir ${CMAKE_SYSTEM_PREFIX_PATH}) - foreach(MpiPackageDir ${_MPI_PREFIX_PATH}) - if(EXISTS ${SystemPrefixDir}/${MpiPackageDir}) - list(APPEND _MPI_PREFIX_PATH "${SystemPrefixDir}/${MpiPackageDir}") - endif() - endforeach() -endforeach() - - -# -# interrogate_mpi_compiler(lang try_libs) -# -# Attempts to extract compiler and linker args from an MPI compiler. The arguments set -# by this function are: -# -# MPI__INCLUDE_PATH MPI__LINK_FLAGS MPI__FOUND -# MPI__COMPILE_FLAGS MPI__LIBRARIES -# -# MPI__COMPILER must be set beforehand to the absolute path to an MPI compiler for -# . Additionally, MPI__INCLUDE_PATH and MPI__LIBRARIES may be set -# to skip autodetection. -# -# If try_libs is TRUE, this will also attempt to find plain MPI libraries in the usual -# way. In general, this is not as effective as interrogating the compilers, as it -# ignores language-specific flags and libraries. However, some MPI implementations -# (Windows implementations) do not have compiler wrappers, so this approach must be used. -# -function (interrogate_mpi_compiler lang try_libs) - # MPI_${lang}_NO_INTERROGATE will be set to a compiler name when the *regular* compiler was - # discovered to be the MPI compiler. This happens on machines like the Cray XE6 that use - # modules to set cc, CC, and ftn to the MPI compilers. If the user force-sets another MPI - # compiler, MPI_${lang}_COMPILER won't be equal to MPI_${lang}_NO_INTERROGATE, and we'll - # inspect that compiler anew. This allows users to set new compilers w/o rm'ing cache. - string(COMPARE NOTEQUAL "${MPI_${lang}_NO_INTERROGATE}" "${MPI_${lang}_COMPILER}" interrogate) - - # If MPI is set already in the cache, don't bother with interrogating the compiler. - if (interrogate AND ((NOT MPI_${lang}_INCLUDE_PATH) OR (NOT MPI_${lang}_LIBRARIES))) - if (MPI_${lang}_COMPILER) - # Check whether the -showme:compile option works. This indicates that we have either OpenMPI - # or a newer version of LAM-MPI, and implies that -showme:link will also work. - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -showme:compile - OUTPUT_VARIABLE MPI_COMPILE_CMDLINE OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_COMPILE_CMDLINE ERROR_STRIP_TRAILING_WHITESPACE - RESULT_VARIABLE MPI_COMPILER_RETURN) - - if (MPI_COMPILER_RETURN EQUAL 0) - # If we appear to have -showme:compile, then we should - # also have -showme:link. Try it. - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -showme:link - OUTPUT_VARIABLE MPI_LINK_CMDLINE OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_LINK_CMDLINE ERROR_STRIP_TRAILING_WHITESPACE - RESULT_VARIABLE MPI_COMPILER_RETURN) - - if (MPI_COMPILER_RETURN EQUAL 0) - # We probably have -showme:incdirs and -showme:libdirs as well, - # so grab that while we're at it. - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -showme:incdirs - OUTPUT_VARIABLE MPI_INCDIRS OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_INCDIRS ERROR_STRIP_TRAILING_WHITESPACE) - - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -showme:libdirs - OUTPUT_VARIABLE MPI_LIBDIRS OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_LIBDIRS ERROR_STRIP_TRAILING_WHITESPACE) - - else() - # reset things here if something went wrong. - set(MPI_COMPILE_CMDLINE) - set(MPI_LINK_CMDLINE) - endif() - endif () - - # Older versions of LAM-MPI have "-showme". Try to find that. - if (NOT MPI_COMPILER_RETURN EQUAL 0) - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -showme - OUTPUT_VARIABLE MPI_COMPILE_CMDLINE OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_COMPILE_CMDLINE ERROR_STRIP_TRAILING_WHITESPACE - RESULT_VARIABLE MPI_COMPILER_RETURN) - endif() - - # MVAPICH uses -compile-info and -link-info. Try them. - if (NOT MPI_COMPILER_RETURN EQUAL 0) - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -compile-info - OUTPUT_VARIABLE MPI_COMPILE_CMDLINE OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_COMPILE_CMDLINE ERROR_STRIP_TRAILING_WHITESPACE - RESULT_VARIABLE MPI_COMPILER_RETURN) - - # If we have compile-info, also have link-info. - if (MPI_COMPILER_RETURN EQUAL 0) - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -link-info - OUTPUT_VARIABLE MPI_LINK_CMDLINE OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_LINK_CMDLINE ERROR_STRIP_TRAILING_WHITESPACE - RESULT_VARIABLE MPI_COMPILER_RETURN) - endif() - - # make sure we got compile and link. Reset vars if something's wrong. - if (NOT MPI_COMPILER_RETURN EQUAL 0) - set(MPI_COMPILE_CMDLINE) - set(MPI_LINK_CMDLINE) - endif() - endif() - - # MPICH just uses "-show". Try it. - if (NOT MPI_COMPILER_RETURN EQUAL 0) - execute_process( - COMMAND ${MPI_${lang}_COMPILER} -show - OUTPUT_VARIABLE MPI_COMPILE_CMDLINE OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_VARIABLE MPI_COMPILE_CMDLINE ERROR_STRIP_TRAILING_WHITESPACE - RESULT_VARIABLE MPI_COMPILER_RETURN) - endif() - - if (MPI_COMPILER_RETURN EQUAL 0) - # We have our command lines, but we might need to copy MPI_COMPILE_CMDLINE - # into MPI_LINK_CMDLINE, if we didn't find the link line. - if (NOT MPI_LINK_CMDLINE) - set(MPI_LINK_CMDLINE ${MPI_COMPILE_CMDLINE}) - endif() - else() - message(STATUS "Unable to determine MPI from MPI driver ${MPI_${lang}_COMPILER}") - set(MPI_COMPILE_CMDLINE) - set(MPI_LINK_CMDLINE) - endif() - - # Here, we're done with the interrogation part, and we'll try to extract args we care - # about from what we learned from the compiler wrapper scripts. - - # If interrogation came back with something, extract our variable from the MPI command line - if (MPI_COMPILE_CMDLINE OR MPI_LINK_CMDLINE) - # Extract compile flags from the compile command line. - string(REGEX MATCHALL "(^| )-[Df]([^\" ]+|\"[^\"]+\")" MPI_ALL_COMPILE_FLAGS "${MPI_COMPILE_CMDLINE}") - set(MPI_COMPILE_FLAGS_WORK) - - foreach(FLAG ${MPI_ALL_COMPILE_FLAGS}) - if (MPI_COMPILE_FLAGS_WORK) - set(MPI_COMPILE_FLAGS_WORK "${MPI_COMPILE_FLAGS_WORK} ${FLAG}") - else() - set(MPI_COMPILE_FLAGS_WORK ${FLAG}) - endif() - endforeach() - - # Extract include paths from compile command line - string(REGEX MATCHALL "(^| )-I([^\" ]+|\"[^\"]+\")" MPI_ALL_INCLUDE_PATHS "${MPI_COMPILE_CMDLINE}") - foreach(IPATH ${MPI_ALL_INCLUDE_PATHS}) - string(REGEX REPLACE "^ ?-I" "" IPATH ${IPATH}) - string(REGEX REPLACE "//" "/" IPATH ${IPATH}) - list(APPEND MPI_INCLUDE_PATH_WORK ${IPATH}) - endforeach() - - # try using showme:incdirs if extracting didn't work. - if (NOT MPI_INCLUDE_PATH_WORK) - set(MPI_INCLUDE_PATH_WORK ${MPI_INCDIRS}) - separate_arguments(MPI_INCLUDE_PATH_WORK) - endif() - - # If all else fails, just search for mpi.h in the normal include paths. - if (NOT MPI_INCLUDE_PATH_WORK) - set(MPI_HEADER_PATH "MPI_HEADER_PATH-NOTFOUND" CACHE FILEPATH "Cleared" FORCE) - find_path(MPI_HEADER_PATH mpi.h - HINTS ${_MPI_BASE_DIR} ${_MPI_PREFIX_PATH} - PATH_SUFFIXES include) - set(MPI_INCLUDE_PATH_WORK ${MPI_HEADER_PATH}) - endif() - - # Extract linker paths from the link command line - string(REGEX MATCHALL "(^| |-Wl,)-L([^\" ]+|\"[^\"]+\")" MPI_ALL_LINK_PATHS "${MPI_LINK_CMDLINE}") - set(MPI_LINK_PATH) - foreach(LPATH ${MPI_ALL_LINK_PATHS}) - string(REGEX REPLACE "^(| |-Wl,)-L" "" LPATH ${LPATH}) - string(REGEX REPLACE "//" "/" LPATH ${LPATH}) - list(APPEND MPI_LINK_PATH ${LPATH}) - endforeach() - - # try using showme:libdirs if extracting didn't work. - if (NOT MPI_LINK_PATH) - set(MPI_LINK_PATH ${MPI_LIBDIRS}) - separate_arguments(MPI_LINK_PATH) - endif() - - # Extract linker flags from the link command line - string(REGEX MATCHALL "(^| )-Wl,([^\" ]+|\"[^\"]+\")" MPI_ALL_LINK_FLAGS "${MPI_LINK_CMDLINE}") - set(MPI_LINK_FLAGS_WORK) - foreach(FLAG ${MPI_ALL_LINK_FLAGS}) - if (MPI_LINK_FLAGS_WORK) - set(MPI_LINK_FLAGS_WORK "${MPI_LINK_FLAGS_WORK} ${FLAG}") - else() - set(MPI_LINK_FLAGS_WORK ${FLAG}) - endif() - endforeach() - - # Extract the set of libraries to link against from the link command - # line - string(REGEX MATCHALL "(^| )-l([^\" ]+|\"[^\"]+\")" MPI_LIBNAMES "${MPI_LINK_CMDLINE}") - - # Determine full path names for all of the libraries that one needs - # to link against in an MPI program - foreach(LIB ${MPI_LIBNAMES}) - string(REGEX REPLACE "^ ?-l" "" LIB ${LIB}) - # MPI_LIB is cached by find_library, but we don't want that. Clear it first. - set(MPI_LIB "MPI_LIB-NOTFOUND" CACHE FILEPATH "Cleared" FORCE) - find_library(MPI_LIB NAMES ${LIB} HINTS ${MPI_LINK_PATH}) - - if (MPI_LIB) - list(APPEND MPI_LIBRARIES_WORK ${MPI_LIB}) - elseif (NOT MPI_FIND_QUIETLY) - message(WARNING "Unable to find MPI library ${LIB}") - endif() - endforeach() - - # Sanity check MPI_LIBRARIES to make sure there are enough libraries - list(LENGTH MPI_LIBRARIES_WORK MPI_NUMLIBS) - list(LENGTH MPI_LIBNAMES MPI_NUMLIBS_EXPECTED) - if (NOT MPI_NUMLIBS EQUAL MPI_NUMLIBS_EXPECTED) - set(MPI_LIBRARIES_WORK "MPI_${lang}_LIBRARIES-NOTFOUND") - endif() - endif() - - elseif(try_libs) - # If we didn't have an MPI compiler script to interrogate, attempt to find everything - # with plain old find functions. This is nasty because MPI implementations have LOTS of - # different library names, so this section isn't going to be very generic. We need to - # make sure it works for MS MPI, though, since there are no compiler wrappers for that. - find_path(MPI_HEADER_PATH mpi.h - HINTS ${_MPI_BASE_DIR} ${_MPI_PREFIX_PATH} - PATH_SUFFIXES include Inc) - set(MPI_INCLUDE_PATH_WORK ${MPI_HEADER_PATH}) - - # Decide between 32-bit and 64-bit libraries for Microsoft's MPI - if("${CMAKE_SIZEOF_VOID_P}" EQUAL 8) - set(MS_MPI_ARCH_DIR amd64) - else() - set(MS_MPI_ARCH_DIR i386) - endif() - - set(MPI_LIB "MPI_LIB-NOTFOUND" CACHE FILEPATH "Cleared" FORCE) - find_library(MPI_LIB - NAMES mpi mpich mpich2 msmpi - HINTS ${_MPI_BASE_DIR} ${_MPI_PREFIX_PATH} - PATH_SUFFIXES lib lib/${MS_MPI_ARCH_DIR} Lib Lib/${MS_MPI_ARCH_DIR}) - set(MPI_LIBRARIES_WORK ${MPI_LIB}) - - # Right now, we only know about the extra libs for C++. - # We could add Fortran here (as there is usually libfmpich, etc.), but - # this really only has to work with MS MPI on Windows. - # Assume that other MPI's are covered by the compiler wrappers. - if (${lang} STREQUAL CXX) - set(MPI_LIB "MPI_LIB-NOTFOUND" CACHE FILEPATH "Cleared" FORCE) - find_library(MPI_LIB - NAMES mpi++ mpicxx cxx mpi_cxx - HINTS ${_MPI_BASE_DIR} ${_MPI_PREFIX_PATH} - PATH_SUFFIXES lib) - if (MPI_LIBRARIES_WORK AND MPI_LIB) - set(MPI_LIBRARIES_WORK "${MPI_LIBRARIES_WORK};${MPI_LIB}") - endif() - endif() - - if (NOT MPI_LIBRARIES_WORK) - set(MPI_LIBRARIES_WORK "MPI_${lang}_LIBRARIES-NOTFOUND") - endif() - endif() - - # If we found MPI, set up all of the appropriate cache entries - set(MPI_${lang}_COMPILE_FLAGS ${MPI_COMPILE_FLAGS_WORK} CACHE STRING "MPI ${lang} compilation flags" FORCE) - set(MPI_${lang}_INCLUDE_PATH ${MPI_INCLUDE_PATH_WORK} CACHE STRING "MPI ${lang} include path" FORCE) - set(MPI_${lang}_LINK_FLAGS ${MPI_LINK_FLAGS_WORK} CACHE STRING "MPI ${lang} linking flags" FORCE) - set(MPI_${lang}_LIBRARIES ${MPI_LIBRARIES_WORK} CACHE STRING "MPI ${lang} libraries to link against" FORCE) - mark_as_advanced(MPI_${lang}_COMPILE_FLAGS MPI_${lang}_INCLUDE_PATH MPI_${lang}_LINK_FLAGS MPI_${lang}_LIBRARIES) - - # clear out our temporary lib/header detectionv variable here. - set(MPI_LIB "MPI_LIB-NOTFOUND" CACHE INTERNAL "Scratch variable for MPI lib detection" FORCE) - set(MPI_HEADER_PATH "MPI_HEADER_PATH-NOTFOUND" CACHE INTERNAL "Scratch variable for MPI header detection" FORCE) - endif() - - # finally set a found variable for each MPI language - if (MPI_${lang}_INCLUDE_PATH AND MPI_${lang}_LIBRARIES) - set(MPI_${lang}_FOUND TRUE PARENT_SCOPE) - else() - set(MPI_${lang}_FOUND FALSE PARENT_SCOPE) - endif() -endfunction() - - -# This function attempts to compile with the regular compiler, to see if MPI programs -# work with it. This is a last ditch attempt after we've tried interrogating mpicc and -# friends, and after we've tried to find generic libraries. Works on machines like -# Cray XE6, where the modules environment changes what MPI version cc, CC, and ftn use. -function(try_regular_compiler lang success) - set(scratch_directory ${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}) - if (${lang} STREQUAL Fortran) - set(test_file ${scratch_directory}/cmake_mpi_test.f90) - file(WRITE ${test_file} - "program hello\n" - "include 'mpif.h'\n" - "integer ierror\n" - "call MPI_INIT(ierror)\n" - "call MPI_FINALIZE(ierror)\n" - "end\n") - else() - if (${lang} STREQUAL CXX) - set(test_file ${scratch_directory}/cmake_mpi_test.cpp) - else() - set(test_file ${scratch_directory}/cmake_mpi_test.c) - endif() - file(WRITE ${test_file} - "#include \n" - "int main(int argc, char **argv) {\n" - " MPI_Init(&argc, &argv);\n" - " MPI_Finalize();\n" - "}\n") - endif() - try_compile(compiler_has_mpi ${scratch_directory} ${test_file}) - if (compiler_has_mpi) - set(MPI_${lang}_NO_INTERROGATE ${CMAKE_${lang}_COMPILER} CACHE STRING "Whether to interrogate MPI ${lang} compiler" FORCE) - set(MPI_${lang}_COMPILER ${CMAKE_${lang}_COMPILER} CACHE STRING "MPI ${lang} compiler" FORCE) - set(MPI_${lang}_COMPILE_FLAGS "" CACHE STRING "MPI ${lang} compilation flags" FORCE) - set(MPI_${lang}_INCLUDE_PATH "" CACHE STRING "MPI ${lang} include path" FORCE) - set(MPI_${lang}_LINK_FLAGS "" CACHE STRING "MPI ${lang} linking flags" FORCE) - set(MPI_${lang}_LIBRARIES "" CACHE STRING "MPI ${lang} libraries to link against" FORCE) - endif() - set(${success} ${compiler_has_mpi} PARENT_SCOPE) - unset(compiler_has_mpi CACHE) -endfunction() - -# End definitions, commence real work here. - -# Most mpi distros have some form of mpiexec which gives us something we can reliably look for. -find_program(MPIEXEC - NAMES ${_MPI_EXEC_NAMES} - PATHS ${_MPI_PREFIX_PATH} - PATH_SUFFIXES bin - DOC "Executable for running MPI programs.") - -# call get_filename_component twice to remove mpiexec and the directory it exists in (typically bin). -# This gives us a fairly reliable base directory to search for /bin /lib and /include from. -get_filename_component(_MPI_BASE_DIR "${MPIEXEC}" PATH) -get_filename_component(_MPI_BASE_DIR "${_MPI_BASE_DIR}" PATH) - -set(MPIEXEC_NUMPROC_FLAG "-np" CACHE STRING "Flag used by MPI to specify the number of processes for MPIEXEC; the next option will be the number of processes.") -set(MPIEXEC_PREFLAGS "" CACHE STRING "These flags will be directly before the executable that is being run by MPIEXEC.") -set(MPIEXEC_POSTFLAGS "" CACHE STRING "These flags will come after all flags given to MPIEXEC.") -set(MPIEXEC_MAX_NUMPROCS "2" CACHE STRING "Maximum number of processors available to run MPI applications.") -mark_as_advanced(MPIEXEC MPIEXEC_NUMPROC_FLAG MPIEXEC_PREFLAGS MPIEXEC_POSTFLAGS MPIEXEC_MAX_NUMPROCS) - - -#============================================================================= -# Backward compatibility input hacks. Propagate the FindMPI hints to C and -# CXX if the respective new versions are not defined. Translate the old -# MPI_LIBRARY and MPI_EXTRA_LIBRARY to respective MPI_${lang}_LIBRARIES. -# -# Once we find the new variables, we translate them back into their old -# equivalents below. -foreach (lang C CXX) - # Old input variables. - set(_MPI_OLD_INPUT_VARS COMPILER COMPILE_FLAGS INCLUDE_PATH LINK_FLAGS) - - # Set new vars based on their old equivalents, if the new versions are not already set. - foreach (var ${_MPI_OLD_INPUT_VARS}) - if (NOT MPI_${lang}_${var} AND MPI_${var}) - set(MPI_${lang}_${var} "${MPI_${var}}") - endif() - endforeach() - - # Special handling for MPI_LIBRARY and MPI_EXTRA_LIBRARY, which we nixed in the - # new FindMPI. These need to be merged into MPI__LIBRARIES - if (NOT MPI_${lang}_LIBRARIES AND (MPI_LIBRARY OR MPI_EXTRA_LIBRARY)) - set(MPI_${lang}_LIBRARIES ${MPI_LIBRARY} ${MPI_EXTRA_LIBRARY}) - endif() -endforeach() -#============================================================================= - - -# This loop finds the compilers and sends them off for interrogation. -foreach (lang C CXX Fortran) - if (CMAKE_${lang}_COMPILER_WORKS) - # If the user supplies a compiler *name* instead of an absolute path, assume that we need to find THAT compiler. - if (MPI_${lang}_COMPILER) - is_file_executable(MPI_${lang}_COMPILER MPI_COMPILER_IS_EXECUTABLE) - if (NOT MPI_COMPILER_IS_EXECUTABLE) - # Get rid of our default list of names and just search for the name the user wants. - set(_MPI_${lang}_COMPILER_NAMES ${MPI_${lang}_COMPILER}) - set(MPI_${lang}_COMPILER "MPI_${lang}_COMPILER-NOTFOUND" CACHE FILEPATH "Cleared" FORCE) - # If the user specifies a compiler, we don't want to try to search libraries either. - set(try_libs FALSE) - endif() - else() - set(try_libs TRUE) - endif() - - find_program(MPI_${lang}_COMPILER - NAMES ${_MPI_${lang}_COMPILER_NAMES} - PATHS "${MPI_HOME}/bin" "$ENV{MPI_HOME}/bin" ${_MPI_PREFIX_PATH}) - interrogate_mpi_compiler(${lang} ${try_libs}) - mark_as_advanced(MPI_${lang}_COMPILER) - - # last ditch try -- if nothing works so far, just try running the regular compiler and - # see if we can create an MPI executable. - set(regular_compiler_worked 0) - if (NOT MPI_${lang}_LIBRARIES OR NOT MPI_${lang}_INCLUDE_PATH) - try_regular_compiler(${lang} regular_compiler_worked) - endif() - - if (regular_compiler_worked) - find_package_handle_standard_args(MPI_${lang} DEFAULT_MSG MPI_${lang}_COMPILER) - else() - find_package_handle_standard_args(MPI_${lang} DEFAULT_MSG MPI_${lang}_LIBRARIES MPI_${lang}_INCLUDE_PATH) - endif() - endif() -endforeach() - - -#============================================================================= -# More backward compatibility stuff -# -# Bare MPI sans ${lang} vars are set to CXX then C, depending on what was found. -# This mimics the behavior of the old language-oblivious FindMPI. -set(_MPI_OLD_VARS FOUND COMPILER INCLUDE_PATH COMPILE_FLAGS LINK_FLAGS LIBRARIES) -if (MPI_CXX_FOUND) - foreach (var ${_MPI_OLD_VARS}) - set(MPI_${var} ${MPI_CXX_${var}}) - endforeach() -elseif (MPI_C_FOUND) - foreach (var ${_MPI_OLD_VARS}) - set(MPI_${var} ${MPI_C_${var}}) - endforeach() -else() - # Note that we might still have found Fortran, but you'll need to use MPI_Fortran_FOUND - set(MPI_FOUND FALSE) -endif() - -# Chop MPI_LIBRARIES into the old-style MPI_LIBRARY and MPI_EXTRA_LIBRARY, and set them in cache. -if (MPI_LIBRARIES) - list(GET MPI_LIBRARIES 0 MPI_LIBRARY_WORK) - set(MPI_LIBRARY ${MPI_LIBRARY_WORK} CACHE FILEPATH "MPI library to link against" FORCE) -else() - set(MPI_LIBRARY "MPI_LIBRARY-NOTFOUND" CACHE FILEPATH "MPI library to link against" FORCE) -endif() - -list(LENGTH MPI_LIBRARIES MPI_NUMLIBS) -if (MPI_NUMLIBS GREATER 1) - set(MPI_EXTRA_LIBRARY_WORK ${MPI_LIBRARIES}) - list(REMOVE_AT MPI_EXTRA_LIBRARY_WORK 0) - set(MPI_EXTRA_LIBRARY ${MPI_EXTRA_LIBRARY_WORK} CACHE STRING "Extra MPI libraries to link against" FORCE) -else() - set(MPI_EXTRA_LIBRARY "MPI_EXTRA_LIBRARY-NOTFOUND" CACHE STRING "Extra MPI libraries to link against" FORCE) -endif() -#============================================================================= - -# unset these vars to cleanup namespace -unset(_MPI_OLD_VARS) -unset(_MPI_PREFIX_PATH) -unset(_MPI_BASE_DIR) -foreach (lang C CXX Fortran) - unset(_MPI_${lang}_COMPILER_NAMES) -endforeach() diff --git a/cmake/Modules/FindOpenNI.cmake b/cmake/Modules/FindOpenNI.cmake index a8e6f51f..cb537be8 100644 --- a/cmake/Modules/FindOpenNI.cmake +++ b/cmake/Modules/FindOpenNI.cmake @@ -6,7 +6,7 @@ # OPENNI_INCLUDE_DIRS - Directories containing the OPENNI include files. # OPENNI_LIBRARIES - Libraries needed to use OPENNI. # OPENNI_DEFINITIONS - Compiler flags for OPENNI. -# +# # For libusb-1.0, add USB_10_ROOT if not found find_package(PkgConfig QUIET) @@ -19,21 +19,21 @@ if(NOT WIN32) PATH_SUFFIXES libusb-1.0) find_library(USB_10_LIBRARY - NAMES usb-1.0 + NAMES usb-1.0 HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" PATH_SUFFIXES lib) - + include(FindPackageHandleStandardArgs) find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) - + if(NOT USB_10_FOUND) - message(STATUS "OpenNI disabled because libusb-1.0 not found.") + message(STATUS "OpenNI disabled because libusb-1.0 not found.") return() else() include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) endif() endif(NOT WIN32) - + if(${CMAKE_VERSION} VERSION_LESS 2.8.2) pkg_check_modules(PC_OPENNI openni-dev) else() @@ -49,11 +49,11 @@ endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) #add a hint so that it can find it without the pkg-config find_path(OPENNI_INCLUDE_DIR XnStatus.h - HINTS ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS} /usr/include/openni /usr/include/ni "${OPENNI_ROOT}" "$ENV{OPENNI_ROOT}" + HINTS ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS} /usr/include/openni /usr/include/ni /opt/local/include/ni "${OPENNI_ROOT}" "$ENV{OPENNI_ROOT}" PATHS "$ENV{OPEN_NI_INSTALL_PATH${OPENNI_SUFFIX}}/Include" PATH_SUFFIXES openni include Include) #add a hint so that it can find it without the pkg-config -find_library(OPENNI_LIBRARY +find_library(OPENNI_LIBRARY NAMES OpenNI${OPENNI_SUFFIX} HINTS ${PC_OPENNI_LIBDIR} ${PC_OPENNI_LIBRARY_DIRS} /usr/lib "${OPENNI_ROOT}" "$ENV{OPENNI_ROOT}" PATHS "$ENV{OPEN_NI_LIB${OPENNI_SUFFIX}}" @@ -67,12 +67,11 @@ endif() include(FindPackageHandleStandardArgs) find_package_handle_standard_args(OpenNI DEFAULT_MSG OPENNI_LIBRARY OPENNI_INCLUDE_DIR) - + mark_as_advanced(OPENNI_LIBRARY OPENNI_INCLUDE_DIR) -if(OPENNI_FOUND) +if(OPENNI_FOUND) # Add the include directories - set(OPENNI_INCLUDE_DIRS ${OPENNI_INCLUDE_DIR}) + set(OPENNI_INCLUDE_DIRS ${OPENNI_INCLUDE_DIR}) message(STATUS "OpenNI found (include: ${OPENNI_INCLUDE_DIRS}, lib: ${OPENNI_LIBRARY})") endif(OPENNI_FOUND) - diff --git a/cmake/Modules/FindOpenNI2.cmake b/cmake/Modules/FindOpenNI2.cmake new file mode 100644 index 00000000..036e4e04 --- /dev/null +++ b/cmake/Modules/FindOpenNI2.cmake @@ -0,0 +1,80 @@ +############################################################################### +# Find OpenNI 2 +# +# This sets the following variables: +# OPENNI2_FOUND - True if OPENNI 2 was found. +# OPENNI2_INCLUDE_DIRS - Directories containing the OPENNI 2 include files. +# OPENNI2_LIBRARIES - Libraries needed to use OPENNI 2. +# OPENNI2_DEFINITIONS - Compiler flags for OPENNI 2. +# +# For libusb-1.0, add USB_10_ROOT if not found + +find_package(PkgConfig QUIET) + +# Find LibUSB +if(NOT WIN32) + pkg_check_modules(PC_USB_10 libusb-1.0) + find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h + HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES libusb-1.0) + + find_library(USB_10_LIBRARY + NAMES usb-1.0 + HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES lib) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) + + if(NOT USB_10_FOUND) + message(STATUS "OpenNI 2 disabled because libusb-1.0 not found.") + return() + else() + include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) + endif() +endif(NOT WIN32) + +if(${CMAKE_VERSION} VERSION_LESS 2.8.2) + pkg_check_modules(PC_OPENNI2 openni2-dev) +else() + pkg_check_modules(PC_OPENNI2 QUIET openni2-dev) +endif() + +set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) + +set(OPENNI2_SUFFIX) +if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + set(OPENNI2_SUFFIX 64) +endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + +find_path(OPENNI2_INCLUDE_DIRS OpenNI.h + PATHS + "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Win64 needs '64' suffix + /usr/include/openni2 # common path for deb packages +) + +find_library(OPENNI2_LIBRARY + NAMES OpenNI2 # No suffix needed on Win64 + libOpenNI2 # Linux + PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" # Windows default path, Win64 needs '64' suffix + "$ENV{OPENNI2_REDIST}" # Linux install does not use a separate 'lib' directory + ) + +if(CMAKE_SYSTEM_NAME STREQUAL "Darwin") + set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} ${LIBUSB_1_LIBRARIES}) +else() + set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) + +mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) + +if(OPENNI2_FOUND) + # Add the include directories + set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) + set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) + message(STATUS "OpenNI 2 found (include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARY}, redist: ${OPENNI2_REDIST_DIR})") +endif(OPENNI2_FOUND) + diff --git a/cmake/Modules/FindPXCAPI.cmake b/cmake/Modules/FindPXCAPI.cmake index 3ea77f4e..be82346e 100644 --- a/cmake/Modules/FindPXCAPI.cmake +++ b/cmake/Modules/FindPXCAPI.cmake @@ -5,36 +5,29 @@ # PXCAPI_FOUND - True if PXCAPI was found. # PXCAPI_INCLUDE_DIRS - Directories containing the PXCAPI include files. # PXCAPI_LIBRARIES - Libraries needed to use PXCAPI. - -#MESSAGE("Searching for PXCAPI in: ${PXCAPI_DIR}") + +find_path(PXCAPI_DIR include/pxcimage.h + PATHS "${PXCAPI_DIR}" "C:/Program Files/Intel/PCSDK" "C:/Program Files (x86)/Intel/PCSDK" + DOC "PXCAPI include directories") if(PXCAPI_DIR) set(PXCAPI_INCLUDE_DIRS ${PXCAPI_DIR}/include ${PXCAPI_DIR}/sample/common/include) - set(PXCAPI_LIB_DIRS ${PXCAPI_DIR}/lib/x64 ${PXCAPI_DIR}/sample/common/lib/x64/v100) - set(PXCAPI_LIBS ${PXCAPI_DIR}/lib/x64/libpxc.lib ${PXCAPI_DIR}/sample/common/lib/x64/v100/libpxcutils.lib) -else() - MESSAGE("Searching PXCAPI includes: ${PXCAPI_DIR}") - find_path(PXCAPI_DIR include/pxcimage.h - PATHS "${PXCAPI_DIR}" "C:/Program Files/Intel/PCSDK" "C:/Program Files (x86)/Intel/PCSDK" - DOC "PXCAPI include directories") - if(PXCAPI_DIR) - set(PXCAPI_INCLUDE_DIRS ${PXCAPI_DIR}/include ${PXCAPI_DIR}/sample/common/include) - set(PXCAPI_LIB_DIRS ${PXCAPI_DIR}/lib/x64 ${PXCAPI_DIR}/sample/common/lib/x64/v100) - set(PXCAPI_LIBS ${PXCAPI_DIR}/lib/x64/libpxc.lib ${PXCAPI_DIR}/sample/common/lib/x64/v100/libpxcutils.lib) - else() - set(PXCAPI_DIR "directory not found (please enter)" CACHE FILEPATH "directory of PXCAPI") - endif() + find_library(PXCAPI_LIB libpxc.lib + PATHS "${PXCAPI_DIR}/lib/" NO_DEFAULT_PATH + PATH_SUFFIXES x64 Win32) + find_library(PXCAPI_SAMPLE_LIB libpxcutils.lib + PATHS "${PXCAPI_DIR}/sample/common/lib" NO_DEFAULT_PATH + PATH_SUFFIXES x64/v100 Win32/v100) + set(PXCAPI_LIBS ${PXCAPI_LIB} ${PXCAPI_SAMPLE_LIB}) endif() -MESSAGE("PXCAPI_INCLUDE_DIR: ${PXCAPI_INCLUDE_DIRS}") -MESSAGE("PXCAPI_LIBS: ${PXCAPI_LIBS}") - include(FindPackageHandleStandardArgs) find_package_handle_standard_args(PXCAPI DEFAULT_MSG - PXCAPI_LIBS PXCAPI_INCLUDE_DIRS PXCAPI_LIB_DIRS) - + PXCAPI_LIBS PXCAPI_INCLUDE_DIRS) + +mark_as_advanced(PXCAPI_LIB PXCAPI_SAMPLE_LIB) + if(MSVC) set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /NODEFAULTLIB:LIBCMT") endif() - \ No newline at end of file diff --git a/cmake/Modules/FindPcap.cmake b/cmake/Modules/FindPcap.cmake index 937b6338..18e4cfa2 100644 --- a/cmake/Modules/FindPcap.cmake +++ b/cmake/Modules/FindPcap.cmake @@ -99,14 +99,9 @@ SET(CMAKE_REQUIRED_INCLUDES ${PCAP_INCLUDE_DIRS}) SET(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARIES}) #Is pcap found ? -IF(PCAP_INCLUDE_DIRS AND PCAP_LIBRARIES) -SET( PCAP_FOUND "YES" ) -message(STATUS "PCAP found (include: ${PCAP_INCLUDE_DIRS}, lib: ${PCAP_LIBRARIES})") -ELSE() -SET( PCAP_FOUND "NO" ) -message(STATUS "PCAP NOT found") -ENDIF(PCAP_INCLUDE_DIRS AND PCAP_LIBRARIES) - +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PCAP DEFAULT_MSG + PCAP_LIBRARIES PCAP_INCLUDE_DIRS) MARK_AS_ADVANCED( PCAP_LIBRARIES diff --git a/cmake/Modules/FindQVTK.cmake b/cmake/Modules/FindQVTK.cmake index 18ae5767..1e9d683d 100644 --- a/cmake/Modules/FindQVTK.cmake +++ b/cmake/Modules/FindQVTK.cmake @@ -7,17 +7,26 @@ # QVTK_LIBRARY - QVTK library. # if QVTK_FOUND then QVTK_INCLUDE_DIR is appended to VTK_INCLUDE_DIRS and # QVTK_LIBRARY is appended to QVTK_LIBRARY_DIR - -find_library (QVTK_LIBRARY QVTK HINTS ${VTK_DIR} ${VTK_DIR}/bin) -find_path (QVTK_INCLUDE_DIR QVTKWidget.h HINT ${VTK_INCLUDE_DIRS}) -find_package_handle_standard_args(QVTK DEFAULT_MSG - QVTK_LIBRARY QVTK_INCLUDE_DIR) - -if(NOT QVTK_FOUND) - set (VTK_USE_QVTK OFF) -else(NOT QVTK_FOUND) - get_filename_component (QVTK_LIBRARY_DIR ${QVTK_LIBRARY} PATH) - set (VTK_LIBRARY_DIRS ${VTK_LIBRARY_DIRS} ${QVTK_LIBRARY_DIR}) - set (VTK_INCLUDE_DIRS ${VTK_INCLUDE_DIRS} ${QVTK_INCLUDE_DIR}) - set (VTK_USE_QVTK ON) -endif(NOT QVTK_FOUND) +if (${VTK_MAJOR_VERSION} VERSION_LESS "6.0") + find_library (QVTK_LIBRARY QVTK HINTS ${VTK_DIR} ${VTK_DIR}/bin) + find_path (QVTK_INCLUDE_DIR QVTKWidget.h HINT ${VTK_INCLUDE_DIRS}) + find_package_handle_standard_args(QVTK DEFAULT_MSG + QVTK_LIBRARY QVTK_INCLUDE_DIR) + if(NOT QVTK_FOUND) + set (VTK_USE_QVTK OFF) + else(NOT QVTK_FOUND) + get_filename_component (QVTK_LIBRARY_DIR ${QVTK_LIBRARY} PATH) + set (VTK_LIBRARY_DIRS ${VTK_LIBRARY_DIRS} ${QVTK_LIBRARY_DIR}) + set (VTK_INCLUDE_DIRS ${VTK_INCLUDE_DIRS} ${QVTK_INCLUDE_DIR}) + set (VTK_USE_QVTK ON) + endif(NOT QVTK_FOUND) +else (${VTK_MAJOR_VERSION} VERSION_LESS "6.0") + list (FIND VTK_MODULES_ENABLED vtkGUISupportQt GUI_SUPPORT_QT_FOUND) + list (FIND VTK_MODULES_ENABLED vtkRenderingQt RENDERING_QT_FOUND) + if (GUI_SUPPORT_QT_FOUND AND RENDERING_QT_FOUND) + set (VTK_USE_QVTK ON) + set (QVTK_LIBRARY vtkRenderingQt vtkGUISupportQt) + else (GUI_SUPPORT_QT_FOUND AND RENDERING_QT_FOUND) + unset(QVTK_FOUND) + endif (GUI_SUPPORT_QT_FOUND AND RENDERING_QT_FOUND) +endif (${VTK_MAJOR_VERSION} VERSION_LESS "6.0") diff --git a/cmake/Modules/FindQhull.cmake b/cmake/Modules/FindQhull.cmake index f5fd269d..698bd151 100644 --- a/cmake/Modules/FindQhull.cmake +++ b/cmake/Modules/FindQhull.cmake @@ -15,8 +15,8 @@ if(QHULL_USE_STATIC) set(QHULL_RELEASE_NAME qhullstatic) set(QHULL_DEBUG_NAME qhullstatic_d) else(QHULL_USE_STATIC) - set(QHULL_RELEASE_NAME qhull qhull${QHULL_MAJOR_VERSION}) - set(QHULL_DEBUG_NAME qhull_d qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION}) + set(QHULL_RELEASE_NAME qhull_p qhull${QHULL_MAJOR_VERSION} qhull) + set(QHULL_DEBUG_NAME qhull_pd qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION} qhull_d) endif(QHULL_USE_STATIC) find_file(QHULL_HEADER @@ -49,6 +49,8 @@ find_library(QHULL_LIBRARY PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" PATH_SUFFIXES project build bin lib) +get_filename_component(QHULL_LIBRARY_NAME "${QHULL_LIBRARY}" NAME) + find_library(QHULL_LIBRARY_DEBUG NAMES ${QHULL_DEBUG_NAME} ${QHULL_RELEASE_NAME} HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" @@ -59,6 +61,8 @@ if(NOT QHULL_LIBRARY_DEBUG) set(QHULL_LIBRARY_DEBUG ${QHULL_LIBRARY}) endif(NOT QHULL_LIBRARY_DEBUG) +get_filename_component(QHULL_LIBRARY_DEBUG_NAME "${QHULL_LIBRARY_DEBUG}" NAME) + set(QHULL_INCLUDE_DIRS ${QHULL_INCLUDE_DIR}) set(QHULL_LIBRARIES optimized ${QHULL_LIBRARY} debug ${QHULL_LIBRARY_DEBUG}) diff --git a/cmake/pcl_find_boost.cmake b/cmake/pcl_find_boost.cmake index 212d2e0e..55fc40e9 100644 --- a/cmake/pcl_find_boost.cmake +++ b/cmake/pcl_find_boost.cmake @@ -1,9 +1,17 @@ # Find and set Boost flags -if(NOT PCL_SHARED_LIBS OR WIN32) - set(Boost_USE_STATIC_LIBS ON) - set(Boost_USE_STATIC ON) -endif(NOT PCL_SHARED_LIBS OR WIN32) +# If we would like to compile against a dynamically linked Boost +if(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32 AND WIN32) + set(Boost_USE_STATIC_LIBS OFF) + set(Boost_USE_STATIC OFF) + set(Boost_USE_MULTITHREAD ON) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBOOST_ALL_DYN_LINK -DBOOST_ALL_NO_LIB") +else(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32 AND WIN32) + if(NOT PCL_SHARED_LIBS OR WIN32) + set(Boost_USE_STATIC_LIBS ON) + set(Boost_USE_STATIC ON) + endif(NOT PCL_SHARED_LIBS OR WIN32) +endif(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32 AND WIN32) if(${CMAKE_VERSION} VERSION_LESS 2.8.5) SET(Boost_ADDITIONAL_VERSIONS "1.43" "1.43.0" "1.44" "1.44.0" "1.45" "1.45.0" "1.46.1" "1.46.0" "1.46" "1.47" "1.47.0") @@ -15,24 +23,15 @@ endif(${CMAKE_VERSION} VERSION_LESS 2.8.5) set(Boost_NO_BOOST_CMAKE ON) # Optional boost modules -find_package(Boost 1.40.0 QUIET COMPONENTS serialization mpi) -if(Boost_MPI_FOUND) - set(BOOST_MPI_FOUND TRUE) -endif(Boost_MPI_FOUND) +find_package(Boost 1.47.0 QUIET COMPONENTS serialization mpi) if(Boost_SERIALIZATION_FOUND) set(BOOST_SERIALIZATION_FOUND TRUE) endif(Boost_SERIALIZATION_FOUND) # Required boost modules -set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams) -# Starting with Boost 1.50, boost_thread depends on chrono. As this is not -# taken care of automatically on Windows, we add an explicit dependency as a -# workaround. -if(WIN32 AND Boost_VERSION VERSION_GREATER "104900") - set(BOOST_REQUIRED_MODULES ${BOOST_REQUIRED_MODULES} chrono) -endif(WIN32 AND Boost_VERSION VERSION_GREATER "104900") +set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams chrono) -find_package(Boost 1.40.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES}) +find_package(Boost 1.47.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES}) if(Boost_FOUND) set(BOOST_FOUND TRUE) diff --git a/cmake/pcl_find_gl.cmake b/cmake/pcl_find_gl.cmake new file mode 100644 index 00000000..d34f26f1 --- /dev/null +++ b/cmake/pcl_find_gl.cmake @@ -0,0 +1,20 @@ +# Try to Find OpenGL and GLUT silently +# In addition sets two flags if the found versions are Apple frameworks +# OPENGL_IS_A_FRAMEWORK +# GLUT_IS_A_FRAMEWORK + +find_package(OpenGL QUIET REQUIRED) + +if(APPLE AND OPENGL_FOUND) + if ("${OPENGL_INCLUDE_DIR}" MATCHES "\\.framework") + set(OPENGL_IS_A_FRAMEWORK TRUE) + endif ("${OPENGL_INCLUDE_DIR}" MATCHES "\\.framework") +endif(APPLE AND OPENGL_FOUND) + +find_package(GLUT QUIET) + +if(APPLE AND GLUT_FOUND) + if ("${GLUT_INCLUDE_DIR}" MATCHES "\\.framework") + set(GLUT_IS_A_FRAMEWORK TRUE) + endif ("${GLUT_INCLUDE_DIR}" MATCHES "\\.framework") +endif(APPLE AND GLUT_FOUND) diff --git a/cmake/pcl_options.cmake b/cmake/pcl_options.cmake index cedf920c..641e8c3a 100644 --- a/cmake/pcl_options.cmake +++ b/cmake/pcl_options.cmake @@ -18,6 +18,10 @@ else(PCL_SHARED_LIBS) endif(PCL_SHARED_LIBS) mark_as_advanced(PCL_SHARED_LIBS) +# Build with dynamic linking for Boost (advanced users) +option(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32 "Build against a dynamically linked Boost on Win32 platforms." OFF) +mark_as_advanced(PCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32) + # Precompile for a minimal set of point types instead of all. option(PCL_ONLY_CORE_POINT_TYPES "Compile explicitly only for a small subset of point types (e.g., pcl::PointXYZ instead of PCL_XYZ_POINT_TYPES)." OFF) mark_as_advanced(PCL_ONLY_CORE_POINT_TYPES) @@ -38,3 +42,13 @@ mark_as_advanced(CMAKE_TIMING_VERBOSE) option(CMAKE_MSVC_CODE_LINK_OPTIMIZATION "Enable the /GL and /LTCG code and link optimization options for MSVC. Enabled by default." ON) mark_as_advanced(CMAKE_MSVC_CODE_LINK_OPTIMIZATION) +# Project folders +option(USE_PROJECT_FOLDERS "Use folders to organize PCL projects in an IDE." OFF) +mark_as_advanced(USE_PROJECT_FOLDERS) +if(USE_PROJECT_FOLDERS) + set_property(GLOBAL PROPERTY USE_FOLDERS ON) +endif(USE_PROJECT_FOLDERS) + +option(BUILD_tools "Useful PCL-based command line tools" ON) + +option(WITH_DOCS "Build doxygen documentation" OFF) diff --git a/cmake/pcl_pclconfig.cmake b/cmake/pcl_pclconfig.cmake index cf16693e..b0ea48ee 100644 --- a/cmake/pcl_pclconfig.cmake +++ b/cmake/pcl_pclconfig.cmake @@ -36,14 +36,32 @@ foreach(_ss ${PCL_SUBSYSTEMS_MODULES}) endforeach(_opt_dep) set(PCLCONFIG_OPTIONAL_DEPENDENCIES "${PCLCONFIG_OPTIONAL_DEPENDENCIES})\n") endif(_opt_deps) + + #look for subsystems + string(TOUPPER "PCL_${_ss}_SUBSYS" PCL_SUBSYS_SUBSYS) + if (${PCL_SUBSYS_SUBSYS}) + string(TOUPPER "PCL_${_ss}_SUBSYS_STATUS" PCL_SUBSYS_SUBSYS_STATUS) + foreach(_sub ${${PCL_SUBSYS_SUBSYS}}) + PCL_GET_SUBSUBSYS_STATUS(_sub_status ${_ss} ${_sub}) + if (_sub_status) + set(PCLCONFIG_AVAILABLE_COMPONENTS "${PCLCONFIG_AVAILABLE_COMPONENTS} ${_sub}") + set(PCLCONFIG_AVAILABLE_COMPONENTS_LIST "${PCLCONFIG_AVAILABLE_COMPONENTS_LIST}\n# - ${_sub}") + GET_IN_MAP(_deps PCL_SUBSYS_DEPS ${_ss}_${sub}) + if(_deps) + set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES}set(pcl_${_sub}_int_dep ") + foreach(_dep ${_deps}) + set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES}${_dep} ") + endforeach(_dep) + set(PCLCONFIG_INTERNAL_DEPENDENCIES "${PCLCONFIG_INTERNAL_DEPENDENCIES})\n") + endif(_deps) + endif (_sub_status) + endforeach(_sub) + endif (${PCL_SUBSYS_SUBSYS}) endif(_status) endforeach(_ss) #Boost modules set(PCLCONFIG_AVAILABLE_BOOST_MODULES "system filesystem thread date_time iostreams") -if(Boost_MPI_FOUND) - set(PCLCONFIG_AVAILABLE_BOOST_MODULES "${PCLCONFIG_AVAILABLE_BOOST_MODULES} mpi") -endif(Boost_MPI_FOUND) if(Boost_SERIALIZATION_FOUND) set(PCLCONFIG_AVAILABLE_BOOST_MODULES "${PCLCONFIG_AVAILABLE_BOOST_MODULES} serialization") endif(Boost_SERIALIZATION_FOUND) diff --git a/cmake/pcl_targets.cmake b/cmake/pcl_targets.cmake index 08b1896a..79f6bed6 100644 --- a/cmake/pcl_targets.cmake +++ b/cmake/pcl_targets.cmake @@ -12,7 +12,7 @@ macro(PCL_SUBSYS_OPTION _var _name _desc _default) PCL_GET_SUBSYS_HYPERSTATUS(subsys_status ${_name}) if(NOT ("${subsys_status}" STREQUAL "AUTO_OFF")) option(${_opt_name} ${_desc} ${_default}) - if(NOT ${_default} AND NOT ${_opt_name}) + if((NOT ${_default} AND NOT ${_opt_name}) OR ("${_default}" STREQUAL "AUTO_OFF")) set(${_var} FALSE) if(${ARGC} GREATER 4) set(_reason ${ARGV4}) @@ -29,11 +29,48 @@ macro(PCL_SUBSYS_OPTION _var _name _desc _default) set(${_var} TRUE) PCL_SET_SUBSYS_STATUS(${_name} TRUE) PCL_ENABLE_DEPENDIES(${_name}) - endif(NOT ${_default} AND NOT ${_opt_name}) + endif((NOT ${_default} AND NOT ${_opt_name}) OR ("${_default}" STREQUAL "AUTO_OFF")) endif(NOT ("${subsys_status}" STREQUAL "AUTO_OFF")) PCL_ADD_SUBSYSTEM(${_name} ${_desc}) endmacro(PCL_SUBSYS_OPTION) +############################################################################### +# Add an option to build a subsystem or not. +# _var The name of the variable to store the option in. +# _parent The name of the parent subsystem +# _name The name of the option's target subsubsystem. +# _desc The description of the subsubsystem. +# _default The default value (TRUE or FALSE) +# ARGV5 The reason for disabling if the default is FALSE. +macro(PCL_SUBSUBSYS_OPTION _var _parent _name _desc _default) + set(_opt_name "BUILD_${_parent}_${_name}") + PCL_GET_SUBSYS_HYPERSTATUS(parent_status ${_parent}) + if(NOT ("${parent_status}" STREQUAL "AUTO_OFF") AND NOT ("${parent_status}" STREQUAL "OFF")) + PCL_GET_SUBSYS_HYPERSTATUS(subsys_status ${_parent}_${_name}) + if(NOT ("${subsys_status}" STREQUAL "AUTO_OFF")) + option(${_opt_name} ${_desc} ${_default}) + if((NOT ${_default} AND NOT ${_opt_name}) OR ("${_default}" STREQUAL "AUTO_OFF")) + set(${_var} FALSE) + if(${ARGC} GREATER 5) + set(_reason ${ARGV5}) + else(${ARGC} GREATER 5) + set(_reason "Disabled by default.") + endif(${ARGC} GREATER 5) + PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE ${_reason}) + PCL_DISABLE_DEPENDIES(${_parent}_${_name}) + elseif(NOT ${_opt_name}) + set(${_var} FALSE) + PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE "Disabled manually.") + PCL_DISABLE_DEPENDIES(${_parent}_${_name}) + else(NOT ${_default} AND NOT ${_opt_name}) + set(${_var} TRUE) + PCL_SET_SUBSYS_STATUS(${_parent}_${_name} TRUE) + PCL_ENABLE_DEPENDIES(${_parent}_${_name}) + endif((NOT ${_default} AND NOT ${_opt_name}) OR ("${_default}" STREQUAL "AUTO_OFF")) + endif(NOT ("${subsys_status}" STREQUAL "AUTO_OFF")) + endif(NOT ("${parent_status}" STREQUAL "AUTO_OFF") AND NOT ("${parent_status}" STREQUAL "OFF")) + PCL_ADD_SUBSUBSYSTEM(${_parent} ${_name} ${_desc}) +endmacro(PCL_SUBSUBSYS_OPTION) ############################################################################### # Make one subsystem depend on one or more other subsystems, and disable it if @@ -82,6 +119,54 @@ macro(PCL_SUBSYS_DEPEND _var _name) endif(${_var} AND (NOT ("${subsys_status}" STREQUAL "AUTO_OFF"))) endmacro(PCL_SUBSYS_DEPEND) +############################################################################### +# Make one subsystem depend on one or more other subsystems, and disable it if +# they are not being built. +# _var The cumulative build variable. This will be set to FALSE if the +# dependencies are not met. +# _parent The parent subsystem name. +# _name The name of the subsubsystem. +# ARGN The subsystems and external libraries to depend on. +macro(PCL_SUBSUBSYS_DEPEND _var _parent _name) + set(options) + set(parentArg) + set(nameArg) + set(multiValueArgs DEPS EXT_DEPS OPT_DEPS) + cmake_parse_arguments(SUBSYS "${options}" "${parentArg}" "${nameArg}" "${multiValueArgs}" ${ARGN} ) + if(SUBSUBSYS_DEPS) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_DEPS ${_parent}_${_name} "${SUBSUBSYS_DEPS}") + endif(SUBSUBSYS_DEPS) + if(SUBSUBSYS_EXT_DEPS) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_EXT_DEPS ${_parent}_${_name} "${SUBSUBSYS_EXT_DEPS}") + endif(SUBSUBSYS_EXT_DEPS) + if(SUBSUBSYS_OPT_DEPS) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_OPT_DEPS ${_parent}_${_name} "${SUBSUBSYS_OPT_DEPS}") + endif(SUBSUBSYS_OPT_DEPS) + GET_IN_MAP(subsys_status PCL_SUBSYS_HYPERSTATUS ${_parent}_${_name}) + if(${_var} AND (NOT ("${subsys_status}" STREQUAL "AUTO_OFF"))) + if(SUBSUBSYS_DEPS) + foreach(_dep ${SUBSUBSYS_DEPS}) + PCL_GET_SUBSYS_STATUS(_status ${_dep}) + if(NOT _status) + set(${_var} FALSE) + PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE "Requires ${_dep}.") + else(NOT _status) + PCL_GET_SUBSYS_INCLUDE_DIR(_include_dir ${_dep}) + include_directories(${PROJECT_SOURCE_DIR}/${_include_dir}/include) + endif(NOT _status) + endforeach(_dep) + endif(SUBSUBSYS_DEPS) + if(SUBSUBSYS_EXT_DEPS) + foreach(_dep ${SUBSUBSYS_EXT_DEPS}) + string(TOUPPER "${_dep}_found" EXT_DEP_FOUND) + if(NOT ${EXT_DEP_FOUND} OR (NOT ("${EXT_DEP_FOUND}" STREQUAL "TRUE"))) + set(${_var} FALSE) + PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE "Requires external library ${_dep}.") + endif(NOT ${EXT_DEP_FOUND} OR (NOT ("${EXT_DEP_FOUND}" STREQUAL "TRUE"))) + endforeach(_dep) + endif(SUBSUBSYS_EXT_DEPS) + endif(${_var} AND (NOT ("${subsys_status}" STREQUAL "AUTO_OFF"))) +endmacro(PCL_SUBSUBSYS_DEPEND) ############################################################################### # Add a set of include files to install. @@ -290,6 +375,8 @@ macro(PCL_ADD_TEST _name _exename) else(${CMAKE_VERSION} VERSION_LESS 2.8.4) add_test(NAME ${_name} COMMAND ${_exename} ${PCL_ADD_TEST_ARGUMENTS}) endif(${CMAKE_VERSION} VERSION_LESS 2.8.4) + + add_dependencies(tests ${_exename}) endmacro(PCL_ADD_TEST) ############################################################################### @@ -420,6 +507,15 @@ endmacro(PCL_MAKE_PKGCONFIG_HEADER_ONLY) ############################################################################### # Reset the subsystem status map. macro(PCL_RESET_MAPS) + foreach(_ss ${PCL_SUBSYSTEMS}) + string(TOUPPER "PCL_${_ss}_SUBSYS" PCL_SUBSYS_SUBSYS) + if (${PCL_SUBSYS_SUBSYS}) + string(TOUPPER "PCL_${_ss}_SUBSYS_DESC" PCL_PARENT_SUBSYS_DESC) + set(${PCL_SUBSYS_SUBSYS_DESC} "" CACHE INTERNAL "" FORCE) + set(${PCL_SUBSYS_SUBSYS} "" CACHE INTERNAL "" FORCE) + endif (${PCL_SUBSYS_SUBSYS}) + endforeach(_ss) + set(PCL_SUBSYS_HYPERSTATUS "" CACHE INTERNAL "To Build Or Not To Build, That Is The Question." FORCE) set(PCL_SUBSYS_STATUS "" CACHE INTERNAL @@ -446,6 +542,20 @@ macro(PCL_ADD_SUBSYSTEM _name _desc) SET_IN_GLOBAL_MAP(PCL_SUBSYS_DESC ${_name} ${_desc}) endmacro(PCL_ADD_SUBSYSTEM) +############################################################################### +# Register a subsubsystem. +# _name Subsystem name. +# _desc Description of the subsystem +macro(PCL_ADD_SUBSUBSYSTEM _parent _name _desc) + string(TOUPPER "PCL_${_parent}_SUBSYS" PCL_PARENT_SUBSYS) + string(TOUPPER "PCL_${_parent}_SUBSYS_DESC" PCL_PARENT_SUBSYS_DESC) + set(_temp ${${PCL_PARENT_SUBSYS}}) + list(APPEND _temp ${_name}) + set(${PCL_PARENT_SUBSYS} ${_temp} CACHE INTERNAL "Internal list of ${_parenr} subsystems" + FORCE) + set_in_global_map(${PCL_PARENT_SUBSYS_DESC} ${_name} ${_desc}) +endmacro(PCL_ADD_SUBSUBSYSTEM) + ############################################################################### # Set the status of a subsystem. @@ -462,6 +572,22 @@ macro(PCL_SET_SUBSYS_STATUS _name _status) SET_IN_GLOBAL_MAP(PCL_SUBSYS_REASONS ${_name} ${_reason}) endmacro(PCL_SET_SUBSYS_STATUS) +############################################################################### +# Set the status of a subsystem. +# _name Subsystem name. +# _status TRUE if being built, FALSE otherwise. +# ARGN[0] Reason for not building. +macro(PCL_SET_SUBSUBSYS_STATUS _parent _name _status) + if(${ARGC} EQUAL 4) + set(_reason ${ARGV2}) + else(${ARGC} EQUAL 4) + set(_reason "No reason") + endif(${ARGC} EQUAL 4) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_STATUS ${_parent}_${_name} ${_status}) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_REASONS ${_parent}_${_name} ${_reason}) +endmacro(PCL_SET_SUBSUBSYS_STATUS) + + ############################################################################### # Get the status of a subsystem # _var Destination variable. @@ -470,6 +596,15 @@ macro(PCL_GET_SUBSYS_STATUS _var _name) GET_IN_MAP(${_var} PCL_SUBSYS_STATUS ${_name}) endmacro(PCL_GET_SUBSYS_STATUS) +############################################################################### +# Get the status of a subsystem +# _var Destination variable. +# _name Name of the subsystem. +macro(PCL_GET_SUBSUBSYS_STATUS _var _parent _name) + GET_IN_MAP(${_var} PCL_SUBSYS_STATUS ${_parent}_${_name}) +endmacro(PCL_GET_SUBSUBSYS_STATUS) + + ############################################################################### # Set the hyperstatus of a subsystem and its dependee # _name Subsystem name. @@ -538,7 +673,33 @@ macro(PCL_WRITE_STATUS_REPORT) foreach(_ss ${PCL_SUBSYSTEMS}) PCL_GET_SUBSYS_STATUS(_status ${_ss}) if(_status) - message(STATUS " ${_ss}") + set(message_text " ${_ss}") + string(TOUPPER "PCL_${_ss}_SUBSYS" PCL_SUBSYS_SUBSYS) + if (${PCL_SUBSYS_SUBSYS}) + set(will_build) + foreach(_sub ${${PCL_SUBSYS_SUBSYS}}) + PCL_GET_SUBSYS_STATUS(_sub_status ${_ss}_${_sub}) + if (_sub_status) + set(will_build "${will_build}\n |_ ${_sub}") + endif (_sub_status) + endforeach(_sub) + if (NOT ("${will_build}" STREQUAL "")) + set(message_text "${message_text}\n building: ${will_build}") + endif (NOT ("${will_build}" STREQUAL "")) + set(wont_build) + foreach(_sub ${${PCL_SUBSYS_SUBSYS}}) + PCL_GET_SUBSYS_STATUS(_sub_status ${_ss}_${_sub}) + PCL_GET_SUBSYS_HYPERSTATUS(_sub_hyper_status ${_ss}_${sub}) + if (NOT _sub_status OR ("${_sub_hyper_status}" STREQUAL "AUTO_OFF")) + GET_IN_MAP(_reason PCL_SUBSYS_REASONS ${_ss}_${_sub}) + set(wont_build "${wont_build}\n |_ ${_sub}: ${_reason}") + endif (NOT _sub_status OR ("${_sub_hyper_status}" STREQUAL "AUTO_OFF")) + endforeach(_sub) + if (NOT ("${wont_build}" STREQUAL "")) + set(message_text "${message_text}\n not building: ${wont_build}") + endif (NOT ("${wont_build}" STREQUAL "")) + endif (${PCL_SUBSYS_SUBSYS}) + message(STATUS "${message_text}") endif(_status) endforeach(_ss) @@ -565,7 +726,7 @@ endmacro(PCL_WRITE_STATUS_REPORT) # exception_list OPTIONAL and contains list of subdirectories not to account macro(collect_subproject_directory_names dirname filename names dirs) file(GLOB globbed RELATIVE "${dirname}" "${dirname}/*/${filename}") - if(${ARGC} GREATER 3) + if(${ARGC} GREATER 4) set(exclusion_list ${ARGN}) foreach(file ${globbed}) get_filename_component(dir ${file} PATH) @@ -574,18 +735,15 @@ macro(collect_subproject_directory_names dirname filename names dirs) set(${dirs} ${${dirs}} ${dir}) endif(excluded EQUAL -1) endforeach() - else(${ARGC} GREATER 3) + else(${ARGC} GREATER 4) foreach(file ${globbed}) get_filename_component(dir ${file} PATH) set(${dirs} ${${dirs}} ${dir}) endforeach(file) - endif(${ARGC} GREATER 3) + endif(${ARGC} GREATER 4) foreach(subdir ${${dirs}}) - file(STRINGS ${dirname}/${subdir}/CMakeLists.txt name REGEX "set.*SUBSYS_NAME .*\\)$") - string(REGEX REPLACE "set.*SUBSYS_NAME" "" name "${name}") - string(REPLACE ")" "" name "${name}") - string(STRIP "${name}" name) -# message(STATUS "setting ${subdir} component name to ${name}") + file(STRINGS ${dirname}/${subdir}/CMakeLists.txt name REGEX "[setSET ]+\\(.*SUBSYS_NAME .*\\)$") + string(REGEX REPLACE "[setSET ]+\\(.*SUBSYS_NAME[ ]+([A-Za-z0-9_]+)[ ]*\\)" "\\1" name "${name}") set(${names} ${${names}} ${name}) file(STRINGS ${dirname}/${subdir}/CMakeLists.txt DEPENDENCIES REGEX "set.*SUBSYS_DEPS .*\\)") string(REGEX REPLACE "set.*SUBSYS_DEPS" "" DEPENDENCIES "${DEPENDENCIES}") diff --git a/cmake/pcl_utils.cmake b/cmake/pcl_utils.cmake index 976bb9dd..55b0820f 100644 --- a/cmake/pcl_utils.cmake +++ b/cmake/pcl_utils.cmake @@ -41,6 +41,20 @@ macro(PREFIX_LIST _output _prefix _list) endforeach(_item) endmacro(PREFIX_LIST) +############################################################################### +# Remove vtk definitions +# This is used for CUDA targets, because nvcc does not like VTK 6+ definitions +# style. +macro(REMOVE_VTK_DEFINITIONS) + get_directory_property(_dir_defs DIRECTORY ${CMAKE_SOURCE_DIR} COMPILE_DEFINITIONS) + set(_vtk_definitions) + foreach(_item ${_dir_defs}) + if(_item MATCHES "vtk*") + list(APPEND _vtk_definitions -D${_item}) + endif() + endforeach() + remove_definitions(${_vtk_definitions}) +endmacro(REMOVE_VTK_DEFINITIONS) ############################################################################### # Pull the component parts out of the version number. @@ -84,8 +98,12 @@ macro(SET_INSTALL_DIRS) if (NOT DEFINED LIB_INSTALL_DIR) set(LIB_INSTALL_DIR "lib") endif (NOT DEFINED LIB_INSTALL_DIR) - set(INCLUDE_INSTALL_ROOT - "include/${PROJECT_NAME_LOWER}-${PCL_MAJOR_VERSION}.${PCL_MINOR_VERSION}") + if(NOT ANDROID) + set(INCLUDE_INSTALL_ROOT + "include/${PROJECT_NAME_LOWER}-${PCL_MAJOR_VERSION}.${PCL_MINOR_VERSION}") + else(NOT ANDROID) + set(INCLUDE_INSTALL_ROOT "include") # Android, don't put into subdir + endif(NOT ANDROID) set(INCLUDE_INSTALL_DIR "${INCLUDE_INSTALL_ROOT}/pcl") set(DOC_INSTALL_DIR "share/doc/${PROJECT_NAME_LOWER}-${PCL_MAJOR_VERSION}.${PCL_MINOR_VERSION}") set(BIN_INSTALL_DIR "bin") diff --git a/cmake/uninstall_target.cmake.in b/cmake/uninstall_target.cmake.in index a3c4053c..d1191233 100644 --- a/cmake/uninstall_target.cmake.in +++ b/cmake/uninstall_target.cmake.in @@ -5,7 +5,6 @@ endif(NOT EXISTS "@PROJECT_BINARY_DIR@/install_manifest.txt") file(READ "@PROJECT_BINARY_DIR@/install_manifest.txt" files) string(REGEX REPLACE "\n" ";" files "${files}") foreach(file ${files}) - message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") if(EXISTS "$ENV{DESTDIR}${file}" OR IS_SYMLINK "$ENV{DESTDIR}${file}") exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" @@ -49,18 +48,19 @@ else(EXISTS "@CMAKE_INSTALL_PREFIX@/@PCLCONFIG_INSTALL_DIR@") "Directory \"@CMAKE_INSTALL_PREFIX@/@PCLCONFIG_INSTALL_DIR@\" does not exist.") endif(EXISTS "@CMAKE_INSTALL_PREFIX@/@PCLCONFIG_INSTALL_DIR@") -# remove pcl directory in share (removes all files in it!) -# created by CMakeLists.txt for PCLConfig.cmake -message(STATUS "Uninstalling \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\"") -if(EXISTS "@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@") - exec_program("@CMAKE_COMMAND@" - ARGS "-E remove_directory \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\"" - OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) - if(NOT "${rm_retval}" STREQUAL 0) - message(FATAL_ERROR - "Problem when removing \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\"") - endif(NOT "${rm_retval}" STREQUAL 0) -else(EXISTS "@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@") - message(STATUS - "Directory \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\" does not exist.") -endif(EXISTS "@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@") +# remove pcl directory in share/doc (removes all files in it!) +if(@WITH_DOCS@) + message(STATUS "Uninstalling \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\"") + if(EXISTS "@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@") + exec_program("@CMAKE_COMMAND@" + ARGS "-E remove_directory \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\"" + OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) + if(NOT "${rm_retval}" STREQUAL 0) + message(FATAL_ERROR + "Problem when removing \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\"") + endif(NOT "${rm_retval}" STREQUAL 0) + else(EXISTS "@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@") + message(STATUS + "Directory \"@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@\" does not exist.") + endif(EXISTS "@CMAKE_INSTALL_PREFIX@/@DOC_INSTALL_DIR@") +endif() diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 54113ceb..34ac2990 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -3,13 +3,14 @@ set(SUBSYS_DESC "Point cloud common library") set(SUBSYS_DEPS) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS eigen boost) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS eigen boost) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(range_image_incs + include/pcl/range_image/bearing_angle_image.h include/pcl/range_image/range_image.h include/pcl/range_image/range_image_planar.h include/pcl/range_image/range_image_spherical.h @@ -22,6 +23,7 @@ if(build) ) set(range_image_srcs + src/bearing_angle_image.cpp src/range_image.cpp src/range_image_planar.cpp ) @@ -33,7 +35,6 @@ if(build) src/common.cpp src/correspondence.cpp src/distances.cpp - src/intersections.cpp src/parse.cpp src/poses_from_matches.cpp src/print.cpp @@ -53,7 +54,6 @@ if(build) include/pcl/point_traits.h include/pcl/point_types_conversion.h include/pcl/point_representation.h - include/pcl/correspondence.h include/pcl/point_types.h include/pcl/for_each_type.h include/pcl/pcl_tests.h @@ -90,6 +90,7 @@ if(build) include/pcl/common/common_headers.h include/pcl/common/distances.h include/pcl/common/eigen.h + include/pcl/common/copy_point.h include/pcl/common/io.h include/pcl/common/file_io.h include/pcl/common/intersections.h @@ -124,6 +125,8 @@ if(build) include/pcl/common/impl/centroid.hpp include/pcl/common/impl/common.hpp include/pcl/common/impl/eigen.hpp + include/pcl/common/impl/intersections.hpp + include/pcl/common/impl/copy_point.hpp include/pcl/common/impl/io.hpp include/pcl/common/impl/file_io.hpp include/pcl/common/impl/norms.hpp @@ -139,6 +142,7 @@ if(build) include/pcl/common/impl/random.hpp include/pcl/common/impl/generate.hpp include/pcl/common/impl/projection_matrix.hpp + include/pcl/common/impl/accumulators.hpp ) set(impl_incs @@ -168,22 +172,22 @@ if(build) src/fft/kiss_fft.c src/fft/kiss_fftr.c) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${kissfft_srcs} ${incs} ${common_incs} ${impl_incs} ${ros_incs} ${tools_incs} ${kissfft_incs} ${common_incs_impl} ${range_image_incs} ${range_image_incs_impl}) - #PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${common_incs} ${impl_incs} ${ros_incs} ${tools_incs} ${common_incs_impl} ${range_image_incs} ${range_image_incs_impl}) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "" "" + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${kissfft_srcs} ${incs} ${common_incs} ${impl_incs} ${ros_incs} ${tools_incs} ${kissfft_incs} ${common_incs_impl} ${range_image_incs} ${range_image_incs_impl}) + #PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${common_incs} ${impl_incs} ${ros_incs} ${tools_incs} ${common_incs_impl} ${range_image_incs} ${range_image_incs_impl}) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} "" ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} common ${common_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} common/fft ${kissfft_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} common/impl ${common_incs_impl}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} impl ${impl_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ros ${ros_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} console ${tools_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} range_image ${range_image_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} range_image/impl ${range_image_incs_impl}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" common ${common_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" common/fft ${kissfft_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" common/impl ${common_incs_impl}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" ros ${ros_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" console ${tools_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" range_image ${range_image_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" range_image/impl ${range_image_incs_impl}) endif(build) diff --git a/common/include/pcl/PCLHeader.h b/common/include/pcl/PCLHeader.h index cdccbe6b..84a64076 100644 --- a/common/include/pcl/PCLHeader.h +++ b/common/include/pcl/PCLHeader.h @@ -18,9 +18,14 @@ namespace pcl PCLHeader (): seq (0), stamp (), frame_id () {} + /** \brief Sequence number */ pcl::uint32_t seq; + /** \brief A timestamp associated with the time when the data was acquired + * + * The value represents microseconds since 1970-01-01 00:00:00 (the UNIX epoch). + */ pcl::uint64_t stamp; - + /** \brief Coordinate frame ID */ std::string frame_id; typedef boost::shared_ptr Ptr; diff --git a/common/include/pcl/common/centroid.h b/common/include/pcl/common/centroid.h index 112b9565..535ddf0c 100644 --- a/common/include/pcl/common/centroid.h +++ b/common/include/pcl/common/centroid.h @@ -58,6 +58,7 @@ namespace pcl * \param[out] centroid the output centroid * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. * \note if return value is 0, the centroid is not changed, thus not valid. + * The last compononent of the vector is set to 1, this allow to transform the centroid vector with 4x4 matrices. * \ingroup common */ template inline unsigned int @@ -83,6 +84,7 @@ namespace pcl * \param[out] centroid the output centroid * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. * \note if return value is 0, the centroid is not changed, thus not valid. + * The last compononent of the vector is set to 1, this allow to transform the centroid vector with 4x4 matrices. * \ingroup common */ template inline unsigned int @@ -110,6 +112,7 @@ namespace pcl * \param[out] centroid the output centroid * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. * \note if return value is 0, the centroid is not changed, thus not valid. + * The last compononent of the vector is set to 1, this allow to transform the centroid vector with 4x4 matrices. * \ingroup common */ template inline unsigned int @@ -140,6 +143,7 @@ namespace pcl * \param[out] centroid the output centroid * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. * \note if return value is 0, the centroid is not changed, thus not valid. + * The last compononent of the vector is set to 1, this allow to transform the centroid vector with 4x4 matrices. * \ingroup common */ template inline unsigned int @@ -945,6 +949,184 @@ namespace pcl return (computeNDCentroid (cloud, indices, centroid)); } +} + +#include + +namespace pcl +{ + + /** A generic class that computes the centroid of points fed to it. + * + * Here by "centroid" we denote not just the mean of 3D point coordinates, + * but also mean of values in the other data fields. The general-purpose + * \ref computeNDCentroid() function also implements this sort of + * functionality, however it does it in a "dumb" way, i.e. regardless of the + * semantics of the data inside a field it simply averages the values. In + * certain cases (e.g. for \c x, \c y, \c z, \c intensity fields) this + * behavior is reasonable, however in other cases (e.g. \c rgb, \c rgba, + * \c label fields) this does not lead to meaningful results. + * + * This class is capable of computing the centroid in a "smart" way, i.e. + * taking into account the meaning of the data inside fields. Currently the + * following fields are supported: + * + * - XYZ (\c x, \c y, \c z) + * + * Separate average for each field. + * + * - Normal (\c normal_x, \c normal_y, \c normal_z) + * + * Separate average for each field, and the resulting vector is normalized. + * + * - Curvature (\c curvature) + * + * Average. + * + * - RGB/RGBA (\c rgb or \c rgba) + * + * Separate average for R, G, B, and alpha channels. + * + * - Intensity (\c intensity) + * + * Average. + * + * - Label (\c label) + * + * Majority vote. If several labels have the same largest support then the + * smaller label wins. + * + * The template parameter defines the type of points that may be accumulated + * with this class. This may be an arbitrary PCL point type, and centroid + * computation will happen only for the fields that are present in it and are + * supported. + * + * Current centroid may be retrieved at any time using get(). Note that the + * function is templated on point type, so it is possible to fetch the + * centroid into a point type that differs from the type of points that are + * being accumulated. All the "extra" fields for which the centroid is not + * being calculated will be left untouched. + * + * Example usage: + * + * \code + * // Create and accumulate points + * CentroidPoint centroid; + * centroid.add (pcl::PointXYZ (1, 2, 3); + * centroid.add (pcl::PointXYZ (5, 6, 7); + * // Fetch centroid using `get()` + * pcl::PointXYZ c1; + * centroid.get (c1); + * // The expected result is: c1.x == 3, c1.y == 4, c1.z == 5 + * // It is also okay to use `get()` with a different point type + * pcl::PointXYZRGB c2; + * centroid.get (c2); + * // The expected result is: c2.x == 3, c2.y == 4, c2.z == 5, + * // and c2.rgb is left untouched + * \endcode + * + * \note Assumes that the points being inserted are valid. + * + * \note This class template can be successfully instantiated for *any* + * PCL point type. Of course, each of the field averages is computed only if + * the point type has the corresponding field. + * + * \ingroup common + * \author Sergey Alexandrov */ + template + class CentroidPoint + { + + public: + + CentroidPoint () + : num_points_ (0) + { + } + + /** Add a new point to the centroid computation. + * + * In this function only the accumulators and point counter are updated, + * actual centroid computation does not happen until get() is called. */ + void + add (const PointT& point) + { + // Invoke add point on each accumulator + boost::fusion::for_each (accumulators_, detail::AddPoint (point)); + ++num_points_; + } + + /** Retrieve the current centroid. + * + * Computation (division of accumulated values by the number of points + * and normalization where applicable) happens here. The result is not + * cached, so any subsequent call to this function will trigger + * re-computation. + * + * If the number of accumulated points is zero, then the point will be + * left untouched. */ + template void + get (PointOutT& point) const + { + if (num_points_ != 0) + { + // Filter accumulators so that only those that are compatible with + // both PointT and requested point type remain + typename pcl::detail::Accumulators::type ca (accumulators_); + // Invoke get point on each accumulator in filtered list + boost::fusion::for_each (ca, detail::GetPoint (point, num_points_)); + } + } + + /** Get the total number of points that were added. */ + size_t + getSize () const + { + return (num_points_); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + + size_t num_points_; + typename pcl::detail::Accumulators::type accumulators_; + + }; + + /** Compute the centroid of a set of points and return it as a point. + * + * Implementation leverages \ref CentroidPoint class and therefore behaves + * differently from \ref compute3DCentroid() and \ref computeNDCentroid(). + * See \ref CentroidPoint documentation for explanation. + * + * \param[in] cloud input point cloud + * \param[out] centroid output centroid + * + * \return number of valid points used to determine the centroid (will be the + * same as the size of the cloud if it is dense) + * + * \note If return value is \c 0, then the centroid is not changed, thus is + * not valid. + * + * \ingroup common */ + template size_t + computeCentroid (const pcl::PointCloud& cloud, + PointOutT& centroid); + + /** Compute the centroid of a set of points and return it as a point. + * \param[in] cloud + * \param[in] indices point cloud indices that need to be used + * \param[out] centroid + * This is an overloaded function provided for convenience. See the + * documentation for computeCentroid(). + * + * \ingroup common */ + template size_t + computeCentroid (const pcl::PointCloud& cloud, + const std::vector& indices, + PointOutT& centroid); + } /*@}*/ #include diff --git a/common/include/pcl/common/copy_point.h b/common/include/pcl/common/copy_point.h new file mode 100644 index 00000000..0e80cfe7 --- /dev/null +++ b/common/include/pcl/common/copy_point.h @@ -0,0 +1,62 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_COPY_POINT_H_ +#define PCL_COMMON_COPY_POINT_H_ + +namespace pcl +{ + + /** \brief Copy the fields of a source point into a target point. + * + * If the source and the target point types are the same, then a complete + * copy is made. Otherwise only those fields that the two point types share + * in common are copied. + * + * \param[in] point_in the source point + * \param[out] point_out the target point + * + * \ingroup common */ + template void + copyPoint (const PointInT& point_in, PointOutT& point_out); + +} + +#include + +#endif // PCL_COMMON_COPY_POINT_H_ + diff --git a/common/include/pcl/common/eigen.h b/common/include/pcl/common/eigen.h index 0f030f41..9557f030 100644 --- a/common/include/pcl/common/eigen.h +++ b/common/include/pcl/common/eigen.h @@ -53,6 +53,7 @@ #endif #include +#include #include #include @@ -70,88 +71,15 @@ namespace pcl * \param[in] c constant parameter * \param[out] roots solutions of x^2 + b*x + c = 0 */ - template inline void - computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots) - { - roots (0) = Scalar (0); - Scalar d = Scalar (b * b - 4.0 * c); - if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN! - d = 0.0; - - Scalar sd = ::std::sqrt (d); - - roots (2) = 0.5f * (b + sd); - roots (1) = 0.5f * (b - sd); - } + template void + computeRoots2 (const Scalar &b, const Scalar &c, Roots &roots); /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues * \param[in] m input matrix * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues */ - template inline void - computeRoots (const Matrix& m, Roots& roots) - { - typedef typename Matrix::Scalar Scalar; - - // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The - // eigenvalues are the roots to this equation, all guaranteed to be - // real-valued, because the matrix is symmetric. - Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2) - + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2) - - m (0, 0) * m (1, 2) * m (1, 2) - - m (1, 1) * m (0, 2) * m (0, 2) - - m (2, 2) * m (0, 1) * m (0, 1); - Scalar c1 = m (0, 0) * m (1, 1) - - m (0, 1) * m (0, 1) + - m (0, 0) * m (2, 2) - - m (0, 2) * m (0, 2) + - m (1, 1) * m (2, 2) - - m (1, 2) * m (1, 2); - Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2); - - - if (fabs (c0) < Eigen::NumTraits::epsilon ())// one root is 0 -> quadratic equation - computeRoots2 (c2, c1, roots); - else - { - const Scalar s_inv3 = Scalar (1.0 / 3.0); - const Scalar s_sqrt3 = std::sqrt (Scalar (3.0)); - // Construct the parameters used in classifying the roots of the equation - // and in solving the equation for the roots in closed form. - Scalar c2_over_3 = c2*s_inv3; - Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3; - if (a_over_3 > Scalar (0)) - a_over_3 = Scalar (0); - - Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1)); - - Scalar q = half_b * half_b + a_over_3 * a_over_3*a_over_3; - if (q > Scalar (0)) - q = Scalar (0); - - // Compute the eigenvalues by solving for the roots of the polynomial. - Scalar rho = std::sqrt (-a_over_3); - Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3; - Scalar cos_theta = std::cos (theta); - Scalar sin_theta = std::sin (theta); - roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta; - roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta); - roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta); - - // Sort in increasing order. - if (roots (0) >= roots (1)) - std::swap (roots (0), roots (1)); - if (roots (1) >= roots (2)) - { - std::swap (roots (1), roots (2)); - if (roots (0) >= roots (1)) - std::swap (roots (0), roots (1)); - } - - if (roots (0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0 - computeRoots2 (c2, c1, roots); - } - } + template void + computeRoots (const Matrix &m, Roots &roots); /** \brief determine the smallest eigenvalue and its corresponding eigenvector * \param[in] mat input matrix that needs to be symmetric and positive semi definite @@ -159,43 +87,8 @@ namespace pcl * \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix * \ingroup common */ - template inline void - eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) - { - // if diagonal matrix, the eigenvalues are the diagonal elements - // and the eigenvectors are not unique, thus set to Identity - if (fabs(mat.coeff (1)) <= std::numeric_limits::min ()) - { - if (mat.coeff (0) < mat.coeff (2)) - { - eigenvalue = mat.coeff (0); - eigenvector [0] = 1.0; - eigenvector [1] = 0.0; - } - else - { - eigenvalue = mat.coeff (2); - eigenvector [0] = 0.0; - eigenvector [1] = 1.0; - } - return; - } - - // 0.5 to optimize further calculations - typename Matrix::Scalar trace = static_cast (0.5) * (mat.coeff (0) + mat.coeff (3)); - typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); - - typename Matrix::Scalar temp = trace * trace - determinant; - - if (temp < 0) - temp = 0; - - eigenvalue = trace - ::std::sqrt (temp); - - eigenvector [0] = - mat.coeff (1); - eigenvector [1] = mat.coeff (0) - eigenvalue; - eigenvector.normalize (); - } + template void + eigen22 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector); /** \brief determine the smallest eigenvalue and its corresponding eigenvector * \param[in] mat input matrix that needs to be symmetric and positive semi definite @@ -203,58 +96,8 @@ namespace pcl * \param[out] eigenvalues the smallest eigenvalue of the input matrix * \ingroup common */ - template inline void - eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues) - { - // if diagonal matrix, the eigenvalues are the diagonal elements - // and the eigenvectors are not unique, thus set to Identity - if (fabs(mat.coeff (1)) <= std::numeric_limits::min ()) - { - if (mat.coeff (0) < mat.coeff (3)) - { - eigenvalues.coeffRef (0) = mat.coeff (0); - eigenvalues.coeffRef (1) = mat.coeff (3); - eigenvectors.coeffRef (0) = 1.0; - eigenvectors.coeffRef (1) = 0.0; - eigenvectors.coeffRef (2) = 0.0; - eigenvectors.coeffRef (3) = 1.0; - } - else - { - eigenvalues.coeffRef (0) = mat.coeff (3); - eigenvalues.coeffRef (1) = mat.coeff (0); - eigenvectors.coeffRef (0) = 0.0; - eigenvectors.coeffRef (1) = 1.0; - eigenvectors.coeffRef (2) = 1.0; - eigenvectors.coeffRef (3) = 0.0; - } - return; - } - - // 0.5 to optimize further calculations - typename Matrix::Scalar trace = static_cast (0.5) * (mat.coeff (0) + mat.coeff (3)); - typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); - - typename Matrix::Scalar temp = trace * trace - determinant; - - if (temp < 0) - temp = 0; - else - temp = ::std::sqrt (temp); - - eigenvalues.coeffRef (0) = trace - temp; - eigenvalues.coeffRef (1) = trace + temp; - - // either this is in a row or column depending on RowMajor or ColumnMajor - eigenvectors.coeffRef (0) = - mat.coeff (1); - eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0); - typename Matrix::Scalar norm = static_cast (1.0) / - static_cast (::std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2))); - eigenvectors.coeffRef (0) *= norm; - eigenvectors.coeffRef (2) *= norm; - eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2); - eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0); - } + template void + eigen22 (const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues); /** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix * \param[in] mat symmetric positive semi definite input matrix @@ -262,36 +105,8 @@ namespace pcl * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue * \ingroup common */ - template inline void - computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector) - { - typedef typename Matrix::Scalar Scalar; - // Scale the matrix so its entries are in [-1,1]. The scaling is applied - // only when at least one matrix entry has magnitude larger than 1. - - Scalar scale = mat.cwiseAbs ().maxCoeff (); - if (scale <= std::numeric_limits::min ()) - scale = Scalar (1.0); - - Matrix scaledMat = mat / scale; - - scaledMat.diagonal ().array () -= eigenvalue / scale; - - Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); - Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); - Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - eigenvector = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - eigenvector = vec2 / std::sqrt (len2); - else - eigenvector = vec3 / std::sqrt (len3); - } + template void + computeCorrespondingEigenVector (const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector); /** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix * \param[in] mat symmetric positive semi definite input matrix @@ -300,59 +115,16 @@ namespace pcl * \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue. * \ingroup common */ - template inline void - eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) - { - typedef typename Matrix::Scalar Scalar; - // Scale the matrix so its entries are in [-1,1]. The scaling is applied - // only when at least one matrix entry has magnitude larger than 1. - - Scalar scale = mat.cwiseAbs ().maxCoeff (); - if (scale <= std::numeric_limits::min ()) - scale = Scalar (1.0); - - Matrix scaledMat = mat / scale; - - Vector eigenvalues; - computeRoots (scaledMat, eigenvalues); - - eigenvalue = eigenvalues (0) * scale; - - scaledMat.diagonal ().array () -= eigenvalues (0); - - Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); - Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); - Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - eigenvector = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - eigenvector = vec2 / std::sqrt (len2); - else - eigenvector = vec3 / std::sqrt (len3); - } + template void + eigen33 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector); /** \brief determines the eigenvalues of the symmetric positive semi definite input matrix * \param[in] mat symmetric positive semi definite input matrix * \param[out] evals resulting eigenvalues in ascending order * \ingroup common */ - template inline void - eigen33 (const Matrix& mat, Vector& evals) - { - typedef typename Matrix::Scalar Scalar; - Scalar scale = mat.cwiseAbs ().maxCoeff (); - if (scale <= std::numeric_limits::min ()) - scale = Scalar (1.0); - - Matrix scaledMat = mat / scale; - computeRoots (scaledMat, evals); - evals *= scale; - } + template void + eigen33 (const Matrix &mat, Vector &evals); /** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix * \param[in] mat symmetric positive semi definite input matrix @@ -360,187 +132,8 @@ namespace pcl * \param[out] evals corresponding eigenvectors in correct order according to eigenvalues * \ingroup common */ - template inline void - eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) - { - typedef typename Matrix::Scalar Scalar; - // Scale the matrix so its entries are in [-1,1]. The scaling is applied - // only when at least one matrix entry has magnitude larger than 1. - - Scalar scale = mat.cwiseAbs ().maxCoeff (); - if (scale <= std::numeric_limits::min ()) - scale = Scalar (1.0); - - Matrix scaledMat = mat / scale; - - // Compute the eigenvalues - computeRoots (scaledMat, evals); - - if ((evals (2) - evals (0)) <= Eigen::NumTraits::epsilon ()) - { - // all three equal - evecs.setIdentity (); - } - else if ((evals (1) - evals (0)) <= Eigen::NumTraits::epsilon () ) - { - // first and second equal - Matrix tmp; - tmp = scaledMat; - tmp.diagonal ().array () -= evals (2); - - Vector vec1 = tmp.row (0).cross (tmp.row (1)); - Vector vec2 = tmp.row (0).cross (tmp.row (2)); - Vector vec3 = tmp.row (1).cross (tmp.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - evecs.col (2) = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - evecs.col (2) = vec2 / std::sqrt (len2); - else - evecs.col (2) = vec3 / std::sqrt (len3); - - evecs.col (1) = evecs.col (2).unitOrthogonal (); - evecs.col (0) = evecs.col (1).cross (evecs.col (2)); - } - else if ((evals (2) - evals (1)) <= Eigen::NumTraits::epsilon () ) - { - // second and third equal - Matrix tmp; - tmp = scaledMat; - tmp.diagonal ().array () -= evals (0); - - Vector vec1 = tmp.row (0).cross (tmp.row (1)); - Vector vec2 = tmp.row (0).cross (tmp.row (2)); - Vector vec3 = tmp.row (1).cross (tmp.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - evecs.col (0) = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - evecs.col (0) = vec2 / std::sqrt (len2); - else - evecs.col (0) = vec3 / std::sqrt (len3); - - evecs.col (1) = evecs.col (0).unitOrthogonal (); - evecs.col (2) = evecs.col (0).cross (evecs.col (1)); - } - else - { - Matrix tmp; - tmp = scaledMat; - tmp.diagonal ().array () -= evals (2); - - Vector vec1 = tmp.row (0).cross (tmp.row (1)); - Vector vec2 = tmp.row (0).cross (tmp.row (2)); - Vector vec3 = tmp.row (1).cross (tmp.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); -#ifdef _WIN32 - Scalar *mmax = new Scalar[3]; -#else - Scalar mmax[3]; -#endif - unsigned int min_el = 2; - unsigned int max_el = 2; - if (len1 >= len2 && len1 >= len3) - { - mmax[2] = len1; - evecs.col (2) = vec1 / std::sqrt (len1); - } - else if (len2 >= len1 && len2 >= len3) - { - mmax[2] = len2; - evecs.col (2) = vec2 / std::sqrt (len2); - } - else - { - mmax[2] = len3; - evecs.col (2) = vec3 / std::sqrt (len3); - } - - tmp = scaledMat; - tmp.diagonal ().array () -= evals (1); - - vec1 = tmp.row (0).cross (tmp.row (1)); - vec2 = tmp.row (0).cross (tmp.row (2)); - vec3 = tmp.row (1).cross (tmp.row (2)); - - len1 = vec1.squaredNorm (); - len2 = vec2.squaredNorm (); - len3 = vec3.squaredNorm (); - if (len1 >= len2 && len1 >= len3) - { - mmax[1] = len1; - evecs.col (1) = vec1 / std::sqrt (len1); - min_el = len1 <= mmax[min_el] ? 1 : min_el; - max_el = len1 > mmax[max_el] ? 1 : max_el; - } - else if (len2 >= len1 && len2 >= len3) - { - mmax[1] = len2; - evecs.col (1) = vec2 / std::sqrt (len2); - min_el = len2 <= mmax[min_el] ? 1 : min_el; - max_el = len2 > mmax[max_el] ? 1 : max_el; - } - else - { - mmax[1] = len3; - evecs.col (1) = vec3 / std::sqrt (len3); - min_el = len3 <= mmax[min_el] ? 1 : min_el; - max_el = len3 > mmax[max_el] ? 1 : max_el; - } - - tmp = scaledMat; - tmp.diagonal ().array () -= evals (0); - - vec1 = tmp.row (0).cross (tmp.row (1)); - vec2 = tmp.row (0).cross (tmp.row (2)); - vec3 = tmp.row (1).cross (tmp.row (2)); - - len1 = vec1.squaredNorm (); - len2 = vec2.squaredNorm (); - len3 = vec3.squaredNorm (); - if (len1 >= len2 && len1 >= len3) - { - mmax[0] = len1; - evecs.col (0) = vec1 / std::sqrt (len1); - min_el = len3 <= mmax[min_el] ? 0 : min_el; - max_el = len3 > mmax[max_el] ? 0 : max_el; - } - else if (len2 >= len1 && len2 >= len3) - { - mmax[0] = len2; - evecs.col (0) = vec2 / std::sqrt (len2); - min_el = len3 <= mmax[min_el] ? 0 : min_el; - max_el = len3 > mmax[max_el] ? 0 : max_el; - } - else - { - mmax[0] = len3; - evecs.col (0) = vec3 / std::sqrt (len3); - min_el = len3 <= mmax[min_el] ? 0 : min_el; - max_el = len3 > mmax[max_el] ? 0 : max_el; - } - - unsigned mid_el = 3 - min_el - max_el; - evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized (); - evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized (); -#ifdef _WIN32 - delete [] mmax; -#endif - } - // Rescale back to the original size. - evals *= scale; - } + template void + eigen33 (const Matrix &mat, Matrix &evecs, Vector &evals); /** \brief Calculate the inverse of a 2x2 matrix * \param[in] matrix matrix to be inverted @@ -549,23 +142,8 @@ namespace pcl * \return determinant of the original matrix => if 0 no inverse exists => result is invalid * \ingroup common */ - template inline typename Matrix::Scalar - invert2x2 (const Matrix& matrix, Matrix& inverse) - { - typedef typename Matrix::Scalar Scalar; - Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2) ; - - if (det != 0) - { - //Scalar inv_det = Scalar (1.0) / det; - inverse.coeffRef (0) = matrix.coeff (3); - inverse.coeffRef (1) = - matrix.coeff (1); - inverse.coeffRef (2) = - matrix.coeff (2); - inverse.coeffRef (3) = matrix.coeff (0); - inverse /= det; - } - return det; - } + template typename Matrix::Scalar + invert2x2 (const Matrix &matrix, Matrix &inverse); /** \brief Calculate the inverse of a 3x3 symmetric matrix. * \param[in] matrix matrix to be inverted @@ -574,39 +152,8 @@ namespace pcl * \return determinant of the original matrix => if 0 no inverse exists => result is invalid * \ingroup common */ - template inline typename Matrix::Scalar - invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse) - { - typedef typename Matrix::Scalar Scalar; - // elements - // a b c - // b d e - // c e f - //| a b c |-1 | fd-ee ce-bf be-cd | - //| b d e | = 1/det * | ce-bf af-cc bc-ae | - //| c e f | | be-cd bc-ae ad-bb | - - //det = a(fd-ee) + b(ec-fb) + c(eb-dc) - - Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5); - Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8); - Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4); - - Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd; - - if (det != 0) - { - //Scalar inv_det = Scalar (1.0) / det; - inverse.coeffRef (0) = fd_ee; - inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf; - inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd; - inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2)); - inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5)); - inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1)); - inverse /= det; - } - return det; - } + template typename Matrix::Scalar + invert3x3SymMatrix (const Matrix &matrix, Matrix &inverse); /** \brief Calculate the inverse of a general 3x3 matrix. * \param[in] matrix matrix to be inverted @@ -614,46 +161,16 @@ namespace pcl * \return determinant of the original matrix => if 0 no inverse exists => result is invalid * \ingroup common */ - template inline typename Matrix::Scalar - invert3x3Matrix (const Matrix& matrix, Matrix& inverse) - { - typedef typename Matrix::Scalar Scalar; - - //| a b c |-1 | ie-hf hc-ib fb-ec | - //| d e f | = 1/det * | gf-id ia-gc dc-fa | - //| g h i | | hd-ge gb-ha ea-db | - //det = a(ie-hf) + d(hc-ib) + g(fb-ec) - - Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5); - Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1); - Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2); - Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec) ; - - if (det != 0) - { - inverse.coeffRef (0) = ie_hf; - inverse.coeffRef (1) = hc_ib; - inverse.coeffRef (2) = fb_ec; - inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3); - inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2); - inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0); - inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4); - inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0); - inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1); - - inverse /= det; - } - return det; - } + template typename Matrix::Scalar + invert3x3Matrix (const Matrix &matrix, Matrix &inverse); - template inline typename Matrix::Scalar - determinant3x3Matrix (const Matrix& matrix) - { - // result is independent of Row/Col Major storage! - return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) + - matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) + - matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ; - } + /** \brief Calculate the determinant of a 3x3 matrix. + * \param[in] matrix matrix + * \return determinant of the matrix + * \ingroup common + */ + template typename Matrix::Scalar + determinant3x3Matrix (const Matrix &matrix); /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) @@ -745,8 +262,20 @@ namespace pcl * \param[in] yaw the resulting yaw angle * \ingroup common */ + template void + getEulerAngles (const Eigen::Transform &t, Scalar &roll, Scalar &pitch, Scalar &yaw); + + inline void + getEulerAngles (const Eigen::Affine3f &t, float &roll, float &pitch, float &yaw) + { + getEulerAngles (t, roll, pitch, yaw); + } + inline void - getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw); + getEulerAngles (const Eigen::Affine3d &t, double &roll, double &pitch, double &yaw) + { + getEulerAngles (t, roll, pitch, yaw); + } /** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation * \param[in] t the input transformation matrix @@ -758,10 +287,26 @@ namespace pcl * \param[out] yaw the resulting yaw angle * \ingroup common */ + template void + getTranslationAndEulerAngles (const Eigen::Transform &t, + Scalar &x, Scalar &y, Scalar &z, + Scalar &roll, Scalar &pitch, Scalar &yaw); + + inline void + getTranslationAndEulerAngles (const Eigen::Affine3f &t, + float &x, float &y, float &z, + float &roll, float &pitch, float &yaw) + { + getTranslationAndEulerAngles (t, x, y, z, roll, pitch, yaw); + } + inline void - getTranslationAndEulerAngles (const Eigen::Affine3f& t, - float& x, float& y, float& z, - float& roll, float& pitch, float& yaw); + getTranslationAndEulerAngles (const Eigen::Affine3d &t, + double &x, double &y, double &z, + double &roll, double &pitch, double &yaw) + { + getTranslationAndEulerAngles (t, x, y, z, roll, pitch, yaw); + } /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention) * \param[in] x the input x translation @@ -773,7 +318,7 @@ namespace pcl * \param[out] t the resulting transformation matrix * \ingroup common */ - template inline void + template void getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform &t); @@ -802,7 +347,12 @@ namespace pcl * \ingroup common */ inline Eigen::Affine3f - getTransformation (float x, float y, float z, float roll, float pitch, float yaw); + getTransformation (float x, float y, float z, float roll, float pitch, float yaw) + { + Eigen::Affine3f t; + getTransformation (x, y, z, roll, pitch, yaw, t); + return (t); + } /** \brief Write a matrix to an output stream * \param[in] matrix the matrix to output @@ -861,6 +411,306 @@ namespace pcl template typename Eigen::internal::umeyama_transform_matrix_type::type umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling = false); + +/** \brief Transform a point using an affine matrix + * \param[in] point_in the vector to be transformed + * \param[out] point_out the transformed vector + * \param[in] transformation the transformation matrix + * + * \note Can be used with \c point_in = \c point_out + */ + template inline void + transformPoint (const Eigen::Matrix &point_in, + Eigen::Matrix &point_out, + const Eigen::Transform &transformation) + { + Eigen::Matrix point; + point << point_in, 1.0; + point_out = (transformation * point).template head<3> (); + } + + inline void + transformPoint (const Eigen::Vector3f &point_in, + Eigen::Vector3f &point_out, + const Eigen::Affine3f &transformation) + { + transformPoint (point_in, point_out, transformation); + } + + inline void + transformPoint (const Eigen::Vector3d &point_in, + Eigen::Vector3d &point_out, + const Eigen::Affine3d &transformation) + { + transformPoint (point_in, point_out, transformation); + } + +/** \brief Transform a vector using an affine matrix + * \param[in] vector_in the vector to be transformed + * \param[out] vector_out the transformed vector + * \param[in] transformation the transformation matrix + * + * \note Can be used with \c vector_in = \c vector_out + */ + template inline void + transformVector (const Eigen::Matrix &vector_in, + Eigen::Matrix &vector_out, + const Eigen::Transform &transformation) + { + vector_out = transformation.linear () * vector_in; + } + + inline void + transformVector (const Eigen::Vector3f &vector_in, + Eigen::Vector3f &vector_out, + const Eigen::Affine3f &transformation) + { + transformVector (vector_in, vector_out, transformation); + } + + inline void + transformVector (const Eigen::Vector3d &vector_in, + Eigen::Vector3d &vector_out, + const Eigen::Affine3d &transformation) + { + transformVector (vector_in, vector_out, transformation); + } + +/** \brief Transform a line using an affine matrix + * \param[in] line_in the line to be transformed + * \param[out] line_out the transformed line + * \param[in] transformation the transformation matrix + * + * Lines must be filled in this form:\n + * line[0-2] = Origin coordinates of the vector\n + * line[3-5] = Direction vector + * + * \note Can be used with \c line_in = \c line_out + */ + template bool + transformLine (const Eigen::Matrix &line_in, + Eigen::Matrix &line_out, + const Eigen::Transform &transformation); + + inline bool + transformLine (const Eigen::VectorXf &line_in, + Eigen::VectorXf &line_out, + const Eigen::Affine3f &transformation) + { + return (transformLine (line_in, line_out, transformation)); + } + + inline bool + transformLine (const Eigen::VectorXd &line_in, + Eigen::VectorXd &line_out, + const Eigen::Affine3d &transformation) + { + return (transformLine (line_in, line_out, transformation)); + } + +/** \brief Transform plane vectors using an affine matrix + * \param[in] plane_in the plane coefficients to be transformed + * \param[out] plane_out the transformed plane coefficients to fill + * \param[in] transformation the transformation matrix + * + * The plane vectors are filled in the form ax+by+cz+d=0 + * Can be used with non Hessian form planes coefficients + * Can be used with \c plane_in = \c plane_out + */ + template void + transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation); + + inline void + transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + + inline void + transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + +/** \brief Transform plane vectors using an affine matrix + * \param[in] plane_in the plane coefficients to be transformed + * \param[out] plane_out the transformed plane coefficients to fill + * \param[in] transformation the transformation matrix + * + * The plane vectors are filled in the form ax+by+cz+d=0 + * Can be used with non Hessian form planes coefficients + * Can be used with \c plane_in = \c plane_out + * \warning ModelCoefficients stores floats only ! + */ + template void + transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation); + + inline void + transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + + inline void + transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + +/** \brief Check coordinate system integrity + * \param[in] line_x the first axis + * \param[in] line_y the second axis + * \param[in] norm_limit the limit to ignore norm rounding errors + * \param[in] dot_limit the limit to ignore dot product rounding errors + * \return True if the coordinate system is consistent, false otherwise. + * + * Lines must be filled in this form:\n + * line[0-2] = Origin coordinates of the vector\n + * line[3-5] = Direction vector + * + * Can be used like this :\n + * line_x = X axis and line_y = Y axis\n + * line_x = Z axis and line_y = X axis\n + * line_x = Y axis and line_y = Z axis\n + * Because X^Y = Z, Z^X = Y and Y^Z = X. + * Do NOT invert line order ! + * + * Determine whether a coordinate system is consistent or not by checking :\n + * Line origins: They must be the same for the 2 lines\n + * Norm: The 2 lines must be normalized\n + * Dot products: Must be 0 or perpendicular vectors + */ + template bool + checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const Scalar norm_limit = 1e-3, + const Scalar dot_limit = 1e-3); + + inline bool + checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const double norm_limit = 1e-3, + const double dot_limit = 1e-3) + { + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + + inline bool + checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const float norm_limit = 1e-3, + const float dot_limit = 1e-3) + { + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + +/** \brief Check coordinate system integrity + * \param[in] origin the origin of the coordinate system + * \param[in] x_direction the first axis + * \param[in] y_direction the second axis + * \param[in] norm_limit the limit to ignore norm rounding errors + * \param[in] dot_limit the limit to ignore dot product rounding errors + * \return True if the coordinate system is consistent, false otherwise. + * + * Read the other variant for more information + */ + template inline bool + checkCoordinateSystem (const Eigen::Matrix &origin, + const Eigen::Matrix &x_direction, + const Eigen::Matrix &y_direction, + const Scalar norm_limit = 1e-3, + const Scalar dot_limit = 1e-3) + { + Eigen::Matrix line_x; + Eigen::Matrix line_y; + line_x << origin, x_direction; + line_y << origin, y_direction; + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + + inline bool + checkCoordinateSystem (const Eigen::Matrix &origin, + const Eigen::Matrix &x_direction, + const Eigen::Matrix &y_direction, + const double norm_limit = 1e-3, + const double dot_limit = 1e-3) + { + Eigen::Matrix line_x; + Eigen::Matrix line_y; + line_x.resize (6); + line_y.resize (6); + line_x << origin, x_direction; + line_y << origin, y_direction; + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + + inline bool + checkCoordinateSystem (const Eigen::Matrix &origin, + const Eigen::Matrix &x_direction, + const Eigen::Matrix &y_direction, + const float norm_limit = 1e-3, + const float dot_limit = 1e-3) + { + Eigen::Matrix line_x; + Eigen::Matrix line_y; + line_x.resize (6); + line_y.resize (6); + line_x << origin, x_direction; + line_y << origin, y_direction; + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + +/** \brief Compute the transformation between two coordinate systems + * \param[in] from_line_x X axis from the origin coordinate system + * \param[in] from_line_y Y axis from the origin coordinate system + * \param[in] to_line_x X axis from the destination coordinate system + * \param[in] to_line_y Y axis from the destination coordinate system + * \param[out] transformation the transformation matrix to fill + * \return true if transformation was filled, false otherwise. + * + * Line must be filled in this form:\n + * line[0-2] = Coordinate system origin coordinates \n + * line[3-5] = Direction vector (norm doesn't matter) + */ + template bool + transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation); + + inline bool + transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation) + { + return (transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation)); + } + + inline bool + transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation) + { + return (transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation)); + } + } #include diff --git a/common/include/pcl/common/impl/accumulators.hpp b/common/include/pcl/common/impl/accumulators.hpp new file mode 100644 index 00000000..7c0b8873 --- /dev/null +++ b/common/include/pcl/common/impl/accumulators.hpp @@ -0,0 +1,296 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP +#define PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP + +#include + +#include +#include +#include +#include + +#include + +namespace pcl +{ + + namespace detail + { + + /* Below are several helper accumulator structures that are used by the + * `CentroidPoint` class. Each of them is capable of accumulating + * information from a particular field(s) of a point. The points are + * inserted via `add()` and extracted via `get()` functions. Note that the + * accumulators are not templated on point type, so in principle it is + * possible to insert and extract points of different types. It is the + * responsibility of the user to make sure that points have corresponding + * fields. */ + + struct AccumulatorXYZ + { + + // Requires that point type has x, y, and z fields + typedef pcl::traits::has_xyz IsCompatible; + + // Storage + Eigen::Vector3f xyz; + + AccumulatorXYZ () : xyz (Eigen::Vector3f::Zero ()) { } + + template void + add (const PointT& t) { xyz += t.getVector3fMap (); } + + template void + get (PointT& t, size_t n) const { t.getVector3fMap () = xyz / n; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + }; + + struct AccumulatorNormal + { + + // Requires that point type has normal_x, normal_y, and normal_z fields + typedef pcl::traits::has_normal IsCompatible; + + // Storage + Eigen::Vector4f normal; + + AccumulatorNormal () : normal (Eigen::Vector4f::Zero ()) { } + + // Requires that the normal of the given point is normalized, otherwise it + // does not make sense to sum it up with the accumulated value. + template void + add (const PointT& t) { normal += t.getNormalVector4fMap (); } + + template void + get (PointT& t, size_t) const + { + t.getNormalVector4fMap () = normal; + t.getNormalVector4fMap ().normalize (); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + }; + + struct AccumulatorCurvature + { + + // Requires that point type has curvature field + typedef pcl::traits::has_curvature IsCompatible; + + // Storage + float curvature; + + AccumulatorCurvature () : curvature (0) { } + + template void + add (const PointT& t) { curvature += t.curvature; } + + template void + get (PointT& t, size_t n) const { t.curvature = curvature / n; } + + }; + + struct AccumulatorRGBA + { + + // Requires that point type has rgb or rgba field + typedef pcl::traits::has_color IsCompatible; + + // Storage + float r, g, b, a; + + AccumulatorRGBA () : r (0), g (0), b (0), a (0) { } + + template void + add (const PointT& t) + { + r += static_cast (t.r); + g += static_cast (t.g); + b += static_cast (t.b); + a += static_cast (t.a); + } + + template void + get (PointT& t, size_t n) const + { + t.rgba = static_cast (a / n) << 24 | + static_cast (r / n) << 16 | + static_cast (g / n) << 8 | + static_cast (b / n); + } + + }; + + struct AccumulatorIntensity + { + + // Requires that point type has intensity field + typedef pcl::traits::has_intensity IsCompatible; + + // Storage + float intensity; + + AccumulatorIntensity () : intensity (0) { } + + template void + add (const PointT& t) { intensity += t.intensity; } + + template void + get (PointT& t, size_t n) const { t.intensity = intensity / n; } + + }; + + struct AccumulatorLabel + { + + // Requires that point type has label field + typedef pcl::traits::has_label IsCompatible; + + // Storage + // A better performance may be achieved with a heap structure + std::map labels; + + AccumulatorLabel () { } + + template void + add (const PointT& t) + { + std::map::iterator itr = labels.find (t.label); + if (itr == labels.end ()) + labels.insert (std::make_pair (t.label, 1)); + else + ++itr->second; + } + + template void + get (PointT& t, size_t) const + { + size_t max = 0; + std::map::const_iterator itr; + for (itr = labels.begin (); itr != labels.end (); ++itr) + if (itr->second > max) + { + max = itr->second; + t.label = itr->first; + } + } + + }; + + /* This is a meta-function that may be used to create a Fusion vector of + * those accumulator types that are compatible with given point type(s). */ + + template + struct Accumulators + { + + // Check if a given accumulator type is compatible with a given point type + template + struct IsCompatible : boost::mpl::apply { }; + + // A Fusion vector with accumulator types that are compatible with given + // point types + typedef + typename boost::fusion::result_of::as_vector< + typename boost::mpl::filter_view< + boost::mpl::vector< + AccumulatorXYZ + , AccumulatorNormal + , AccumulatorCurvature + , AccumulatorRGBA + , AccumulatorIntensity + , AccumulatorLabel + > + , boost::mpl::and_< + IsCompatible + , IsCompatible + > + > + >::type + type; + }; + + /* Fusion function object to invoke point addition on every accumulator in + * a fusion sequence. */ + + template + struct AddPoint + { + + const PointT& p; + + AddPoint (const PointT& point) : p (point) { } + + template void + operator () (AccumulatorT& accumulator) const + { + accumulator.add (p); + } + + }; + + /* Fusion function object to invoke get point on every accumulator in a + * fusion sequence. */ + + template + struct GetPoint + { + + PointT& p; + size_t n; + + GetPoint (PointT& point, size_t num) : p (point), n (num) { } + + template void + operator () (AccumulatorT& accumulator) const + { + accumulator.get (p, n); + } + + }; + + } + +} + +#endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */ + diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp index 22a04e48..2c13eb10 100644 --- a/common/include/pcl/common/impl/centroid.hpp +++ b/common/include/pcl/common/impl/centroid.hpp @@ -70,8 +70,8 @@ pcl::compute3DCentroid (ConstCloudIterator &cloud_iterator, ++cp; ++cloud_iterator; } - centroid[3] = 0; centroid /= static_cast (cp); + centroid[3] = 1; return (cp); } @@ -95,8 +95,8 @@ pcl::compute3DCentroid (const pcl::PointCloud &cloud, centroid[1] += cloud[i].y; centroid[2] += cloud[i].z; } - centroid[3] = 0; centroid /= static_cast (cloud.size ()); + centroid[3] = 1; return (static_cast (cloud.size ())); } @@ -115,8 +115,8 @@ pcl::compute3DCentroid (const pcl::PointCloud &cloud, centroid[2] += cloud[i].z; ++cp; } - centroid[3] = 0; centroid /= static_cast (cp); + centroid[3] = 1; return (cp); } @@ -142,8 +142,8 @@ pcl::compute3DCentroid (const pcl::PointCloud &cloud, centroid[1] += cloud[indices[i]].y; centroid[2] += cloud[indices[i]].z; } - centroid[3] = 0; centroid /= static_cast (indices.size ()); + centroid[3] = 1; return (static_cast (indices.size ())); } // NaN or Inf values could exist => check for them @@ -161,8 +161,8 @@ pcl::compute3DCentroid (const pcl::PointCloud &cloud, centroid[2] += cloud[indices[i]].z; ++cp; } - centroid[3] = 0; centroid /= static_cast (cp); + centroid[3] = 1; return (cp); } } @@ -536,7 +536,7 @@ pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, { //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; - centroid[3] = 0; + centroid[3] = 1; covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; @@ -603,7 +603,7 @@ pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, //centroid.head<3> () = vec;//= accu.tail<3> (); //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; - centroid[3] = 0; + centroid[3] = 1; covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; @@ -859,5 +859,44 @@ pcl::computeNDCentroid (const pcl::PointCloud &cloud, return (pcl::computeNDCentroid (cloud, indices.indices, centroid)); } +///////////////////////////////////////////////////////////////////////////////////////////// +template size_t +pcl::computeCentroid (const pcl::PointCloud& cloud, + PointOutT& centroid) +{ + pcl::CentroidPoint cp; + + if (cloud.is_dense) + for (size_t i = 0; i < cloud.size (); ++i) + cp.add (cloud[i]); + else + for (size_t i = 0; i < cloud.size (); ++i) + if (pcl::isFinite (cloud[i])) + cp.add (cloud[i]); + + cp.get (centroid); + return (cp.getSize ()); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template size_t +pcl::computeCentroid (const pcl::PointCloud& cloud, + const std::vector& indices, + PointOutT& centroid) +{ + pcl::CentroidPoint cp; + + if (cloud.is_dense) + for (size_t i = 0; i < indices.size (); ++i) + cp.add (cloud[indices[i]]); + else + for (size_t i = 0; i < indices.size (); ++i) + if (pcl::isFinite (cloud[indices[i]])) + cp.add (cloud[indices[i]]); + + cp.get (centroid); + return (cp.getSize ()); +} + #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_ diff --git a/common/include/pcl/common/impl/copy_point.hpp b/common/include/pcl/common/impl/copy_point.hpp new file mode 100644 index 00000000..b4070a0e --- /dev/null +++ b/common/include/pcl/common/impl/copy_point.hpp @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_IMPL_COPY_POINT_HPP_ +#define PCL_COMMON_IMPL_COPY_POINT_HPP_ + +#include +#include +#include +#include + +namespace pcl +{ + + namespace detail + { + + /* CopyPointHelper and its specializations copy the contents of a source + * point to a target point. There are three cases: + * + * - Points have the same type. + * In this case a single `memcpy` is used. + * + * - Points have different types and one of the following is true: + * * both have RGB fields; + * * both have RGBA fields; + * * one or both have no RGB/RGBA fields. + * In this case we find the list of common fields and copy their + * contents one by one with `NdConcatenateFunctor`. + * + * - Points have different types and one of these types has RGB field, and + * the other has RGBA field. + * In this case we also find the list of common fields and copy their + * contents. In order to account for the fact that RGB and RGBA do not + * match we have an additional `memcpy` to copy the contents of one into + * another. + * + * An appropriate version of CopyPointHelper is instantiated during + * compilation time automatically, so there is absolutely no run-time + * overhead. */ + + template + struct CopyPointHelper { }; + + template + struct CopyPointHelper >::type> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + memcpy (&point_out, &point_in, sizeof (PointInT)); + } + }; + + template + struct CopyPointHelper >, + boost::mpl::or_ >, + boost::mpl::not_ >, + boost::mpl::and_, + pcl::traits::has_field >, + boost::mpl::and_, + pcl::traits::has_field > > > >::type> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + typedef typename pcl::traits::fieldList::type FieldListInT; + typedef typename pcl::traits::fieldList::type FieldListOutT; + typedef typename pcl::intersect::type FieldList; + pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, point_out)); + } + }; + + template + struct CopyPointHelper >, + boost::mpl::or_, + pcl::traits::has_field >, + boost::mpl::and_, + pcl::traits::has_field > > > >::type> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + typedef typename pcl::traits::fieldList::type FieldListInT; + typedef typename pcl::traits::fieldList::type FieldListOutT; + typedef typename pcl::intersect::type FieldList; + const uint32_t offset_in = boost::mpl::if_, + pcl::traits::offset, + pcl::traits::offset >::type::value; + const uint32_t offset_out = boost::mpl::if_, + pcl::traits::offset, + pcl::traits::offset >::type::value; + pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, point_out)); + memcpy (reinterpret_cast (&point_out) + offset_out, + reinterpret_cast (&point_in) + offset_in, + 4); + } + }; + + } + +} + +template void +pcl::copyPoint (const PointInT& point_in, PointOutT& point_out) +{ + detail::CopyPointHelper copy; + copy (point_in, point_out); +} + +#endif //PCL_COMMON_IMPL_COPY_POINT_HPP_ + diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index c9175b58..552224e6 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -39,7 +39,543 @@ #ifndef PCL_COMMON_EIGEN_IMPL_HPP_ #define PCL_COMMON_EIGEN_IMPL_HPP_ -#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots) +{ + roots (0) = Scalar (0); + Scalar d = Scalar (b * b - 4.0 * c); + if (d < 0.0) // no real roots ! THIS SHOULD NOT HAPPEN! + d = 0.0; + + Scalar sd = ::std::sqrt (d); + + roots (2) = 0.5f * (b + sd); + roots (1) = 0.5f * (b - sd); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeRoots (const Matrix& m, Roots& roots) +{ + typedef typename Matrix::Scalar Scalar; + + // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The + // eigenvalues are the roots to this equation, all guaranteed to be + // real-valued, because the matrix is symmetric. + Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2) + + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2) + - m (0, 0) * m (1, 2) * m (1, 2) + - m (1, 1) * m (0, 2) * m (0, 2) + - m (2, 2) * m (0, 1) * m (0, 1); + Scalar c1 = m (0, 0) * m (1, 1) - + m (0, 1) * m (0, 1) + + m (0, 0) * m (2, 2) - + m (0, 2) * m (0, 2) + + m (1, 1) * m (2, 2) - + m (1, 2) * m (1, 2); + Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2); + + if (fabs (c0) < Eigen::NumTraits < Scalar > ::epsilon ()) // one root is 0 -> quadratic equation + computeRoots2 (c2, c1, roots); + else + { + const Scalar s_inv3 = Scalar (1.0 / 3.0); + const Scalar s_sqrt3 = std::sqrt (Scalar (3.0)); + // Construct the parameters used in classifying the roots of the equation + // and in solving the equation for the roots in closed form. + Scalar c2_over_3 = c2 * s_inv3; + Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3; + if (a_over_3 > Scalar (0)) + a_over_3 = Scalar (0); + + Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1)); + + Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3; + if (q > Scalar (0)) + q = Scalar (0); + + // Compute the eigenvalues by solving for the roots of the polynomial. + Scalar rho = std::sqrt (-a_over_3); + Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3; + Scalar cos_theta = std::cos (theta); + Scalar sin_theta = std::sin (theta); + roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta; + roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta); + roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta); + + // Sort in increasing order. + if (roots (0) >= roots (1)) + std::swap (roots (0), roots (1)); + if (roots (1) >= roots (2)) + { + std::swap (roots (1), roots (2)); + if (roots (0) >= roots (1)) + std::swap (roots (0), roots (1)); + } + + if (roots (0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0 + computeRoots2 (c2, c1, roots); + } +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) +{ + // if diagonal matrix, the eigenvalues are the diagonal elements + // and the eigenvectors are not unique, thus set to Identity + if (fabs (mat.coeff (1)) <= std::numeric_limits::min ()) + { + if (mat.coeff (0) < mat.coeff (2)) + { + eigenvalue = mat.coeff (0); + eigenvector[0] = 1.0; + eigenvector[1] = 0.0; + } + else + { + eigenvalue = mat.coeff (2); + eigenvector[0] = 0.0; + eigenvector[1] = 1.0; + } + return; + } + + // 0.5 to optimize further calculations + typename Matrix::Scalar trace = static_cast (0.5) * (mat.coeff (0) + mat.coeff (3)); + typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); + + typename Matrix::Scalar temp = trace * trace - determinant; + + if (temp < 0) + temp = 0; + + eigenvalue = trace - ::std::sqrt (temp); + + eigenvector[0] = -mat.coeff (1); + eigenvector[1] = mat.coeff (0) - eigenvalue; + eigenvector.normalize (); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues) +{ + // if diagonal matrix, the eigenvalues are the diagonal elements + // and the eigenvectors are not unique, thus set to Identity + if (fabs (mat.coeff (1)) <= std::numeric_limits::min ()) + { + if (mat.coeff (0) < mat.coeff (3)) + { + eigenvalues.coeffRef (0) = mat.coeff (0); + eigenvalues.coeffRef (1) = mat.coeff (3); + eigenvectors.coeffRef (0) = 1.0; + eigenvectors.coeffRef (1) = 0.0; + eigenvectors.coeffRef (2) = 0.0; + eigenvectors.coeffRef (3) = 1.0; + } + else + { + eigenvalues.coeffRef (0) = mat.coeff (3); + eigenvalues.coeffRef (1) = mat.coeff (0); + eigenvectors.coeffRef (0) = 0.0; + eigenvectors.coeffRef (1) = 1.0; + eigenvectors.coeffRef (2) = 1.0; + eigenvectors.coeffRef (3) = 0.0; + } + return; + } + + // 0.5 to optimize further calculations + typename Matrix::Scalar trace = static_cast (0.5) * (mat.coeff (0) + mat.coeff (3)); + typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); + + typename Matrix::Scalar temp = trace * trace - determinant; + + if (temp < 0) + temp = 0; + else + temp = ::std::sqrt (temp); + + eigenvalues.coeffRef (0) = trace - temp; + eigenvalues.coeffRef (1) = trace + temp; + + // either this is in a row or column depending on RowMajor or ColumnMajor + eigenvectors.coeffRef (0) = -mat.coeff (1); + eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0); + typename Matrix::Scalar norm = static_cast (1.0) + / static_cast (::std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2))); + eigenvectors.coeffRef (0) *= norm; + eigenvectors.coeffRef (2) *= norm; + eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2); + eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector) +{ + typedef typename Matrix::Scalar Scalar; + // Scale the matrix so its entries are in [-1,1]. The scaling is applied + // only when at least one matrix entry has magnitude larger than 1. + + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + + scaledMat.diagonal ().array () -= eigenvalue / scale; + + Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); + Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); + Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); + + Scalar len1 = vec1.squaredNorm (); + Scalar len2 = vec2.squaredNorm (); + Scalar len3 = vec3.squaredNorm (); + + if (len1 >= len2 && len1 >= len3) + eigenvector = vec1 / std::sqrt (len1); + else if (len2 >= len1 && len2 >= len3) + eigenvector = vec2 / std::sqrt (len2); + else + eigenvector = vec3 / std::sqrt (len3); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) +{ + typedef typename Matrix::Scalar Scalar; + // Scale the matrix so its entries are in [-1,1]. The scaling is applied + // only when at least one matrix entry has magnitude larger than 1. + + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + + Vector eigenvalues; + computeRoots (scaledMat, eigenvalues); + + eigenvalue = eigenvalues (0) * scale; + + scaledMat.diagonal ().array () -= eigenvalues (0); + + Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); + Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); + Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); + + Scalar len1 = vec1.squaredNorm (); + Scalar len2 = vec2.squaredNorm (); + Scalar len3 = vec3.squaredNorm (); + + if (len1 >= len2 && len1 >= len3) + eigenvector = vec1 / std::sqrt (len1); + else if (len2 >= len1 && len2 >= len3) + eigenvector = vec2 / std::sqrt (len2); + else + eigenvector = vec3 / std::sqrt (len3); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen33 (const Matrix& mat, Vector& evals) +{ + typedef typename Matrix::Scalar Scalar; + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + computeRoots (scaledMat, evals); + evals *= scale; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) +{ + typedef typename Matrix::Scalar Scalar; + // Scale the matrix so its entries are in [-1,1]. The scaling is applied + // only when at least one matrix entry has magnitude larger than 1. + + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + + // Compute the eigenvalues + computeRoots (scaledMat, evals); + + if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ()) + { + // all three equal + evecs.setIdentity (); + } + else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ()) + { + // first and second equal + Matrix tmp; + tmp = scaledMat; + tmp.diagonal ().array () -= evals (2); + + Vector vec1 = tmp.row (0).cross (tmp.row (1)); + Vector vec2 = tmp.row (0).cross (tmp.row (2)); + Vector vec3 = tmp.row (1).cross (tmp.row (2)); + + Scalar len1 = vec1.squaredNorm (); + Scalar len2 = vec2.squaredNorm (); + Scalar len3 = vec3.squaredNorm (); + + if (len1 >= len2 && len1 >= len3) + evecs.col (2) = vec1 / std::sqrt (len1); + else if (len2 >= len1 && len2 >= len3) + evecs.col (2) = vec2 / std::sqrt (len2); + else + evecs.col (2) = vec3 / std::sqrt (len3); + + evecs.col (1) = evecs.col (2).unitOrthogonal (); + evecs.col (0) = evecs.col (1).cross (evecs.col (2)); + } + else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ()) + { + // second and third equal + Matrix tmp; + tmp = scaledMat; + tmp.diagonal ().array () -= evals (0); + + Vector vec1 = tmp.row (0).cross (tmp.row (1)); + Vector vec2 = tmp.row (0).cross (tmp.row (2)); + Vector vec3 = tmp.row (1).cross (tmp.row (2)); + + Scalar len1 = vec1.squaredNorm (); + Scalar len2 = vec2.squaredNorm (); + Scalar len3 = vec3.squaredNorm (); + + if (len1 >= len2 && len1 >= len3) + evecs.col (0) = vec1 / std::sqrt (len1); + else if (len2 >= len1 && len2 >= len3) + evecs.col (0) = vec2 / std::sqrt (len2); + else + evecs.col (0) = vec3 / std::sqrt (len3); + + evecs.col (1) = evecs.col (0).unitOrthogonal (); + evecs.col (2) = evecs.col (0).cross (evecs.col (1)); + } + else + { + Matrix tmp; + tmp = scaledMat; + tmp.diagonal ().array () -= evals (2); + + Vector vec1 = tmp.row (0).cross (tmp.row (1)); + Vector vec2 = tmp.row (0).cross (tmp.row (2)); + Vector vec3 = tmp.row (1).cross (tmp.row (2)); + + Scalar len1 = vec1.squaredNorm (); + Scalar len2 = vec2.squaredNorm (); + Scalar len3 = vec3.squaredNorm (); +#ifdef _WIN32 + Scalar *mmax = new Scalar[3]; +#else + Scalar mmax[3]; +#endif + unsigned int min_el = 2; + unsigned int max_el = 2; + if (len1 >= len2 && len1 >= len3) + { + mmax[2] = len1; + evecs.col (2) = vec1 / std::sqrt (len1); + } + else if (len2 >= len1 && len2 >= len3) + { + mmax[2] = len2; + evecs.col (2) = vec2 / std::sqrt (len2); + } + else + { + mmax[2] = len3; + evecs.col (2) = vec3 / std::sqrt (len3); + } + + tmp = scaledMat; + tmp.diagonal ().array () -= evals (1); + + vec1 = tmp.row (0).cross (tmp.row (1)); + vec2 = tmp.row (0).cross (tmp.row (2)); + vec3 = tmp.row (1).cross (tmp.row (2)); + + len1 = vec1.squaredNorm (); + len2 = vec2.squaredNorm (); + len3 = vec3.squaredNorm (); + if (len1 >= len2 && len1 >= len3) + { + mmax[1] = len1; + evecs.col (1) = vec1 / std::sqrt (len1); + min_el = len1 <= mmax[min_el] ? 1 : min_el; + max_el = len1 > mmax[max_el] ? 1 : max_el; + } + else if (len2 >= len1 && len2 >= len3) + { + mmax[1] = len2; + evecs.col (1) = vec2 / std::sqrt (len2); + min_el = len2 <= mmax[min_el] ? 1 : min_el; + max_el = len2 > mmax[max_el] ? 1 : max_el; + } + else + { + mmax[1] = len3; + evecs.col (1) = vec3 / std::sqrt (len3); + min_el = len3 <= mmax[min_el] ? 1 : min_el; + max_el = len3 > mmax[max_el] ? 1 : max_el; + } + + tmp = scaledMat; + tmp.diagonal ().array () -= evals (0); + + vec1 = tmp.row (0).cross (tmp.row (1)); + vec2 = tmp.row (0).cross (tmp.row (2)); + vec3 = tmp.row (1).cross (tmp.row (2)); + + len1 = vec1.squaredNorm (); + len2 = vec2.squaredNorm (); + len3 = vec3.squaredNorm (); + if (len1 >= len2 && len1 >= len3) + { + mmax[0] = len1; + evecs.col (0) = vec1 / std::sqrt (len1); + min_el = len3 <= mmax[min_el] ? 0 : min_el; + max_el = len3 > mmax[max_el] ? 0 : max_el; + } + else if (len2 >= len1 && len2 >= len3) + { + mmax[0] = len2; + evecs.col (0) = vec2 / std::sqrt (len2); + min_el = len3 <= mmax[min_el] ? 0 : min_el; + max_el = len3 > mmax[max_el] ? 0 : max_el; + } + else + { + mmax[0] = len3; + evecs.col (0) = vec3 / std::sqrt (len3); + min_el = len3 <= mmax[min_el] ? 0 : min_el; + max_el = len3 > mmax[max_el] ? 0 : max_el; + } + + unsigned mid_el = 3 - min_el - max_el; + evecs.col (min_el) = evecs.col ( (min_el + 1) % 3).cross (evecs.col ( (min_el + 2) % 3)).normalized (); + evecs.col (mid_el) = evecs.col ( (mid_el + 1) % 3).cross (evecs.col ( (mid_el + 2) % 3)).normalized (); +#ifdef _WIN32 + delete [] mmax; +#endif + } + // Rescale back to the original size. + evals *= scale; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::invert2x2 (const Matrix& matrix, Matrix& inverse) +{ + typedef typename Matrix::Scalar Scalar; + Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2); + + if (det != 0) + { + //Scalar inv_det = Scalar (1.0) / det; + inverse.coeffRef (0) = matrix.coeff (3); + inverse.coeffRef (1) = -matrix.coeff (1); + inverse.coeffRef (2) = -matrix.coeff (2); + inverse.coeffRef (3) = matrix.coeff (0); + inverse /= det; + } + return det; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse) +{ + typedef typename Matrix::Scalar Scalar; + // elements + // a b c + // b d e + // c e f + //| a b c |-1 | fd-ee ce-bf be-cd | + //| b d e | = 1/det * | ce-bf af-cc bc-ae | + //| c e f | | be-cd bc-ae ad-bb | + + //det = a(fd-ee) + b(ec-fb) + c(eb-dc) + + Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5); + Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8); + Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4); + + Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd; + + if (det != 0) + { + //Scalar inv_det = Scalar (1.0) / det; + inverse.coeffRef (0) = fd_ee; + inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf; + inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd; + inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2)); + inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5)); + inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1)); + inverse /= det; + } + return det; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::invert3x3Matrix (const Matrix& matrix, Matrix& inverse) +{ + typedef typename Matrix::Scalar Scalar; + + //| a b c |-1 | ie-hf hc-ib fb-ec | + //| d e f | = 1/det * | gf-id ia-gc dc-fa | + //| g h i | | hd-ge gb-ha ea-db | + //det = a(ie-hf) + d(hc-ib) + g(fb-ec) + + Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5); + Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1); + Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2); + Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec); + + if (det != 0) + { + inverse.coeffRef (0) = ie_hf; + inverse.coeffRef (1) = hc_ib; + inverse.coeffRef (2) = fb_ec; + inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3); + inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2); + inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0); + inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4); + inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0); + inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1); + + inverse /= det; + } + return det; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::determinant3x3Matrix (const Matrix& matrix) +{ + // result is independent of Row/Col Major storage! + return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) + + matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) + + matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ; +} ////////////////////////////////////////////////////////////////////////////////////////// void @@ -124,26 +660,26 @@ pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_dire } ////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw) +template void +pcl::getEulerAngles (const Eigen::Transform &t, Scalar &roll, Scalar &pitch, Scalar &yaw) { - roll = atan2f(t(2,1), t(2,2)); - pitch = asinf(-t(2,0)); - yaw = atan2f(t(1,0), t(0,0)); + roll = atan2 (t (2, 1), t (2, 2)); + pitch = asin (-t (2, 0)); + yaw = atan2 (t (1, 0), t (0, 0)); } ////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::getTranslationAndEulerAngles (const Eigen::Affine3f& t, - float& x, float& y, float& z, - float& roll, float& pitch, float& yaw) +template void +pcl::getTranslationAndEulerAngles (const Eigen::Transform &t, + Scalar &x, Scalar &y, Scalar &z, + Scalar &roll, Scalar &pitch, Scalar &yaw) { - x = t(0,3); - y = t(1,3); - z = t(2,3); - roll = atan2f(t(2,1), t(2,2)); - pitch = asinf(-t(2,0)); - yaw = atan2f(t(1,0), t(0,0)); + x = t (0, 3); + y = t (1, 3); + z = t (2, 3); + roll = atan2 (t (2, 1), t (2, 2)); + pitch = asin (-t (2, 0)); + yaw = atan2 (t (1, 0), t (0, 0)); } ////////////////////////////////////////////////////////////////////////////////////////// @@ -161,15 +697,6 @@ pcl::getTransformation (Scalar x, Scalar y, Scalar z, t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1; } -////////////////////////////////////////////////////////////////////////////////////////// -Eigen::Affine3f -pcl::getTransformation (float x, float y, float z, float roll, float pitch, float yaw) -{ - Eigen::Affine3f t; - getTransformation (x, y, z, roll, pitch, yaw, t); - return (t); -} - ////////////////////////////////////////////////////////////////////////////////////////// template void pcl::saveBinary (const Eigen::MatrixBase& matrix, std::ostream& file) @@ -299,5 +826,176 @@ pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase bool +pcl::transformLine (const Eigen::Matrix &line_in, + Eigen::Matrix &line_out, + const Eigen::Transform &transformation) +{ + if (line_in.innerSize () != 6 || line_out.innerSize () != 6) + { + PCL_DEBUG ("transformLine: lines size != 6\n"); + return (false); + } + + Eigen::Matrix point, vector; + point << line_in.template head<3> (); + vector << line_out.template tail<3> (); + + pcl::transformPoint (point, point, transformation); + pcl::transformVector (vector, vector, transformation); + line_out << point, vector; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation) +{ + Eigen::Hyperplane < Scalar, 3 > plane; + plane.coeffs () << plane_in; + plane.transform (transformation); + plane_out << plane.coeffs (); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation) +{ + Eigen::Matrix < Scalar, 4, 1 > v_plane_in (std::vector < Scalar > (plane_in->values.begin (), plane_in->values.end ()).data ()); + pcl::transformPlane (v_plane_in, v_plane_in, transformation); + plane_out->values.resize (4); + for (int i = 0; i < 4; i++) + plane_in->values[i] = v_plane_in[i]; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const Scalar norm_limit, + const Scalar dot_limit) +{ + if (line_x.innerSize () != 6 || line_y.innerSize () != 6) + { + PCL_DEBUG ("checkCoordinateSystem: lines size != 6\n"); + return (false); + } + + if (line_x.template head<3> () != line_y.template head<3> ()) + { + PCL_DEBUG ("checkCoorZdinateSystem: vector origins are different !\n"); + return (false); + } + + // Make a copy of vector directions + // X^Y = Z | Y^Z = X | Z^X = Y + Eigen::Matrix v_line_x (line_x.template tail<3> ()), + v_line_y (line_y.template tail<3> ()), + v_line_z (v_line_x.cross (v_line_y)); + + // Check vectors norms + if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit) + { + PCL_DEBUG ("checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ()); + return (false); + } + + if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit) + { + PCL_DEBUG ("checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ()); + return (false); + } + + if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit) + { + PCL_DEBUG ("checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ()); + return (false); + } + + // Check vectors perendicularity + if (std::abs (v_line_x.dot (v_line_y)) > dot_limit) + { + PCL_DEBUG ("checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit); + return (false); + } + + if (std::abs (v_line_x.dot (v_line_z)) > dot_limit) + { + PCL_DEBUG ("checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit); + return (false); + } + + if (std::abs (v_line_y.dot (v_line_z)) > dot_limit) + { + PCL_DEBUG ("checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation) +{ + if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6) + { + PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n"); + return (false); + } + + // Check if coordinate systems are valid + if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y)) + { + PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n"); + return (false); + } + + // Convert lines into Vector3 : + Eigen::Matrix fr0 (from_line_x.template head<3>()), + fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()), + fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()), + + to0 (to_line_x.template head<3>()), + to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()), + to2 (to_line_y.template head<3>() + to_line_y.template tail<3>()); + + // Code is inspired from http://stackoverflow.com/a/15277421/1816078 + // Define matrices and points : + Eigen::Transform T2, T3 = Eigen::Transform::Identity (); + Eigen::Matrix x1, y1, z1, x2, y2, z2; + + // Axes of the coordinate system "fr" + x1 = (fr1 - fr0).normalized (); // the versor (unitary vector) of the (fr1-fr0) axis vector + y1 = (fr2 - fr0).normalized (); + + // Axes of the coordinate system "to" + x2 = (to1 - to0).normalized (); + y2 = (to2 - to0).normalized (); + + // Transform from CS1 to CS2 + // Note: if fr0 == (0,0,0) --> CS1 == CS2 --> T2 = Identity + T2.linear () << x1, y1, x1.cross (y1); + + // Transform from CS1 to CS3 + T3.linear () << x2, y2, x2.cross (y2); + + // Identity matrix = transform to CS2 to CS3 + // Note: if CS1 == CS2 --> transformation = T3 + transformation = Eigen::Transform::Identity (); + transformation.linear () = T3.linear () * T2.linear ().inverse (); + transformation.translation () = to0 - (transformation.linear () * fr0); + return (true); +} + #endif //PCL_COMMON_EIGEN_IMPL_HPP_ diff --git a/common/include/pcl/common/impl/intersections.hpp b/common/include/pcl/common/impl/intersections.hpp new file mode 100644 index 00000000..619a888e --- /dev/null +++ b/common/include/pcl/common/impl/intersections.hpp @@ -0,0 +1,172 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_INTERSECTIONS_IMPL_HPP_ +#define PCL_COMMON_INTERSECTIONS_IMPL_HPP_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// + +bool +pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, + const Eigen::VectorXf &line_b, + Eigen::Vector4f &point, double sqr_eps) +{ + Eigen::Vector4f p1, p2; + lineToLineSegment (line_a, line_b, p1, p2); + + // If the segment size is smaller than a pre-given epsilon... + double sqr_dist = (p1 - p2).squaredNorm (); + if (sqr_dist < sqr_eps) + { + point = p1; + return (true); + } + point.setZero (); + return (false); +} + +bool +pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a, + const pcl::ModelCoefficients &line_b, + Eigen::Vector4f &point, double sqr_eps) +{ + Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.values[0], line_a.values.size ()); + Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.values[0], line_b.values.size ()); + return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps)); +} + +template bool +pcl::planeWithPlaneIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + Eigen::Matrix &line, + double angular_tolerance) +{ + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector5; + typedef Eigen::Matrix Matrix5; + + // Normalize plane normals + Vector3 plane_a_norm (plane_a.template head<3> ()); + Vector3 plane_b_norm (plane_b.template head<3> ()); + plane_a_norm.normalize (); + plane_b_norm.normalize (); + + // Test if planes are parallel (test_cos == 1) + double test_cos = plane_a_norm.dot (plane_b_norm); + double upper_limit = 1 + angular_tolerance; + double lower_limit = 1 - angular_tolerance; + + if ((test_cos > lower_limit) && (test_cos < upper_limit)) + { + PCL_DEBUG ("Plane A and Plane B are parallel.\n"); + return (false); + } + + Vector4 line_direction = plane_a.cross3 (plane_b); + line_direction.normalized(); + + // Construct system of equations using lagrange multipliers with one objective function and two constraints + Matrix5 langrange_coefs; + langrange_coefs << 2,0,0, plane_a[0], plane_b[0], + 0,2,0, plane_a[1], plane_b[1], + 0,0,2, plane_a[2], plane_b[2], + plane_a[0], plane_a[1], plane_a[2], 0, 0, + plane_b[0], plane_b[1], plane_b[2], 0, 0; + + Vector5 b; + b << 0, 0, 0, -plane_a[3], -plane_b[3]; + + line.resize(6); + // Solve for the lagrange multipliers + line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> (); + line.template tail<3>() = line_direction.template head<3>(); + return (true); +} + +template bool +pcl::threePlanesIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + const Eigen::Matrix &plane_c, + Eigen::Matrix &intersection_point, + double determinant_tolerance) +{ + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Matrix3; + + // TODO: Using Eigen::HyperPlanes is better to solve this problem + // Check if some planes are parallel + Matrix3 normals_in_lines; + + for (int i = 0; i < 3; i++) + { + normals_in_lines (i, 0) = plane_a[i]; + normals_in_lines (i, 1) = plane_b[i]; + normals_in_lines (i, 2) = plane_c[i]; + } + + Scalar determinant = normals_in_lines.determinant (); + if (fabs (determinant) < determinant_tolerance) + { + // det ~= 0 + PCL_DEBUG ("At least two planes are parralel.\n"); + return (false); + } + + // Left part of the 3 equations + Matrix3 left_member; + + for (int i = 0; i < 3; i++) + { + left_member (0, i) = plane_a[i]; + left_member (1, i) = plane_b[i]; + left_member (2, i) = plane_c[i]; + } + + // Right side of the 3 equations + Vector3 right_member; + right_member << -plane_a[3], -plane_b[3], -plane_c[3]; + + // Solve the system + intersection_point = left_member.fullPivLu ().solve (right_member); + return (true); +} + +#endif //PCL_COMMON_INTERSECTIONS_IMPL_HPP diff --git a/common/include/pcl/common/impl/io.hpp b/common/include/pcl/common/impl/io.hpp index 3d40c718..9ddd7797 100644 --- a/common/include/pcl/common/impl/io.hpp +++ b/common/include/pcl/common/impl/io.hpp @@ -42,6 +42,7 @@ #define PCL_IO_IMPL_IO_HPP_ #include +#include #include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -107,14 +108,10 @@ pcl::getFieldsList (const pcl::PointCloud &) ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out) { - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - + // Allocate enough space and copy the basics cloud_out.header = cloud_in.header; cloud_out.width = cloud_in.width; cloud_out.height = cloud_in.height; @@ -123,57 +120,13 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_origin_ = cloud_in.sensor_origin_; cloud_out.points.resize (cloud_in.points.size ()); - // If the point types are the same, don't copy one by one if (isSamePointType ()) - { + // Copy the whole memory block memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < cloud_in.points.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[i], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different - for (size_t i = 0; i < cloud_in.points.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[i], cloud_out.points[i])); + else + // Iterate over each point + for (size_t i = 0; i < cloud_in.points.size (); ++i) + copyPoint (cloud_in.points[i], cloud_out.points[i]); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -232,7 +185,7 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out) { @@ -245,69 +198,14 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each point - for (size_t i = 0; i < indices.size (); ++i) - memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; int (i) < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < indices.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different + // Iterate over each point for (size_t i = 0; i < indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); + copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]); } ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const std::vector > &indices, pcl::PointCloud &cloud_out) { @@ -320,64 +218,9 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each point - for (size_t i = 0; i < indices.size (); ++i) - memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < indices.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different + // Iterate over each point for (size_t i = 0; i < indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); + copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -409,77 +252,11 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out) { - // Allocate enough space and copy the basics - cloud_out.points.resize (indices.indices.size ()); - cloud_out.header = cloud_in.header; - cloud_out.width = indices.indices.size (); - cloud_out.height = 1; - cloud_out.is_dense = cloud_in.is_dense; - cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; - cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each point - for (size_t i = 0; i < indices.indices.size (); ++i) - memcpy (&cloud_out.points[i], &cloud_in.points[indices.indices[i]], sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < indices.indices.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices.indices[i]], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[indices.indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different - for (size_t i = 0; i < indices.indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices.indices[i]], cloud_out.points[i])); + copyPointCloud (cloud_in, indices.indices, cloud_out); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -548,75 +325,6 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each cluster - int cp = 0; - for (size_t cc = 0; cc < indices.size (); ++cc) - { - // Iterate over each idx - for (size_t i = 0; i < indices[cc].indices.size (); ++i) - { - cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]]; - ++cp; - } - } - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - // Iterate over each cluster - int cp = 0; - for (size_t cc = 0; cc < indices.size (); ++cc) - { - // Iterate over each idx - for (size_t i = 0; i < indices[cc].indices.size (); ++i) - { - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[cp]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices[cp].indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); - ++cp; - } - } - return; - } - } - // Iterate over each cluster int cp = 0; for (size_t cc = 0; cc < indices.size (); ++cc) @@ -624,8 +332,7 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, // Iterate over each idx for (size_t i = 0; i < indices[cc].indices.size (); ++i) { - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); + copyPoint (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp]); ++cp; } } @@ -665,5 +372,131 @@ pcl::concatenateFields (const pcl::PointCloud &cloud1_in, } } +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, + int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value) +{ + if (top < 0 || left < 0 || bottom < 0 || right < 0) + { + std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right"; + PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!"); + return; + } + + if (top == 0 && left == 0 && bottom == 0 && right == 0) + cloud_out = cloud_in; + else + { + // Allocate enough space and copy the basics + cloud_out.header = cloud_in.header; + cloud_out.width = cloud_in.width + left + right; + cloud_out.height = cloud_in.height + top + bottom; + if (cloud_out.size () != cloud_out.width * cloud_out.height) + cloud_out.resize (cloud_out.width * cloud_out.height); + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + if (border_type == pcl::BORDER_TRANSPARENT) + { + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + } + } + else + { + // Copy the data + if (border_type != pcl::BORDER_CONSTANT) + { + try + { + std::vector padding (cloud_out.width - cloud_in.width); + int right = cloud_out.width - cloud_in.width - left; + int bottom = cloud_out.height - cloud_in.height - top; + + for (int i = 0; i < left; i++) + padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type); + + for (int i = 0; i < right; i++) + padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type); + + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + + for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + + for (int j = 0; j < left; j++) + out_inner[j - left] = in[padding[j]]; + + for (int j = 0; j < right; j++) + out_inner[j + cloud_in.width] = in[padding[j + left]]; + } + + for (int i = 0; i < top; i++) + { + int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type); + memcpy (out + i*cloud_out.width, + out + (j+top) * cloud_out.width, + sizeof (PointT) * cloud_out.width); + } + + for (int i = 0; i < bottom; i++) + { + int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type); + memcpy (out + (i + cloud_in.height + top)*cloud_out.width, + out + (j+top)*cloud_out.width, + cloud_out.width * sizeof (PointT)); + } + } + catch (pcl::BadArgumentException &e) + { + PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type); + } + } + else + { + int right = cloud_out.width - cloud_in.width - left; + int bottom = cloud_out.height - cloud_in.height - top; + std::vector buff (cloud_out.width, value); + PointT* buff_ptr = &(buff[0]); + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + + for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + + memcpy (out_inner - left, buff_ptr, left * sizeof (PointT)); + memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT)); + } + + for (int i = 0; i < top; i++) + { + memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT)); + } + + for (int i = 0; i < bottom; i++) + { + memcpy (out + (i + cloud_in.height + top)*cloud_out.width, + buff_ptr, + cloud_out.width * sizeof (PointT)); + } + } + } + } +} + #endif // PCL_IO_IMPL_IO_H_ diff --git a/common/include/pcl/common/impl/pca.hpp b/common/include/pcl/common/impl/pca.hpp index b274f5c0..68d5b61c 100644 --- a/common/include/pcl/common/impl/pca.hpp +++ b/common/include/pcl/common/impl/pca.hpp @@ -47,15 +47,15 @@ ///////////////////////////////////////////////////////////////////////////////////////// /** \brief Constructor with direct computation - * \param[in] X input m*n matrix (ie n vectors of R(m)) + * \param[in] cloud input m*n matrix (ie n vectors of R(m)) * \param[in] basis_only flag to compute only the PCA basis */ template -pcl::PCA::PCA (const pcl::PointCloud& X, bool basis_only) +pcl::PCA::PCA (const pcl::PointCloud &cloud, bool basis_only) { Base (); basis_only_ = basis_only; - setInputCloud (X.makeShared ()); + setInputCloud (cloud.makeShared ()); compute_done_ = initCompute (); } diff --git a/common/include/pcl/common/intensity.h b/common/include/pcl/common/intensity.h index 33f0baab..673cfe1e 100644 --- a/common/include/pcl/common/intensity.h +++ b/common/include/pcl/common/intensity.h @@ -51,7 +51,7 @@ namespace pcl struct IntensityFieldAccessor { /** \brief get intensity field - * \param[in] point p + * \param[in] p point * \return p.intensity */ inline float @@ -60,7 +60,7 @@ namespace pcl return p.intensity; } /** \brief gets the intensity value of a point - * \param[in/out] p point for which intensity to be get + * \param p point for which intensity to be get * \param[in] intensity value of the intensity field */ inline void @@ -69,7 +69,7 @@ namespace pcl intensity = p.intensity; } /** \brief sets the intensity value of a point - * \param[in/out] p point for which intensity to be set + * \param p point for which intensity to be set * \param[in] intensity value of the intensity field */ inline void @@ -78,7 +78,7 @@ namespace pcl p.intensity = intensity; } /** \brief subtract value from intensity field - * \param[in/out] p point for which to modify inetnsity + * \param p point for which to modify inetnsity * \param[in] value value to be subtracted from point intensity */ inline void @@ -87,7 +87,7 @@ namespace pcl p.intensity -= value; } /** \brief add value to intensity field - * \param[in/out] p point for which to modify inetnsity + * \param p point for which to modify inetnsity * \param[in] value value to be added to point intensity */ inline void diff --git a/common/include/pcl/common/intersections.h b/common/include/pcl/common/intersections.h index fb81d36e..7cf30b12 100644 --- a/common/include/pcl/common/intersections.h +++ b/common/include/pcl/common/intersections.h @@ -34,6 +34,7 @@ * $Id$ * */ + #ifndef PCL_INTERSECTIONS_H_ #define PCL_INTERSECTIONS_H_ @@ -57,10 +58,11 @@ namespace pcl * \param[in] sqr_eps maximum allowable squared distance to the true solution * \ingroup common */ - PCL_EXPORTS bool + PCL_EXPORTS inline bool lineWithLineIntersection (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, - Eigen::Vector4f &point, double sqr_eps = 1e-4); + Eigen::Vector4f &point, + double sqr_eps = 1e-4); /** \brief Get the intersection of a two 3D lines in space as a 3D point * \param[in] line_a the coefficients of the first line (point, direction) @@ -69,24 +71,90 @@ namespace pcl * \param[in] sqr_eps maximum allowable squared distance to the true solution * \ingroup common */ - PCL_EXPORTS bool + + PCL_EXPORTS inline bool lineWithLineIntersection (const pcl::ModelCoefficients &line_a, const pcl::ModelCoefficients &line_b, - Eigen::Vector4f &point, double sqr_eps = 1e-4); + Eigen::Vector4f &point, + double sqr_eps = 1e-4); /** \brief Determine the line of intersection of two non-parallel planes using lagrange multipliers * \note Described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA" * \param[in] plane_a coefficients of plane A and plane B in the form ax + by + cz + d = 0 - * \param[out] plane_b coefficients of line where line.tail<3>() = direction vector and + * \param[in] plane_b coefficients of line where line.tail<3>() = direction vector and * line.head<3>() the point on the line clossest to (0, 0, 0) + * \param[out] line the intersected line to be filled + * \param[in] angular_tolerance tolerance in radians * \return true if succeeded/planes aren't parallel */ - PCL_EXPORTS bool + PCL_EXPORTS template bool + planeWithPlaneIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + Eigen::Matrix &line, + double angular_tolerance = 0.1); + + PCL_EXPORTS inline bool planeWithPlaneIntersection (const Eigen::Vector4f &plane_a, - const Eigen::Vector4f &fplane_b, + const Eigen::Vector4f &plane_b, Eigen::VectorXf &line, - double angular_tolerance = 0.1); + double angular_tolerance = 0.1) + { + return (planeWithPlaneIntersection (plane_a, plane_b, line, angular_tolerance)); + } + + PCL_EXPORTS inline bool + planeWithPlaneIntersection (const Eigen::Vector4d &plane_a, + const Eigen::Vector4d &plane_b, + Eigen::VectorXd &line, + double angular_tolerance = 0.1) + { + return (planeWithPlaneIntersection (plane_a, plane_b, line, angular_tolerance)); + } + + /** \brief Determine the point of intersection of three non-parallel planes by solving the equations. + * \note If using nearly parralel planes you can lower the determinant_tolerance value. This can + * lead to inconsistent results. + * If the three planes intersects in a line the point will be anywhere on the line. + * \param[in] plane_a are the coefficients of the first plane in the form ax + by + cz + d = 0 + * \param[in] plane_b are the coefficients of the second plane + * \param[in] plane_c are the coefficients of the third plane + * \param[in] determinant_tolerance is a limit to determine whether planes are parallel or not + * \param[out] intersection_point the three coordinates x, y, z of the intersection point + * \return true if succeeded/planes aren't parallel + */ + PCL_EXPORTS template bool + threePlanesIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + const Eigen::Matrix &plane_c, + Eigen::Matrix &intersection_point, + double determinant_tolerance = 1e-6); + + + PCL_EXPORTS inline bool + threePlanesIntersection (const Eigen::Vector4f &plane_a, + const Eigen::Vector4f &plane_b, + const Eigen::Vector4f &plane_c, + Eigen::Vector3f &intersection_point, + double determinant_tolerance = 1e-6) + { + return (threePlanesIntersection (plane_a, plane_b, plane_c, + intersection_point, determinant_tolerance)); + } + + PCL_EXPORTS inline bool + threePlanesIntersection (const Eigen::Vector4d &plane_a, + const Eigen::Vector4d &plane_b, + const Eigen::Vector4d &plane_c, + Eigen::Vector3d &intersection_point, + double determinant_tolerance = 1e-6) + { + return (threePlanesIntersection (plane_a, plane_b, plane_c, + intersection_point, determinant_tolerance)); + } + } /*@}*/ -#endif //#ifndef PCL_INTERSECTIONS_H_ +#include + +#endif //#ifndef PCL_INTERSECTIONS_H_ diff --git a/common/include/pcl/common/io.h b/common/include/pcl/common/io.h index f2c415ff..ec72feda 100644 --- a/common/include/pcl/common/io.h +++ b/common/include/pcl/common/io.h @@ -45,6 +45,7 @@ #include #include #include +#include #include namespace pcl @@ -223,6 +224,24 @@ namespace pcl } } + typedef enum + { + BORDER_CONSTANT = 0, BORDER_REPLICATE = 1, + BORDER_REFLECT = 2, BORDER_WRAP = 3, + BORDER_REFLECT_101 = 4, BORDER_TRANSPARENT = 5, + BORDER_DEFAULT = BORDER_REFLECT_101 + } InterpolationType; + + /** \brief \return the right index according to the interpolation type. + * \note this is adapted from OpenCV + * \param p the index of point to interpolate + * \param length the top/bottom row or left/right column index + * \param type the requested interpolation + * \throws pcl::BadArgumentException if type is unknown + */ + PCL_EXPORTS int + interpolatePointIndex (int p, int length, InterpolationType type); + /** \brief Concatenate two pcl::PCLPointCloud2. * \param[in] cloud1 the first input point cloud dataset * \param[in] cloud2 the second input point cloud dataset @@ -380,6 +399,31 @@ namespace pcl const std::vector &indices, pcl::PointCloud &cloud_out); + /** \brief Copy a point cloud inside a larger one interpolating borders. + * \param[in] cloud_in the input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \param top + * \param bottom + * \param left + * \param right + * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right. + * \param[in] border_type the interpolating method (pcl::BORDER_XXX) + * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh + * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb + * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba + * BORDER_WRAP: cdefgh|abcdefgh|abcdefg + * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' + * BORDER_TRANSPARENT: mnopqr|abcdefgh|tuvwxyz where m-r and t-z are orignal values of cloud_out + * \param value + * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + int top, int bottom, int left, int right, + pcl::InterpolationType border_type, const PointT& value); + /** \brief Concatenate two datasets representing different fields. * * \note If the input datasets have overlapping fields (i.e., both contain diff --git a/common/include/pcl/common/pca.h b/common/include/pcl/common/pca.h index a414539d..509cbdc9 100644 --- a/common/include/pcl/common/pca.h +++ b/common/include/pcl/common/pca.h @@ -98,8 +98,8 @@ namespace pcl * X input m*n matrix (ie n vectors of R(m)) * basis_only flag to compute only the PCA basis */ - PCL_DEPRECATED (PCA (const pcl::PointCloud& X, bool basis_only = false), - "Use PCA (bool basis_only); setInputCloud (X.makeShared ()); instead"); + PCL_DEPRECATED ("Use PCA (bool basis_only); setInputCloud (X.makeShared ()); instead") + PCA (const pcl::PointCloud& X, bool basis_only = false); /** Copy Constructor * \param[in] pca PCA object diff --git a/common/include/pcl/common/point_tests.h b/common/include/pcl/common/point_tests.h index 7dbafdaf..f202af7a 100644 --- a/common/include/pcl/common/point_tests.h +++ b/common/include/pcl/common/point_tests.h @@ -66,6 +66,7 @@ namespace pcl template<> inline bool isFinite (const pcl::RGB&) { return (true); } template<> inline bool isFinite (const pcl::Label&) { return (true); } template<> inline bool isFinite (const pcl::Axis&) { return (true); } + template<> inline bool isFinite (const pcl::Intensity&) { return (true); } template<> inline bool isFinite (const pcl::MomentInvariants&) { return (true); } template<> inline bool isFinite (const pcl::PrincipalRadiiRSD&) { return (true); } template<> inline bool isFinite (const pcl::Boundary&) { return (true); } diff --git a/common/include/pcl/common/random.h b/common/include/pcl/common/random.h index 696018d1..da446086 100644 --- a/common/include/pcl/common/random.h +++ b/common/include/pcl/common/random.h @@ -109,7 +109,7 @@ namespace pcl /** Constructor * \param parameters uniform distribution parameters and generator seed */ - UniformGenerator(const Parameters& paramters); + UniformGenerator(const Parameters& parameters); /** Change seed value * \param[in] seed new generator seed value diff --git a/common/include/pcl/console/parse.h b/common/include/pcl/console/parse.h index a4163e9a..00158752 100644 --- a/common/include/pcl/console/parse.h +++ b/common/include/pcl/console/parse.h @@ -62,7 +62,7 @@ namespace pcl * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] argument_name the string value to search for - * \return index of found argument or -1 of arguments does not appear in list + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int find_argument (int argc, char** argv, const char* argument_name); @@ -72,7 +72,7 @@ namespace pcl * \param[in] argv the command line arguments * \param[in] argument_name the name of the argument to search for * \param[out] value The value of the argument - * \return index of found argument or -1 of arguments does not appear in list + * \return index of found argument or -1 if arguments do not appear in list */ template int parse (int argc, char** argv, const char* argument_name, Type& value) @@ -90,114 +90,117 @@ namespace pcl return (index - 1); } - /** \brief Parse for a specific given command line argument. Returns the value - * sent as a string. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, std::string &val); - /** \brief Parse for a specific given command line argument. Returns the value - * sent as a boolean. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, bool &val); - /** \brief Parse for a specific given command line argument. Returns the value - * sent as a double. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, float &val); - /** \brief Parse for a specific given command line argument. Returns the value - * sent as a double. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, double &val); - /** \brief Parse for a specific given command line argument. Returns the value - * sent as an int. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, int &val); - /** \brief Parse for a specific given command line argument. Returns the value - * sent as an unsigned int. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, unsigned int &val); - /** \brief Parse for a specific given command line argument. Returns the value - * sent as an int. + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_argument (int argc, char** argv, const char* str, char &val); /** \brief Parse for specific given command line arguments (2x values comma - * separated). Returns the values sent as doubles. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] f the first output value * \param[out] s the second output value * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_2x_arguments (int argc, char** argv, const char* str, float &f, float &s, bool debug = true); /** \brief Parse for specific given command line arguments (2x values comma - * separated). Returns the values sent as doubles. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] f the first output value * \param[out] s the second output value * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_2x_arguments (int argc, char** argv, const char* str, double &f, double &s, bool debug = true); /** \brief Parse for specific given command line arguments (2x values comma - * separated). Returns the values sent as ints. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] f the first output value * \param[out] s the second output value * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_2x_arguments (int argc, char** argv, const char* str, int &f, int &s, bool debug = true); /** \brief Parse for specific given command line arguments (3x values comma - * separated). Returns the values sent as doubles. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for @@ -205,12 +208,13 @@ namespace pcl * \param[out] s the second output value * \param[out] t the third output value * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_3x_arguments (int argc, char** argv, const char* str, float &f, float &s, float &t, bool debug = true); /** \brief Parse for specific given command line arguments (3x values comma - * separated). Returns the values sent as doubles. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for @@ -218,12 +222,13 @@ namespace pcl * \param[out] s the second output value * \param[out] t the third output value * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_3x_arguments (int argc, char** argv, const char* str, double &f, double &s, double &t, bool debug = true); /** \brief Parse for specific given command line arguments (3x values comma - * separated). Returns the values sent as ints. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for @@ -231,103 +236,111 @@ namespace pcl * \param[out] s the second output value * \param[out] t the third output value * \param[in] debug whether to print debug info or not + * return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_3x_arguments (int argc, char** argv, const char* str, int &f, int &s, int &t, bool debug = true); /** \brief Parse for specific given command line arguments (3x values comma - * separated). Returns the values sent as doubles. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] v the vector into which the parsed values will be copied + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_x_arguments (int argc, char** argv, const char* str, std::vector& v); /** \brief Parse for specific given command line arguments (N values comma - * separated). Returns the values sent as ints. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] v the vector into which the parsed values will be copied + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_x_arguments (int argc, char** argv, const char* str, std::vector& v); /** \brief Parse for specific given command line arguments (N values comma - * separated). Returns the values sent as ints. + * separated). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] v the vector into which the parsed values will be copied + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS int parse_x_arguments (int argc, char** argv, const char* str, std::vector& v); - /** \brief Parse for specific given command line arguments (multiple occurances - * of the same command line parameter). Returns the values sent as a vector. + /** \brief Parse for specific given command line arguments (multiple occurences + * of the same command line parameter). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] values the resultant output values + * \return index of found argument or -1 if arguments do not appear in list */ PCL_EXPORTS bool parse_multiple_arguments (int argc, char** argv, const char* str, std::vector &values); - /** \brief Parse for specific given command line arguments (multiple occurances - * of the same command line parameter). Returns the values sent as a vector. + /** \brief Parse for specific given command line arguments (multiple occurences + * of the same command line parameter). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] values the resultant output values + * \return true if found, false otherwise */ PCL_EXPORTS bool parse_multiple_arguments (int argc, char** argv, const char* str, std::vector &values); - /** \brief Parse for specific given command line arguments (multiple occurances - * of the same command line parameter). Returns the values sent as a vector. + /** \brief Parse for specific given command line arguments (multiple occurences + * of the same command line parameter). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] values the resultant output values + * \return true if found, false otherwise */ PCL_EXPORTS bool parse_multiple_arguments (int argc, char** argv, const char* str, std::vector &values); /** \brief Parse for a specific given command line argument (multiple occurences - * of the same command line parameter). Returns the value sent as a vector. + * of the same command line parameter). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the string value to search for * \param[out] values the resultant output values + * \return true if found, false otherwise */ PCL_EXPORTS bool parse_multiple_arguments (int argc, char** argv, const char* str, std::vector &values); - /** \brief Parse for specific given command line arguments (multiple occurances - * of 2x argument groups, separated by commas). Returns 2 vectors holding the - * given values. + /** \brief Parse command line arguments for file names with given extension (multiple occurences + * of 2x argument groups, separated by commas). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] values_f the first vector of output values * \param[out] values_s the second vector of output values + * \return true if found, false otherwise */ PCL_EXPORTS bool parse_multiple_2x_arguments (int argc, char** argv, const char* str, std::vector &values_f, std::vector &values_s); - /** \brief Parse for specific given command line arguments (multiple occurances - * of 3x argument groups, separated by commas). Returns 3 vectors holding the - * given values. + /** \brief Parse command line arguments for file names with given extension (multiple occurences + * of 3x argument groups, separated by commas). * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] str the command line argument to search for * \param[out] values_f the first vector of output values * \param[out] values_s the second vector of output values * \param[out] values_t the third vector of output values + * \return true if found, false otherwise */ PCL_EXPORTS bool parse_multiple_3x_arguments (int argc, char** argv, const char* str, @@ -335,11 +348,11 @@ namespace pcl std::vector &values_s, std::vector &values_t); - /** \brief Parse command line arguments for file names. Returns a vector with - * file names indices. + /** \brief Parse command line arguments for file names with given extension * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments * \param[in] ext the extension to search for + * \return a vector with file names indices */ PCL_EXPORTS std::vector parse_file_extension_argument (int argc, char** argv, const std::string &ext); diff --git a/common/include/pcl/exceptions.h b/common/include/pcl/exceptions.h index 57c8ba65..b3462da9 100644 --- a/common/include/pcl/exceptions.h +++ b/common/include/pcl/exceptions.h @@ -251,6 +251,18 @@ namespace pcl : pcl::PCLException (error_description, file_name, function_name, line_number) { } }; + /** \class BadArgumentException + * \brief An exception that is thrown when the argments number or type is wrong/unhandled. + */ + class BadArgumentException : public PCLException + { + public: + BadArgumentException (const std::string& error_description, + const std::string& file_name = "", + const std::string& function_name = "" , + unsigned line_number = 0) throw () + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + }; } diff --git a/common/include/pcl/impl/pcl_base.hpp b/common/include/pcl/impl/pcl_base.hpp index 67c2b6ea..7d4ae738 100644 --- a/common/include/pcl/impl/pcl_base.hpp +++ b/common/include/pcl/impl/pcl_base.hpp @@ -153,7 +153,7 @@ pcl::PCLBase::initCompute () } catch (std::bad_alloc) { - PCL_ERROR ("[initCompute] Failed to allocate %zu indices.\n", input_->points.size ()); + PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", input_->points.size ()); } for (size_t i = 0; i < indices_->size (); ++i) { (*indices_)[i] = static_cast(i); } } diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp index 748cd19d..c4a69ce4 100644 --- a/common/include/pcl/impl/point_types.hpp +++ b/common/include/pcl/impl/point_types.hpp @@ -72,6 +72,7 @@ (pcl::PFHSignature125) \ (pcl::PFHRGBSignature250) \ (pcl::PPFSignature) \ + (pcl::CPPFSignature) \ (pcl::PPFRGBSignature) \ (pcl::NormalBasedSignature12) \ (pcl::FPFHSignature33) \ @@ -131,6 +132,7 @@ (pcl::PFHSignature125) \ (pcl::PFHRGBSignature250) \ (pcl::PPFSignature) \ + (pcl::CPPFSignature) \ (pcl::PPFRGBSignature) \ (pcl::NormalBasedSignature12) \ (pcl::FPFHSignature33) \ @@ -141,7 +143,23 @@ namespace pcl { -#define PCL_ADD_POINT4D \ + typedef Eigen::Map Array3fMap; + typedef const Eigen::Map Array3fMapConst; + typedef Eigen::Map Array4fMap; + typedef const Eigen::Map Array4fMapConst; + typedef Eigen::Map Vector3fMap; + typedef const Eigen::Map Vector3fMapConst; + typedef Eigen::Map Vector4fMap; + typedef const Eigen::Map Vector4fMapConst; + + typedef Eigen::Matrix Vector3c; + typedef Eigen::Map Vector3cMap; + typedef const Eigen::Map Vector3cMapConst; + typedef Eigen::Matrix Vector4c; + typedef Eigen::Map Vector4cMap; + typedef const Eigen::Map Vector4cMapConst; + +#define PCL_ADD_UNION_POINT4D \ union EIGEN_ALIGN16 { \ float data[4]; \ struct { \ @@ -149,17 +167,23 @@ namespace pcl float y; \ float z; \ }; \ - }; \ - inline Eigen::Map getVector3fMap () { return (Eigen::Vector3f::Map (data)); } \ - inline const Eigen::Map getVector3fMap () const { return (Eigen::Vector3f::Map (data)); } \ - inline Eigen::Map getVector4fMap () { return (Eigen::Vector4f::MapAligned (data)); } \ - inline const Eigen::Map getVector4fMap () const { return (Eigen::Vector4f::MapAligned (data)); } \ - inline Eigen::Map getArray3fMap () { return (Eigen::Array3f::Map (data)); } \ - inline const Eigen::Map getArray3fMap () const { return (Eigen::Array3f::Map (data)); } \ - inline Eigen::Map getArray4fMap () { return (Eigen::Array4f::MapAligned (data)); } \ - inline const Eigen::Map getArray4fMap () const { return (Eigen::Array4f::MapAligned (data)); } + }; -#define PCL_ADD_NORMAL4D \ +#define PCL_ADD_EIGEN_MAPS_POINT4D \ + inline pcl::Vector3fMap getVector3fMap () { return (pcl::Vector3fMap (data)); } \ + inline pcl::Vector3fMapConst getVector3fMap () const { return (pcl::Vector3fMapConst (data)); } \ + inline pcl::Vector4fMap getVector4fMap () { return (pcl::Vector4fMap (data)); } \ + inline pcl::Vector4fMapConst getVector4fMap () const { return (pcl::Vector4fMapConst (data)); } \ + inline pcl::Array3fMap getArray3fMap () { return (pcl::Array3fMap (data)); } \ + inline pcl::Array3fMapConst getArray3fMap () const { return (pcl::Array3fMapConst (data)); } \ + inline pcl::Array4fMap getArray4fMap () { return (pcl::Array4fMap (data)); } \ + inline pcl::Array4fMapConst getArray4fMap () const { return (pcl::Array4fMapConst (data)); } + +#define PCL_ADD_POINT4D \ + PCL_ADD_UNION_POINT4D \ + PCL_ADD_EIGEN_MAPS_POINT4D + +#define PCL_ADD_UNION_NORMAL4D \ union EIGEN_ALIGN16 { \ float data_n[4]; \ float normal[3]; \ @@ -168,13 +192,19 @@ namespace pcl float normal_y; \ float normal_z; \ }; \ - }; \ - inline Eigen::Map getNormalVector3fMap () { return (Eigen::Vector3f::Map (data_n)); } \ - inline const Eigen::Map getNormalVector3fMap () const { return (Eigen::Vector3f::Map (data_n)); } \ - inline Eigen::Map getNormalVector4fMap () { return (Eigen::Vector4f::MapAligned (data_n)); } \ - inline const Eigen::Map getNormalVector4fMap () const { return (Eigen::Vector4f::MapAligned (data_n)); } + }; -#define PCL_ADD_RGB \ +#define PCL_ADD_EIGEN_MAPS_NORMAL4D \ + inline pcl::Vector3fMap getNormalVector3fMap () { return (pcl::Vector3fMap (data_n)); } \ + inline pcl::Vector3fMapConst getNormalVector3fMap () const { return (pcl::Vector3fMapConst (data_n)); } \ + inline pcl::Vector4fMap getNormalVector4fMap () { return (pcl::Vector4fMap (data_n)); } \ + inline pcl::Vector4fMapConst getNormalVector4fMap () const { return (pcl::Vector4fMapConst (data_n)); } + +#define PCL_ADD_NORMAL4D \ + PCL_ADD_UNION_NORMAL4D \ + PCL_ADD_EIGEN_MAPS_NORMAL4D + +#define PCL_ADD_UNION_RGB \ union \ { \ union \ @@ -191,6 +221,22 @@ namespace pcl uint32_t rgba; \ }; +#define PCL_ADD_EIGEN_MAPS_RGB \ + inline Eigen::Vector3i getRGBVector3i () { return (Eigen::Vector3i (r, g, b)); } \ + inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); } \ + inline Eigen::Vector4i getRGBVector4i () { return (Eigen::Vector4i (r, g, b, a)); } \ + inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } \ + inline Eigen::Vector4i getRGBAVector4i () { return (Eigen::Vector4i (r, g, b, a)); } \ + inline const Eigen::Vector4i getRGBAVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } \ + inline pcl::Vector3cMap getBGRVector3cMap () { return (pcl::Vector3cMap (reinterpret_cast (&rgba))); } \ + inline pcl::Vector3cMapConst getBGRVector3cMap () const { return (pcl::Vector3cMapConst (reinterpret_cast (&rgba))); } \ + inline pcl::Vector4cMap getBGRAVector4cMap () { return (pcl::Vector4cMap (reinterpret_cast (&rgba))); } \ + inline pcl::Vector4cMapConst getBGRAVector4cMap () const { return (pcl::Vector4cMapConst (reinterpret_cast (&rgba))); } + +#define PCL_ADD_RGB \ + PCL_ADD_UNION_RGB \ + PCL_ADD_EIGEN_MAPS_RGB + #define PCL_ADD_INTENSITY \ struct \ { \ @@ -209,15 +255,6 @@ namespace pcl uint32_t intensity; \ }; \ - typedef Eigen::Map Array3fMap; - typedef const Eigen::Map Array3fMapConst; - typedef Eigen::Map Array4fMap; - typedef const Eigen::Map Array4fMapConst; - typedef Eigen::Map Vector3fMap; - typedef const Eigen::Map Vector3fMapConst; - typedef Eigen::Map Vector4fMap; - typedef const Eigen::Map Vector4fMapConst; - struct _PointXYZ { @@ -346,6 +383,13 @@ namespace pcl intensity = 0; } +#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 + operator unsigned char() const + { + return intensity; + } +#endif + friend std::ostream& operator << (std::ostream& os, const Intensity8u& p); }; @@ -492,19 +536,10 @@ namespace pcl { x = y = z = 0.0f; data[3] = 1.0f; - r = g = b = a = 0; - } - inline Eigen::Vector3i getRGBVector3i () - { - return (Eigen::Vector3i (r, g, b)); - } - inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); } - inline Eigen::Vector4i getRGBVector4i () - { - return (Eigen::Vector4i (r, g, b, a)); + r = g = b = 0; + a = 255; } - inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } - + friend std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p); }; @@ -580,18 +615,6 @@ namespace pcl a = 0; } - inline Eigen::Vector3i getRGBVector3i () - { - return (Eigen::Vector3i (r, g, b)); - } - inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); } - inline Eigen::Vector4i getRGBVector4i () - { - return (Eigen::Vector4i (r, g, b, a)); - } - inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } - - friend std::ostream& operator << (std::ostream& os, const PointXYZRGB& p); EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -847,23 +870,12 @@ namespace pcl { struct { - // RGB union - union - { - struct - { - uint8_t b; - uint8_t g; - uint8_t r; - uint8_t a; - }; - float rgb; - uint32_t rgba; - }; + PCL_ADD_UNION_RGB; float curvature; }; float data_c[4]; }; + PCL_ADD_EIGEN_MAPS_RGB; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -916,17 +928,6 @@ namespace pcl curvature = 0; } - inline Eigen::Vector3i getRGBVector3i () - { - return (Eigen::Vector3i (r, g, b)); - } - inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); } - inline Eigen::Vector4i getRGBVector4i () - { - return (Eigen::Vector4i (r, g, b, a)); - } - inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } - friend std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p); }; @@ -1077,6 +1078,13 @@ namespace pcl struct Boundary { uint8_t boundary_point; + +#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 + operator unsigned char() const + { + return boundary_point; + } +#endif friend std::ostream& operator << (std::ostream& os, const Boundary& p); }; @@ -1110,6 +1118,7 @@ namespace pcl struct PFHSignature125 { float histogram[125]; + static int descriptorSize () { return 125; } friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p); }; @@ -1121,7 +1130,8 @@ namespace pcl struct PFHRGBSignature250 { float histogram[250]; - + static int descriptorSize () { return 250; } + friend std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p); }; @@ -1137,6 +1147,18 @@ namespace pcl friend std::ostream& operator << (std::ostream& os, const PPFSignature& p); }; + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const CPPFSignature& p); + /** \brief A point structure for storing the Point Pair Feature (CPPF) values + * \ingroup common + */ + struct CPPFSignature + { + float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10; + float alpha_m; + + friend std::ostream& operator << (std::ostream& os, const CPPFSignature& p); + }; + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p); /** \brief A point structure for storing the Point Pair Color Feature (PPFRGB) values * \ingroup common @@ -1170,7 +1192,8 @@ namespace pcl { float descriptor[1980]; float rf[9]; - + static int descriptorSize () { return 1980; } + friend std::ostream& operator << (std::ostream& os, const ShapeContext1980& p); }; @@ -1183,7 +1206,8 @@ namespace pcl { float descriptor[352]; float rf[9]; - + static int descriptorSize () { return 352; } + friend std::ostream& operator << (std::ostream& os, const SHOT352& p); }; @@ -1196,7 +1220,8 @@ namespace pcl { float descriptor[1344]; float rf[9]; - + static int descriptorSize () { return 1344; } + friend std::ostream& operator << (std::ostream& os, const SHOT1344& p); }; @@ -1256,7 +1281,8 @@ namespace pcl struct FPFHSignature33 { float histogram[33]; - + static int descriptorSize () { return 33; } + friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p); }; @@ -1267,7 +1293,8 @@ namespace pcl struct VFHSignature308 { float histogram[308]; - + static int descriptorSize () { return 308; } + friend std::ostream& operator << (std::ostream& os, const VFHSignature308& p); }; @@ -1278,7 +1305,8 @@ namespace pcl struct ESFSignature640 { float histogram[640]; - + static int descriptorSize () { return 640; } + friend std::ostream& operator << (std::ostream& os, const ESFSignature640& p); }; @@ -1289,7 +1317,7 @@ namespace pcl struct GFPFHSignature16 { float histogram[16]; - static int descriptorSize() { return 16; } + static int descriptorSize () { return 16; } friend std::ostream& operator << (std::ostream& os, const GFPFHSignature16& p); }; @@ -1302,7 +1330,8 @@ namespace pcl { float x, y, z, roll, pitch, yaw; float descriptor[36]; - + static int descriptorSize () { return 36; } + friend std::ostream& operator << (std::ostream& os, const Narf36& p); }; @@ -1347,6 +1376,7 @@ namespace pcl struct Histogram { float histogram[N]; + static int descriptorSize () { return N; } }; struct EIGEN_ALIGN16 _PointWithScale @@ -1431,25 +1461,14 @@ namespace pcl { struct { - // RGB union - union - { - struct - { - uint8_t b; - uint8_t g; - uint8_t r; - uint8_t a; - }; - float rgb; - uint32_t rgba; - }; + PCL_ADD_UNION_RGB; float radius; float confidence; float curvature; }; float data_c[4]; }; + PCL_ADD_EIGEN_MAPS_RGB; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/common/include/pcl/pcl_base.h b/common/include/pcl/pcl_base.h index aabc529b..f678d99c 100644 --- a/common/include/pcl/pcl_base.h +++ b/common/include/pcl/pcl_base.h @@ -96,7 +96,7 @@ namespace pcl /** \brief Get a pointer to the input point cloud dataset. */ inline PointCloudConstPtr const - getInputCloud () { return (input_); } + getInputCloud () const { return (input_); } /** \brief Provide a pointer to the vector of indices that represents the input data. * \param[in] indices a pointer to the indices that represent the input data. @@ -131,12 +131,16 @@ namespace pcl inline IndicesPtr const getIndices () { return (indices_); } + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesConstPtr const + getIndices () const { return (indices_); } + /** \brief Override PointCloud operator[] to shorten code * \note this method can be called instead of (*input_)[(*indices_)[pos]] * or input_->points[(*indices_)[pos]] * \param[in] pos position in indices_ vector */ - inline const PointT& operator[] (size_t pos) + inline const PointT& operator[] (size_t pos) const { return ((*input_)[(*indices_)[pos]]); } diff --git a/common/include/pcl/pcl_macros.h b/common/include/pcl/pcl_macros.h index 52594420..10969f9e 100644 --- a/common/include/pcl/pcl_macros.h +++ b/common/include/pcl/pcl_macros.h @@ -101,9 +101,9 @@ namespace pcl #elif ANDROID // Use the math.h macros # include -# define pcl_isnan(x) isnan(x) -# define pcl_isfinite(x) isfinite(x) -# define pcl_isinf(x) isinf(x) +# define pcl_isnan(x) std::isnan(x) +# define pcl_isfinite(x) std::isfinite(x) +# define pcl_isinf(x) std::isinf(x) #elif _GLIBCXX_USE_C99_MATH // Are the C++ cmath functions enabled? @@ -301,21 +301,21 @@ log2f (float x) #endif #if (defined(__GNUC__) && PCL_LINEAR_VERSION(__GNUC__,__GNUC_MINOR__,__GNUC_PATCHLEVEL__) < PCL_LINEAR_VERSION(4,5,0) && ! defined(__clang__)) || defined(__INTEL_COMPILER) -#define PCL_DEPRECATED(func, message) func __attribute__ ((deprecated)) +#define PCL_DEPRECATED(message) __attribute__ ((deprecated)) #endif // gcc supports this starting from 4.5 : http://gcc.gnu.org/bugzilla/show_bug.cgi?id=43666 #if (defined(__GNUC__) && PCL_LINEAR_VERSION(__GNUC__,__GNUC_MINOR__,__GNUC_PATCHLEVEL__) >= PCL_LINEAR_VERSION(4,5,0)) || (defined(__clang__) && __has_extension(attribute_deprecated_with_message)) -#define PCL_DEPRECATED(func, message) func __attribute__ ((deprecated(message))) +#define PCL_DEPRECATED(message) __attribute__ ((deprecated(message))) #endif #ifdef _MSC_VER -#define PCL_DEPRECATED(func, message) __declspec(deprecated(message)) func +#define PCL_DEPRECATED(message) __declspec(deprecated(message)) #endif #ifndef PCL_DEPRECATED #pragma message("WARNING: You need to implement PCL_DEPRECATED for this compiler") -#define PCL_DEPRECATED(func) func +#define PCL_DEPRECATED(message) #endif diff --git a/common/include/pcl/pcl_tests.h b/common/include/pcl/pcl_tests.h index 10e7f99a..f9b8f136 100644 --- a/common/include/pcl/pcl_tests.h +++ b/common/include/pcl/pcl_tests.h @@ -40,16 +40,27 @@ #ifndef PCL_TEST_MACROS #define PCL_TEST_MACROS +#include + +/** \file pcl_tests.h + * Helper macros for testing equality of various data fields in PCL points */ + namespace pcl { + /** test_macros.h provide helper macros for testing vectors, matrices etc. * We took some liberty with upcasing names to make them look like googletest * macros names so that reader is not confused. * - * \author Nizar Sallem + * This file also provides a family of googletest-style macros for asserting + * equality or nearness of xyz, normal, and rgba fields. + * + * \author Nizar Sallem, Sergey Alexandrov */ + namespace test { + template void EXPECT_EQ_VECTORS (const V1& v1, const V2& v2) { @@ -69,7 +80,192 @@ namespace pcl for (size_t i = 0; i < length; ++i) EXPECT_NEAR (v1[i], v2[i], epsilon); } + + namespace internal + { + + template + ::testing::AssertionResult XYZEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getVector3fMap ().cwiseEqual ((p2).getVector3fMap ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getVector3fMap ()" << std::endl + << " Actual: " << p2.getVector3fMap ().transpose () << std::endl + << "Expected: " << expr1 << ".getVector3fMap ()" << std::endl + << "Which is: " << p1.getVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult XYZNear (const char* expr1, + const char* expr2, + const char* abs_error_expr, + const Point1T& p1, + const Point2T& p2, + double abs_error) + { + const Eigen::Vector3f diff = ((p1).getVector3fMap () - + (p2).getVector3fMap ()).cwiseAbs (); + if ((diff.array () < abs_error).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Some of the element-wise differences exceed " << abs_error_expr + << " (which evaluates to " << abs_error << ")" << std::endl + << "Difference: " << diff.transpose () << std::endl + << " Value of: " << expr2 << ".getVector3fMap ()" << std::endl + << " Actual: " << p2.getVector3fMap ().transpose () << std::endl + << " Expected: " << expr1 << ".getVector3fMap ()" << std::endl + << " Which is: " << p1.getVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult NormalEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getNormalVector3fMap ().cwiseEqual ((p2).getNormalVector3fMap ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl + << " Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl + << "Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl + << "Which is: " << p1.getNormalVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult NormalNear (const char* expr1, + const char* expr2, + const char* abs_error_expr, + const Point1T& p1, + const Point2T& p2, + double abs_error) + { + const Eigen::Vector3f diff = ((p1).getNormalVector3fMap () - + (p2).getNormalVector3fMap ()).cwiseAbs (); + if ((diff.array () < abs_error).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Some of the element-wise differences exceed " << abs_error_expr + << " (which evaluates to " << abs_error << ")" << std::endl + << "Difference: " << diff.transpose () << std::endl + << " Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl + << " Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl + << " Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl + << " Which is: " << p1.getNormalVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult RGBEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getRGBVector3i ().cwiseEqual ((p2).getRGBVector3i ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getRGBVector3i ()" << std::endl + << " Actual: " << p2.getRGBVector3i ().transpose () << std::endl + << "Expected: " << expr1 << ".getRGBVector3i ()" << std::endl + << "Which is: " << p1.getRGBVector3i ().transpose (); + } + + template + ::testing::AssertionResult RGBAEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getRGBAVector4i ().cwiseEqual ((p2).getRGBAVector4i ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getRGBAVector4i ()" << std::endl + << " Actual: " << p2.getRGBAVector4i ().transpose () << std::endl + << "Expected: " << expr1 << ".getRGBAVector4i ()" << std::endl + << "Which is: " << p1.getRGBAVector4i ().transpose (); + } + + } + } + } +/// Expect that each of x, y, and z fields are equal in +/// two points. +#define EXPECT_XYZ_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::XYZEQ, \ + (expected), (actual)) + +/// Assert that each of x, y, and z fields are equal in +/// two points. +#define ASSERT_XYZ_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::XYZEQ, \ + (expected), (actual)) + +/// Expect that differences between x, y, and z fields in +/// two points are each within abs_error. +#define EXPECT_XYZ_NEAR(expected, actual, abs_error) \ + EXPECT_PRED_FORMAT3(::pcl::test::internal::XYZNear, \ + (expected), (actual), abs_error) + +/// Assert that differences between x, y, and z fields in +/// two points are each within abs_error. +#define ASSERT_XYZ_NEAR(expected, actual, abs_error) \ + EXPECT_PRED_FORMAT3(::pcl::test::internal::XYZNear, \ + (expected), (actual), abs_error) + +/// Expect that each of normal_x, normal_y, and normal_z +/// fields are equal in two points. +#define EXPECT_NORMAL_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::NormalEQ, \ + (expected), (actual)) + +/// Assert that each of normal_x, normal_y, and normal_z +/// fields are equal in two points. +#define ASSERT_NORMAL_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::NormalEQ, \ + (expected), (actual)) + +/// Expect that differences between normal_x, normal_y, +/// and normal_z fields in two points are each within +/// abs_error. +#define EXPECT_NORMAL_NEAR(expected, actual, abs_error) \ + EXPECT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \ + (expected), (actual), abs_error) + +/// Assert that differences between normal_x, normal_y, +/// and normal_z fields in two points are each within +/// abs_error. +#define ASSERT_NORMAL_NEAR(expected, actual, abs_error) \ + EXPECT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \ + (expected), (actual), abs_error) + +/// Expect that each of r, g, and b fields are equal in +/// two points. +#define EXPECT_RGB_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBEQ, \ + (expected), (actual)) + +/// Assert that each of r, g, and b fields are equal in +/// two points. +#define ASSERT_RGB_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBEQ, \ + (expected), (actual)) + +/// Expect that each of r, g, b, and a fields are equal +/// in two points. +#define EXPECT_RGBA_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ, \ + (expected), (actual)) + +/// Assert that each of r, g, b, and a fields are equal +/// in two points. +#define ASSERT_RGBA_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ, \ + (expected), (actual)) + #endif diff --git a/common/include/pcl/point_traits.h b/common/include/pcl/point_traits.h index f10d9374..7d3d97a1 100644 --- a/common/include/pcl/point_traits.h +++ b/common/include/pcl/point_traits.h @@ -53,6 +53,12 @@ #include #endif +// This is required for the workaround at line 109 +#ifdef _MSC_VER +#include +#include +#endif + namespace pcl { @@ -100,6 +106,24 @@ namespace pcl typedef PointT type; }; +#ifdef _MSC_VER + + /* Sometimes when calling functions like `copyPoint()` or `copyPointCloud` + * without explicitly specifying point types, MSVC deduces them to be e.g. + * `Eigen::internal::workaround_msvc_stl_support` instead of + * plain `pcl::PointXYZ`. Subsequently these types are passed to meta- + * functions like `has_field` or `fieldList` and make them choke. This hack + * makes use of the fact that internally `fieldList` always applies `POD` to + * its argument type. This specialization therefore allows to unwrap the + * contained point type. */ + template + struct POD > + { + typedef PointT type; + }; + +#endif + // name /* This really only depends on Tag, but we go through some gymnastics to avoid ODR violations. We template it on the point type PointT to avoid ODR violations when registering multiple @@ -185,7 +209,19 @@ namespace pcl } }; - /** \brief A helper functor that can copy a specific value if the given field exists. */ + /** \brief A helper functor that can copy a specific value if the given field exists. + * + * \note In order to actually copy the value an instance of this functor should be passed + * to a pcl::for_each_type loop. See the example below. + * + * \code + * PointInT p; + * bool exists; + * float value; + * typedef typename pcl::traits::fieldList::type FieldList; + * pcl::for_each_type (pcl::CopyIfFieldExists (p, "intensity", exists, value)); + * \endcode + */ template struct CopyIfFieldExists { @@ -240,7 +276,17 @@ namespace pcl OutT &value_; }; - /** \brief A helper functor that can set a specific value in a field if the field exists. */ + /** \brief A helper functor that can set a specific value in a field if the field exists. + * + * \note In order to actually set the value an instance of this functor should be passed + * to a pcl::for_each_type loop. See the example below. + * + * \code + * PointT p; + * typedef typename pcl::traits::fieldList::type FieldList; + * pcl::for_each_type (pcl::SetIfFieldExists (p, "intensity", 42.0f)); + * \endcode + */ template struct SetIfFieldExists { diff --git a/common/include/pcl/point_types.h b/common/include/pcl/point_types.h index 6635426b..0bce7712 100644 --- a/common/include/pcl/point_types.h +++ b/common/include/pcl/point_types.h @@ -42,6 +42,9 @@ #include #include #include +#include +#include +#include /** * \file pcl/point_types.h @@ -227,6 +230,11 @@ namespace pcl */ struct PPFSignature; + /** \brief Members: float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10, alpha_m + * \ingroup common + */ + struct CPPFSignature; + /** \brief Members: float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio, alpha_m * \ingroup common */ @@ -513,6 +521,20 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFSignature, (float, alpha_m, alpha_m) ) +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature, + (float, f1, f1) + (float, f2, f2) + (float, f3, f3) + (float, f4, f4) + (float, f5, f5) + (float, f6, f6) + (float, f7, f7) + (float, f8, f8) + (float, f9, f9) + (float, f10, f10) + (float, alpha_m, alpha_m) +) + POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFRGBSignature, (float, f1, f1) (float, f2, f2) @@ -636,6 +658,84 @@ namespace pcl } } }; + + namespace traits + { + + /** \brief Metafunction to check if a given point type has a given field. + * + * Example usage at run-time: + * + * \code + * bool curvature_available = pcl::traits::has_field::value; + * \endcode + * + * Example usage at compile-time: + * + * \code + * BOOST_MPL_ASSERT_MSG ((pcl::traits::has_field::value), + * POINT_TYPE_SHOULD_HAVE_LABEL_FIELD, + * (PointT)); + * \endcode + */ + template + struct has_field : boost::mpl::contains::type, Field>::type + { }; + + /** Metafunction to check if a given point type has all given fields. */ + template + struct has_all_fields : boost::mpl::fold, + boost::mpl::and_ > >::type + { }; + + /** Metafunction to check if a given point type has any of the given fields. */ + template + struct has_any_field : boost::mpl::fold, + boost::mpl::or_ > >::type + { }; + + /** Metafunction to check if a given point type has x, y, and z fields. */ + template + struct has_xyz : has_all_fields > + { }; + + /** Metafunction to check if a given point type has normal_x, normal_y, and + * normal_z fields. */ + template + struct has_normal : has_all_fields > + { }; + + /** Metafunction to check if a given point type has curvature field. */ + template + struct has_curvature : has_field + { }; + + /** Metafunction to check if a given point type has intensity field. */ + template + struct has_intensity : has_field + { }; + + /** Metafunction to check if a given point type has either rgb or rgba field. */ + template + struct has_color : has_any_field > + { }; + + /** Metafunction to check if a given point type has label field. */ + template + struct has_label : has_field + { }; + + } + } // namespace pcl #if defined _MSC_VER diff --git a/common/include/pcl/point_types_conversion.h b/common/include/pcl/point_types_conversion.h index fe867f53..1f445ca8 100644 --- a/common/include/pcl/point_types_conversion.h +++ b/common/include/pcl/point_types_conversion.h @@ -179,7 +179,7 @@ namespace pcl out.x = in.x; out.y = in.y; out.z = in.z; if (in.s == 0) { - out.r = out.g = out.b = static_cast (in.v); + out.r = out.g = out.b = static_cast (255 * in.v); return; } float a = in.h / 60; @@ -350,24 +350,24 @@ namespace pcl * \param[in] focal the focal length * \param[out] out the output pointcloud * **/ - void + inline void PointCloudDepthAndRGBtoXYZRGBA (PointCloud& depth, PointCloud& image, float& focal, PointCloud& out) { float bad_point = std::numeric_limits::quiet_NaN(); - int width_ = depth.width; - int height_ = depth.height; + size_t width_ = depth.width; + size_t height_ = depth.height; float constant_ = 1.0f / focal; - for(unsigned int v = 0; v < height_; v++) + for (size_t v = 0; v < height_; v++) { - for(unsigned int u = 0; u < width_; u++) + for (size_t u = 0; u < width_; u++) { PointXYZRGBA pt; pt.a = 0; - float depth_ = depth.at(u,v).intensity; + float depth_ = depth.at (u, v).intensity; if (depth_ == 0) { @@ -379,11 +379,11 @@ namespace pcl pt.x = static_cast (u) * pt.z * constant_; pt.y = static_cast (v) * pt.z * constant_; } - pt.r = image.at(u,v).r; - pt.g = image.at(u,v).g; - pt.b = image.at(u,v).b; + pt.r = image.at (u, v).r; + pt.g = image.at (u, v).g; + pt.b = image.at (u, v).b; - out.points.push_back(pt); + out.points.push_back (pt); } } out.width = width_; diff --git a/common/include/pcl/range_image/bearing_angle_image.h b/common/include/pcl/range_image/bearing_angle_image.h new file mode 100644 index 00000000..2245ea52 --- /dev/null +++ b/common/include/pcl/range_image/bearing_angle_image.h @@ -0,0 +1,89 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Intelligent Robotics Lab, DLUT. + * Author: Qinghua Li, Yan Zhuang, Fei Yan + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Intelligent Robotics Lab, DLUT. nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file bearing_angle_image.h + * \Created on: July 07, 2012 + */ + +#ifndef PCL_BEARING_ANGLE_IMAGE_H_ +#define PCL_BEARING_ANGLE_IMAGE_H_ + +#include +#include +#include + +namespace pcl +{ + /** \brief class BearingAngleImage is used as an interface to generate Bearing Angle(BA) image. + * \author: Qinghua Li (qinghua__li@163.com) + */ + class BearingAngleImage : public pcl::PointCloud + { + public: + // ===== TYPEDEFS ===== + typedef pcl::PointCloud BaseClass; + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + BearingAngleImage (); + /** Destructor */ + virtual ~BearingAngleImage (); + + public: + /** \brief Reset all values to an empty Bearing Angle image */ + void + reset (); + + /** \brief Calculate the angle between the laser beam and the segment joining two consecutive + * measurement points. + * \param point1 + * \param point2 + */ + double + getAngle (const PointXYZ &point1, const PointXYZ &point2); + + /** \brief Transform 3D point cloud into a 2D Bearing Angle(BA) image */ + void + generateBAImage (PointCloud& point_cloud); + + protected: + /**< This point is used to be able to return a reference to a unknown gray point */ + PointXYZRGBA unobserved_point_; + }; +} + +#endif // PCL_BEARING_ANGLE_IMAGE_H_ diff --git a/common/include/pcl/range_image/range_image.h b/common/include/pcl/range_image/range_image.h index 765921b4..79b1d65f 100644 --- a/common/include/pcl/range_image/range_image.h +++ b/common/include/pcl/range_image/range_image.h @@ -203,7 +203,6 @@ namespace pcl * individual pixels in the image in the x-direction * \param angular_resolution_y the angular difference (in radians) between the * individual pixels in the image in the y-direction - * \param angular_resolution the angle (in radians) between each sample in the depth image * \param point_cloud_center the center of bounding sphere * \param point_cloud_radius the radius of the bounding sphere * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to diff --git a/common/include/pcl/ros/conversions.h b/common/include/pcl/ros/conversions.h index b1544434..41d47f93 100644 --- a/common/include/pcl/ros/conversions.h +++ b/common/include/pcl/ros/conversions.h @@ -62,12 +62,9 @@ namespace pcl * createMapping (msg.fields, field_map); * \endcode */ - PCL_DEPRECATED (template void fromROSMsg ( - const pcl::PCLPointCloud2& msg, pcl::PointCloud& cloud, - const MsgFieldMap& field_map), - "pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead."); - - template void + template + PCL_DEPRECATED ("pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead.") + void fromROSMsg (const pcl::PCLPointCloud2& msg, pcl::PointCloud& cloud, const MsgFieldMap& field_map) { @@ -78,10 +75,9 @@ namespace pcl * \param[in] msg the PCLPointCloud2 binary blob * \param[out] cloud the resultant pcl::PointCloud */ - PCL_DEPRECATED (template void fromROSMsg ( - const pcl::PCLPointCloud2& msg, pcl::PointCloud& cloud), - "pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead."); - template void + template + PCL_DEPRECATED ("pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead.") + void fromROSMsg (const pcl::PCLPointCloud2& msg, pcl::PointCloud& cloud) { fromPCLPointCloud2 (msg, cloud); @@ -91,10 +87,9 @@ namespace pcl * \param[in] cloud the input pcl::PointCloud * \param[out] msg the resultant PCLPointCloud2 binary blob */ - PCL_DEPRECATED (template void toROSMsg ( - const pcl::PointCloud& cloud, pcl::PCLPointCloud2& msg), - "pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead."); - template void + template + PCL_DEPRECATED ("pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead.") + void toROSMsg (const pcl::PointCloud& cloud, pcl::PCLPointCloud2& msg) { toPCLPointCloud2 (cloud, msg); @@ -106,10 +101,9 @@ namespace pcl * CloudT cloud type, CloudT should be akin to pcl::PointCloud * \note will throw std::runtime_error if there is a problem */ - PCL_DEPRECATED (template void toROSMsg ( - const CloudT& cloud, pcl::PCLImage& msg), - "pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead."); - template void + template + PCL_DEPRECATED ("pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead.") + void toROSMsg (const CloudT& cloud, pcl::PCLImage& msg) { toPCLPointCloud2 (cloud, msg); @@ -120,10 +114,8 @@ namespace pcl * \param msg the resultant pcl::PCLImage * will throw std::runtime_error if there is a problem */ - PCL_DEPRECATED (inline void toROSMsg ( - const pcl::PCLPointCloud2& cloud, pcl::PCLImage& msg), - "pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead."); inline void + PCL_DEPRECATED ("pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead.") toROSMsg (const pcl::PCLPointCloud2& cloud, pcl::PCLImage& msg) { toPCLPointCloud2 (cloud, msg); diff --git a/common/src/bearing_angle_image.cpp b/common/src/bearing_angle_image.cpp new file mode 100644 index 00000000..bdb72b57 --- /dev/null +++ b/common/src/bearing_angle_image.cpp @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Intelligent Robotics Lab, DLUT. + * Author: Qinghua Li, Yan Zhuang, Fei Yan + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Intelligent Robotics Lab, DLUT. nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file bearing_angle_image.cpp + * \created on: July 07, 2012 + * \author: Qinghua Li (qinghua__li@163.com) + */ + +#include +#include + +namespace pcl +{ +///////////////////////////////////////////////////////// +BearingAngleImage::BearingAngleImage () : + BearingAngleImage::BaseClass (), + unobserved_point_ () +{ + reset (); + unobserved_point_.x = unobserved_point_.y = unobserved_point_.z = 0.0; + unobserved_point_.rgba = 255; +} + +///////////////////////////////////////////////////////// +BearingAngleImage::~BearingAngleImage () +{ +} + +///////////////////////////////////////////////////////// +void +BearingAngleImage::reset () +{ + width = height = 0; + points.clear (); +} + +///////////////////////////////////////////////////////// +double +BearingAngleImage::getAngle (const PointXYZ &point1, const PointXYZ &point2) +{ + double a, b, c; + double theta; + const Eigen::Vector3f& p1 = point1.getVector3fMap (); + const Eigen::Vector3f& p2 = point2.getVector3fMap (); + a = p1.squaredNorm (); + b = (p1 - p2).squaredNorm (); + c = p2.squaredNorm (); + + if (a != 0 && b != 0) + { + theta = acos ((a + b - c) / (2 * sqrt (a) * sqrt (b))) * 180 / M_PI; + } + else + { + theta = 0.0; + } + + return theta; +} + +///////////////////////////////////////////////////////// +void +BearingAngleImage::generateBAImage (PointCloud& point_cloud) +{ + width = point_cloud.width; + height = point_cloud.height; + unsigned int size = width * height; + points.clear (); + points.resize (size, unobserved_point_); + + double theta; + uint8_t r, g, b, gray; + + // primary transformation process + for (int i = 0; i < static_cast (height) - 1; ++i) + { + for (int j = 0; j < static_cast (width) - 1; ++j) + { + theta = getAngle (point_cloud.at (j, i + 1), point_cloud.at (j + 1, i)); + + // based on the theta, calculate the gray value of every pixel point + gray = theta * 255 / 180; + r = gray; + g = gray; + b = gray; + + points[(i + 1) * width + j].x = point_cloud.at (j, i + 1).x; + points[(i + 1) * width + j].y = point_cloud.at (j, i + 1).y; + points[(i + 1) * width + j].z = point_cloud.at (j, i + 1).z; + // set the gray value for every pixel point + points[(i + 1) * width + j].rgba = ((int)r) << 24 | ((int)g) << 16 | ((int)b) << 8 | 0xff; + } + } +} + +} // namespace end diff --git a/common/src/intersections.cpp b/common/src/intersections.cpp deleted file mode 100644 index 6176ba08..00000000 --- a/common/src/intersections.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * - */ - -#include -#include - -bool -pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, - const Eigen::VectorXf &line_b, - Eigen::Vector4f &point, double sqr_eps) -{ - Eigen::Vector4f p1, p2; - lineToLineSegment (line_a, line_b, p1, p2); - - // If the segment size is smaller than a pre-given epsilon... - double sqr_dist = (p1 - p2).squaredNorm (); - if (sqr_dist < sqr_eps) - { - point = p1; - return (true); - } - point.setZero (); - return (false); -} - -bool -pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a, - const pcl::ModelCoefficients &line_b, - Eigen::Vector4f &point, double sqr_eps) -{ - Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.values[0], line_a.values.size ()); - Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.values[0], line_b.values.size ()); - return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps)); -} - -bool -pcl::planeWithPlaneIntersection (const Eigen::Vector4f &plane_a, - const Eigen::Vector4f &plane_b, - Eigen::VectorXf &line, - double angular_tolerance) -{ - //planes shouldn't be parallel - double test_cosine = plane_a.head<3>().dot(plane_b.head<3>()); - double upper_limit = 1 + angular_tolerance; - double lower_limit = 1 - angular_tolerance; - - if ((test_cosine < upper_limit) && (test_cosine > lower_limit)) - { - PCL_ERROR ("Plane A and Plane B are Parallel"); - return (false); - } - - if ((test_cosine > -upper_limit) && (test_cosine < -lower_limit)) - { - PCL_ERROR ("Plane A and Plane B are Parallel"); - return (false); - } - - Eigen::Vector4f line_direction = plane_a.cross3(plane_b); - line_direction.normalized(); - - //construct system of equations using lagrange multipliers with one objective function and two constraints - Eigen::MatrixXf langegrange_coefs(5,5); - langegrange_coefs << 2,0,0,plane_a[0],plane_b[0], 0,2,0,plane_a[1],plane_b[1], 0,0,2, plane_a[2], plane_b[2], plane_a[0], plane_a[1] , plane_a[2], 0,0, plane_b[0], plane_b[1], plane_b[2], 0,0; - - Eigen::VectorXf b; - b.resize(5); - b << 0, 0, 0, -plane_a[3], -plane_b[3]; - - //solve for the lagrange Multipliers - Eigen::VectorXf x; - x.resize(5); - x = langegrange_coefs.colPivHouseholderQr().solve(b); - - line.resize(6); - line.head<3>() = x.head<3>(); // the x[3] and x[4] are the values of the lagrange multipliers and are neglected - line[3] = line_direction[0]; - line[4] = line_direction[1]; - line[5] = line_direction[2]; - return true; -} diff --git a/common/src/io.cpp b/common/src/io.cpp index 561d0b13..cd260873 100644 --- a/common/src/io.cpp +++ b/common/src/io.cpp @@ -474,3 +474,44 @@ pcl::copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, cloud_out.data = cloud_in.data; } +//////////////////////////////////////////////////////////////////////////////// +int +pcl::interpolatePointIndex (int p, int len, InterpolationType type) +{ + if (static_cast (p) >= static_cast (len)) + { + if (type == BORDER_REPLICATE) + p = p < 0 ? 0 : len - 1; + else if (type == BORDER_REFLECT || type == BORDER_REFLECT_101) + { + int delta = type == BORDER_REFLECT_101; + if (len == 1) + return 0; + do + { + if (p < 0) + p = -p - 1 + delta; + else + p = len - 1 - (p - len) - delta; + } + while (static_cast (p) >= static_cast (len)); + } + else if (type == BORDER_WRAP) + { + if (p < 0) + p -= ((p-len+1)/len)*len; + if (p >= len) + p %= len; + } + else if (type == BORDER_CONSTANT) + p = -1; + else + { + PCL_THROW_EXCEPTION (BadArgumentException, + "[pcl::interpolate_point_index] error: Unhandled interpolation type " + << type << " !"); + } + } + + return (p); +} diff --git a/common/src/parse.cpp b/common/src/parse.cpp index 8cf87d1a..308243ae 100644 --- a/common/src/parse.cpp +++ b/common/src/parse.cpp @@ -191,7 +191,7 @@ pcl::console::parse_2x_arguments (int argc, char** argv, const char* str, float boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 2 && debug) { - print_error ("[parse_2x_arguments] Number of values for %s (%zu) different than 2!\n", str, values.size ()); + print_error ("[parse_2x_arguments] Number of values for %s (%lu) different than 2!\n", str, values.size ()); return (-2); } f = static_cast (atof (values.at (0).c_str ())); @@ -216,7 +216,7 @@ pcl::console::parse_2x_arguments (int argc, char** argv, const char* str, double boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 2 && debug) { - print_error ("[parse_2x_arguments] Number of values for %s (%zu) different than 2!\n", str, values.size ()); + print_error ("[parse_2x_arguments] Number of values for %s (%lu) different than 2!\n", str, values.size ()); return (-2); } f = atof (values.at (0).c_str ()); @@ -241,7 +241,7 @@ pcl::console::parse_2x_arguments (int argc, char** argv, const char* str, int &f boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 2 && debug) { - print_error ("[parse_2x_arguments] Number of values for %s (%zu) different than 2!\n", str, values.size ()); + print_error ("[parse_2x_arguments] Number of values for %s (%lu) different than 2!\n", str, values.size ()); return (-2); } f = atoi (values.at (0).c_str ()); @@ -266,7 +266,7 @@ pcl::console::parse_3x_arguments (int argc, char** argv, const char* str, float boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 3 && debug) { - print_error ("[parse_3x_arguments] Number of values for %s (%zu) different than 3!\n", str, values.size ()); + print_error ("[parse_3x_arguments] Number of values for %s (%lu) different than 3!\n", str, values.size ()); return (-2); } f = static_cast (atof (values.at (0).c_str ())); @@ -292,7 +292,7 @@ pcl::console::parse_3x_arguments (int argc, char** argv, const char* str, double boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 3 && debug) { - print_error ("[parse_3x_arguments] Number of values for %s (%zu) different than 3!\n", str, values.size ()); + print_error ("[parse_3x_arguments] Number of values for %s (%lu) different than 3!\n", str, values.size ()); return (-2); } f = atof (values.at (0).c_str ()); @@ -318,7 +318,7 @@ pcl::console::parse_3x_arguments (int argc, char** argv, const char* str, int &f boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 3 && debug) { - print_error ("[parse_3x_arguments] Number of values for %s (%zu) different than 3!\n", str, values.size ()); + print_error ("[parse_3x_arguments] Number of values for %s (%lu) different than 3!\n", str, values.size ()); return (-2); } f = atoi (values.at (0).c_str ()); @@ -489,7 +489,7 @@ pcl::console::parse_multiple_2x_arguments (int argc, char** argv, const char* st boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 2) { - print_error ("[parse_multiple_2x_arguments] Number of values for %s (%zu) different than 2!\n", str, values.size ()); + print_error ("[parse_multiple_2x_arguments] Number of values for %s (%lu) different than 2!\n", str, values.size ()); return (false); } f = atof (values.at (0).c_str ()); @@ -522,7 +522,7 @@ pcl::console::parse_multiple_3x_arguments (int argc, char** argv, const char* st boost::split (values, argv[i], boost::is_any_of (","), boost::token_compress_on); if (values.size () != 3) { - print_error ("[parse_multiple_3x_arguments] Number of values for %s (%zu) different than 3!\n", str, values.size ()); + print_error ("[parse_multiple_3x_arguments] Number of values for %s (%lu) different than 3!\n", str, values.size ()); return (false); } f = atof (values.at (0).c_str ()); diff --git a/common/src/pcl_base.cpp b/common/src/pcl_base.cpp index 963faa03..215c7243 100644 --- a/common/src/pcl_base.cpp +++ b/common/src/pcl_base.cpp @@ -142,7 +142,7 @@ pcl::PCLBase::initCompute () } catch (std::bad_alloc) { - PCL_ERROR ("[initCompute] Failed to allocate %zu indices.\n", (input_->width * input_->height)); + PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", (input_->width * input_->height)); } for (size_t i = 0; i < indices_->size (); ++i) { (*indices_)[i] = static_cast(i); } } diff --git a/common/src/point_types.cpp b/common/src/point_types.cpp index 4c78ce04..cdecebd1 100644 --- a/common/src/point_types.cpp +++ b/common/src/point_types.cpp @@ -254,6 +254,13 @@ namespace pcl return (os); } + std::ostream& + operator << (std::ostream& os, const CPPFSignature& p) + { + os << "(" << p.f1 << ", " << p.f2 << ", " << p.f3 << ", " << p.f4 << ", " << p.f5 << ", " << p.f6 << ", " << p.f7 << ", " << p.f8 << ", " << p.f9 << ", " << p.f10 << ", " << p.alpha_m << ")"; + return (os); + } + std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p) { diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index c6300b8c..c43826f2 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -1,2 +1,14 @@ +include(CMakeDependentOption) + +cmake_dependent_option(WITH_TUTORIALS "Build tutorials (requires Sphinx)" FALSE "WITH_DOCS" FALSE) +if(WITH_TUTORIALS) + find_package(Sphinx) + if(SPHINX_FOUND) + set(SPHINX_CACHE_DIR "${CMAKE_CURRENT_BINARY_DIR}/_doctrees") + set(SPHINX_HTML_FILE_SUFFIX "html" CACHE STRING "Suffix (extension) of the HTML files generated by Sphinx") + endif(SPHINX_FOUND) +endif(WITH_TUTORIALS) + add_subdirectory(doxygen) +add_subdirectory(advanced) add_subdirectory(tutorials) diff --git a/doc/advanced/CMakeLists.txt b/doc/advanced/CMakeLists.txt new file mode 100644 index 00000000..d066c438 --- /dev/null +++ b/doc/advanced/CMakeLists.txt @@ -0,0 +1,14 @@ +if(SPHINX_FOUND) + add_custom_target(advanced ALL + COMMAND "${SPHINX_EXECUTABLE}" -b html -a -d "${SPHINX_CACHE_DIR}" -D html_file_suffix=".${SPHINX_HTML_FILE_SUFFIX}" "${CMAKE_CURRENT_SOURCE_DIR}/content" html) + add_dependencies(advanced doc) + if(USE_PROJECT_FOLDERS) + set_target_properties(advanced PROPERTIES FOLDER "Documentation (Advanced)") + endif(USE_PROJECT_FOLDERS) + install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html" + DESTINATION "${DOC_INSTALL_DIR}/advanced" + COMPONENT doc) + install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/content/files" + DESTINATION "${DOC_INSTALL_DIR}/advanced" + COMPONENT doc) +endif(SPHINX_FOUND) diff --git a/doc/advanced/Makefile b/doc/advanced/Makefile deleted file mode 100644 index 46fe7754..00000000 --- a/doc/advanced/Makefile +++ /dev/null @@ -1,6 +0,0 @@ -.PHONY: html - -html: - -rm -rf html /tmp/doctrees - sphinx-build -b html -a -d /tmp/doctrees content html - diff --git a/doc/advanced/content/_static/basic.css b/doc/advanced/content/_static/basic.css new file mode 100644 index 00000000..efe60182 --- /dev/null +++ b/doc/advanced/content/_static/basic.css @@ -0,0 +1,538 @@ +/* + * basic.css + * ~~~~~~~~~ + * + * Sphinx stylesheet -- basic theme. + * + * :copyright: Copyright 2007-2011 by the Sphinx team, see AUTHORS. + * :license: BSD, see LICENSE for details. + * + */ + +/* -- main layout ----------------------------------------------------------- */ + +div.clearer { + clear: both; +} + +/* -- relbar ---------------------------------------------------------------- */ + +div.related { + width: 100%; + font-size: 90%; +} + +div.related h3 { + display: none; +} + +div.related ul { + margin: 0; + padding: 0 0 0 10px; + list-style: none; +} + +div.related li { + display: inline; +} + +div.related li.right { + float: right; + margin-right: 5px; +} + +/* -- sidebar --------------------------------------------------------------- */ + +div.sphinxsidebarwrapper { + padding: 10px 5px 0 10px; +} + +div.sphinxsidebar { + float: left; + width: 230px; + margin-left: -100%; + font-size: 90%; +} + +div.sphinxsidebar ul { + list-style: none; +} + +div.sphinxsidebar ul ul, +div.sphinxsidebar ul.want-points { + margin-left: 20px; + list-style: square; +} + +div.sphinxsidebar ul ul { + margin-top: 0; + margin-bottom: 0; +} + +div.sphinxsidebar form { + margin-top: 10px; +} + +div.sphinxsidebar input { + border: 1px solid #98dbcc; + font-family: sans-serif; + font-size: 1em; +} + +img { + border: 0; +} + +/* -- search page ----------------------------------------------------------- */ + +ul.search { + margin: 10px 0 0 20px; + padding: 0; +} + +ul.search li { + padding: 5px 0 5px 20px; + background-image: url(file.png); + background-repeat: no-repeat; + background-position: 0 7px; +} + +ul.search li a { + font-weight: bold; +} + +ul.search li div.context { + color: #888; + margin: 2px 0 0 30px; + text-align: left; +} + +ul.keywordmatches li.goodmatch a { + font-weight: bold; +} + +/* -- index page ------------------------------------------------------------ */ + +table.contentstable { + width: 90%; +} + +table.contentstable p.biglink { + line-height: 150%; +} + +a.biglink { + font-size: 1.3em; +} + +span.linkdescr { + font-style: italic; + padding-top: 5px; + font-size: 90%; +} + +/* -- general index --------------------------------------------------------- */ + +table.indextable { + width: 100%; +} + +table.indextable td { + text-align: left; + vertical-align: top; +} + +table.indextable dl, table.indextable dd { + margin-top: 0; + margin-bottom: 0; +} + +table.indextable tr.pcap { + height: 10px; +} + +table.indextable tr.cap { + margin-top: 10px; + background-color: #f2f2f2; +} + +img.toggler { + margin-right: 3px; + margin-top: 3px; + cursor: pointer; +} + +div.modindex-jumpbox { + border-top: 1px solid #ddd; + border-bottom: 1px solid #ddd; + margin: 1em 0 1em 0; + padding: 0.4em; +} + +div.genindex-jumpbox { + border-top: 1px solid #ddd; + border-bottom: 1px solid #ddd; + margin: 1em 0 1em 0; + padding: 0.4em; +} + +/* -- general body styles --------------------------------------------------- */ + +a.headerlink { + visibility: hidden; +} + +h1:hover > a.headerlink, +h2:hover > a.headerlink, +h3:hover > a.headerlink, +h4:hover > a.headerlink, +h5:hover > a.headerlink, +h6:hover > a.headerlink, +dt:hover > a.headerlink { + visibility: visible; +} + +div.body p.caption { + text-align: inherit; +} + +div.body td { + text-align: left; +} + +.field-list ul { + padding-left: 1em; +} + +.first { +font-family: "Droid Serif", "DejaVu Serif", "Garamond", serif; +font-size: 1.3em; +color: #64794c; +margin-bottom: 0; + margin-top: 0 !important; +} + +.first img { +max-width: none !important; +} + +p.rubric { + margin-top: 30px; + font-weight: bold; +} + +img.align-left, .figure.align-left, object.align-left { + clear: left; + float: left; + margin-right: 1em; +} + +img.align-right, .figure.align-right, object.align-right { + clear: right; + float: right; + margin-left: 1em; +} + +img.align-center, .figure.align-center, object.align-center { + display: block; + margin-left: auto; + margin-right: auto; +} + +.align-left { + text-align: left; +} + +.align-center { + clear: both; + text-align: center; +} + +.align-right { + text-align: right; +} + +/* -- sidebars -------------------------------------------------------------- */ + +div.sidebar { + margin: 0 0 0.5em 1em; + border: 1px solid #ddb; + padding: 7px 7px 0 7px; + background-color: #ffe; + width: 40%; + float: right; +} + +p.sidebar-title { + font-weight: bold; +} + +/* -- topics ---------------------------------------------------------------- */ + +div.topic { + border: 1px solid #ccc; + padding: 7px 7px 0 7px; + margin: 10px 0 10px 0; +} + +p.topic-title { + font-size: 1.1em; + font-weight: bold; + margin-top: 10px; +} + +/* -- admonitions ----------------------------------------------------------- */ + +div.admonition { + margin-top: 10px; + margin-bottom: 10px; + padding: 7px; +} + +div.admonition dt { + font-weight: bold; +} + +div.admonition dl { + margin-bottom: 0; +} + +p.admonition-title { + margin: 0px 10px 5px 0px; + font-weight: bold; +} + +div.body p.centered { + text-align: center; + margin-top: 25px; +} + +/* -- tables ---------------------------------------------------------------- */ + +table.docutils { + border: 0; + border-collapse: collapse; +} + +table.docutils td, table.docutils th { + padding: 1px 8px 1px 5px; + border-top: 0; + border-left: 0; + border-right: 0; + border-bottom: 1px solid #aaa; +} + +table.field-list td, table.field-list th { + border: 0 !important; +} + +table.footnote td, table.footnote th { + border: 0 !important; +} + +th { + text-align: left; + padding-right: 5px; +} + +table.citation { + border-left: solid 1px gray; + margin-left: 1px; +} + +table.citation td { + border-bottom: none; +} + +/* -- other body styles ----------------------------------------------------- */ + +ol.arabic { + list-style: decimal; +} + +ol.loweralpha { + list-style: lower-alpha; +} + +ol.upperalpha { + list-style: upper-alpha; +} + +ol.lowerroman { + list-style: lower-roman; +} + +ol.upperroman { + list-style: upper-roman; +} + +dl { + margin-bottom: 15px; +} + +dd p { + margin-top: 0px; +} + +dd ul, dd table { + margin-bottom: 10px; +} + +dd { + margin-top: 3px; + margin-bottom: 10px; + margin-left: 30px; +} + +dt:target, .highlighted { + background-color: #fbe54e; +} + +dl.glossary dt { + font-weight: bold; + font-size: 1.1em; +} + +.field-list ul { + margin: 0; + padding-left: 1em; +} + +.field-list p { + margin: 0; +} + +.refcount { + color: #060; +} + +.optional { + font-size: 1.3em; +} + +.versionmodified { + font-style: italic; +} + +.system-message { + background-color: #fda; + padding: 5px; + border: 3px solid red; +} + +.footnote:target { + background-color: #ffa; +} + +.line-block { + display: block; + margin-top: 1em; + margin-bottom: 1em; +} + +.line-block .line-block { + margin-top: 0; + margin-bottom: 0; + margin-left: 1.5em; +} + +.guilabel, .menuselection { + font-family: sans-serif; +} + +.accelerator { + text-decoration: underline; +} + +.classifier { + font-style: oblique; +} + +/* -- code displays --------------------------------------------------------- */ + +pre { + overflow: auto; + overflow-y: hidden; /* fixes display issues on Chrome browsers */ +white-space : pre-wrap; /*for Mozilla*/ +word-wrap: break-word; /*for IE*/ +} + +td.linenos pre { + padding: 5px 0px; + border: 0; + background-color: transparent; + color: #aaa; +} + +table.highlighttable { + margin-left: 0.5em; +} + +table.highlighttable td { + padding: 0 0.5em 0 0.5em; +} + +tt.descname { + background-color: transparent; + font-weight: bold; + font-size: 1.2em; +} + +tt.descclassname { + background-color: transparent; +} + +tt.xref, a tt { + background-color: transparent; + font-weight: bold; +} + +h1 tt, h2 tt, h3 tt, h4 tt, h5 tt, h6 tt { + background-color: transparent; +} + +.viewcode-link { + float: right; +} + +.viewcode-back { + float: right; + font-family: sans-serif; +} + +div.viewcode-block:target { + margin: -1px -10px; + padding: 0 10px; +} + +/* -- math display ---------------------------------------------------------- */ + +img.math { + vertical-align: middle; +} + +div.math p { + text-align: center; +} + +span.eqno { + float: right; +} + +/* -- printout stylesheet --------------------------------------------------- */ + +@media print { + div.document, + div.documentwrapper, + div.bodywrapper { + margin: 0 !important; + width: 100%; + } + + div.sphinxsidebar, + div.related, + div.footer, + #top-link { + display: none; + } +} diff --git a/doc/advanced/content/_static/sphinxdoc.css b/doc/advanced/content/_static/sphinxdoc.css new file mode 100644 index 00000000..62ff7fd6 --- /dev/null +++ b/doc/advanced/content/_static/sphinxdoc.css @@ -0,0 +1,315 @@ +/* + * sphinxdoc.css_t + * ~~~~~~~~~~~~~~~ + * + * Sphinx stylesheet -- sphinxdoc theme. Originally created by + * Armin Ronacher for Werkzeug. + * + * :copyright: Copyright 2007-2011 by the Sphinx team, see AUTHORS. + * :license: BSD, see LICENSE for details. + * + */ + +@import url("basic.css"); + +/* -- page layout ----------------------------------------------------------- */ + +body { + color: black; + padding: 0; + margin: 0px 80px 0px 80px; + min-width: 740px; +} + +div.document { + + text-align: left; + + +} + +div.bodywrapper { + margin: 0 240px 0 0; + border-right: 1px solid #ccc; +} + +div.body { + margin: 0; + padding: 0.5em 20px 20px 20px; +} + +div.related { + font-size: 1em; +} + +div.related ul { + background-image: url(navigation.png); + height: 2em; + border-top: 1px solid #ddd; + border-bottom: 1px solid #ddd; +} + +div.related ul li { + margin: 0; + padding: 0; + height: 2em; + float: left; +} + +div.related ul li.right { + float: right; + margin-right: 5px; +} + +div.related ul li a { + margin: 0; + padding: 0 5px 0 5px; + line-height: 1.75em; + color: #EE9816; +} + +div.related ul li a:hover { + color: #3CA8E7; +} + +div.sphinxsidebarwrapper { + padding: 0; +} + +div.sphinxsidebar { + margin: 0; + padding: 0.5em 15px 15px 0; + width: 210px; + float: right; + font-size: 1em; + text-align: left; +} + +div.sphinxsidebar h3, div.sphinxsidebar h4 { + margin: 1em 0 0.5em 0; + font-size: 1em; + padding: 0.1em 0 0.1em 0.5em; + color: white; + border: 1px solid #86989B; + background-color: #AFC1C4; +} + +div.sphinxsidebar h3 a { + color: white; +} + +div.sphinxsidebar ul { + padding-left: 1.5em; + margin-top: 7px; + padding: 0; + line-height: 130%; +} + +div.sphinxsidebar ul ul { + margin-left: 20px; +} + +div.footer { + background-color: #E3EFF1; + color: #86989B; + padding: 3px 8px 3px 0; + clear: both; + font-size: 0.8em; + text-align: right; +} + +div.footer a { + color: #86989B; + text-decoration: underline; +} + +/* -- body styles ----------------------------------------------------------- */ + +p { + margin: 0.8em 0 0.5em 0; +} + +div.body a { + text-decoration: underline; +} + +h2 { +/* color: #11557C;*/ + margin: 1.3em 0 0.2em 0; + font-size: 1.35em; + padding: 0; +} + +h3 { + margin: 1em 0 -0.3em 0; + font-size: 1.2em; +} + +div.body h1 a, div.body h2 a, div.body h3 a, div.body h4 a, div.body h5 a, div.body h6 a { + color: black!important; +} + +h1 a.anchor, h2 a.anchor, h3 a.anchor, h4 a.anchor, h5 a.anchor, h6 a.anchor { + display: none; + margin: 0 0 0 0.3em; + padding: 0 0.2em 0 0.2em; + color: #aaa!important; +} + +h1:hover a.anchor, h2:hover a.anchor, h3:hover a.anchor, h4:hover a.anchor, +h5:hover a.anchor, h6:hover a.anchor { + display: inline; +} + +h1 a.anchor:hover, h2 a.anchor:hover, h3 a.anchor:hover, h4 a.anchor:hover, +h5 a.anchor:hover, h6 a.anchor:hover { + color: #777; + background-color: #eee; +} + +a.headerlink { + color: #c60f0f!important; + font-size: 1em; + margin-left: 6px; + padding: 0 4px 0 4px; + text-decoration: none!important; +} + +a.headerlink:hover { + background-color: #ccc; + color: white!important; +} + +cite, code, tt { + font-family: 'Consolas', 'Deja Vu Sans Mono', + 'Bitstream Vera Sans Mono', monospace; + font-size: 0.95em; + letter-spacing: 0.01em; +} + +tt { + background-color: #f2f2f2; + border-bottom: 1px solid #ddd; + color: #333; +} + +tt.descname, tt.descclassname, tt.xref { + border: 0; +} + +hr { + border: 1px solid #abc; + margin: 2em; +} + +a tt { + border: 0; + color: #CA7900; +} + +a tt:hover { + color: #2491CF; +} + +pre { + font-family: 'Consolas', 'Deja Vu Sans Mono', + 'Bitstream Vera Sans Mono', monospace; + font-size: 0.95em; + letter-spacing: 0.015em; + line-height: 120%; + padding: 0.5em; + border: 1px solid #ccc; + background-color: #f8f8f8; +} + +pre a { + color: inherit; + text-decoration: underline; +} + +td.linenos pre { + padding: 0.5em 0; +} + +div.quotebar { + background-color: #f8f8f8; + max-width: 250px; + float: right; + padding: 2px 7px; + border: 1px solid #ccc; +} + +div.topic { + background-color: #f8f8f8; +} + +table { + border-collapse: collapse; + margin: 0 -0.5em 0 -0.5em; +} + +table td, table th { + padding: 0.2em 0.5em 0.2em 0.5em; +} + +div.admonition, div.warning { + font-size: 0.9em; + margin: 1em 0 1em 0; + border: 1px solid #86989B; + background-color: #f7f7f7; + padding: 0; +} + +div.admonition p, div.warning p { + margin: 0.5em 1em 0.5em 1em; + padding: 0; +} + +div.admonition pre, div.warning pre { + margin: 0.4em 1em 0.4em 1em; +} + +div.admonition p.admonition-title, +div.warning p.admonition-title { + margin: 0; + padding: 0.1em 0 0.1em 0.5em; + color: white; + border-bottom: 1px solid #86989B; + font-weight: bold; + background-color: #AFC1C4; +} + +div.warning { + border: 1px solid #940000; +} + +div.warning p.admonition-title { + background-color: #CF0000; + border-bottom-color: #940000; +} + +div.admonition ul, div.admonition ol, +div.warning ul, div.warning ol { + margin: 0.1em 0.5em 0.5em 3em; + padding: 0; +} + +div.versioninfo { + margin: 1em 0 0 0; + border: 1px solid #ccc; + background-color: #DDEAF0; + padding: 8px; + line-height: 1.3em; + font-size: 0.9em; +} + +.viewcode-back { + font-family: 'Lucida Grande', 'Lucida Sans Unicode', 'Geneva', + 'Verdana', sans-serif; +} + +div.viewcode-block:target { + background-color: #f4debf; + border-top: 1px solid #ac9; + border-bottom: 1px solid #ac9; +} diff --git a/doc/advanced/content/_templates/layout.html b/doc/advanced/content/_templates/layout.html index 5603ce5f..0516be55 100644 --- a/doc/advanced/content/_templates/layout.html +++ b/doc/advanced/content/_templates/layout.html @@ -1,8 +1,47 @@ + + + +Documentation - Point Cloud Library (PCL) + {% extends "!layout.html" %} {% block extrahead %} +initialize('web'); + +$snip = $modx->runSnippet("getSiteNavigation", array('id'=>5, 'phLevels'=>'sitenav.level0,sitenav.level1', 'showPageNav'=>'n')); +$chunkOutput = $modx->getChunk("site-header", array('sitenav'=>$snip)); +$bodytag = str_replace("[[+showSubmenus:notempty=`", "", $chunkOutput); +$bodytag = str_replace("`]]", "", $bodytag); +echo $bodytag; +echo "\n"; +?> +
+

Documentation

+ +
+
{% endblock %} +{% block relbar1 %}{% endblock %} +{% block relbar2 %}{% endblock %} {% block rootrellink %}{% endblock %} {% block sidebarsearch %}{% endblock %} + +{% block footer %} +
+ +getChunk("site-footer"); +echo $chunkOutput; +?> +{% endblock %} + + diff --git a/doc/advanced/content/branches_repository.rst b/doc/advanced/content/branches_repository.rst deleted file mode 100644 index 2b5c0c9e..00000000 --- a/doc/advanced/content/branches_repository.rst +++ /dev/null @@ -1,48 +0,0 @@ -.. _branches_repository: - -How to commit concurrently to trunk and branches ------------------------------------------------- - -Most often as a developer you will have to deal with the problem of either applying a commit to a particular branch, or pushing a modification that you just made on your machine to more than one branch. This simple example will show you one of the many ways of doing this. - -The example is given for a piece of code/patch that has to be committed to both **trunk** and one of the **branches** (1.x in this case). - -Before continuing, make sure your code compiles and tests carefully, and follows the indentation guidelines/C++ programming style (see :ref:`pcl_style_guide`). Then, follow this set of simple steps: - - 1. In case you checked out only one of the branches or trunk, go ahead and check out **svn+ssh://svn@svn.pointclouds.org/pcl** instead. As a developer you shouldn't worry too much for the extra space that the tags/branches will consume on your hard drive, but if you do, visit **http://svn.pointclouds.org** and check out all the branches that you want to work on individually. For the former:: - - svn+ssh://svn@svn.pointclouds.org/pcl pcl - - - This will give you both trunk, and all the branches, and tags. - - 2. Assuming that you're working in trunk, go ahead and do the following:: - - - cd pcl/trunk # change directory to where trunk is - svn st -q # check to see what the commit queue contains - svn diff . > /tmp/a.patch # obtain a patch of your code - - At this point */tmp/a.patch* will contain the patch that has to be pushed in the repository. - - 3. Change directory to the branch that you want to commit the same patch to, and do:: - - - cd pcl/branches/pcl-1.x # change directory to where the branch is - patch -p0 < /tmp/a.patch # apply the patch - - - One problem that we will have to deal with now is adding new files that have been added by your patch:: - - svn st # check to see which files have "?" in the branch but should have "A" instead - svn add # add all files that need to be added - - 4. If possible, commit both patches at the same time. Here, we're assuming that branches/pcl-1.x and trunk have the same root directory:: - - cd pcl # change directory to where trunk+branches is - svn st -q - svn commit branches/ trunk/ - - Alternatively, you can perform two individual commits in steps 2 and 3. - - diff --git a/doc/advanced/content/c_cache.rst b/doc/advanced/content/c_cache.rst index fb2864bf..21da47ba 100644 --- a/doc/advanced/content/c_cache.rst +++ b/doc/advanced/content/c_cache.rst @@ -24,7 +24,7 @@ but it actually runs the equivalent of 'ccache g++'. Using colorgcc to colorize output --------------------------------- -`colorgcc`_ is a colorizer for the output +`colorgcc `_ is a colorizer for the output of GCC, and allows you to better interpret the compiler warnings/errors. To enable both colorgcc and ccache, perform the following steps: diff --git a/doc/advanced/content/distcc.rst b/doc/advanced/content/distcc.rst index e254706b..59ce2984 100644 --- a/doc/advanced/content/distcc.rst +++ b/doc/advanced/content/distcc.rst @@ -56,6 +56,11 @@ In each distributed build environment, there are usually two different roles: [pcl] $ mkdir build && cd build [pcl/build] $ CC="distcc gcc" CXX="distcc g++" cmake .. + Sometimes compiling on systems supporting different SSE extensions will lead + to problems. Setting PCL_ENABLE_SSE to false will solve this, like:: + + [pcl/build] $ CC="distcc gcc" CXX="distcc g++" cmake -DPCL_ENABLE_SSE:BOOL=FALSE ../pcl + The output of ``CC="distcc gcc" CXX="distcc g++" cmake ..`` will generate something like this. Please note that this is just an example and that the messages might vary depending on your operating system and the way your diff --git a/doc/advanced/content/files/PCL_eclipse_profile.xml b/doc/advanced/content/files/PCL_eclipse_profile.xml index 25dab64b..5b76b8f2 100644 --- a/doc/advanced/content/files/PCL_eclipse_profile.xml +++ b/doc/advanced/content/files/PCL_eclipse_profile.xml @@ -1,54 +1,54 @@ - + - + - + - + - + - - + + - + - + - - + + - - + + - + @@ -56,102 +56,103 @@ - - + + + - - + + + + - - - - + + - + - + - + - - + + - - + + - - + + - + - + - - + + - + - - + + - + - - + + - - - + + + - + diff --git a/doc/advanced/content/how_to_write_a_tutorial.rst b/doc/advanced/content/how_to_write_a_tutorial.rst index 9437a731..4b1f5b1b 100644 --- a/doc/advanced/content/how_to_write_a_tutorial.rst +++ b/doc/advanced/content/how_to_write_a_tutorial.rst @@ -25,8 +25,8 @@ beautiful HTML documents. Both documentation sources are stored in our `Source repository -`_ and the web pages are generated hourly by -our server via `crontab` jobs. +`_ and the web pages are generated +hourly by our server via `crontab` jobs. In the next two sections we will address both of the above, and present a small example for each. We'll begin with the easiest of the two: adding a new @@ -44,12 +44,12 @@ you read the following resources: * http://www.siafoo.net/help/reST - has a nice tutorial/set of examples Once you understand how reST works, look over our current set of tutorials for -examples at http://svn.pointclouds.org/pcl/trunk/doc/tutorials/content/. +examples at https://github.com/PointCloudLibrary/pcl/tree/master/doc/tutorials/content. To add a new tutorial, simply create a new file, and send it to us together with the images/videos that you want included in the tutorial. The best way to -do this is to login to http://dev.pointclouds.org, create an issue on the -tracker, and add the new tutorial as an attachement. +do this is to login to https://github.com/PointCloudLibrary/pcl and send it as +a pull request. Improving the API documentation @@ -69,7 +69,7 @@ To help us improve the API documentation, all that you need to do is simply check out the source code of PCL (we recommend trunk if you're going to start editing the sources), like:: - svn co http://svn.pointclouds.org/pcl/trunk pcl + git clone https://github.com/PointCloudLibrary/pcl Then, edit the file containing the function/class that you want to improve the documentation for, say *common/include/pcl/point_cloud.h*, and go to the @@ -81,11 +81,7 @@ element that you want to improve. Let's take *points* for example:: What you have to modify is the Doxygen-style comment starting with /\*\* and ending with \*/. See http://www.doxygen.org for more information. -To send us the modification, please login to http://dev.pointclouds.org, create -an issue on the tracker, and add the output of the following command to the -issue:: - - svn diff common/include/pcl/point_cloud.h +To send us the modification, please send a pull request through Github. Testing the modified API documentation -------------------------------------- diff --git a/doc/advanced/content/index.rst b/doc/advanced/content/index.rst index 7332a08b..99716d5a 100644 --- a/doc/advanced/content/index.rst +++ b/doc/advanced/content/index.rst @@ -65,9 +65,9 @@ development that everyone should follow: .. topic:: Rules - * if you make any commits, please **_add the commit log_** or something similar **_to + * if you make important commits, please **_add the commit log_** or something similar **_to the changelist page_** - (http://dev.pointclouds.org/projects/pcl/wiki/ChangeList); + (https://github.com/PointCloudLibrary/pcl/blob/master/CHANGES.md); * if you change anything in an existing algorithm, **_make sure that there are unit tests_** for it and **_make sure that they pass before you commit_** the code; @@ -92,17 +92,12 @@ development that everyone should follow: Short documentation on how to add new, throw and handle exceptions in PCL. -* :ref:`branches_repository` - - If you're not sure how to best make concurrent commits to the repository to both **trunk**, and any of the existing **branches**, see this example. - - * :ref:`pcl2` An in-depth discussion about the PCL 2.x API can be found here. -Commiting changes to trunk --------------------------- +Commiting changes to the git master +----------------------------------- In order to oversee the commit messages more easier and that the changelist looks homogenous please keep the following format: "* X in @@ (#)" @@ -116,20 +111,10 @@ Improving the PCL documentation documentation and tutorials/examples, please read our short guide on how to start. -.. - Profiling your code +How to build a minimal example +------------------------------ +* :ref:`minimal_example` -Contents --------- - -.. toctree:: - - c_cache - distcc - compiler_optimizations - single_compile_unit - pcl_style_guide - branches_repository - how_to_write_a_tutorial + In case you need help to debug your code, please follow this guidelines to write a minimal example. diff --git a/doc/advanced/content/pcl2.rst b/doc/advanced/content/pcl2.rst index ae5faba3..27a27cd0 100644 --- a/doc/advanced/content/pcl2.rst +++ b/doc/advanced/content/pcl2.rst @@ -41,7 +41,6 @@ Proposals for the 2.x API: * make sure we can access a slice of the data as a *2D image*, thus allowing fast 2D displaying, [u, v] operations, etc * make sure we can access a slice of the data as a subpoint cloud: only certain points are chosen from the main point cloud * implement channels (of a single type!) as data holders, e.g.: - * cloud["xyz"] => gets all 3D x,y,z data * cloud["normals"] => gets all surface normal data * etc @@ -71,7 +70,6 @@ Proposals for the 2.x API: pos_space = ( "float with euclidean 2-norm distance", { "x", "y", "z" }, [[(0.3,0,1.3) , ... , (1.2,3.1,2)], ... , [(1,0.3,1) , ... , (2,0,3.5)] ) color_space = ( "uint8 with rgb distance", { "r", "g", "b" }, [[(0,255,0), ... , (128,255,32)] ... [(12,54,31) ... (255,0,192)]] ) - 1.2 PointTypes ^^^^^^^^^^^^^^ @@ -114,7 +112,7 @@ Anything involving a slice of data should use size_t for indices and not int. E. 1.6 RANSAC ^^^^^^^^^^ - * Renaming the functions and internal variables: everything should be named with _src and _tgt: we have confusing names like indices_ and indices_tgt_ (and no indices_src_), setInputCloud and setInputTarget (duuh, everything is an input, it should be setTarget, setSource), in the code, a sample is named: selection, model_ and samples. getModelCoefficients is confusing with getModel (this one should be getBestSample). + * Renaming the functions and internal variables: everything should be named with _src and _tgt: we have confusing names like \indices_ and \indices_tgt_ (and no \indices_src_), setInputCloud and setInputTarget (duuh, everything is an input, it should be setTarget, setSource), in the code, a sample is named: selection, \model_ and samples. getModelCoefficients is confusing with getModel (this one should be getBestSample). * no const-correctness all over, it's pretty scary: all the get should be const, selectWithinDistance and so on too. * the getModel, getInliers function should not force you to fill a vector: you should just return a const reference to the internal vector: that could allow you to save a useless copy * some private members should be made protected in the sub sac models (like sac_model_registration) so that we can inherit from them. diff --git a/doc/advanced/content/pcl_reg_eval.rst b/doc/advanced/content/pcl_reg_eval.rst index ae1b4cca..9102e4d0 100644 --- a/doc/advanced/content/pcl_reg_eval.rst +++ b/doc/advanced/content/pcl_reg_eval.rst @@ -7,23 +7,23 @@ Data generation =============== - synthetic data - real word data (how to get ground truth?) - - Kinect - - PR2 laser scanner - - SICK laser data - - small range 3D scanner - - mid range 3D scanner (Faro) - - high end 3D scanner (Riegl, Velodyne) + - Kinect + - PR2 laser scanner + - SICK laser data + - small range 3D scanner + - mid range 3D scanner (Faro) + - high end 3D scanner (Riegl, Velodyne) - Point Types - - 2D(?) - - 3D - - RGB + - 2D(?) + - 3D + - RGB - dynamics - - static scans - - scanning while driving (e.g. robots) + - static scans + - scanning while driving (e.g. robots) - size - - room - - building - - outdoor (street) + - room + - building + - outdoor (street) Architecture ============ @@ -41,12 +41,14 @@ ICP ^^^ - how does the algorithm cope with outliers - how are the point pairs evaluated: - - does it use normal or RGB information - - does it weight the pairs differently - - which kind of point pairs are used: - - one-to-one - - one-to-many - - many-to-many + + - does it use normal or RGB information + - does it weight the pairs differently + - which kind of point pairs are used: + + - one-to-one + - one-to-many + - many-to-many Similar Projects ================ diff --git a/doc/advanced/content/pcl_registration.rst b/doc/advanced/content/pcl_registration.rst index c00038b2..9f203a4b 100644 --- a/doc/advanced/content/pcl_registration.rst +++ b/doc/advanced/content/pcl_registration.rst @@ -60,7 +60,7 @@ Graph ^^^^^ This should hold the SLAM graph. I would propose to use Boost::Graph for it, as it allows us to access a lot of algorithms. -.. todo:: +.. note:: define abstract structure. @@ -159,7 +159,7 @@ GraphHandler void addConstraint (Graph &gr, PointCloud &from, PointCloud &to, Pose &pose); } -.. todo:: +.. note:: I'm not sure about this one. diff --git a/doc/advanced/content/pcl_style_guide.rst b/doc/advanced/content/pcl_style_guide.rst index 8869b27e..e3c0a546 100644 --- a/doc/advanced/content/pcl_style_guide.rst +++ b/doc/advanced/content/pcl_style_guide.rst @@ -336,7 +336,7 @@ download it to some known location and then: * open .emacs * add the following before any C/C++ custom hooks -.. code-block:: lisp +:: (load-file "/location/to/pcl-c-style.el") (add-hook 'c-mode-common-hook 'pcl-set-c-style) @@ -350,8 +350,37 @@ You can find a semi-finished config for `Uncrustify `_. +| To add the new formatting style go to: Windows > Preferences > C/C++ > Code Style > Formatter + +| To format portion of codes, select the code and press Ctrl + Shift + F. +| If you want to format the whole code in your project go to the tree and right click on the project: Source > Format. + +Note that the Eclipse formatter style is configured to wrap all arguments in a function, feel free to re-arange the arguments if you feel the need; for example, +this improves readability: + +.. code-block:: cpp + + int + displayPoint (float x, float y, float z, + float r, float g, float b + ); + +This eclipse formatter fails to add a space before brackets when using PCL macros: + +.. code-block:: cpp + + PCL_ERROR("Text\n"); + +should be + +.. code-block:: cpp + + PCL_ERROR ("Text\n"); + +.. note:: + + This style sheet is not perfect, please mention errors on the user mailing list and feel free to patch! 3. Structuring ============== diff --git a/doc/advanced/content/vertical_sse.rst b/doc/advanced/content/vertical_sse.rst index cfb0902c..de4bcab8 100644 --- a/doc/advanced/content/vertical_sse.rst +++ b/doc/advanced/content/vertical_sse.rst @@ -677,9 +677,9 @@ clouds. The point clouds used are: -* `capture000X.pcd `_ -* `table_scene_mug_stereo_textured.pcd `_ -* `table_scene_lms400.pcd `_ +* `capture000X.pcd `_ +* `table_scene_mug_stereo_textured.pcd `_ +* `table_scene_lms400.pcd `_ ``capture0001.pcd`` (organized, 640x480, 57553 NaNs):: diff --git a/doc/doxygen/CMakeLists.txt b/doc/doxygen/CMakeLists.txt index 1ec72af1..f4660ffe 100644 --- a/doc/doxygen/CMakeLists.txt +++ b/doc/doxygen/CMakeLists.txt @@ -1,5 +1,13 @@ +if(WITH_DOCS) + find_package(Doxygen) + if(DOXYGEN_FOUND) + find_package(HTMLHelp) + endif(DOXYGEN_FOUND) +endif(WITH_DOCS) + message(STATUS "DOXYGEN_FOUND ${DOXYGEN_FOUND}") message(STATUS "HTML_HELP_COMPILER ${HTML_HELP_COMPILER}") + if(DOXYGEN_FOUND) if(HTML_HELP_COMPILER) set(DOCUMENTATION_HTML_HELP YES) @@ -12,15 +20,22 @@ if(DOXYGEN_FOUND) set(HAVE_DOT NO) endif(DOXYGEN_DOT_EXECUTABLE) + option(DOXYGEN_USE_SHORT_NAMES "Generate shorter (but less readable) file names" OFF) + if(DOXYGEN_USE_SHORT_NAMES) + set(SHORT_NAMES YES) + else(DOXYGEN_USE_SHORT_NAMES) + set(SHORT_NAMES NO) + endif(DOXYGEN_USE_SHORT_NAMES) + set(STRIPPED_HEADERS "${PCL_SOURCE_DIR}/${PCL_MODULES_NAMES}/include") string(REPLACE ";" "/include \\\n\t\t\t\t\t\t\t\t\t\t\t\t ${PCL_SOURCE_DIR}/" STRIPPED_HEADERS "${STRIPPED_HEADERS}") set(DOC_SOURCE_DIR "\"${PCL_SOURCE_DIR}\"\\") file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html") set(doxyfile "${CMAKE_CURRENT_BINARY_DIR}/doxyfile") - configure_file("${CMAKE_CURRENT_SOURCE_DIR}/doxyfile.in" ${doxyfile}) - add_custom_target(doc ${DOXYGEN_EXECUTABLE} ${doxyfile}) + configure_file("${CMAKE_CURRENT_SOURCE_DIR}/doxyfile.in" "${doxyfile}") + add_custom_target(doc ALL "${DOXYGEN_EXECUTABLE}" "${doxyfile}") if(USE_PROJECT_FOLDERS) - set_target_properties(doc PROPERTIES FOLDER "Documentation") + set_target_properties(doc PROPERTIES FOLDER "Documentation (Doxygen)") endif(USE_PROJECT_FOLDERS) if(DOCUMENTATION_HTML_HELP STREQUAL YES) install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html" diff --git a/doc/doxygen/doxyfile.in b/doc/doxygen/doxyfile.in index e96587ff..a51b59d4 100644 --- a/doc/doxygen/doxyfile.in +++ b/doc/doxygen/doxyfile.in @@ -27,7 +27,7 @@ INLINE_INHERITED_MEMB = NO FULL_PATH_NAMES = YES STRIP_FROM_PATH = STRIP_FROM_INC_PATH = @STRIPPED_HEADERS@ -SHORT_NAMES = YES +SHORT_NAMES = @SHORT_NAMES@ JAVADOC_AUTOBRIEF = YES QT_AUTOBRIEF = NO MULTILINE_CPP_IS_BRIEF = NO @@ -51,7 +51,6 @@ SUBGROUPING = YES INLINE_GROUPED_CLASSES = NO INLINE_SIMPLE_STRUCTS = NO TYPEDEF_HIDES_STRUCT = NO -SYMBOL_CACHE_SIZE = 0 LOOKUP_CACHE_SIZE = 0 #--------------------------------------------------------------------------- @@ -88,7 +87,6 @@ ENABLED_SECTIONS = MAX_INITIALIZER_LINES = 30 SHOW_USED_FILES = YES SHOW_FILES = NO -SHOW_DIRECTORIES = NO SHOW_NAMESPACES = YES FILE_VERSION_FILTER = LAYOUT_FILE = "@PCL_SOURCE_DIR@/doc/doxygen/doxygen_layout.xml" @@ -115,8 +113,7 @@ FILE_PATTERNS = *.h \ *.hpp \ *.doxy RECURSIVE = YES -EXCLUDE = */.svn \ - "@PCL_SOURCE_DIR@/cmake" \ +EXCLUDE = "@PCL_SOURCE_DIR@/cmake" \ "@PCL_SOURCE_DIR@/3rdparty" \ "@PCL_SOURCE_DIR@/test" \ "@PCL_SOURCE_DIR@/android" \ @@ -169,9 +166,9 @@ HTML_HEADER = HTML_FOOTER = @PCL_SOURCE_DIR@/doc/doxygen/footer.html HTML_STYLESHEET = HTML_EXTRA_FILES = -HTML_COLORSTYLE_HUE = 220 -HTML_COLORSTYLE_SAT = 100 -HTML_COLORSTYLE_GAMMA = 80 +HTML_COLORSTYLE_HUE = 87 +HTML_COLORSTYLE_SAT = 46 +HTML_COLORSTYLE_GAMMA = 73 HTML_TIMESTAMP = YES HTML_DYNAMIC_SECTIONS = YES GENERATE_DOCSET = YES @@ -277,9 +274,10 @@ EXPAND_ONLY_PREDEF = YES SEARCH_INCLUDES = YES INCLUDE_PATH = INCLUDE_FILE_PATTERNS = *.h -#PREDEFINED = protected=private \ -PREDEFINED = "HAVE_QHULL=1" \ - "HAVE_OPENNI=1" +#PREDEFINED = protected=private \ +PREDEFINED = = "HAVE_QHULL=1" \ + "HAVE_OPENNI=1" \ + "PCL_DEPRECATED(x)=" EXPAND_AS_DEFINED = SKIP_FUNCTION_MACROS = YES diff --git a/doc/doxygen/doxygen_layout.xml b/doc/doxygen/doxygen_layout.xml index 5cf6db94..fb664ebe 100644 --- a/doc/doxygen/doxygen_layout.xml +++ b/doc/doxygen/doxygen_layout.xml @@ -18,7 +18,6 @@ - diff --git a/doc/doxygen/pcl.doxy b/doc/doxygen/pcl.doxy index 8f873cce..0e273fff 100644 --- a/doc/doxygen/pcl.doxy +++ b/doc/doxygen/pcl.doxy @@ -29,9 +29,8 @@ Please visit http://www.pointclouds.org for more information.

Quick Links

  • Main website: http://www.pointclouds.org
    • -
  • Developer Zone: http://dev.pointclouds.org
    • -
  • SVN Repository: http://svn.pointclouds.org
    • -
  • Build farm: http://build.pointclouds.org
    • +
  • Developer Zone: https://github.com/PointCloudLibrary
    • +
  • Build farm: https://travis-ci.org/PointCloudLibrary/pcl

References

diff --git a/doc/overview/Makefile b/doc/overview/Makefile deleted file mode 100644 index 46fe7754..00000000 --- a/doc/overview/Makefile +++ /dev/null @@ -1,6 +0,0 @@ -.PHONY: html - -html: - -rm -rf html /tmp/doctrees - sphinx-build -b html -a -d /tmp/doctrees content html - diff --git a/doc/overview/content/_templates/layout.html b/doc/overview/content/_templates/layout.html deleted file mode 100644 index 5603ce5f..00000000 --- a/doc/overview/content/_templates/layout.html +++ /dev/null @@ -1,8 +0,0 @@ -{% extends "!layout.html" %} - -{% block extrahead %} -{% endblock %} - -{% block rootrellink %}{% endblock %} - -{% block sidebarsearch %}{% endblock %} diff --git a/doc/overview/content/conf.py b/doc/overview/content/conf.py deleted file mode 100644 index 3c6ae9e2..00000000 --- a/doc/overview/content/conf.py +++ /dev/null @@ -1,135 +0,0 @@ -# All configuration values have a default; values that are commented out -# serve to show the default. - -import sys, os - -# -- General configuration ----------------------------------------------------- -# Add any Sphinx extension module names here, as strings. They can be extensions -# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. -extensions = [] - -# Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] - -# The suffix of source filenames. -source_suffix = '.rst' - -# The master toctree document. -master_doc = 'index' - -# General information about the project. -project = u'PCL' -copyright = '' - -# The version info for the project you're documenting, acts as replacement for -# |version| and |release|, also used in various other places throughout the -# built documents. -# -# The short X.Y version. -version = '0.0' -# The full version, including alpha/beta/rc tags. -release = '0.0' - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -#language = None - -# There are two options for replacing |today|: either, you set today to some -# non-false value, then it is used: -#today = '' -# Else, today_fmt is used as the format for a strftime call. -#today_fmt = '%B %d, %Y' - -# List of documents that shouldn't be included in the build. -#unused_docs = [] - -# List of directories, relative to source directory, that shouldn't be searched -# for source files. -exclude_trees = [] - -# The reST default role (used for this markup: `text`) to use for all documents. -default_role = None - -# If true, '()' will be appended to :func: etc. cross-reference text. -#add_function_parentheses = True - -# If true, the current module name will be prepended to all description -# unit titles (such as .. function::). -add_module_names = False - -# If true, sectionauthor and moduleauthor directives will be shown in the -# output. They are ignored by default. -show_authors = False - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = 'sphinx' - -# A list of ignored prefixes for module index sorting. -#modindex_common_prefix = [] - -# -- Options for HTML output --------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. Major themes that come with -# Sphinx are currently 'default' and 'sphinxdoc'. -html_theme = 'sphinxdoc' - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# html_theme_options = { 'rightsidebar' : 'true' } - -# Add any paths that contain custom themes here, relative to this directory. -html_theme_path = ['.'] - -# The name for this set of Sphinx documents. If None, it defaults to -# " v documentation". -html_title = None - -# A shorter title for the navigation bar. Default is the same as html_title. -html_short_title = 'Home' - -# The name of an image file (within the static path) to use as favicon of the -# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 -# pixels large. -html_favicon = None - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] - -# If true, SmartyPants will be used to convert quotes and dashes to -# typographically correct entities. -#html_use_smartypants = True - -# If false, no module index is generated. -html_use_modindex = False - -# If false, no index is generated. -html_use_index = False - -# If true, the index is split into individual pages for each letter. -html_split_index = False - -# If true, links to the reST sources are added to the pages. -html_show_sourcelink = False - -# If true, an OpenSearch description file will be output, and all pages will -# contain a tag referring to it. The value of this option must be the -# base URL from which the finished HTML is served. -#html_use_opensearch = '' - -# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). -html_file_suffix = '.html' - -html_sidebars = { - '**': [], - 'using/windows': [], -} -html_show_copyright = False -html_show_sphinx = False -html_add_permalinks = None -needs_sphinx = 1.0 -file_insertion_enabled = True -raw_enabled = True - diff --git a/doc/overview/content/images/visualization/bunny.jpg b/doc/overview/content/images/visualization/bunny.jpg deleted file mode 100644 index c3738896..00000000 Binary files a/doc/overview/content/images/visualization/bunny.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/ex1.jpg b/doc/overview/content/images/visualization/ex1.jpg deleted file mode 100644 index 8a61fa8b..00000000 Binary files a/doc/overview/content/images/visualization/ex1.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/ex2.jpg b/doc/overview/content/images/visualization/ex2.jpg deleted file mode 100644 index 00699df9..00000000 Binary files a/doc/overview/content/images/visualization/ex2.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/ex3.jpg b/doc/overview/content/images/visualization/ex3.jpg deleted file mode 100644 index 1695e644..00000000 Binary files a/doc/overview/content/images/visualization/ex3.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/ex4.jpg b/doc/overview/content/images/visualization/ex4.jpg deleted file mode 100644 index b22a821b..00000000 Binary files a/doc/overview/content/images/visualization/ex4.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/ex5.jpg b/doc/overview/content/images/visualization/ex5.jpg deleted file mode 100644 index 1cc3ac02..00000000 Binary files a/doc/overview/content/images/visualization/ex5.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/histogram.jpg b/doc/overview/content/images/visualization/histogram.jpg deleted file mode 100644 index 6951cf7a..00000000 Binary files a/doc/overview/content/images/visualization/histogram.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/normals.jpg b/doc/overview/content/images/visualization/normals.jpg deleted file mode 100644 index ae100129..00000000 Binary files a/doc/overview/content/images/visualization/normals.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/pcs.jpg b/doc/overview/content/images/visualization/pcs.jpg deleted file mode 100644 index 6f222a02..00000000 Binary files a/doc/overview/content/images/visualization/pcs.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/range_image.jpg b/doc/overview/content/images/visualization/range_image.jpg deleted file mode 100644 index 4865d9c8..00000000 Binary files a/doc/overview/content/images/visualization/range_image.jpg and /dev/null differ diff --git a/doc/overview/content/images/visualization/shapes.jpg b/doc/overview/content/images/visualization/shapes.jpg deleted file mode 100644 index 351de84a..00000000 Binary files a/doc/overview/content/images/visualization/shapes.jpg and /dev/null differ diff --git a/doc/overview/content/index.rst b/doc/overview/content/index.rst deleted file mode 100644 index a3db1caf..00000000 --- a/doc/overview/content/index.rst +++ /dev/null @@ -1,35 +0,0 @@ -.. toctree:: - -Overview --------- - -PCL presents an advanced and extensive approach to the subject of 3D -perception, and it's meant to provide support for all the common 3D building -blocks that applications need. Written from a perspective of online sensor data -processing, PCL deals with point cloud data acquired from real sensing devices -including laser scanners, stereo cameras, and TOF cameras. - -PCL is a fully templated, modern C++ library, with efficient backends for -modern CPUs (SSE optimized), and GPUs (CUDA). The plethora of state-of-the-art -algorithms implemented is large, with many new ones added on a weekly to -monthly basis -- acquisition, filtering, feature estimation, surface -reconstruction, registration, model fitting, and segmentation, are just a few -of the many topics covered by PCL. - -The project is supported by an international community of robotics and perception -researchers, and would not exist without the contributions of many people and -companies. We thank everyone for their support! - -Presentation ------------- - -The following overview presentation was given at the ROS Fall School at TUM in -November 2010. - -.. raw:: html - -

- - (download here) -

- diff --git a/doc/overview/content/visualization.rst b/doc/overview/content/visualization.rst deleted file mode 100644 index 19624090..00000000 --- a/doc/overview/content/visualization.rst +++ /dev/null @@ -1,192 +0,0 @@ -.. _visualization: - -PCL Visualization overview --------------------------- - -The **pcl_visualization** library was built for the purpose of being able to -quickly prototype and visualize the results of algorithms operating on 3D point -cloud data. Similar to OpenCV's **highgui** routines for displaying 2D images -and for drawing basic 2D shapes on screen, the library offers: - - * methods for rendering and setting visual properties (colors, point sizes, - opacity, etc) for any n-D point cloud datasets in pcl::PointCloud format; - - .. image:: images/visualization/bunny.jpg - - * methods for drawing basic 3D shapes on screen (e.g., cylinders, spheres, - lines, polygons, etc) either from sets of points or from parametric - equations; - - .. image:: images/visualization/shapes.jpg - - * a histogram visualization module (PCLHistogramVisualizer) for 2D plots; - - .. image:: images/visualization/histogram.jpg - - * a multitude of Geometry and Color handler for pcl::PointCloud datasets; - - .. image:: images/visualization/normals.jpg - .. image:: images/visualization/pcs.jpg - - * a pcl::RangeImage visualization module. - - .. image:: images/visualization/range_image.jpg - -The package makes use of the VTK library for 3D rendering for -range image and 2D operations. - -For implementing your own visualizers, take a look at the tests and examples -accompanying the library. - -.. note:: - - Due to historical reasons, PCL 1.x stores RGB data as a packed float (to - preserve backward compatibility). To learn more about this, please see the - `PointXYZRGB - `_. - -Simple Cloud Visualization --------------------------- - -If you just want to visualize something in your app with a few lines of code, -use a snippet like the following one: - -.. code-block:: cpp - :linenos: - - #include - //... - void - foo () - { - pcl::PointCloud cloud; - //... populate cloud - pcl_visualization::CloudViewer viewer("Simple Cloud Viewer"); - viewer.showCloud(cloud); - while (!viewer.wasStopped()) - { - } - } - -PCD Viewer ----------- - -A quick way for visualizing PCD (Point Cloud Data) files is by using -**pcl_viewer**. As of 0.2.7, pcl_viewer's help screen looks like:: - - Syntax is: pcl_viewer .pcd - where options are: - -bc r,g,b = background color - -fc r,g,b = foreground color - -ps X = point size (1..64) - -opaque X = rendered point cloud opacity (0..1) - -ax n = enable on-screen display of XYZ axes and scale them to n - -ax_pos X,Y,Z = if axes are enabled, set their X,Y,Z position in space (default 0,0,0) - - -cam (*) = use given camera settings as initial view - (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / Window Size / Window Pos] or use a that contains the same information. - - -multiview 0/1 = enable/disable auto-multi viewport rendering (default disabled) - - - -normals 0/X = disable/enable the display of every Xth point's surface normal as lines (default disabled) - -normals_scale X = resize the normal unit vector size to X (default 0.02) - - -pc 0/X = disable/enable the display of every Xth point's principal curvatures as lines (default disabled) - -pc_scale X = resize the principal curvatures vectors size to X (default 0.02) - - - (Note: for multiple .pcd files, provide multiple -{fc,ps} parameters; they will be automatically assigned to the right file) - -Usage examples --------------- - -.. code-block:: bash - - $ pcl_viewer -multiview 1 data/partial_cup_model.pcd data/partial_cup_model.pcd data/partial_cup_model.pcd - - -The above will load the ``partial_cup_model.pcd`` file 3 times, and will create a -multi-viewport rendering (``-multiview 1``). - -.. image:: images/visualization/ex1.jpg - -Pressing ``h`` while the point clouds are being rendered will output the -following information on the console:: - - | Help: - ------- - p, P : switch to a point-based representation - w, W : switch to a wireframe-based representation (where available) - s, S : switch to a surface-based representation (where available) - - j, J : take a .PNG snapshot of the current window view - c, C : display current camera/window parameters - - + / - : increment/decrement overall point size - - g, G : display scale grid (on/off) - u, U : display lookup table (on/off) - - r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}] - - ALT + s, S : turn stereo mode on/off - ALT + f, F : switch between maximized window mode and original size - - l, L : list all available geometric and color handlers for the current actor map - ALT + 0..9 [+ CTRL] : switch between different geometric handlers (where available) - 0..9 [+ CTRL] : switch between different color handlers (where available) - - -Pressing ``l`` will show the current list of available geometry/color handlers -for the datasets that we loaded. In this example:: - - List of available geometry handlers for actor partial_cup_model.pcd-0: xyz(1) normal_xyz(2) - List of available color handlers for actor partial_cup_model.pcd-0: [random](1) x(2) y(3) z(4) normal_x(5) normal_y(6) normal_z(7) curvature(8) boundary(9) k(10) principal_curvature_x(11) principal_curvature_y(12) principal_curvature_z(13) pc1(14) pc2(15) - -Switching to a ``normal_xyz`` geometric handler using ``ALT+1`` and then -pressing ``8`` to switch to a curvature color handler, should result in the -following:: - - $ pcl_viewer -normals 100 data/partial_cup_model.pcd - -.. image:: images/visualization/ex2.jpg - - -The above will load the ``partial_cup_model.pcd`` file and render its every -``100`` th surface normal on screen. - -.. code-block:: bash - - $ pcl_viewer -pc 100 data/partial_cup_model.pcd - -.. image:: images/visualization/ex3.jpg - -The above will load the ``partial_cup_model.pcd`` file and render its every -``100`` th principal curvature (+surface normal) on screen. - -.. image:: images/visualization/ex4.jpg - -.. code-block:: bash - - $ pcl_viewer data/bun000.pcd data/bun045.pcd -ax 0.5 -ps 3 -ps 1 - -The above assumes that the ``bun000.pcd`` and ``bun045.pcd`` datasets have been -downloaded and are available. The results shown in the following picture were -obtained after pressing ``u`` and ``g`` to enable the lookup table and on-grid -display. - -.. image:: images/visualization/ex5.jpg - - -Range Image Visualizer ----------------------- - -A quick way for visualizing range images is by using the binary of the tutorial -for range_image_visualization:: - - $ tutorial_range_image_visualization data/office_scene.pcd - -The above will load the ``office_scene.pcd`` point cloud file, create a range -image from it and visualize both, the point cloud and the range image. - diff --git a/doc/tutorials/CMakeLists.txt b/doc/tutorials/CMakeLists.txt index 0d3d98dc..b1efe963 100644 --- a/doc/tutorials/CMakeLists.txt +++ b/doc/tutorials/CMakeLists.txt @@ -1,24 +1,14 @@ -find_package(Sphinx) if(SPHINX_FOUND) - file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html") - if(WIN32) - set(TMPDIR "$ENV{TEMP}") - else() - set(TMPDIR "/tmp") - endif() - file(TO_CMAKE_PATH "${TMPDIR}" TMPDIR) - add_custom_target(Tutorials - COMMAND ${CMAKE_COMMAND} -E remove_directory "${TMPDIR}/doctrees" - COMMAND ${SPHINX_EXECUTABLE} -b html -a -d "${TMPDIR}/doctrees" "${CMAKE_CURRENT_SOURCE_DIR}/content" html - ) + add_custom_target(tutorials ALL + COMMAND "${SPHINX_EXECUTABLE}" -b html -a -d "${SPHINX_CACHE_DIR}" -D html_file_suffix=".${SPHINX_HTML_FILE_SUFFIX}" "${CMAKE_CURRENT_SOURCE_DIR}/content" html) + add_dependencies(tutorials doc) if(USE_PROJECT_FOLDERS) - set_target_properties(Tutorials PROPERTIES FOLDER "Documentation") + set_target_properties(tutorials PROPERTIES FOLDER "Documentation (Tutorials)") endif(USE_PROJECT_FOLDERS) install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html" DESTINATION "${DOC_INSTALL_DIR}/tutorials" COMPONENT doc) install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/content/sources" DESTINATION "${DOC_INSTALL_DIR}/tutorials" - COMPONENT doc - PATTERN ".svn" EXCLUDE) + COMPONENT doc) endif(SPHINX_FOUND) diff --git a/doc/tutorials/Makefile b/doc/tutorials/Makefile deleted file mode 100644 index 46fe7754..00000000 --- a/doc/tutorials/Makefile +++ /dev/null @@ -1,6 +0,0 @@ -.PHONY: html - -html: - -rm -rf html /tmp/doctrees - sphinx-build -b html -a -d /tmp/doctrees content html - diff --git a/doc/tutorials/content/_static/basic.css b/doc/tutorials/content/_static/basic.css new file mode 100644 index 00000000..efe60182 --- /dev/null +++ b/doc/tutorials/content/_static/basic.css @@ -0,0 +1,538 @@ +/* + * basic.css + * ~~~~~~~~~ + * + * Sphinx stylesheet -- basic theme. + * + * :copyright: Copyright 2007-2011 by the Sphinx team, see AUTHORS. + * :license: BSD, see LICENSE for details. + * + */ + +/* -- main layout ----------------------------------------------------------- */ + +div.clearer { + clear: both; +} + +/* -- relbar ---------------------------------------------------------------- */ + +div.related { + width: 100%; + font-size: 90%; +} + +div.related h3 { + display: none; +} + +div.related ul { + margin: 0; + padding: 0 0 0 10px; + list-style: none; +} + +div.related li { + display: inline; +} + +div.related li.right { + float: right; + margin-right: 5px; +} + +/* -- sidebar --------------------------------------------------------------- */ + +div.sphinxsidebarwrapper { + padding: 10px 5px 0 10px; +} + +div.sphinxsidebar { + float: left; + width: 230px; + margin-left: -100%; + font-size: 90%; +} + +div.sphinxsidebar ul { + list-style: none; +} + +div.sphinxsidebar ul ul, +div.sphinxsidebar ul.want-points { + margin-left: 20px; + list-style: square; +} + +div.sphinxsidebar ul ul { + margin-top: 0; + margin-bottom: 0; +} + +div.sphinxsidebar form { + margin-top: 10px; +} + +div.sphinxsidebar input { + border: 1px solid #98dbcc; + font-family: sans-serif; + font-size: 1em; +} + +img { + border: 0; +} + +/* -- search page ----------------------------------------------------------- */ + +ul.search { + margin: 10px 0 0 20px; + padding: 0; +} + +ul.search li { + padding: 5px 0 5px 20px; + background-image: url(file.png); + background-repeat: no-repeat; + background-position: 0 7px; +} + +ul.search li a { + font-weight: bold; +} + +ul.search li div.context { + color: #888; + margin: 2px 0 0 30px; + text-align: left; +} + +ul.keywordmatches li.goodmatch a { + font-weight: bold; +} + +/* -- index page ------------------------------------------------------------ */ + +table.contentstable { + width: 90%; +} + +table.contentstable p.biglink { + line-height: 150%; +} + +a.biglink { + font-size: 1.3em; +} + +span.linkdescr { + font-style: italic; + padding-top: 5px; + font-size: 90%; +} + +/* -- general index --------------------------------------------------------- */ + +table.indextable { + width: 100%; +} + +table.indextable td { + text-align: left; + vertical-align: top; +} + +table.indextable dl, table.indextable dd { + margin-top: 0; + margin-bottom: 0; +} + +table.indextable tr.pcap { + height: 10px; +} + +table.indextable tr.cap { + margin-top: 10px; + background-color: #f2f2f2; +} + +img.toggler { + margin-right: 3px; + margin-top: 3px; + cursor: pointer; +} + +div.modindex-jumpbox { + border-top: 1px solid #ddd; + border-bottom: 1px solid #ddd; + margin: 1em 0 1em 0; + padding: 0.4em; +} + +div.genindex-jumpbox { + border-top: 1px solid #ddd; + border-bottom: 1px solid #ddd; + margin: 1em 0 1em 0; + padding: 0.4em; +} + +/* -- general body styles --------------------------------------------------- */ + +a.headerlink { + visibility: hidden; +} + +h1:hover > a.headerlink, +h2:hover > a.headerlink, +h3:hover > a.headerlink, +h4:hover > a.headerlink, +h5:hover > a.headerlink, +h6:hover > a.headerlink, +dt:hover > a.headerlink { + visibility: visible; +} + +div.body p.caption { + text-align: inherit; +} + +div.body td { + text-align: left; +} + +.field-list ul { + padding-left: 1em; +} + +.first { +font-family: "Droid Serif", "DejaVu Serif", "Garamond", serif; +font-size: 1.3em; +color: #64794c; +margin-bottom: 0; + margin-top: 0 !important; +} + +.first img { +max-width: none !important; +} + +p.rubric { + margin-top: 30px; + font-weight: bold; +} + +img.align-left, .figure.align-left, object.align-left { + clear: left; + float: left; + margin-right: 1em; +} + +img.align-right, .figure.align-right, object.align-right { + clear: right; + float: right; + margin-left: 1em; +} + +img.align-center, .figure.align-center, object.align-center { + display: block; + margin-left: auto; + margin-right: auto; +} + +.align-left { + text-align: left; +} + +.align-center { + clear: both; + text-align: center; +} + +.align-right { + text-align: right; +} + +/* -- sidebars -------------------------------------------------------------- */ + +div.sidebar { + margin: 0 0 0.5em 1em; + border: 1px solid #ddb; + padding: 7px 7px 0 7px; + background-color: #ffe; + width: 40%; + float: right; +} + +p.sidebar-title { + font-weight: bold; +} + +/* -- topics ---------------------------------------------------------------- */ + +div.topic { + border: 1px solid #ccc; + padding: 7px 7px 0 7px; + margin: 10px 0 10px 0; +} + +p.topic-title { + font-size: 1.1em; + font-weight: bold; + margin-top: 10px; +} + +/* -- admonitions ----------------------------------------------------------- */ + +div.admonition { + margin-top: 10px; + margin-bottom: 10px; + padding: 7px; +} + +div.admonition dt { + font-weight: bold; +} + +div.admonition dl { + margin-bottom: 0; +} + +p.admonition-title { + margin: 0px 10px 5px 0px; + font-weight: bold; +} + +div.body p.centered { + text-align: center; + margin-top: 25px; +} + +/* -- tables ---------------------------------------------------------------- */ + +table.docutils { + border: 0; + border-collapse: collapse; +} + +table.docutils td, table.docutils th { + padding: 1px 8px 1px 5px; + border-top: 0; + border-left: 0; + border-right: 0; + border-bottom: 1px solid #aaa; +} + +table.field-list td, table.field-list th { + border: 0 !important; +} + +table.footnote td, table.footnote th { + border: 0 !important; +} + +th { + text-align: left; + padding-right: 5px; +} + +table.citation { + border-left: solid 1px gray; + margin-left: 1px; +} + +table.citation td { + border-bottom: none; +} + +/* -- other body styles ----------------------------------------------------- */ + +ol.arabic { + list-style: decimal; +} + +ol.loweralpha { + list-style: lower-alpha; +} + +ol.upperalpha { + list-style: upper-alpha; +} + +ol.lowerroman { + list-style: lower-roman; +} + +ol.upperroman { + list-style: upper-roman; +} + +dl { + margin-bottom: 15px; +} + +dd p { + margin-top: 0px; +} + +dd ul, dd table { + margin-bottom: 10px; +} + +dd { + margin-top: 3px; + margin-bottom: 10px; + margin-left: 30px; +} + +dt:target, .highlighted { + background-color: #fbe54e; +} + +dl.glossary dt { + font-weight: bold; + font-size: 1.1em; +} + +.field-list ul { + margin: 0; + padding-left: 1em; +} + +.field-list p { + margin: 0; +} + +.refcount { + color: #060; +} + +.optional { + font-size: 1.3em; +} + +.versionmodified { + font-style: italic; +} + +.system-message { + background-color: #fda; + padding: 5px; + border: 3px solid red; +} + +.footnote:target { + background-color: #ffa; +} + +.line-block { + display: block; + margin-top: 1em; + margin-bottom: 1em; +} + +.line-block .line-block { + margin-top: 0; + margin-bottom: 0; + margin-left: 1.5em; +} + +.guilabel, .menuselection { + font-family: sans-serif; +} + +.accelerator { + text-decoration: underline; +} + +.classifier { + font-style: oblique; +} + +/* -- code displays --------------------------------------------------------- */ + +pre { + overflow: auto; + overflow-y: hidden; /* fixes display issues on Chrome browsers */ +white-space : pre-wrap; /*for Mozilla*/ +word-wrap: break-word; /*for IE*/ +} + +td.linenos pre { + padding: 5px 0px; + border: 0; + background-color: transparent; + color: #aaa; +} + +table.highlighttable { + margin-left: 0.5em; +} + +table.highlighttable td { + padding: 0 0.5em 0 0.5em; +} + +tt.descname { + background-color: transparent; + font-weight: bold; + font-size: 1.2em; +} + +tt.descclassname { + background-color: transparent; +} + +tt.xref, a tt { + background-color: transparent; + font-weight: bold; +} + +h1 tt, h2 tt, h3 tt, h4 tt, h5 tt, h6 tt { + background-color: transparent; +} + +.viewcode-link { + float: right; +} + +.viewcode-back { + float: right; + font-family: sans-serif; +} + +div.viewcode-block:target { + margin: -1px -10px; + padding: 0 10px; +} + +/* -- math display ---------------------------------------------------------- */ + +img.math { + vertical-align: middle; +} + +div.math p { + text-align: center; +} + +span.eqno { + float: right; +} + +/* -- printout stylesheet --------------------------------------------------- */ + +@media print { + div.document, + div.documentwrapper, + div.bodywrapper { + margin: 0 !important; + width: 100%; + } + + div.sphinxsidebar, + div.related, + div.footer, + #top-link { + display: none; + } +} diff --git a/doc/tutorials/content/_static/sphinxdoc.css b/doc/tutorials/content/_static/sphinxdoc.css new file mode 100644 index 00000000..62ff7fd6 --- /dev/null +++ b/doc/tutorials/content/_static/sphinxdoc.css @@ -0,0 +1,315 @@ +/* + * sphinxdoc.css_t + * ~~~~~~~~~~~~~~~ + * + * Sphinx stylesheet -- sphinxdoc theme. Originally created by + * Armin Ronacher for Werkzeug. + * + * :copyright: Copyright 2007-2011 by the Sphinx team, see AUTHORS. + * :license: BSD, see LICENSE for details. + * + */ + +@import url("basic.css"); + +/* -- page layout ----------------------------------------------------------- */ + +body { + color: black; + padding: 0; + margin: 0px 80px 0px 80px; + min-width: 740px; +} + +div.document { + + text-align: left; + + +} + +div.bodywrapper { + margin: 0 240px 0 0; + border-right: 1px solid #ccc; +} + +div.body { + margin: 0; + padding: 0.5em 20px 20px 20px; +} + +div.related { + font-size: 1em; +} + +div.related ul { + background-image: url(navigation.png); + height: 2em; + border-top: 1px solid #ddd; + border-bottom: 1px solid #ddd; +} + +div.related ul li { + margin: 0; + padding: 0; + height: 2em; + float: left; +} + +div.related ul li.right { + float: right; + margin-right: 5px; +} + +div.related ul li a { + margin: 0; + padding: 0 5px 0 5px; + line-height: 1.75em; + color: #EE9816; +} + +div.related ul li a:hover { + color: #3CA8E7; +} + +div.sphinxsidebarwrapper { + padding: 0; +} + +div.sphinxsidebar { + margin: 0; + padding: 0.5em 15px 15px 0; + width: 210px; + float: right; + font-size: 1em; + text-align: left; +} + +div.sphinxsidebar h3, div.sphinxsidebar h4 { + margin: 1em 0 0.5em 0; + font-size: 1em; + padding: 0.1em 0 0.1em 0.5em; + color: white; + border: 1px solid #86989B; + background-color: #AFC1C4; +} + +div.sphinxsidebar h3 a { + color: white; +} + +div.sphinxsidebar ul { + padding-left: 1.5em; + margin-top: 7px; + padding: 0; + line-height: 130%; +} + +div.sphinxsidebar ul ul { + margin-left: 20px; +} + +div.footer { + background-color: #E3EFF1; + color: #86989B; + padding: 3px 8px 3px 0; + clear: both; + font-size: 0.8em; + text-align: right; +} + +div.footer a { + color: #86989B; + text-decoration: underline; +} + +/* -- body styles ----------------------------------------------------------- */ + +p { + margin: 0.8em 0 0.5em 0; +} + +div.body a { + text-decoration: underline; +} + +h2 { +/* color: #11557C;*/ + margin: 1.3em 0 0.2em 0; + font-size: 1.35em; + padding: 0; +} + +h3 { + margin: 1em 0 -0.3em 0; + font-size: 1.2em; +} + +div.body h1 a, div.body h2 a, div.body h3 a, div.body h4 a, div.body h5 a, div.body h6 a { + color: black!important; +} + +h1 a.anchor, h2 a.anchor, h3 a.anchor, h4 a.anchor, h5 a.anchor, h6 a.anchor { + display: none; + margin: 0 0 0 0.3em; + padding: 0 0.2em 0 0.2em; + color: #aaa!important; +} + +h1:hover a.anchor, h2:hover a.anchor, h3:hover a.anchor, h4:hover a.anchor, +h5:hover a.anchor, h6:hover a.anchor { + display: inline; +} + +h1 a.anchor:hover, h2 a.anchor:hover, h3 a.anchor:hover, h4 a.anchor:hover, +h5 a.anchor:hover, h6 a.anchor:hover { + color: #777; + background-color: #eee; +} + +a.headerlink { + color: #c60f0f!important; + font-size: 1em; + margin-left: 6px; + padding: 0 4px 0 4px; + text-decoration: none!important; +} + +a.headerlink:hover { + background-color: #ccc; + color: white!important; +} + +cite, code, tt { + font-family: 'Consolas', 'Deja Vu Sans Mono', + 'Bitstream Vera Sans Mono', monospace; + font-size: 0.95em; + letter-spacing: 0.01em; +} + +tt { + background-color: #f2f2f2; + border-bottom: 1px solid #ddd; + color: #333; +} + +tt.descname, tt.descclassname, tt.xref { + border: 0; +} + +hr { + border: 1px solid #abc; + margin: 2em; +} + +a tt { + border: 0; + color: #CA7900; +} + +a tt:hover { + color: #2491CF; +} + +pre { + font-family: 'Consolas', 'Deja Vu Sans Mono', + 'Bitstream Vera Sans Mono', monospace; + font-size: 0.95em; + letter-spacing: 0.015em; + line-height: 120%; + padding: 0.5em; + border: 1px solid #ccc; + background-color: #f8f8f8; +} + +pre a { + color: inherit; + text-decoration: underline; +} + +td.linenos pre { + padding: 0.5em 0; +} + +div.quotebar { + background-color: #f8f8f8; + max-width: 250px; + float: right; + padding: 2px 7px; + border: 1px solid #ccc; +} + +div.topic { + background-color: #f8f8f8; +} + +table { + border-collapse: collapse; + margin: 0 -0.5em 0 -0.5em; +} + +table td, table th { + padding: 0.2em 0.5em 0.2em 0.5em; +} + +div.admonition, div.warning { + font-size: 0.9em; + margin: 1em 0 1em 0; + border: 1px solid #86989B; + background-color: #f7f7f7; + padding: 0; +} + +div.admonition p, div.warning p { + margin: 0.5em 1em 0.5em 1em; + padding: 0; +} + +div.admonition pre, div.warning pre { + margin: 0.4em 1em 0.4em 1em; +} + +div.admonition p.admonition-title, +div.warning p.admonition-title { + margin: 0; + padding: 0.1em 0 0.1em 0.5em; + color: white; + border-bottom: 1px solid #86989B; + font-weight: bold; + background-color: #AFC1C4; +} + +div.warning { + border: 1px solid #940000; +} + +div.warning p.admonition-title { + background-color: #CF0000; + border-bottom-color: #940000; +} + +div.admonition ul, div.admonition ol, +div.warning ul, div.warning ol { + margin: 0.1em 0.5em 0.5em 3em; + padding: 0; +} + +div.versioninfo { + margin: 1em 0 0 0; + border: 1px solid #ccc; + background-color: #DDEAF0; + padding: 8px; + line-height: 1.3em; + font-size: 0.9em; +} + +.viewcode-back { + font-family: 'Lucida Grande', 'Lucida Sans Unicode', 'Geneva', + 'Verdana', sans-serif; +} + +div.viewcode-block:target { + background-color: #f4debf; + border-top: 1px solid #ac9; + border-bottom: 1px solid #ac9; +} diff --git a/doc/tutorials/content/_templates/layout.html b/doc/tutorials/content/_templates/layout.html index 5603ce5f..0516be55 100644 --- a/doc/tutorials/content/_templates/layout.html +++ b/doc/tutorials/content/_templates/layout.html @@ -1,8 +1,47 @@ + + + +Documentation - Point Cloud Library (PCL) + {% extends "!layout.html" %} {% block extrahead %} +initialize('web'); + +$snip = $modx->runSnippet("getSiteNavigation", array('id'=>5, 'phLevels'=>'sitenav.level0,sitenav.level1', 'showPageNav'=>'n')); +$chunkOutput = $modx->getChunk("site-header", array('sitenav'=>$snip)); +$bodytag = str_replace("[[+showSubmenus:notempty=`", "", $chunkOutput); +$bodytag = str_replace("`]]", "", $bodytag); +echo $bodytag; +echo "\n"; +?> +
+

Documentation

+ +
+
{% endblock %} +{% block relbar1 %}{% endblock %} +{% block relbar2 %}{% endblock %} {% block rootrellink %}{% endblock %} {% block sidebarsearch %}{% endblock %} + +{% block footer %} +
+ +getChunk("site-footer"); +echo $chunkOutput; +?> +{% endblock %} + + diff --git a/doc/tutorials/content/adding_custom_ptype.rst b/doc/tutorials/content/adding_custom_ptype.rst index 46de5606..51914fea 100644 --- a/doc/tutorials/content/adding_custom_ptype.rst +++ b/doc/tutorials/content/adding_custom_ptype.rst @@ -64,7 +64,7 @@ What `PointT` types are available in PCL? To cover all possible cases that we could think of, we defined a plethora of point types in PCL. The following might be only a snippet, please see -`point_types.hpp `_ +`point_types.hpp `_ for the complete list. This list is important, because before defining your own custom type, you need @@ -807,8 +807,8 @@ make sense to try to use explicit instantiations for your `MyPointType` types, for any classes that you expose (from PCL our outside PCL). .. note:: -Starting with PCL-1.7 you need to define PCL_NO_PRECOMPILE before you include -any PCL headers to include the templated algorithms as well. + Starting with PCL-1.7 you need to define PCL_NO_PRECOMPILE before you include + any PCL headers to include the templated algorithms as well. Example ------- diff --git a/doc/tutorials/content/alignment_prerejective.rst b/doc/tutorials/content/alignment_prerejective.rst index 98bc8459..ddfad3f0 100644 --- a/doc/tutorials/content/alignment_prerejective.rst +++ b/doc/tutorials/content/alignment_prerejective.rst @@ -9,7 +9,7 @@ In this tutorial, we show how to find the alignment pose of a rigid object in a The code -------- -First, download the datasets from :download:`here <./sources/alignment_prerejective/data/alignment_prerejective.tar.gz>` and extract the files. +First, download the test models: :download:`object <./sources/alignment_prerejective/chef.pcd>` and :download:`scene <./sources/alignment_prerejective/rs1.pcd>`. Next, copy and paste the following code into your editor and save it as ``alignment_prerejective.cpp`` (or download the source file :download:`here <./sources/alignment_prerejective/alignment_prerejective.cpp>`). @@ -54,27 +54,27 @@ We are now ready to setup the alignment process. We use the class :pcl:`SampleCo .. literalinclude:: sources/alignment_prerejective/alignment_prerejective.cpp :language: cpp - :lines: 79-90 + :lines: 79-91 .. note:: Apart from the usual input point clouds and features, this class takes some additional runtime parameters which have great influence on the performance of the alignment algorithm. The first two have the same meaning as in the alignment class :pcl:`SampleConsensusInitialAlignment `: - Number of samples - *setNumberOfSamples ()*: The number of point correspondences to sample between the object and the scene. At minimum, 3 points are required to calculate a pose. - Correspondence randomness - *setCorrespondenceRandomness ()*: Instead of matching each object FPFH descriptor to its nearest matching feature in the scene, we can choose between the *N* best matches at random. This increases the iterations necessary, but also makes the algorithm robust towards outlier matches. - - Polygonal similarity threshold - *setSimlarityThreshold ()*: The alignment class uses the :pcl:`CorrespondenceRejectorPoly ` class for early elimination of bad poses based on pose-invariant geometric consistencies of the inter-distances between sampled points on the object and the scene. The closer this value is set to 1, the more greedy and thereby fast the algorithm becomes. However, this also increases the risk of eliminating good poses when noise is present. + - Polygonal similarity threshold - *setSimilarityThreshold ()*: The alignment class uses the :pcl:`CorrespondenceRejectorPoly ` class for early elimination of bad poses based on pose-invariant geometric consistencies of the inter-distances between sampled points on the object and the scene. The closer this value is set to 1, the more greedy and thereby fast the algorithm becomes. However, this also increases the risk of eliminating good poses when noise is present. - Inlier threshold - *setMaxCorrespondenceDistance ()*: This is the Euclidean distance threshold used for determining whether a transformed object point is correctly aligned to the nearest scene point or not. In this example, we have used a heuristic value of 1.5 times the point cloud resolution. - - Inlier fraction - *setInlierFraction ()*: In many practical scenarios, large parts of the observed object in the scene are not visible, either due to clutter, occlusions or both. In such cases, we need to allow for pose hypotheses that do not align all object points to the scene. The absolute number of correctly aligned points is determined using the inlier threshold, and if the ratio of this number to the total number of points in the object is higher than the specified inlier fraction, we accept a pose hypothesis as valid. Furthermore, if a pose generates the highest number of inliers so far, the pose is stored as the current output. In other words, this class tries to maximize the inliers instead of minimizing the fit error during the alignment process. + - Inlier fraction - *setInlierFraction ()*: In many practical scenarios, large parts of the observed object in the scene are not visible, either due to clutter, occlusions or both. In such cases, we need to allow for pose hypotheses that do not align all object points to the scene. The absolute number of correctly aligned points is determined using the inlier threshold, and if the ratio of this number to the total number of points in the object is higher than the specified inlier fraction, we accept a pose hypothesis as valid. Finally, we are ready to execute the alignment process. .. literalinclude:: sources/alignment_prerejective/alignment_prerejective.cpp :language: cpp - :lines: 91 + :lines: 92-95 The aligned object is stored in the point cloud *object_aligned*. If a pose with enough inliers was found (more than 25 % of the total number of object points), the algorithm is said to converge, and we can print and visualize the results. .. literalinclude:: sources/alignment_prerejective/alignment_prerejective.cpp :language: cpp - :lines: 95-109 + :lines: 99-114 Compiling and running the program @@ -91,14 +91,16 @@ After you have made the executable, you can run it like so:: $ ./alignment_prerejective chef.pcd rs1.pcd After a few seconds, you will see a visualization and a terminal output similar to:: - - | -0.003 -0.972 0.235 | - R = | -0.993 -0.026 -0.119 | - | 0.122 -0.233 -0.965 | - t = < 0.095, -0.022, 0.069 > + Alignment took 352ms. - Inliers: 890/3432 + | 0.040 -0.929 -0.369 | + R = | -0.999 -0.035 -0.020 | + | 0.006 0.369 -0.929 | + + t = < -0.287, 0.045, 0.126 > + + Inliers: 987/3432 The visualization window should look something like the below figures. The scene is shown with green color, and the aligned object model is shown with blue color. Note the high number of non-visible object points. diff --git a/doc/tutorials/content/bspline_fitting.rst b/doc/tutorials/content/bspline_fitting.rst new file mode 100644 index 00000000..518261ed --- /dev/null +++ b/doc/tutorials/content/bspline_fitting.rst @@ -0,0 +1,278 @@ +.. _bspline_fitting: + +Fitting trimmed B-splines to unordered point clouds +--------------------------------------------------- + +This tutorial explains how to run a B-spline fitting algorithm on a +point-cloud, to obtain a smooth, parametric surface representation. +The algorithm consists of the following steps: + +* Initialization of the B-spline surface by using the Principal Component Analysis (PCA). This + assumes that the point-cloud has two main orientations, i.e. that it is roughly planar. + +* Refinement and fitting of the B-spline surface. + +* Circular initialization of the B-spline curve. Here we assume that the point-cloud is + compact, i.e. no separated clusters. + +* Fitting of the B-spline curve. + +* Triangulation of the trimmed B-spline surface. + +In this video, the algorithm is applied to the frontal scan of the stanford bunny (204800 points): + +.. raw:: html + + + + +Theoretical background +---------------------- + +Theoretical information on the algorithm can be found in this `report +`_ and in my `PhD thesis +`_. + + +PCL installation settings +------------------------- + +Please note that the modules for NURBS and B-splines are not enabled by default. +Make sure you enable "BUILD_surface_on_nurbs" in your ccmake configuration, by setting it to ON. + +If your license permits, also enable "USE_UMFPACK" for sparse linear solving. +This requires SuiteSparse (libsuitesparse-dev in Ubuntu) which is faster, +allows more degrees of freedom (i.e. control points) and more data points. + +The program created during this tutorial is available in +*pcl/examples/surface/example_nurbs_fitting_surface.cpp* and is built when +"BUILD_examples" is set to ON. This will create the binary called *pcl_example_nurbs_fitting_surface* +in your *bin* folder. + + +The code +-------- + +The cpp file used in this tutorial can be found in *pcl/doc/tutorials/content/sources/bspline_fitting/bspline_fitting.cpp*. +You can find the input file at *pcl/test/bunny.pcd*. + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :linenos: + :lines: 1-220 + + +The explanation +--------------- +Now, let's break down the code piece by piece. +Lets start with the choice of the parameters for B-spline surface fitting: + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :linenos: + :lines: 56-66 + +* *order* is the polynomial order of the B-spline surface. + +* *refinement* is the number of refinement iterations, where for each iteration control-points + are inserted, approximately doubling the control points in each parametric direction + of the B-spline surface. + +* *iterations* is the number of iterations that are performed after refinement is completed. + +* *mesh_resolution* the number of vertices in each parametric direction, + used for triangulation of the B-spline surface. + +Fitting: + +* *interior_smoothness* is the smoothness of the surface interior. + +* *interior_weight* is the weight for optimization for the surface interior. + +* *boundary_smoothness* is the smoothness of the surface boundary. + +* *boundary_weight* is the weight for optimization for the surface boundary. + +Note, that the boundary in this case is not the trimming curve used later on. +The boundary can be used when a point-set exists that defines the boundary. Those points +can be declared in *pcl::on_nurbs::NurbsDataSurface::boundary*. In that case, when the +*boundary_weight* is greater than 0.0, the algorithm tries to align the domain boundaries +to these points. In our example we are trimming the surface anyway, so there is no need +for aligning the boundary. + +Initialization of the B-spline surface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 68-72 + +The command *initNurbsPCABoundingBox* uses PCA to create a coordinate systems, where the principal +eigenvectors point into the direction of the maximum, middle and minimum extension of the point-cloud. +The center of the coordinate system is located at the mean of the points. +To estimate the extension of the B-spline surface domain, a bounding box is computed in the plane formed +by the maximum and middle eigenvectors. That bounding box is used to initialize the B-spline surface with +its minimum number of control points, according to the polynomial degree chosen. + +The surface fitting class *pcl::on_nurbs::FittingSurface* is initialized with the point data and the initial +B-spline. + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 74-80 + +The *on_nurbs::Triangulation* class allows easy conversion between the *ON_NurbsSurface* and the *PolygonMesh* class, +for visualization of the B-spline surfaces. Note that NURBS are a generalization of B-splines, +and are therefore a valid container for B-splines, with all control-point weights = 1. + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 82-92 + +Refinement and fitting of the B-spline surface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +At this point of the code we have a B-spline surface with minimal number of control points. +Typically they are not enough to represent finer details of the underlying geometry +of the point-cloud. However, if we increase the control-points to our desired level of detail and +subsequently fit the refined B-spline, we run into problems. For robust fitting B-spline surfaces +the rule is: +"The higher the degree of freedom of the B-spline surface, the closer we have to be to the points to be approximated". + +This is the reason why we iteratively increase the degree of freedom by refinement in both directions (line 85-86), +and fit the B-spline surface to the point-cloud, getting closer to the final solution. + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 94-102 + +After we reached the final level of refinement, the surface is further fitted to the point-cloud +for a pleasing end result. + +Initialization of the B-spline curve +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Now that we have the surface fitted to the point-cloud, we want to cut off the overlapping regions of the surface. +To achieve this we project the point-cloud into the parametric domain using the closest points to the B-spline surface. +In this domain of R^2 we perform the weighted B-spline curve fitting, that creates a closed trimming curve that approximately +contains all the points. + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 107-120 + +The topic of curve fitting goes a bit deeper into the thematics of B-splines. Here we assume that you are +familiar with the concept of B-splines, knot vectors, control-points, and so forth. +Please consider the curve being split into supporting regions which is bound by consecutive knots. +Also note that points that are inside and outside the curve are distinguished. + +* *addCPsAccuracy* the distance of the supporting region of the curve to the closest data points has to be below + this value, otherwise a control point is inserted. + +* *addCPsIteration* inner iterations without inserting control points. + +* *maxCPs* the maximum total number of control-points. + +* *accuracy* the average fitting accuracy of the curve, w.r.t. the supporting regions. + +* *iterations* maximum number of iterations performed. + +* *closest_point_resolution* number of control points that must lie within each supporting region. (0 turns this constraint off) + +* *closest_point_weight* weight for fitting the curve to its closest points. + +* *closest_point_sigma2* threshold for closest points (disregard points that are further away from the curve). + +* *interior_sigma2* threshold for interior points (disregard points that are further away from and lie within the curve). + +* *smooth_concavity* value that leads to inward bending of the curve (0 = no bending; <0 inward bending; >0 outward bending). + +* *smoothness* weight of smoothness term. + + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 122-127 + +The curve is initialized using a minimum number of control points to represent a circle, with the center located +at the mean of the point-cloud and the radius of the maximum distance of a point to the center. +Please note that interior weighting is enabled for all points with the command *curve_data.interior_weight_function.push_back (true)*. + +Fitting of the B-spline curve +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 129-133 + +Similar to the surface fitting approach, the curve is iteratively fitted and refined, as shown in the video. +Note how the curve tends to bend inwards at regions where it is not supported by any points. + +Triangulation of the trimmed B-spline surface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 136-142 + +After the curve fitting terminated, our geometric representation consists of a B-spline surface and a closed +B-spline curved, defined within the parametric domain of the B-spline surface. This is called trimmed B-spline surface. +In line 140 we can use the trimmed B-spline to create a triangular mesh. The triangulation algorithm first triangulates +the whole domain and afterwards removes triangles that lie outside of the trimming curve. Vertices of triangles +that intersect the trimming curve are clamped to the curve. + +When running this example and switch to wire-frame mode (w), you will notice that the triangles are ordered in +a rectangular way, which is a result of the rectangular domain of the surface. + +Some hints +---------- +Please bear in mind that the robustness of this algorithm heavily depends on the underlying data. +The parameters for B-spline fitting are designed to model the characteristics of this data. + +* If you have holes or steps in your data, you might want to work with lower refinement levels and lower accuracy to + prevent the B-spline from folding and twisting. Moderately increasing of the smoothness might also work. + +* Try to introduce as much pre-conditioning and constraints to the parameters. E.g. if you know, that + the trimming curve is rather simple, then limit the number of maximum control points. + +* Start simple! Before giving up on gaining control over twisting and bending B-splines, I highly recommend + to start your fitting trials with a small number of control points (low refinement), + low accuracy but also low smoothness (B-splines have implicit smoothing property). + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/bspline_fitting/CMakeLists.txt + :language: cmake + :linenos: + +After you have made the executable, you can run it. Simply do: + + $ ./bspline_fitting ${PCL_ROOT}/test/bunny.pcd + + +Saving and viewing the result +----------------------------- + +* Saving as OpenNURBS (3dm) file + +You can save the B-spline surface by using the commands provided by OpenNurbs: + +.. literalinclude:: sources/bspline_fitting/bspline_fitting.cpp + :language: cpp + :lines: 145-163 + +The files generated can be viewed with the pcl/examples/surface/example_nurbs_viewer_surface.cpp. + +* Saving as triangle mesh into a vtk file + +You can save the triangle mesh for example by saving into a VTK file by: + + #include + ... + pcl::io::saveVTKFile ("mesh.vtk", mesh); + +PCL also provides vtk conversion into other formats (PLY, OBJ). + diff --git a/doc/tutorials/content/building_pcl.rst b/doc/tutorials/content/building_pcl.rst index 06d0f877..f0b8fafc 100644 --- a/doc/tutorials/content/building_pcl.rst +++ b/doc/tutorials/content/building_pcl.rst @@ -36,7 +36,6 @@ Let's have a look at what `cmake` options got enabled:: You should see something like the following on screen:: - BUILD_TESTS ON BUILD_common ON BUILD_features ON BUILD_filters ON @@ -61,8 +60,6 @@ You should see something like the following on screen:: The explanation --------------- -* `BUILD_TESTS`: option to enable/disable building of tests - * `BUILD_common`: option to enable/disable building of common library * `BUILD_features`: option to enable/disable building of features library @@ -110,9 +107,9 @@ Tweaking basic settings Depending on your project/system, you might want to enable/disable certain options. For example, you can prevent the building of: -* tests: setting BUILD_TESTS and BUILD_global_tests to OFF +* tests: setting `BUILD_global_tests` to `OFF` -* a library: setting BUILD_LIBRARY_NAME to OFF +* a library: setting `BUILD_LIBRARY_NAME` to `OFF` Note that if you disable a XXX library that is required for building YYY then XXX will be built but won't appear in the cache. @@ -143,8 +140,26 @@ you have all the dependencies installed. In this section we will discuss each dependency entry so that you can configure/build or update/build PCL according to your system. -General remarks -^^^^^^^^^^^^^^^^ +Building unit tests +^^^^^^^^^^^^^^^^^^^ + +If you want to contribute to PCL, or are modifying the code, you need +to turn on building of unit tests. This is accomplished by setting the `BUILD_global_tests` +option to `ON`, with a few caveats. If you're using `ccmake` and you find that `BUILD_global_tests` +is reverting to `OFF` when you configure, you can move the cursor up to the `BUILD_global_tests` line to see the +error message. + +Two options which will need to be turned ON before `BUILD_global_tests` are `BUILD_outofcore` and +`BUILD_people`. Your mileage may vary. + +Also required for unit tests is the source code for the Google C++ Testing Framework. That is +usually as simple as downloading the source, extracting it, and pointing the `GTEST_SRC_DIR` and `GTEST_INCLUDE_DIR` +options to the applicable source locations. On Ubuntu, you can simply run `apt-get install libgtest-dev`. + +These steps enable the `tests` make target, so you can use `make tests` to run tests. + +General remarks +^^^^^^^^^^^^^^^ Under ${PCL_ROOT}/cmake/Modules there is a list of FindXXX.cmake files used to locate dependencies and set their related variables. They have a list of default searchable paths where to look for them. In addition, diff --git a/doc/tutorials/content/cloud_viewer.rst b/doc/tutorials/content/cloud_viewer.rst index 1b8e634f..939914f9 100644 --- a/doc/tutorials/content/cloud_viewer.rst +++ b/doc/tutorials/content/cloud_viewer.rst @@ -10,7 +10,7 @@ to get you up and viewing clouds in as little code as possible. The CloudViewer class is **NOT** meant to be used in multi-threaded applications! Please check the documentation on - :pcl:`PCLVisualizer` or read the :ref:`pcl_visualizer` tutorial + :pcl:`PCLVisualizer` or read the :ref:`pcl_visualizer` tutorial for thread safe visualization. Simple Cloud Visualization diff --git a/doc/tutorials/content/compiling_pcl_dependencies_windows.rst b/doc/tutorials/content/compiling_pcl_dependencies_windows.rst index fe6fa3e4..f8c4dace 100644 --- a/doc/tutorials/content/compiling_pcl_dependencies_windows.rst +++ b/doc/tutorials/content/compiling_pcl_dependencies_windows.rst @@ -32,7 +32,7 @@ compile a series of 3rd party library dependencies: used as the matrix backend for SSE optimized math. **mandatory** - - **FLANN** version >= 1.6.8 (http://www.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN) + - **FLANN** version >= 1.6.8 (http://www.cs.ubc.ca/research/flann/) used in `kdtree` for fast approximate nearest neighbors search. **mandatory** @@ -52,19 +52,15 @@ compile a series of 3rd party library dependencies: used to grab point clouds from OpenNI compliant devices. **optional** - - **Qt** version >= 4.6 (http://qt.nokia.com/) + - **Qt** version >= 4.6 (http://qt.digia.com/) used for developing applications with a graphical user interface (GUI) **optional** - - **MPI** version >= 1.4 (http://www.mcs.anl.gov/research/projects/mpich2/) - - **optional** - .. note:: Though not a dependency per se, don't forget that you also need the CMake - build system (http://www.cmake.org/), at least version **2.8.3**. A Subversion client for Windows, i.e. TortoiseSVN - (http://tortoisesvn.tigris.org/), is also required to download the PCL source code. + build system (http://www.cmake.org/), at least version **2.8.3**. A Git + client for Windows is also required to download the PCL source code. Building dependencies --------------------- @@ -92,9 +88,6 @@ like:: Let's start with `Boost`. We will be using the `CMake-able Boost` project which provide a CMake based build system for Boost. - As a dependency of MPI Boost module (optional), you need first to download and install MPI from the link above. Choose "Win IA32 binary" - if you are building 32 bit PCL libraries, or "Win X86_64 binary" if you are building 64 bit binaries. - If you do not need it, you can skip this, and remove "mpi" from the modules list later. To build Boost, open the CMake-gui and fill in the fields:: @@ -142,20 +135,19 @@ like:: then fill the **BUILD_PROJECTS** CMake entry (which is set to `ALL` by default) with a semicolon-seperated list of boost modules:: - BUILD_PROJECTS : system;filesystem;date_time;thread;iostreams;tr1;serialization;mpi + BUILD_PROJECTS : system;filesystem;date_time;thread;iostreams;tr1;serialization Also, uncheck the **ENABLE_STATIC_RUNTIME** checkbox. Then, click "Configure" again. If you get some errors related to Python, then uncheck **WITH_PYTHON** checkbox, and click "Configure" again. Now, in the CMake log, you should see something like:: Reading boost project directories (per BUILD_PROJECTS) - + + date_time + thread + serialization + system + filesystem - + mpi +-- optional python bindings disabled since PYTHON_FOUND is false. + tr1 @@ -165,20 +157,6 @@ like:: in the solution file, and then will install the build libraries along with the header files to the default installation folder (e.g. C:/Program Files (x86)/Boost). - .. note:: - - If you are building the mpi boost module, and you are using CMake <= 2.8.7, you may run into the following error:: - - LINK : fatal error LNK1104: cannot open file 'C:\Program Files\MPICH2\lib\mpi.lib C:\Program Files\MPICH2\lib\cxx.lib' - - As a workaround (until CMake 2.8.8 is out), go back to CMake gui, check the "Advanced" checkbox at the top right of - CMake window, and edit these entries as follows (please adjust the paths according to your system):: - - MPI_CXX_LIBRARIES : C:/Program Files/MPICH2/lib/cxx.lib;C:/Program Files/MPICH2/lib/mpi.lib - MPI_LIBRARY : C:/Program Files/MPICH2/lib/mpi.lib - - Then, click "Generate". Visual Studio will ask you to reload the solution, then re build the **INSTALL** project. - .. note:: If you get some errors during the installation process, it could be caused by the UAC of MS Windows @@ -221,7 +199,7 @@ like:: If you don't have a Python interpreter installed CMake would probably not allow you to generate the project files. To solve this problem you can install the Python interpreter - (http://www.python.org/download/windows/) or comment the `add_subdirectory( test )` line + (https://www.python.org/download/windows/) or comment the `add_subdirectory( test )` line from C:/PCL_dependencies/flann-1.7.1-src/CMakeLists.txt . - **QHull** : diff --git a/doc/tutorials/content/compiling_pcl_macosx.rst b/doc/tutorials/content/compiling_pcl_macosx.rst index 9c3da398..cecaceb6 100644 --- a/doc/tutorials/content/compiling_pcl_macosx.rst +++ b/doc/tutorials/content/compiling_pcl_macosx.rst @@ -20,7 +20,7 @@ Prerequisites Before getting started download and install the following prerequisites for Mac OS X: -- **XCode** (http://developer.apple.com/xcode) +- **XCode** (https://developer.apple.com/xcode/) Apple’s powerful integrated development environment @@ -60,7 +60,7 @@ The following libraries are **Required** to build PCL. Unified matrix library. Used as the matrix backend for SSE optimized math. - **FLANN** version >= 1.6.8 - (http://www.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN) + (http://www.cs.ubc.ca/research/flann/) Library for performing fast approximate nearest neighbor searches in high dimensional spaces. Used in `kdtree` for fast approximate nearest neighbors search. @@ -104,7 +104,7 @@ for PCL developers: A documentation system for C++, C, Java, Objective-C, Python, IDL (Corba and Microsoft flavors), Fortran, VHDL, PHP, C#, and to some extent D. -- **Sphinx** (http://sphinx.pocoo.org/) +- **Sphinx** (http://sphinx-doc.org/) A tool that makes it easy to create intelligent and beautiful documentation. @@ -235,9 +235,9 @@ Building PCL At this point you should have everything needed installed to build PCL with almost no additional configuration. -Checkout the PCL source from the trunk:: +Checkout the PCL source from the Github: - $ svn co http://svn.pointclouds.org/pcl/trunk pcl + $ git clone https://github.com/PointCloudLibrary/pcl $ cd pcl Create the build directories, configure CMake, build and install:: @@ -292,7 +292,7 @@ using Sphinx. The easiest way to get this installed is using pythons $ easy_install -U Sphinx The Sphinx documentation also requires the third party contrib extension -`sphinxcontrib-doxylink` (http://pypi.python.org/pypi/sphinxcontrib-doxylink) +`sphinxcontrib-doxylink` (https://pypi.python.org/pypi/sphinxcontrib-doxylink) to reference the Doxygen built documentation. To install from source you'll also need Mercurial:: diff --git a/doc/tutorials/content/compiling_pcl_windows.rst b/doc/tutorials/content/compiling_pcl_windows.rst index bf906ae7..f3d7dcc4 100644 --- a/doc/tutorials/content/compiling_pcl_windows.rst +++ b/doc/tutorials/content/compiling_pcl_windows.rst @@ -65,9 +65,8 @@ is needed only to build PCL tests. We do not provide GTest installers. **optiona .. note:: Though not a dependency per se, don't forget that you also need the CMake - build system (http://www.cmake.org/), at least version **2.8.7**. A Subversion client - for Windows, i.e. TortoiseSVN (http://tortoisesvn.tigris.org/), is also required - to download the PCL source code. + build system (http://www.cmake.org/), at least version **2.8.7**. A Git client + for Windows is also required to download the PCL source code. Downloading PCL source code --------------------------- @@ -86,37 +85,9 @@ The invocation to download the source code is thus, using a command line: cd wherever/you/want/to/put/the/repo/ git clone https://github.com/PointCloudLibrary/pcl.git -You could also use Github for Windows( http://windows.github.com/ ), but that is potentially more +You could also use Github for Windows (https://windows.github.com/), but that is potentially more troublesome than setting up git on windows. -Alternatively you could use the old subversion repository: - -For this, -you will need Tortoise SVN to download sources from PCL svn server. - -Subversion is a version control system similar to CVS which allows developers to simultaneously work on PCL. -The download operation of the most recent source from the main development line, known as trunk, is called `checkout`. - -.. note:: - In this tutorial, we will build the svn trunk of PCL. If you want, you can build a PCL branch instead. - You can also build an official release using the source archive from http://pointclouds.org/downloads/. - You can grab PCL branches using Tortoise SVN from : - - - pcl-1.x branch from http://svn.pointclouds.org/pcl/branches/pcl-1.x - - - pcl-1.5.x branch from http://svn.pointclouds.org/pcl/branches/pcl-1.5.x - -First create a folder that will holds PCL source code and binaries. In the remaining of this tutorial we will be using C:\\PCL. -To checkout PCL source code, navigate to the C:\\PCL folder using Windows file manager. Then right click and choose -`SVN Checkout...` from the contextual menu. Set "URL of repository" to http://svn.pointclouds.org/pcl/trunk and -"Checkout directory" to C:\\PCL\\trunk. - -.. image:: images/windows/SVNCheckout_pcl_trunk.png - :alt: SVN Checkout dialog - :align: center - -Click "OK" and the download should start. At the end of this process, you will have PCL source code in C:\\PCL\\trunk. - Configuring PCL --------------- @@ -127,7 +98,7 @@ You can also build static PCL libraries if you want. Run the CMake-gui application and fill in the fields:: - Where is the source code : C:/PCL/trunk + Where is the source code : C:/PCL/pcl Where to build the binaries: C:/PCL Now hit the "Configure" button. You will be asked for a `generator`. A generator is simply a compiler. @@ -352,7 +323,7 @@ Advanced topics Then, you need to enable the `documentation` project in Visual Studio by checking the **BUILD_DOCUMENTATION** checkbox in CMake. You can also build one single CHM file that will gather all the generated html files into one file. You need the `Microsoft - HTML HELP Workshop `_. + HTML HELP Workshop `_. After you install the `Microsoft HTML HELP Workshop`, hit `Configure`. If CMake is not able to find **HTML_HEL_COMPILER**, then fill it manually with the path to `hhc.exe` (e.g. C:/Program Files (x86)/HTML Help Workshop/hhc.exe), then click `Configure` and `Generate`. diff --git a/doc/tutorials/content/correspondence_grouping.rst b/doc/tutorials/content/correspondence_grouping.rst index 272c968a..5331cfe9 100644 --- a/doc/tutorials/content/correspondence_grouping.rst +++ b/doc/tutorials/content/correspondence_grouping.rst @@ -10,8 +10,8 @@ For each cluster, representing a possible model instance in the scene, the Corre The code -------- -Before you begin, you should download the dataset used in this tutorial from `github.com/PointCloudLibrary/data/tree/master/tutorials/correspondence_grouping `_ -and extract the files in a folder of your convenience. +Before you begin, you should download the PCD dataset used in this tutorial from GitHub (`milk.pcd `_ and +`milk_cartoon_all_small_clorox.pcd `_) and put the files in a folder of your convenience. Also, copy and paste the following code into your editor and save it as ``correspondence_grouping.cpp`` (or download the source file :download:`here <./sources/correspondence_grouping/correspondence_grouping.cpp>`). @@ -140,7 +140,7 @@ Alternatively to Hough3DGrouping, and by means of the appropriate command line s :lines: 327-339 .. note:: - The ``recognize`` method returns a vector of ``Eigen::Matrix4f`` representing a transformation (rotation + translation) for each instance of the model found in the scene (obtained via Absolute Orientation) and a **vector** of :pcl:`Correspondences ` (a vector of vectors of :pcl:`Correspondence `) representing the output of the clustering i.e. each element of this vector is in turn a set of correspondences, representing the correspondences associated to a specific model instance in the scene. + The ``recognize`` method returns a vector of ``Eigen::Matrix4f`` representing a transformation (rotation + translation) for each instance of the model found in the scene (obtained via Absolute Orientation) and a **vector** of :pcl:`Correspondences ` (a vector of vectors of :pcl:`Correspondence `) representing the output of the clustering i.e. each element of this vector is in turn a set of correspondences, representing the correspondences associated to a specific model instance in the scene. If you **only** need the clustered correspondences because you are planning to use them in a different way, you can use the ``cluster`` method. @@ -155,7 +155,7 @@ As a first thing we are showing, for each instance of the model found into the s :language: cpp :lines: 344-360 -The program then shows in a :pcl:`PCLVisualizer ` window the scene cloud with a red overlay where an instance of the model has been found. +The program then shows in a :pcl:`PCLVisualizer ` window the scene cloud with a red overlay where an instance of the model has been found. If the command line switches ``-k`` and ``-c`` have been used, the program also shows a "stand-alone" rendering of the model cloud. If keypoint visualization is enabled, keypoints are displayed as blue dots and if correspondence visualization has been enabled they are shown as a green line for each correspondence which *survived* the clustering process. .. literalinclude:: sources/correspondence_grouping/correspondence_grouping.cpp diff --git a/doc/tutorials/content/don_segmentation.rst b/doc/tutorials/content/don_segmentation.rst index 70652f42..4d5b0569 100644 --- a/doc/tutorials/content/don_segmentation.rst +++ b/doc/tutorials/content/don_segmentation.rst @@ -8,7 +8,6 @@ In this tutorial we will learn how to use Difference of Normals features, implem This algorithm performs a scale based segmentation of the given input point cloud, finding points that belong within the scale parameters given. -.. donpipeline:: .. figure:: images/donpipelinesmall.jpg :align: center @@ -28,7 +27,6 @@ Formally the Difference of Normals operator is defined, where :math:`$r_s, r_l \in \mathbb{R}$`, :math:`$r_s`_ +`pcl_features `_ library. The default FPFH implementation uses 11 binning subdivisions (e.g., each of the diff --git a/doc/tutorials/content/generate_local_doc.rst b/doc/tutorials/content/generate_local_doc.rst new file mode 100644 index 00000000..0c71f5e6 --- /dev/null +++ b/doc/tutorials/content/generate_local_doc.rst @@ -0,0 +1,74 @@ +.. _generate_local_doc: + +====================================== +Generate a local documentation for PCL +====================================== + +For practical reasons you might want to have a local documentation which corresponds to your +PCL version. In this tutorial you will learn how to generate it and how to set up Apache so that +the search bar works. + +This tutorial was written for Ubuntu 12.04 and 14.04, feel free to edit it on GitHub to add your platform. + +Dependencies +============ + +You need to install a few dependencies in order to be able to generate the documentation:: + + $ sudo apt-get install doxygen graphviz sphinx3 python-pip + $ sudo pip install sphinxcontrib-doxylink + +Generate the documentation +========================== + +Go into the build folder of PCL where you've configured it (`see tutorial `_) and enter:: + + $ make doc + +Then you can open the documentation with your browser, for example:: + + $ firefox doc/doxygen/html/index.html + +The documentation has been generated in your PCL build directory but it is not installed; if you wish to install it just do:: + + $ sudo make install + +The default PCL ``CMAKE_INSTALL_PREFIX`` is ``/usr/local``, this means the documentation will be located in ``/usr/local/share/doc/pcl-1.7/html/index.html`` + +.. note:: + You will quickly notice that the search bar doesn't work! (searching opens "search.php" instead of searching) + +Installing and configuring Apache +================================= + +Apache (`The Apache HTTP Server `_) is a web server application, in this section you will +learn how to configure Apache in order to be able to use the search feature within your offline documentation. + +First you need to install Apache and php:: + + $ sudo apt-get install apache2 php5 libapache2-mod-php5 + +Then you need to edit the default website location:: + + $ sudo gedit /etc/apache2/sites-available/000-default.conf + +Change ``DocumentRoot`` (default = ``/var/www/html``) to ``/usr/local/share/doc/pcl-1.7/html/`` (or your local PCL doc build path) + +After that change the Apache directory options:: + + $ sudo gedit +153 /etc/apache2/apache2.conf + +Replace the paragraph at line 153 with:: + + + #Options FollowSymLinks + Options Indexes FollowSymLinks Includes ExecCGI + AllowOverride All + Order deny,allow + Allow from all + + +Restart Apache and the search bar will now work if you open ``localhost``:: + + $ sudo /etc/init.d/apache2 restart + $ firefox localhost diff --git a/doc/tutorials/content/images/bspline_bunny.png b/doc/tutorials/content/images/bspline_bunny.png new file mode 100644 index 00000000..90fbd1b9 Binary files /dev/null and b/doc/tutorials/content/images/bspline_bunny.png differ diff --git a/doc/tutorials/content/images/eigen_vectors.png b/doc/tutorials/content/images/eigen_vectors.png new file mode 100644 index 00000000..d49dc620 Binary files /dev/null and b/doc/tutorials/content/images/eigen_vectors.png differ diff --git a/doc/tutorials/content/images/ihs_lion_photo.JPG b/doc/tutorials/content/images/ihs_lion_photo.JPG deleted file mode 100644 index f3218594..00000000 Binary files a/doc/tutorials/content/images/ihs_lion_photo.JPG and /dev/null differ diff --git a/doc/tutorials/content/images/ihs_lion_photo.jpg b/doc/tutorials/content/images/ihs_lion_photo.jpg new file mode 100644 index 00000000..f3218594 Binary files /dev/null and b/doc/tutorials/content/images/ihs_lion_photo.jpg differ diff --git a/doc/tutorials/content/images/interactive_icp/add_monkey.png b/doc/tutorials/content/images/interactive_icp/add_monkey.png new file mode 100644 index 00000000..aff9cee1 Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/add_monkey.png differ diff --git a/doc/tutorials/content/images/interactive_icp/add_sub.png b/doc/tutorials/content/images/interactive_icp/add_sub.png new file mode 100644 index 00000000..df6b4720 Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/add_sub.png differ diff --git a/doc/tutorials/content/images/interactive_icp/animation.gif b/doc/tutorials/content/images/interactive_icp/animation.gif new file mode 100644 index 00000000..7cdea51b Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/animation.gif differ diff --git a/doc/tutorials/content/images/interactive_icp/del_cube.png b/doc/tutorials/content/images/interactive_icp/del_cube.png new file mode 100644 index 00000000..6d5dcdcb Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/del_cube.png differ diff --git a/doc/tutorials/content/images/interactive_icp/export.png b/doc/tutorials/content/images/interactive_icp/export.png new file mode 100644 index 00000000..f6a0475d Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/export.png differ diff --git a/doc/tutorials/content/images/interactive_icp/icp-1.png b/doc/tutorials/content/images/interactive_icp/icp-1.png new file mode 100644 index 00000000..e02bfa4c Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/icp-1.png differ diff --git a/doc/tutorials/content/images/interactive_icp/monkey.png b/doc/tutorials/content/images/interactive_icp/monkey.png new file mode 100644 index 00000000..ebe3467e Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/monkey.png differ diff --git a/doc/tutorials/content/images/interactive_icp/sub2.png b/doc/tutorials/content/images/interactive_icp/sub2.png new file mode 100644 index 00000000..84accc23 Binary files /dev/null and b/doc/tutorials/content/images/interactive_icp/sub2.png differ diff --git a/doc/tutorials/content/images/matrix_transform/cube.png b/doc/tutorials/content/images/matrix_transform/cube.png new file mode 100644 index 00000000..0ba4187d Binary files /dev/null and b/doc/tutorials/content/images/matrix_transform/cube.png differ diff --git a/doc/tutorials/content/images/matrix_transform/cube_big.png b/doc/tutorials/content/images/matrix_transform/cube_big.png new file mode 100644 index 00000000..26bad1c0 Binary files /dev/null and b/doc/tutorials/content/images/matrix_transform/cube_big.png differ diff --git a/doc/tutorials/content/images/moment_of_inertia.png b/doc/tutorials/content/images/moment_of_inertia.png new file mode 100644 index 00000000..d5f3fe9d Binary files /dev/null and b/doc/tutorials/content/images/moment_of_inertia.png differ diff --git a/doc/tutorials/content/images/pcl_with_eclipse/build_tab.gif b/doc/tutorials/content/images/pcl_with_eclipse/build_tab.gif new file mode 100644 index 00000000..4fcb208b Binary files /dev/null and b/doc/tutorials/content/images/pcl_with_eclipse/build_tab.gif differ diff --git a/doc/tutorials/content/images/pcl_with_eclipse/eclipse.png b/doc/tutorials/content/images/pcl_with_eclipse/eclipse.png new file mode 100644 index 00000000..f6f48952 Binary files /dev/null and b/doc/tutorials/content/images/pcl_with_eclipse/eclipse.png differ diff --git a/doc/tutorials/content/images/pcl_with_eclipse/lrun_obj.gif b/doc/tutorials/content/images/pcl_with_eclipse/lrun_obj.gif new file mode 100644 index 00000000..57f41022 Binary files /dev/null and b/doc/tutorials/content/images/pcl_with_eclipse/lrun_obj.gif differ diff --git a/doc/tutorials/content/images/progressive_morphological_filter.png b/doc/tutorials/content/images/progressive_morphological_filter.png new file mode 100644 index 00000000..50e91e50 Binary files /dev/null and b/doc/tutorials/content/images/progressive_morphological_filter.png differ diff --git a/doc/tutorials/content/images/projected_cloud.png b/doc/tutorials/content/images/projected_cloud.png new file mode 100644 index 00000000..98ed5303 Binary files /dev/null and b/doc/tutorials/content/images/projected_cloud.png differ diff --git a/doc/tutorials/content/images/qt_visualizer/pcl_visualizer.gif b/doc/tutorials/content/images/qt_visualizer/pcl_visualizer.gif new file mode 100644 index 00000000..abaf97ab Binary files /dev/null and b/doc/tutorials/content/images/qt_visualizer/pcl_visualizer.gif differ diff --git a/doc/tutorials/content/images/qt_visualizer/qt.png b/doc/tutorials/content/images/qt_visualizer/qt.png new file mode 100644 index 00000000..25cc52dc Binary files /dev/null and b/doc/tutorials/content/images/qt_visualizer/qt.png differ diff --git a/doc/tutorials/content/images/qt_visualizer/qt_config.png b/doc/tutorials/content/images/qt_visualizer/qt_config.png new file mode 100644 index 00000000..52a53c27 Binary files /dev/null and b/doc/tutorials/content/images/qt_visualizer/qt_config.png differ diff --git a/doc/tutorials/content/images/qt_visualizer/ui.png b/doc/tutorials/content/images/qt_visualizer/ui.png new file mode 100644 index 00000000..566db6b3 Binary files /dev/null and b/doc/tutorials/content/images/qt_visualizer/ui.png differ diff --git a/doc/tutorials/content/images/rops_feature.png b/doc/tutorials/content/images/rops_feature.png new file mode 100644 index 00000000..7d2ad88a Binary files /dev/null and b/doc/tutorials/content/images/rops_feature.png differ diff --git a/doc/tutorials/content/images/visualization/bunny.jpg b/doc/tutorials/content/images/visualization/bunny.jpg new file mode 100644 index 00000000..c3738896 Binary files /dev/null and b/doc/tutorials/content/images/visualization/bunny.jpg differ diff --git a/doc/tutorials/content/images/visualization/ex1.jpg b/doc/tutorials/content/images/visualization/ex1.jpg new file mode 100644 index 00000000..8a61fa8b Binary files /dev/null and b/doc/tutorials/content/images/visualization/ex1.jpg differ diff --git a/doc/tutorials/content/images/visualization/ex2.jpg b/doc/tutorials/content/images/visualization/ex2.jpg new file mode 100644 index 00000000..00699df9 Binary files /dev/null and b/doc/tutorials/content/images/visualization/ex2.jpg differ diff --git a/doc/tutorials/content/images/visualization/ex3.jpg b/doc/tutorials/content/images/visualization/ex3.jpg new file mode 100644 index 00000000..1695e644 Binary files /dev/null and b/doc/tutorials/content/images/visualization/ex3.jpg differ diff --git a/doc/tutorials/content/images/visualization/ex4.jpg b/doc/tutorials/content/images/visualization/ex4.jpg new file mode 100644 index 00000000..b22a821b Binary files /dev/null and b/doc/tutorials/content/images/visualization/ex4.jpg differ diff --git a/doc/tutorials/content/images/visualization/ex5.jpg b/doc/tutorials/content/images/visualization/ex5.jpg new file mode 100644 index 00000000..1cc3ac02 Binary files /dev/null and b/doc/tutorials/content/images/visualization/ex5.jpg differ diff --git a/doc/tutorials/content/images/visualization/histogram.jpg b/doc/tutorials/content/images/visualization/histogram.jpg new file mode 100644 index 00000000..6951cf7a Binary files /dev/null and b/doc/tutorials/content/images/visualization/histogram.jpg differ diff --git a/doc/tutorials/content/images/visualization/normals.jpg b/doc/tutorials/content/images/visualization/normals.jpg new file mode 100644 index 00000000..ae100129 Binary files /dev/null and b/doc/tutorials/content/images/visualization/normals.jpg differ diff --git a/doc/tutorials/content/images/visualization/pcs.jpg b/doc/tutorials/content/images/visualization/pcs.jpg new file mode 100644 index 00000000..6f222a02 Binary files /dev/null and b/doc/tutorials/content/images/visualization/pcs.jpg differ diff --git a/doc/tutorials/content/images/visualization/range_image.jpg b/doc/tutorials/content/images/visualization/range_image.jpg new file mode 100644 index 00000000..4865d9c8 Binary files /dev/null and b/doc/tutorials/content/images/visualization/range_image.jpg differ diff --git a/doc/tutorials/content/images/visualization/shapes.jpg b/doc/tutorials/content/images/visualization/shapes.jpg new file mode 100644 index 00000000..351de84a Binary files /dev/null and b/doc/tutorials/content/images/visualization/shapes.jpg differ diff --git a/doc/tutorials/content/images/windows/SVNCheckout_pcl_trunk.png b/doc/tutorials/content/images/windows/SVNCheckout_pcl_trunk.png deleted file mode 100644 index 05f4316f..00000000 Binary files a/doc/tutorials/content/images/windows/SVNCheckout_pcl_trunk.png and /dev/null differ diff --git a/doc/tutorials/content/implicit_shape_model.rst b/doc/tutorials/content/implicit_shape_model.rst index a29e41bc..38de1a73 100644 --- a/doc/tutorials/content/implicit_shape_model.rst +++ b/doc/tutorials/content/implicit_shape_model.rst @@ -4,7 +4,7 @@ Implicit Shape Model -------------------- In this tutorial we will learn how to use the implicit shape model algorithm implemented in the ``pcl::ism::ImplicitShapeModel`` class. -This algorithm was described in the article `"Hough Transforms and 3D SURF for robust three dimensional classification" `_ by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool. +This algorithm was described in the article `"Hough Transforms and 3D SURF for robust three dimensional classification" `_ by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool. This algorithm is a combination of generalized Hough transform and the Bag of Features approach and its purpose is as follows. Having some training set - point clouds of different objects of the known class - the algorithm computes a certain model which will be later used to predict an object center in the given cloud that wasn't a part of the training set. Theoretical Primer @@ -44,7 +44,7 @@ After the training process is done and the trained model (weights, directions et #. Previous step gives us a set of directions to the expected center and the power for each vote. In order to get single point that corresponds to center these votes need to be analysed. For this purpose algorithm uses the non maxima suppression approach. User just needs to pass the radius of the object of interest and the rest will be done by the ``ISMVoteList::findStrongestPeaks ()`` method. For more comprehensive information please refer to the article -`"Hough Transforms and 3D SURF for robust three dimensional classification" `_. +`"Hough Transforms and 3D SURF for robust three dimensional classification" `_. The code -------- @@ -53,11 +53,11 @@ First of all you will need the set of point clouds for this tutorial - training Below is the list of clouds that are well suited for this tutorial (they were borrowed from the Ohio dataset). Clouds for training: - * `Cat `_ - * `Horse `_ - * `Lioness `_ - * `Michael `_ - * `Wolf `_ + * `Cat (train) `_ + * `Horse (train) `_ + * `Lioness (train) `_ + * `Michael (train) `_ + * `Wolf (train) `_ Clouds for testing: * `Cat `_ diff --git a/doc/tutorials/content/in_hand_scanner.rst b/doc/tutorials/content/in_hand_scanner.rst index df1a1843..de3ef4d0 100644 --- a/doc/tutorials/content/in_hand_scanner.rst +++ b/doc/tutorials/content/in_hand_scanner.rst @@ -125,7 +125,7 @@ How to use it In the following section I will go through the steps to scan in a model of the 'lion' object which is about 15 cm high. -.. image:: images/ihs_lion_photo.JPG +.. image:: images/ihs_lion_photo.jpg :alt: Lion object. :width: 500px :height: 782px diff --git a/doc/tutorials/content/index.rst b/doc/tutorials/content/index.rst index b47e96b5..b927a2ad 100644 --- a/doc/tutorials/content/index.rst +++ b/doc/tutorials/content/index.rst @@ -3,7 +3,7 @@ The following links describe a set of basic PCL tutorials. Please note that their source codes may already be provided as part of the PCL regular releases, so check there before you start copy & pasting the code. The list of tutorials -below is automatically generated from reST files located in our SVN repository. +below is automatically generated from reST files located in our git repository. .. note:: @@ -56,7 +56,7 @@ Basic Usage * :ref:`basic_structures` ====== ====== - |mi_0| Title: **Getting Started / Basic Structures** + |mi_1| Title: **Getting Started / Basic Structures** Author: *Radu B. Rusu* @@ -65,13 +65,13 @@ Basic Usage Presents the basic data structures in PCL and discusses their usage with a simple code example. ====== ====== - .. |mi_0| image:: images/pcl_logo.png + .. |mi_1| image:: images/pcl_logo.png :height: 75px * :ref:`using_pcl_pcl_config` ====== ====== - |mi_1| Title: **Using PCL in your own project** + |mi_2| Title: **Using PCL in your own project** Author: *Nizar Sallem* @@ -80,13 +80,13 @@ Basic Usage In this tutorial, we will learn how to link your own project to PCL using cmake. ====== ====== - .. |mi_1| image:: images/pcl_logo.png + .. |mi_2| image:: images/pcl_logo.png :height: 75px * :ref:`building_pcl` ====== ====== - |mi_2| Title: **Explaining PCL's cmake options** + |mi_3| Title: **Explaining PCL's cmake options** Author: *Nizar Sallem* @@ -95,13 +95,13 @@ Basic Usage In this tutorial, we will explain the basic PCL cmake options, and ways to tweak them to fit your project. ====== ====== - .. |mi_2| image:: images/pcl_ccmake.png + .. |mi_3| image:: images/pcl_ccmake.png :height: 100px * :ref:`compiling_pcl_dependencies_windows` ====== ====== - |mi_3| Title: **Compiling PCL's dependencies from source on Windows** + |mi_4| Title: **Compiling PCL's dependencies from source on Windows** Authors: *Alessio Placitelli* and *Mourad Boufarguine* @@ -110,13 +110,13 @@ Basic Usage In this tutorial, we will explain how to compile PCL's 3rd party dependencies from source on Microsoft Windows. ====== ====== - .. |mi_3| image:: images/windows_logo.png + .. |mi_4| image:: images/windows_logo.png :height: 100px * :ref:`compiling_pcl_windows` ====== ====== - |mi_4| Title: **Compiling PCL on Windows** + |mi_5| Title: **Compiling PCL on Windows** Author: *Mourad Boufarguine* @@ -125,13 +125,13 @@ Basic Usage In this tutorial, we will explain how to compile PCL on Microsoft Windows. ====== ====== - .. |mi_4| image:: images/windows_logo.png + .. |mi_5| image:: images/windows_logo.png :height: 100px * :ref:`compiling_pcl_macosx` ====== ====== - |mi_5| Title: **Compiling PCL and its dependencies from MacPorts and source on Mac OS X** + |mi_6| Title: **Compiling PCL and its dependencies from MacPorts and source on Mac OS X** Author: *Justin Rosen* @@ -140,13 +140,13 @@ Basic Usage This tutorial explains how to build the Point Cloud Library **from MacPorts and source** on Mac OS X platforms. ====== ====== - .. |mi_5| image:: images/macosx_logo.png + .. |mi_6| image:: images/macosx_logo.png :height: 100px * :ref:`installing_homebrew` ====== ====== - |mi_6| Title: **Installing on Mac OS X using Homebrew** + |mi_7| Title: **Installing on Mac OS X using Homebrew** Author: *Geoffrey Biggs* @@ -155,24 +155,69 @@ Basic Usage This tutorial explains how to install the Point Cloud Library on Mac OS X using Homebrew. Both direct installation and compiling PCL from source are explained. ====== ====== - .. |mi_6| image:: images/macosx_logo.png + .. |mi_7| image:: images/macosx_logo.png :height: 100px * :ref:`using_pcl_with_eclipse` ====== ====== - |mi_7| Title: **Using Eclipse as your PCL trunk editor** + |mi_8| Title: **Using Eclipse as your PCL editor** Author: *Koen Buys* - Compatibility: > PCL 1.7 + Compatibility: PCL git master - This tutorial shows you how to get your PCL trunk as a project in Eclipse. + This tutorial shows you how to get your PCL as a project in Eclipse. ====== ====== - .. |mi_7| image:: images/pcl_logo.png + .. |mi_8| image:: images/pcl_with_eclipse/eclipse.png + :height: 100px + + * :ref:`generate_local_doc` + + ======= ====== + |mi_11| Title: **Generate a local documentation for PCL** + + Author: *Victor Lamoine* + + Compatibility: PCL > 1.0 + + This tutorial shows you how to generate and use a local documentation for PCL. + ======= ====== + + .. |mi_11| image:: images/pcl_logo.png :height: 75px + * :ref:`qt_visualizer` + + ====== ====== + |mi_9| Title: **Create a PCL visualizer in Qt with cmake** + + Author: *Victor Lamoine* + + Compatibility: > PCL 1.5 + + This tutorial shows you how to create a PCL visualizer within a Qt application. + ====== ====== + + .. |mi_9| image:: images/qt_visualizer/qt.png + :height: 128px + + * :ref:`matrix_transform` + + ======= ====== + |mi_10| Title: **Using matrixes to transform a point cloud** + + Author: *Victor Lamoine* + + Compatibility: > PCL 1.5 + + This tutorial shows you how to transform a point cloud using a matrix. + ======= ====== + + .. |mi_10| image:: images/matrix_transform/cube.png + :height: 120px + .. _advanced_usage: Advanced Usage @@ -319,6 +364,36 @@ Features .. |fe_7| image:: images/narf_keypoint_extraction.png :height: 100px + * :ref:`moment_of_inertia` + + ====== ====== + |fe_8| Title: **Moment of inertia and eccentricity based descriptors** + + Author: *Sergey Ushakov* + + Compatibility: > PCL 1.7 + + In this tutorial we will learn how to compute moment of inertia and eccentricity of the cloud. In addition to this we will learn how to extract AABB and OBB. + ====== ====== + + .. |fe_8| image:: images/moment_of_inertia.png + :height: 100px + + * :ref:`rops_feature` + + ====== ====== + |fe_9| Title: **RoPs (Rotational Projection Statistics) feature** + + Author: *Sergey Ushakov* + + Compatibility: > PCL 1.7 + + In this tutorial we will learn how to compute RoPS feature. + ====== ====== + + .. |fe_9| image:: images/rops_feature.png + :height: 100px + .. _filtering_tutorial: Filtering @@ -736,6 +811,21 @@ Registration .. |re_3| image:: images/iterative_closest_point.gif :height: 100px + * :ref:`interactive_icp` + + ====== ====== + |re_7| Title: **Interactive ICP** + + Author: *Victor Lamoine* + + Compatibility: > PCL 1.5 + + This tutorial will teach you how to build an interactive ICP program + ====== ====== + + .. |re_7| image:: images/interactive_icp/monkey.png + :height: 120px + * :ref:`normal_distributions_transform` ====== ====== @@ -758,7 +848,7 @@ Registration Author: *Martin Saelzle* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 This document shows how to use the In-hand scanner applications to obtain colored models of small objects with RGB-D cameras. ====== ====== @@ -860,7 +950,7 @@ Segmentation Author: *Sergey Ushakov* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 In this tutorial we will learn how to use region growing segmentation algorithm. ====== ====== @@ -875,7 +965,7 @@ Segmentation Author: *Sergey Ushakov* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 In this tutorial we will learn how to use color-based region growing segmentation algorithm. ====== ====== @@ -890,7 +980,7 @@ Segmentation Author: *Sergey Ushakov* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 In this tutorial we will learn how to use min-cut based segmentation algorithm. ====== ====== @@ -905,7 +995,7 @@ Segmentation Author: *Frits Florentinus* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 This tutorial describes how to use the Conditional Euclidean Clustering class in PCL: A segmentation algorithm that clusters points based on Euclidean distance and a user-customizable condition that needs to hold. @@ -921,7 +1011,7 @@ Segmentation Author: *Yani Ioannou* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 In this tutorial we will learn how to use the difference of normals feature for segmentation. ====== ====== @@ -936,13 +1026,43 @@ Segmentation Author: *Jeremie Papon* - Compatibility: > PCL 1.7 + Compatibility: >= PCL 1.7 In this tutorial, we show to break a pointcloud into the mid-level supervoxel representation. ====== ====== .. |se_9| image:: images/supervoxel_clustering_small.png :height: 100px + + * :ref:`progressive_morphological_filtering` + + ======= ====== + |se_10| Title: **Progressive Morphological Filtering** + + Author: *Brad Chambers* + + Compatibility: >= PCL 1.8 + + In this tutorial, we show how to segment a point cloud into ground and non-ground returns. + ======= ====== + + .. |se_10| image:: images/progressive_morphological_filter.png + :height: 100px + + * :ref:`model_outlier_removal` + + ======= ====== + |se_11| Title: **Model outlier removal** + + Author: *Timo Häckel* + + Compatibility: >= PCL 1.7.2 + + This tutorial describes how to extract points from a point cloud using SAC models + ======= ====== + + .. |se_11| image:: images/pcl_logo.png + :height: 75px .. _surface_tutorial: @@ -994,6 +1114,22 @@ Surface .. |su_3| image:: images/greedy_triangulation.png :height: 100px + * :ref:`bspline_fitting` + + ====== ====== + |su_4| Title: **Fitting trimmed B-splines to unordered point clouds** + + Author: *Thomas Mörwald* + + Compatibility: > PCL 1.7 + + In this tutorial we will learn how to reconstruct a smooth surface from an unordered point-cloud by fitting trimmed B-splines. + ====== ====== + + .. |su_4| image:: images/bspline_bunny.png + :height: 100px + + .. _visualization_tutorial: Visualization @@ -1059,6 +1195,21 @@ Visualization .. |vi_4| image:: images/pcl_plotter_comprational.png :height: 100px + * :ref:`visualization` + + ====== ====== + |vi_5| Title: **PCL Visualization overview** + + Author: *Radu B. Rusu* + + Compatibility: >= PCL 1.0 + + This tutorial will give an overview on the usage of the PCL visualization tools. + ====== ====== + + .. |vi_5| image:: images/visualization_small.png + :height: 120px + .. _applications_tutorial: Applications @@ -1109,21 +1260,6 @@ Applications .. |ap_3| image:: images/mobile_streaming_1.jpg :height: 100px - * :ref:`using_kinfu_large_scale` - - ====== ====== - |ap_4| Title: **Using Kinfu Large Scale to generate a textured mesh** - - Author: *Francisco Heredia and Raphael Favier* - - Compatibility: > PCL 1.5 - - This tutorial demonstrates how to use KinFu Large Scale to produce a mesh from a room, and apply texture information in post-processing for a more appealing visual result. - ====== ====== - - .. |ap_4| image:: images/using_kinfu_large_scale.jpg - :height: 100px - * :ref:`ground_based_rgbd_people_detection` ====== ====== @@ -1131,7 +1267,7 @@ Applications Author: *Matteo Munaro* - Compatibility: > PCL 1.7 trunk + Compatibility: >= PCL 1.7 This tutorial presents a method for detecting people on a ground plane with RGB-D data. ====== ====== diff --git a/doc/tutorials/content/installing_homebrew.rst b/doc/tutorials/content/installing_homebrew.rst index 132457d5..060a57cd 100644 --- a/doc/tutorials/content/installing_homebrew.rst +++ b/doc/tutorials/content/installing_homebrew.rst @@ -4,9 +4,7 @@ Installing on Mac OS X using Homebrew ===================================== This tutorial explains how to install the Point Cloud Library on Mac OS -X using Homebrew. Two approaches are described: using the existing PCL -formula to automatically install, and using Homebrew only for the -dependencies with PCL compiled and installed from source. +X using Homebrew. .. image:: images/macosx_logo.png :alt: Mac OS X logo @@ -17,308 +15,54 @@ dependencies with PCL compiled and installed from source. Prerequisites ============= -No matter which method you choose, you will need to have Homebrew -installed. If you do not already have a Homebrew installation, see the +You will need to have Homebrew installed. If you do not already have a Homebrew installation, see the `Homebrew homepage`_ for installation instructions. .. _`Homebrew homepage`: - http://mxcl.github.com/homebrew/ + http://brew.sh/ .. _homebrew_all: Using the formula ================= -Homebrew includes a formula for installing PCL. This will automatically -install all necessary dependencies and provides options for controlling +The PCL formula is not in Homebrew official repositories yet, but it will be after 1.7.2. +For now it resides at `Fran6co's repository `_ and can be tapped from there. +This will automatically install all necessary dependencies and provides options for controlling which parts of PCL are installed. .. note:: - The PCL formula is currently in development. It will be submitted to - Homebrew shortly. Until then, you can download it from - `PCL.RB `_. To prepare it, - follow these steps: + To prepare it, follow these steps: #. Install Homebrew. See the Homebrew website for instructions. #. Execute ``brew update``. - #. Download the formula and place it in - ``/usr/local/Library/Formula`` (or an appropriate location if you - installed Homebrew somewhere else). + #. Execute ``brew tap homebrew/versions``. + #. Execute ``brew tap homebrew/science``. + #. Execute ``brew tap fran6co/cv``. -To install using the formula, execute the following command:: +To install the latest version using the formula, execute the following command:: - $ brew install pcl + $ brew install pcl --HEAD You can specify options to control which parts of PCL are installed. For -example, to disable the Python bindings and visualisation, and enable the -documentation, execute the following command:: +example, to build just the libraries without extra dependencies, execute the following command:: - $ brew install pcl --nopython --novis --doc + $ brew install pcl --HEAD --without-apps --without-tools --without-vtk --without-qvtk --without-qt For a full list of the available options, see the formula's help:: $ brew options pcl -You can test the installation by executing the tests included with PCL:: - - $ brew test pcl - Once PCL is installed, you may wish to periodically upgrade it. Update Homebrew and, if a PCL update is available, upgrade:: $ brew update $ brew upgrade pcl - -.. _homebrew_deps: - -Installing from source -====================== - -In order to compile every component of PCL, several dependencies must be -installed. Homebrew includes formulae for all PCL dependencies except -OpenNI, so this step is relatively easy. - -Dependency information ----------------------- - -Required -'''''''' - -The following libraries are **Required** to build PCL. - -- **CMake** version >= 2.8.3 (http://www.cmake.org) - Cross-platform, open-source build system. - - .. note:: - - Though not a dependency per se, the PCL community relies heavily on CMake - for the libraries build process. - -- **Boost** version >= 1.46.1 (http://www.boost.org/) - Provides free peer-reviewed portable C++ source libraries. Used for shared - pointers, and threading. - -- **Eigen** version >= 3.0.0 (http://eigen.tuxfamily.org/) - Unified matrix library. Used as the matrix backend for SSE optimized math. - -- **FLANN** version >= 1.6.8 - (http://www.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN) - Library for performing fast approximate nearest neighbor searches in high - dimensional spaces. Used in `kdtree` for fast approximate nearest neighbors - search. - -- **Visualization ToolKit (VTK)** version >= 5.6.1 (http://www.vtk.org/) - Software system for 3D computer graphics, image processing and visualization. - Used in `visualization` for 3D point cloud rendering and visualization. - - .. note:: - - The current release of PCL (1.2) does not support visualisation on - Mac OS X. PCL 1.3 is expected to correct this. - -Optional -'''''''' - -The following libraries are *optional* and provide extended functionality -within PCL, such as Kinect support. - -- **Qhull** version >= 2011.1 (http://www.qhull.org/) - computes the convex hull, Delaunay triangulation, Voronoi diagram, halfspace - intersection about a point, furthest-site Delaunay triangulation, and - furthest-site Voronoi diagram. Used for convex/concave hull decompositions - in `surface`. - -- **libusb** (http://www.libusb.org/) - A library that gives user level applications uniform access to USB devices - across many different operating systems. - -- **PCL Patched OpenNI/Sensor** (http://www.openni.org/) - The OpenNI Framework provides the interface for physical devices and for - middleware components. Used to grab point clouds from OpenNI compliant - devices. - -- **Doxygen** (http://www.doxygen.org) - A documentation system for C++, C, Java, Objective-C, Python, IDL (Corba and - Microsoft flavors), Fortran, VHDL, PHP, C#, and to some extent D. - -- **Sphinx** (http://sphinx.pocoo.org/) - A tool that makes it easy to create intelligent and beautiful - documentation. PCL uses this and Doxygen to compile the - documentation. - -Advanced (Developers) -''''''''''''''''''''' - -The following libraries are *advanced* and provide additional functionality -for PCL developers: - -- **googletest** version >= 1.6.0 (http://code.google.com/p/googletest/) - Google's framework for writing C++ tests on a variety of platforms. Used - to build test units. - -Installing dependencies ------------------------ - -Most of the dependencies will be installed via Homebrew. The remainder, -we will compile from source. - -Install CMake -''''''''''''' -:: - - $ brew install cmake - -Install Boost -''''''''''''' -:: - - $ brew install boost - -Install Eigen -''''''''''''' -:: - - $ brew install eigen - -Install FLANN -''''''''''''' -:: - - $ brew install flann - -Install VTK -''''''''''' - -To install VTK, you need a modified Homebrew formula for VTK. Please -download it from `VTK.RB `_. - -:: - - $ brew install vtk --qt OR --qt-extern [if you have your own Qt installation already] - -.. note:: - - If you are installing PCL 1.2, you may skip this dependency. - -Install Qhull (optional) -'''''''''''''''''''''''' -:: - - $ brew install qhull - -Install libusb (optional) -''''''''''''''''''''''''' -:: - - $ brew install libusb - -Install Doxygen (optional) -'''''''''''''''''''''''''' -:: - - $ brew install doxygen - -Install Sphinx (optional) -''''''''''''''''''''''''' -:: - - $ brew install sphinx - -Install patched OpenNI and Sensor -''''''''''''''''''''''''''''''''' - -Download the patched versions of OpenNI and Sensor: `openni_osx.zip -`_ and -`ps_engine_osx.zip -`_. - -Extract, build, fix permissions and install OpenNI:: - - $ unzip openni_osx.zip -d openni_osx - $ cd openni_osx/Redist - $ chmod -R a+r Bin Include Lib - $ chmod -R a+x Bin Lib - $ chmod a+x Include/MacOSX Include/Linux-* - $ sudo ./install.sh - -In addition the PrimeSense XML configuration file found within the -patched OpenNI download needs its permissions fixed and to be copied to -the correct location to for the Kinect to work on Mac OS X:: - - $ chmod a+r openni_osx/Redist/Samples/Config/SamplesConfig.xml - $ sudo cp openni_osx/Redist/Samples/Config/SamplesConfig.xml /etc/primesense/ - -Extract, build, fix permissions and install Sensor:: - - $ unzip ps_engine_osx.zip -d ps_engine_osx - $ cd ps_engine_osx/Redist - $ chmod -R a+r Bin Lib Config Install - $ chmod -R a+x Bin Lib - $ sudo ./install.sh - -Compiling PCL -------------- - -At this point you should have everything needed installed to build PCL -with almost no additional configuration. - -Check out the PCL source from the trunk:: - - $ svn co http://svn.pointclouds.org/pcl/trunk pcl - $ cd pcl - -Create the build directories, configure CMake, build and install:: - - $ mkdir build - $ cd build - $ cmake .. - $ make - $ sudo make install - -.. note:: - - If you are installing PCL 1.2, disable the visualisation module, or - compilation will fail:: - - $ cmake .. -DBUILD_visualization:BOOL=OFF - -The customization of the build process is out of the scope of this tutorial and -is covered in greater detail in the :ref:`building_pcl` tutorial. - -Compiling the documentation (optional) --------------------------------------- - -If you installed the Doxygen and Sphinx dependencies, you can compile -the documentation after compiling PCL. To do so, use this command:: - - $ make doc - -The tutorials can be built using this command:: - - $ make Tutorials - -.. note:: - - The Homebrew formula for Sphinx may not install the extension - necessary to link to the Doxygen-generated documentation. In this - case, you will need to install Sphinx and the extension manually. - Start by installing Sphinx using easy_install:: - - $ easy_install -U Sphinx - - Next, install Mercurial (see the Mercurial documentation) and the - extension:: - - $ hg clone http://bitbucket.org/birkenfeld/sphinx-contrib - $ cd sphinx-contrib/doxylink - $ python setup.py install - Using PCL --------- Now that PCL in installed, you can start using the library in your own projects by following the :ref:`using_pcl_pcl_config` tutorial. - diff --git a/doc/tutorials/content/interactive_icp.rst b/doc/tutorials/content/interactive_icp.rst new file mode 100644 index 00000000..f2c41cb5 --- /dev/null +++ b/doc/tutorials/content/interactive_icp.rst @@ -0,0 +1,282 @@ +.. _interactive_icp: + +=================================== +Interactive Iterative Closest Point +=================================== + +This tutorial will teach you how to write an interactive ICP viewer. The program will +load a point cloud and apply a rigid transformation on it. After that the ICP algorithm will +align the transformed point cloud with the original. Each time the user presses "space" +an ICP iteration is done and the viewer is refreshed. + +.. contents:: + +Creating a mesh with Blender +============================ +You can easily create a sample point cloud with Blender. +Install and open Blender then delete the cube in the scene by pressing "Del" key : + +.. image:: images/interactive_icp/del_cube.png + :height: 285 + +Add a monkey mesh in the scene : + +.. image:: images/interactive_icp/add_monkey.png + :height: 328 + +Subdivide the original mesh to make it more dense : + +.. image:: images/interactive_icp/add_sub.png + :height: 500 + +Configure the subdivision to 2 or 3 for example : dont forget to apply the modifier + +.. image:: images/interactive_icp/sub2.png + :height: 203 + +Export the mesh into a PLY file : + +.. image:: images/interactive_icp/export.png + :height: 481 + +The code +======== + +First, create a file, let's say, ``interactive_icp.cpp`` in your favorite +editor, and place the following code inside it: + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :linenos: + +The explanations +================ + +Now, let's break down the code piece by piece. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 1-8 + +We include all the headers we will make use of. +**#include ** allows us to use **pcl::transformPointCloud** function. +**#include >** allows us to use parse the arguments given to the program. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 9-12 + +Two typedefs to simplify declarations and code reading. +The bool will help us know when the user asks for the next iteration of ICP + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 14-24 + +This functions takes the reference of a 4x4 matrix and prints the rigid transformation in an human +readable way. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 25-32 + +This function is the callback for the viewer. This function will be called whenever a key is pressed +when the viewer window is on top. If "space" is hit; set the bool to true. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 38-41 + +The 3 point clouds we will use to store the data. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 42-71 + +We check the arguments of the program, set the number of initial ICP iterations +and try to load the PLY file. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 72-91 + +We transform the original point cloud using a rigid matrix transformation. +See the related tutorial in PCL documentation for more information. +**cloud_in** contains the original point cloud. +**cloud_tr** and **cloud_icp** contains the translated/rotated point cloud. +**cloud_tr** is a backup we will use for display (green point cloud). + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 93-101 + +This is the creation of the ICP object. We set the parameters of the ICP algorithm. +**setMaximumIterations(iterations)** sets the number of initial iterations to do (1 +is the default value). We then transform the point cloud into **cloud_icp**. +After the first alignment we set ICP max iterations to 1 for all the next times this +ICP object will be used (when the user presses "space"). + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 103-115 + +Check if the ICP algorithm converged; otherwise exit the program. +In case of success we store the transformation matrix in a 4x4 matrix and +then print the rigid matrix transformation. The reason why we store this +matrix is explained later. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 117-127 + +For the visualization we create two viewports in the visualizer vertically +separated. **bckgr_gray_level** and **txt_gray_lvl** are variables to easily +switch from white background & black text/point cloud to black background & +white text/point cloud. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 129-141 + +We add the original point cloud in the 2 viewports and display it the same color +as **txt_gray_lvl**. We add the point cloud we transformed using the matrix in the left +viewport in green and the point cloud aligned with ICP in red (right viewport). + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 143-150 + +We add descriptions for the point clouds in each viewport so the user knows what is what. +The string stream ss is needed to transform the integer **iterations** into a string. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 152-161 + +We set the two viewports background color according to **bckgr_gray_level**. +To get the camera parameters I simply pressed "C" in the viewer. Then I copied the +parameters into this function to save the camera position / orientation / focal point. +The function **registerKeyboardCallback** allows us to call a function whenever the +users pressed a keyboard key when viewer windows is on top. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 163-166 + +This is the normal behaviour if no key is pressed. The viewer waits to exit. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 169-172 + +If the user press any key of the keyboard, the function **keyboardEventOccurred** is called; +this function checks if the key is "space" or not. If yes the global bool **next_iteration** +is set to true, allowing the viewer loop to enter the next part of the code: the ICP object +is called to align the meshes. Remember we already configured this object input/output clouds +and we set max iterations to 1 in lines 90-93. + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 167-194 + +As before we check if ICP as converged, if not we exit the program. +**printf("\033[11A");** is a little trick to go up 11 lines in the terminal to write +over the last matrix displayed. In short it allows to replace text instead of writing +new lines; making the ouptut more readable. +We increment **iterations** to update the text value in the visualizer. + +Now we want to display the rigid transformation from the original transformed point cloud to +the current alignment made by ICP. The function **getFinalTransformation()** returns the rigid +matrix transformation done during the iterations (here: 1 iteration). This means that if you have already +done 10 iterations this function returns the matrix to transform the point cloud from the iteration 10 to 11. + +This is not what we want. If we multiply the last matrix with the new one the result is the transformation matrix from +the start to the current iteration. This is basically how it works :: + + matrix[ICP 0->1]*matrix[ICP 1->2]*matrix[ICP 2->3] = matrix[ICP 0->3] + +While this is mathematically true, you will easilly notice that this is not true in this program due to roundings. +This is why I introduced the initial ICP iteration parameters. Try to launch the program with 20 initial iterations +and save the matrix in a text file. Launch the same program with 1 initial iteration and press space till you go to 20 +iterations. You will a notice a slight difference. The matrix with 20 initial iterations is much more accurate than the +one multiplied 19 times. + + +.. literalinclude:: sources/interactive_icp/interactive_icp.cpp + :language: cpp + :lines: 195-199 + +We set the bool to false and the rest is the ending of the program. + +Compiling and running the program +================================= + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/interactive_icp/CMakeLists.txt + :language: cmake + :linenos: + +After you have made the executable, you can run it. Simply do:: + + $ ./interactive_icp monkey.ply 1 + +Remember that the matrix displayed is not very accurate if you do a lot of iterations +by pressing "space". + +You will see something similar to this:: + + $ ./interactive_icp ../monkey.ply 5 + [pcl::PLYReader] ../monkey.ply:12: property 'list uint8 uint32 vertex_indices' of element 'face' is not handled + + Loaded file ../monkey.ply (125952 points) in 578 ms + + Applying this rigid transformation to: cloud_in -> cloud_icp + Rotation matrix : + | 0.924 -0.383 0.000 | + R = | 0.383 0.924 0.000 | + | 0.000 0.000 1.000 | + Translation vector : + t = < 0.000, 0.000, 0.400 > + + Applied 1 ICP iteration(s) in 2109 ms + + ICP has converged, score is 0.0182442 + + ICP transformation 1 : cloud_icp -> cloud_in + Rotation matrix : + | 0.998 0.066 -0.003 | + R = | -0.066 0.997 0.033 | + | 0.005 -0.033 0.999 | + Translation vector : + t = < 0.022, -0.017, -0.097 > + +If ICP did a perfect job the two matrices should have exactly the same values and +the matrix found by ICP should have inverted signs outside the diagonal. For example :: + + | 0.924 -0.383 0.000 | + R = | 0.383 0.924 0.000 | + | 0.000 0.000 1.000 | + Translation vector : + t = < 0.000, 0.000, 0.400 > + + | 0.924 0.383 0.000 | + R = | -0.383 0.924 0.000 | + | 0.000 0.000 1.000 | + Translation vector : + t = < 0.000, 0.000, -0.400 > + +.. DANGER:: + If you iterate several times manually using "space"; the results will become more and more erroned because + of the matrix multiplication (see line 181 of the original code) + If you seek precision, provide an initial number of iterations to the program + +.. image:: images/interactive_icp/icp-1.png + :height: 605 + +After 25 iterations the models fits perfectly the original cloud. Remember that this is an easy job for ICP because +you are asking to align two identical point clouds ! + +.. image:: images/interactive_icp/animation.gif + :height: 630 + diff --git a/doc/tutorials/content/matrix_transform.rst b/doc/tutorials/content/matrix_transform.rst new file mode 100644 index 00000000..3357d92e --- /dev/null +++ b/doc/tutorials/content/matrix_transform.rst @@ -0,0 +1,185 @@ +.. _matrix_transform: + +Using a matrix to transform a point cloud +----------------------------------------- + +In this tutorial we will learn how to transform a point cloud using a 4x4 matrix. +We will apply a rotation and a translation to a loaded point cloud and display then +result. + +This program is able to load one PCD or PLY file; apply a matrix transformation on it +and display the original and transformed point cloud. + +The code +-------- + +First, create a file, let's say, ``matrix_transform.cpp`` in your favorite +editor, and place the following code inside it: + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :linenos: + +The explanation +--------------- + +Now, let's break down the code piece by piece. + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 1-8 + +We include all the headers we will make use of. +**#include ** allows us to use **pcl::transformPointCloud** function. + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 10-17 + +This function display the help in case the user didn't provide expected arguments. + + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 24-28 + +We parse the arguments on the command line, either using **-h** or **--help** will +display the help. This terminates the program + + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 30-45 + +We look for .ply or .pcd filenames in the arguments. If not found; terminate the program. +The bool **file_is_pcd** will help us choose between loading PCD or PLY file. + + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 47-62 + +We now load the PCD/PLY file and check if the file was loaded successfuly. Otherwise terminate +the program. + + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 64-75 + +This is a first approach to create a transformation. This will help you understand how transformation matrices work. +We initialize a 4x4 matrix to identity; :: + + | 1 0 0 0 | + i = | 0 1 0 0 | + | 0 0 1 0 | + | 0 0 0 1 | + +.. note:: + + The identity matrix is the equivalent of "1" when multiplying numbers; it changes nothing. + It is a square matrix with ones on the main diagonal and zeros elsewhere. + +This means no transformation (no rotation and no translation). We do not use the +last row of the matrix. + +The first 3 rows and colums (top left) components are the rotation +matrix. The first 3 rows of the last column is the translation. + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 77-90 + +Here we defined a 45° (PI/4) rotation around the Z axis and a translation on the X axis. +This is the transformation we just defined :: + + | cos(θ) -sin(θ) 0.0 | + R = | sin(θ) cos(θ) 0.0 | + | 0.0 0.0 1.0 | + + t = < 2.5, 0.0, 0.0 > + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 92-105 + +This second approach is easier to understand and is less error prone. +Be carefull if you want to apply several rotations; rotations are not commutative ! This means than in most cases: +rotA * rotB != rotB * rotA. + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 107-110 + +Now we apply this matrix on the point cloud **source_cloud** and we save the result in the +newly created **transformed_cloud**. + +.. literalinclude:: sources/matrix_transform/matrix_transform.cpp + :language: cpp + :lines: 112-135 + +We then visualize the result using the **PCLVisualizer**. The original point cloud will be +displayed white and the transformed one in red. The coordoniates axis will be displayed. +We also set the background color of the visualizer and the point display size. + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/matrix_transform/CMakeLists.txt + :language: cmake + :linenos: + +After you have made the executable, you can run it. Simply do:: + + $ ./matrix_transform cube.ply + +You will see something similar to this:: + + ./matrix_transform cube.ply + [pcl::PLYReader] /home/victor/cube.ply:12: property 'list uint8 uint32 vertex_indices' of element 'face' is not handled + Method #1: using a Matrix4f + 0.707107 -0.707107 0 2.5 + 0.707107 0.707107 0 0 + 0 0 1 0 + 0 0 0 1 + + Method #2: using an Affine3f + 0.707107 -0.707107 0 2.5 + 0.707107 0.707107 0 0 + 0 0 1 0 + 0 0 0 1 + + Point cloud colors : white = original point cloud + red = transformed point cloud + +.. image:: images/matrix_transform/cube_big.png + :height: 614 + +More about transformations +-------------------------- + +| So now you successfully transformed a point cloud using a transformation matrix. +| What if you want to transform a single point ? A vector ? + +| A point is defined in 3D space with its three coordinates; x,y,z (in a cartesian coordinate system). +| How can you multiply a vector (with 3 coordinates) with a 4x4 matrix ? You simply can't ! If you don't know why please refer to `matrix multiplications on wikipedia `_. + +We need a vector with 4 components. What do you put in the last component ? It depends on what you want to do: + +* If you want to transform a point: put 1 at the end of the vector so that the translation is taken in account. +* If you want to transform the direction of a vector: put 0 at the end of the vector to ignore the translation. + +Here's a quick example, we want to transform the following vector: :: + + [10, 5, 0, 3, 0, -1] + +| Where the first 3 components defines the origin coordinates and the last 3 components the direction. +| This vector starts at point 10, 5, 0 and ends at 13, 5, -1. + +This is what you need to do to transform the vector: :: + + [10, 5, 0, 1] * 4x4_transformation_matrix + [3, 0, -1, 0] * 4x4_transformation_matrix + diff --git a/doc/tutorials/content/mobile_streaming.rst b/doc/tutorials/content/mobile_streaming.rst index c4a114d6..56aa99bd 100644 --- a/doc/tutorials/content/mobile_streaming.rst +++ b/doc/tutorials/content/mobile_streaming.rst @@ -41,7 +41,7 @@ The client is an Android app named *Point Cloud Streaming*. The app is implemented using the Android `NativeActivity `_. Using *NativeActivity*, an Android app can be implemented in pure C++ code without writing components in Java. The app uses APIs provided by the `Android -NDK `_ to handle touch events +NDK `_ to handle touch events and app life cycle events. While this is suitable for an example app, apps that demand extra features and user interface elements will require implementations that mix native code and Java components and APIs. diff --git a/doc/tutorials/content/model_outlier_removal.rst b/doc/tutorials/content/model_outlier_removal.rst new file mode 100644 index 00000000..d45a6688 --- /dev/null +++ b/doc/tutorials/content/model_outlier_removal.rst @@ -0,0 +1,81 @@ +.. _model_outlier_removal: + +Filtering a PointCloud using ModelOutlierRemoval +------------------------------------------------ + +This tutorial demonstrates how to extract parametric models for example for planes or spheres +out of a PointCloud by using SAC_Models with known coefficients. +If you don't know the models coefficients take a look at the :ref:`random_sample_consensus` tutorial. + +The code +-------- + +First, create a file, let's call it ``model_outlier_removal.cpp``, in your favorite +editor, and place the following inside it: + +.. literalinclude:: sources/model_outlier_removal/model_outlier_removal.cpp + :language: cpp + :linenos: + +The explanation +--------------- + +Now, let's break down the code piece by piece. + +In the following lines, we define the PointClouds structures, fill in noise, random points +on a plane as well as random points on a sphere and display its content to screen. + +.. literalinclude:: sources/model_outlier_removal/model_outlier_removal.cpp + :language: cpp + :lines: 7-45 + +Finally we extract the sphere using ModelOutlierRemoval. + +.. literalinclude:: sources/model_outlier_removal/model_outlier_removal.cpp + :language: cpp + :lines: 50-61 + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/model_outlier_removal/CMakeLists.txt + :language: cmake + :linenos: + + +After you have made the executable, you can run it. Simply do:: + + $ ./model_outlier_removal + +You will see something similar to:: + + Cloud before filtering: + 0.352222 -0.151883 -0.106395 + -0.397406 -0.473106 0.292602 + -0.731898 0.667105 0.441304 + -0.734766 0.854581 -0.0361733 + -0.4607 -0.277468 -0.916762 + -0.82 -0.341666 0.4592 + -0.728589 0.667873 0.152 + -0.3134 -0.873043 -0.3736 + 0.62553 0.590779 0.5096 + -0.54048 0.823588 -0.172 + -0.707627 0.424576 0.5648 + -0.83153 0.523556 0.1856 + -0.513903 -0.719464 0.4672 + 0.291534 0.692393 0.66 + 0.258758 0.654505 -0.7104 + Sphere after filtering: + -0.82 -0.341666 0.4592 + -0.728589 0.667873 0.152 + -0.3134 -0.873043 -0.3736 + 0.62553 0.590779 0.5096 + -0.54048 0.823588 -0.172 + -0.707627 0.424576 0.5648 + -0.83153 0.523556 0.1856 + -0.513903 -0.719464 0.4672 + 0.291534 0.692393 0.66 + 0.258758 0.654505 -0.7104 + diff --git a/doc/tutorials/content/moment_of_inertia.rst b/doc/tutorials/content/moment_of_inertia.rst new file mode 100644 index 00000000..6e9e9862 --- /dev/null +++ b/doc/tutorials/content/moment_of_inertia.rst @@ -0,0 +1,119 @@ +.. _moment_of_inertia: + +Moment of inertia and eccentricity based descriptors +---------------------------------------------------- + +In this tutorial we will learn how to use the `pcl::MomentOfInertiaEstimation` class in order to obtain descriptors based on +eccentricity and moment of inertia. This class also allows to extract axis aligned and oriented bounding boxes of the cloud. +But keep in mind that extracted OBB is not the minimal possible bounding box. + +Theoretical Primer +------------------ + +The idea of the feature extraction method is as follows. +First of all the covariance matrix of the point cloud is calculated and its eigen values and vectors are extracted. +You can consider that the resultant eigen vectors are normalized and always form the right-handed coordinate system +(major eigen vector represents X-axis and the minor vector represents Z-axis). On the next step the iteration process takes place. +On each iteration major eigen vector is rotated. Rotation order is always the same and is performed around the other +eigen vectors, this provides the invariance to rotation of the point cloud. Henceforth, we will refer to this rotated major vector as current axis. + +.. image:: images/eigen_vectors.png + :height: 360px + +For every current axis moment of inertia is calculated. Moreover, current axis is also used for eccentricity calculation. +For this reason current vector is treated as normal vector of the plane and the input cloud is projected onto it. +After that eccentricity is calculated for the obtained projection. + +.. image:: images/projected_cloud.png + :height: 360px + +Implemented class also provides methods for getting AABB and OBB. Oriented bounding box is computed as AABB along eigen vectors. + +The code +-------- + +First of all you will need the point cloud for this tutorial. +`This `_ is the one presented on the screenshots. +Next what you need to do is to create a file ``moment_of_inertia.cpp`` in any editor you prefer and copy the following code inside of it: + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :linenos: + +The explanation +--------------- + +Now let's study out what is the purpose of this code. First few lines will be omitted, as they are obvious. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 13-15 + +These lines are simply loading the cloud from the .pcd file. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 17-19 + +Here is the line where the instantiation of the ``pcl::MomentOfInertiaEstimation`` class takes place. +Immediately after that we set the input cloud and start the computational process, that easy. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 21-31 + +This is were we declare all necessary variables needed to store descriptors and bounding boxes. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 33-39 + +These lines show how to access computed descriptors and other features. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 41-46 + +These lines simply create the instance of ``PCLVisualizer`` class for result visualization. +Here we also add the cloud and the AABB for visualization. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 48-50 + +Visualization of the OBB is little more complex. So here we create a quaternion from the rotational matrix, set OBBs position +and pass it to the visualizer. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 52-58 + +This lines are responsible for eigen vectors visualization. + +.. literalinclude:: sources/moment_of_inertia/moment_of_inertia.cpp + :language: cpp + :lines: 60-98 + +This huge amount of code shows how to work with the oriented bounding box. Note that you need to rotate each of the vertices of the OBB. +This code does the same thing as ``PCLVisualizer::addCube ()`` method. Its only purpose is to show how to work with OBB +if you don't have such usable method as ``PCLVisualizer::addCube ()``. + +Few lines that left simply launch the visualization process. + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/moment_of_inertia/CMakeLists.txt + :language: cmake + :linenos: + +After you have made the executable, you can run it. Simply do:: + + $ ./moment_of_inertia lamppost.pcd + +You should see something similar to this image. Here AABB is yellow, OBB is red. You can also see the eigen vectors. + +.. image:: images/moment_of_inertia.png + :height: 360px diff --git a/doc/tutorials/content/openni_grabber.rst b/doc/tutorials/content/openni_grabber.rst index ba11c739..f1a84e6f 100644 --- a/doc/tutorials/content/openni_grabber.rst +++ b/doc/tutorials/content/openni_grabber.rst @@ -12,7 +12,7 @@ a breeze to request data streams from OpenNI compatible cameras. This tutorial presents how to set up and use the grabber, and since it's so simple, we can keep it short :). -The cameras that we have tested so far are the `Primesense Reference Design `_, `Microsoft Kinect `_ and `Asus Xtion Pro `_ cameras: +The cameras that we have tested so far are the `Primesense Reference Design `_, `Microsoft Kinect `_ and `Asus Xtion Pro `_ cameras: .. image:: images/openni_cams.jpg diff --git a/doc/tutorials/content/pairwise_incremental_registration.rst b/doc/tutorials/content/pairwise_incremental_registration.rst index a13d95d8..e93a0afc 100644 --- a/doc/tutorials/content/pairwise_incremental_registration.rst +++ b/doc/tutorials/content/pairwise_incremental_registration.rst @@ -10,7 +10,7 @@ to incrementally register a series of point clouds two by two. | This is done by finding the best transform between each consecutive cloud, and accumulating these transforms over the whole set of clouds. | Your data set should consist of clouds that have been roughly pre-aligned in a common frame (e.g. in a robot's odometry or map frame) and overlap with one another. -| We provide a set of clouds at `github.com/PointCloudLibrary/data/tree/master/tutorials/pairwise/ `_. +| We provide a set of clouds at `github.com/PointCloudLibrary/data/tree/master/tutorials/pairwise/ `_. The code @@ -173,17 +173,6 @@ Create CMakeLists.txt file and add the following line in it: :language: cmake :linenos: -Note that the line - -.. code-block:: cmake - - add_definitions(-Wno-deprecated -DEIGEN_DONT_VECTORIZE -DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) - -is usefull only on 32-bit systems, that would (sometimes) trigger the following Eigen exception:: - - Eigen::internal::plain_array::plain_array() [with T = float, int Size = 16, int MatrixOrArrayOptions = 0]: Assertion `(reinterpret_cast(array) & 0xf) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.`` - - Copy the files from `github.com/PointCloudLibrary/data/tree/master/tutorials/pairwise `_ in your working folder. diff --git a/doc/tutorials/content/pcl_plotter.rst b/doc/tutorials/content/pcl_plotter.rst index e6f7cafd..036b5c1a 100644 --- a/doc/tutorials/content/pcl_plotter.rst +++ b/doc/tutorials/content/pcl_plotter.rst @@ -5,7 +5,7 @@ PCLPlotter PCLPlotter provides a very straightforward and easy interface for plotting graphs. One can visualize all sort of important plots - from polynomial functions to histograms - inside the library without going to any other softwares (like MATLAB). -Please go through the `documentation `_ when some specific concepts are introduced in this tutorial to know the exact method signatures. +Please go through the `documentation `_ when some specific concepts are introduced in this tutorial to know the exact method signatures. The code for the visualization of a plot are usually as simple as the following snippet. @@ -174,7 +174,7 @@ PCLPlotter provides few other important functionalities other than plotting give 'Plotting' Histogram -------------------- -PCLPlotter provides a very convenient MATLAB like histogram plotting function (`hist() `_ in MATLAB). It takes raw data and bins them according to their frequency and plot them as bar chart. +PCLPlotter provides a very convenient MATLAB like histogram plotting function (`hist() `_ in MATLAB). It takes raw data and bins them according to their frequency and plot them as bar chart. .. code-block:: cpp @@ -249,4 +249,4 @@ The following video shows the the output of the demo. - \ No newline at end of file + diff --git a/doc/tutorials/content/pfh_estimation.rst b/doc/tutorials/content/pfh_estimation.rst index a4351a74..194f0111 100644 --- a/doc/tutorials/content/pfh_estimation.rst +++ b/doc/tutorials/content/pfh_estimation.rst @@ -116,13 +116,13 @@ Estimating PFH features ----------------------- Point Feature Histograms are implemented in PCL as part of the `pcl_features -`_ library. +`_ library. The default PFH implementation uses 5 binning subdivisions (e.g., each of the four feature values will use this many bins from its value interval), and does not include the distances (as explained above -- although the **computePairFeatures** method can be called by the user to obtain the -distances too, if desired) which results in a 125-byte array (:math:`3^5`) of +distances too, if desired) which results in a 125-byte array (:math:`5^3`) of float values. These are stored in a **pcl::PFHSignature125** point type. The following code snippet will estimate a set of PFH features for all the diff --git a/doc/tutorials/content/planar_segmentation.rst b/doc/tutorials/content/planar_segmentation.rst index 3a85b556..3c397599 100644 --- a/doc/tutorials/content/planar_segmentation.rst +++ b/doc/tutorials/content/planar_segmentation.rst @@ -36,7 +36,7 @@ Lines: .. important:: - Please visit http://docs.pointclouds.org/trunk/group__sample__consensus.html + Please visit http://docs.pointclouds.org/trunk/a02954.html for more information on various other implemented Sample Consensus models and robust estimators. diff --git a/doc/tutorials/content/progressive_morphological_filtering.rst b/doc/tutorials/content/progressive_morphological_filtering.rst new file mode 100644 index 00000000..99db074c --- /dev/null +++ b/doc/tutorials/content/progressive_morphological_filtering.rst @@ -0,0 +1,129 @@ +.. _progressive_morphological_filtering: + +Identifying ground returns using ProgressiveMorphologicalFilter segmentation +---------------------------------------------------------------------------- + +Implements the Progressive Morphological Filter for segmentation of ground +points. + +Background +---------- + +A complete description of the algorithm can be found in the article `"A +Progressive Morphological Filter for Removing Nonground Measurements from +Airborne LIDAR Data" `_ by K. +Zhang, S. Chen, D. Whitman, M. Shyu, J. Yan, and C. Zhang. + +The code +-------- + +First, download the dataset `samp11-utm.pcd +`_ +and save it somewhere to disk. + +Then, create a file, let's say, ``bare_earth.cpp`` in your favorite editor, and +place the following inside it: + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :linenos: + +The explanation +--------------- + +Now, let's break down the code piece by piece. + +The following lines of code will read the point cloud data from disk. + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :lines: 14-17 + + +Then, a *pcl::ProgressiveMorphologicalFilter* filter is created. The output +(the indices of ground returns) is computed and stored in *ground*. + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :lines: 22-29 + + +To extract the ground points, the ground indices are passed into a +*pcl::ExtractIndices* filter. + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :lines: 31-35 + + +The ground returns are written to disk for later inspection. + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :lines: 40-41 + + +Then, the filter is called with the same parameters, but with the output +negated, to obtain the non-ground (object) returns. + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :lines: 43-45 + + +And the data is written back to disk. + +.. literalinclude:: sources/bare_earth/bare_earth.cpp + :language: cpp + :lines: 50 + + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/bare_earth/CMakeLists.txt + :language: cmake + :linenos: + +After you have made the executable, you can run it. Simply do:: + + $ ./bare_earth + +You will see something similar to:: + + Cloud before filtering: + points[]: 38010 + width: 38010 + height: 1 + is_dense: 1 + sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1] + + Ground cloud after filtering: + points[]: 18667 + width: 18667 + height: 1 + is_dense: 1 + sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1] + + Object cloud after filtering: + points[]: 19343 + width: 19343 + height: 1 + is_dense: 1 + sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1] + + +You can also look at your outputs samp11-utm_inliers.pcd and +samp11-utm_outliers.pcd:: + + $ ./pcl_viewer samp11-utm_ground.pcd samp11-utm_object.pcd + +You are now able to see both the ground and object returns in one viewer. You +should see something similar to this: + +.. image:: images/progressive_morphological_filter.png + :alt: Output Progressive Morphological Filter + :align: center + :width: 600px diff --git a/doc/tutorials/content/qt_visualizer.rst b/doc/tutorials/content/qt_visualizer.rst new file mode 100644 index 00000000..d30b7e88 --- /dev/null +++ b/doc/tutorials/content/qt_visualizer.rst @@ -0,0 +1,255 @@ +.. _qt_visualizer: + +======================================== +Create a PCL visualizer in Qt with cmake +======================================== + +In this tutorial we will learn how to create a PCL + Qt project, we will use Cmake rather than Qmake.The program we are going to +write is a simple PCL visualizer which allow to change a randomly generated point cloud color. + +| The tutorial was tested on Linux Ubuntu 12.04 and 14.04. It also seems to be working fine on Windows 8.1 x64. +| Feel free to push modifications into the git repo to make this code/tutorial compatible with your platform ! + +.. contents:: + +The project +=========== + +For this project Qt is of course mandatory, make sure it is installed and PCL deals with it. +`qmake `_ is a tool that helps simplify the build process for development project across different platforms, +we will use `cmake `_ instead because most projects in PCL uses cmake and it is simpler in my opinion. + +This is how I organized the project: the build folder contains all built files and the src folder holds all sources files :: + + . + ├── build + └── src + ├── CMakeLists.txt + ├── main.cpp + ├── pclviewer.cpp + ├── pclviewer.h + ├── pclviewer.ui + ├── pcl_visualizer.pro + └── pcl_visualizer.pro.user + +If you want to change this layout you will have to do minor modifications in the code, especially line 2 of ``pclviewer.cpp`` +Create the folder tree and download the sources files from `github `_. + +.. note:: + File paths should not contain any special caracter or the compilation might fail with a ``moc: Cannot open options file specified with @`` error message. + +Qt configuration +================ + +First we will take a look at how Qt is configured to build this project. Simply open ``pcl_visualizer.pro`` with Qt (or double click on the file) + and go to the **Projects** tab + +.. image:: images/qt_visualizer/qt_config.png + :height: 757 + +In this example note that I deleted the **Debug** configuration and only kept the **Release** config. +Use relative paths like this is better than absolute paths; this project should work wherever it has been put. + +We specify in the general section that we want to build in the folder ``../build`` (this is a relative path from the ``.pro`` file). + +The first step of the building is to call ``cmake`` (from the ``build`` folder) with argument ``../src``; this is gonna create all files in the +``build`` folder without modifying anything in the ``src`` foler; thus keeping it clean. + +Then we just have to compile our program; the argument ``-j2`` allow to specify how many thread of your CPU you want to use for compilation. The more thread you use +the faster the compilation will be (especially on big projects); but if you take all threads from the CPU your OS will likely be unresponsive during +the compilation process. +See `compiler optimizations `_ for more information. + +If you don't want to use Qt Creator but Eclipse instead; see `using PCL with Eclipse `_. + +User interface (UI) +=================== + +The point of using Qt for your projects is that you can easily build cross-platform UIs. The UI is held in the ``.ui`` file +You can open it with a text editor or with Qt Creator, in this example the UI is very simple and it consists of : + + * `QMainWindow `_, QWidget: the window (frame) of your application + * qvtkWidget: The VTK widget which holds the PCLVisualizer + * `QLabel `_: Display text on the user interface + * `QSlider `_: A slider to choose a value (here; an integer value) + * `QLCDNumber `_: A digital display, 8 segment styled + +.. image:: images/qt_visualizer/ui.png + :height: 518 + +If you click on Edit `Signals/Slots `_ at the top of the Qt window you will see the relationships +between some of the UI objects. In our example the sliderMoved(int) signal is connected to the display(int) slot; this means that everytime we move the slider +the digital display is updated accordingly to the slider value. + +The code +======== + +Now, let's break down the code piece by piece. + +main.cpp +-------- + +.. literalinclude:: sources/qt_visualizer/main.cpp + :language: cpp + +| Here we include the headers for the class PCLViewer and the headers for QApplication and QMainWindow. +| Then the main functions consists of instanciating a QApplication `a` which manages the GUI application's control flow and main settings. +| A ``PCLViewer`` object called `w` is instanciated and it's method ``show()`` is called. +| Finally we return the state of our program exit through the QApplication `a`. + +pclviewer.h +----------- + +.. literalinclude:: sources/qt_visualizer/pclviewer.h + :language: cpp + :lines: 1-18 + +This file is the header for the class PCLViewer; we include ``QMainWindow`` because this class contains UI elements, we include the PCL headers we will +be using and the VTK header for the ``qvtkWidget``. We also define typedefs of the point types and point clouds, this improves readabily. + + +.. literalinclude:: sources/qt_visualizer/pclviewer.h + :language: cpp + :lines: 20-23 + +We declare the namespace ``Ui`` and the class PCLViewer inside it. + +.. literalinclude:: sources/qt_visualizer/pclviewer.h + :language: cpp + :lines: 25-27 + +This is the definition of the PCLViewer class; the macro ``Q_OBJECT`` tells the compiler that this object contains UI elements; +this imply that this file will be processed through `the Meta-Object Compiler (moc) `_. + +.. literalinclude:: sources/qt_visualizer/pclviewer.h + :language: cpp + :lines: 29-31 + +The constructor and destructor of the PCLViewer class. + +.. literalinclude:: sources/qt_visualizer/pclviewer.h + :language: cpp + :lines: 33-50 + +These are the public slots; these functions will be linked with UI elements actions. + +.. literalinclude:: sources/qt_visualizer/pclviewer.h + :language: cpp + :lines: 52-58 + +| A boost shared pointer to a PCLVisualier and a pointer to a point cloud are defined here. +| The integers ``red``, ``green``, ``blue`` will help us store the value of the sliders. + +pclviewer.cpp +------------- + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 1-14 + +We include the class header and the header for the UI object; note that this file is generated by the moc and it's path depend on +where you call cmake ! + +After that is the constructor implementation; we setup the ui and the window title name. +| Then we initialize the cloud pointer member of the class at a newly allocated point cloud pointer. +| The cloud is resized to be able to hold 200 points. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 16-31 + +| ``red`` ``green`` and ``blue`` protected members are initialized to their default values. +| The cloud is filled with random points (in a cube) and accordingly to ``red`` ``green`` and ``blue`` colors. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 33-37 + +| Here we create a PCL Visualizer name ``viewer`` and we also specify that we don't want an interactor to be created. +| We don't want an interactor to be created because our ``qvtkWidget`` is already an interactor and it's the one we want to use. +| So the next step is to configure our newly created PCL Visualiser interactor to use the ``qvtkWidget``. + +The ``update()`` method of the ``qvtkWidget`` should be called each time you modify the PCL visualizer; if you don't call it you don't know if the +visualizer will be updated before the user try to pan/spin/zoom. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 39-51 + +Here we connect slots and signals, this links UI actions to functions. Here is a summary of what we have linked : + * ``pushButton_random``: + | if button is pressed call ``randomButtonPressed ()`` + * ``horizontalSlider_R``: + | if slider value is changed call ``redSliderValueChanged(int)`` with the new value as argument + | if slider is released call ``RGBsliderReleased()`` + * ``horizontalSlider_G``: + | if slider value is changed call ``redSliderValueChanged(int)`` with the new value as argument + | if slider is released call ``RGBsliderReleased()`` + * ``horizontalSlider_B``: + | if slider value is changed call ``redSliderValueChanged(int)`` with the new value as argument + | if slider is released call ``RGBsliderReleased()`` + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 53-57 + +| This is the last part of our constructor; we add the point cloud to the visualizer, call the method ``pSliderValueChanged`` to change the point size to 2. + +We finaly reset the camera within the PCL Visualizer not avoid the user having to zoom out and update the qvtkwidget to be +sure the modifications will be displayed. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 59-74 + +| This is the public slot function member called when the push button "Random" is pressed. +| The ``for`` loop iterates through the point cloud and changes point cloud color to a random number (between 0 and 255). +| The point cloud is then updated and so the ``qtvtkwidget`` is. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 76-88 + +| This is the public slot function member called whenever the red, green or blue slider is released +| The ``for`` loop iterates through the point cloud and changes point cloud color to ``red``, ``green`` and ``blue`` member values. +| The point cloud is then updated and so the ``qtvtkwidget`` is. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 97-116 + +| These are the public slot function member called whenever the red, green or blue slider value is changed +| These functions just changes the member value accordingly to the slider value. +| Here the point cloud is not updated; so until you release the slider you won't see any change in the visualizer. + +.. literalinclude:: sources/qt_visualizer/pclviewer.cpp + :language: cpp + :lines: 118-121 + +The destructor. + +Compiling and running +===================== + +There are two options here : + * You have configured the Qt project and you can compile/run just by clicking on the bottom left "Play" button. + * You didn't configure the Qt project; just go to the build folder an run ``cmake ../src && make -j2 && ./pcl_visualizer`` + +| Notice that when changing the slider color, the cloud is not updated until you release the slider (``sliderReleased ()`` slot). + +If you wanted to update the point cloud when the slider value is changed you could just call the ``RGBsliderReleased ()`` function inside the +``*sliderValueChanged (int)`` functions. The connect between ``sliderReleased ()`` / ``RGBsliderReleased ()`` would become useless then. + +| When using the slider for the point size; the size of the point is updated without having to release the slider. + +.. image:: images/qt_visualizer/pcl_visualizer.gif + :height: 527 + +More on Qt and PCL +================== + +If you want to know more about Qt and PCL go take a look at `PCL apps `_ like +`PCD video player `_ +or `manual registration `_. + +Re-use the :download:`CMakeLists.txt <./sources/qt_visualizer/CMakeLists.txt>` from this tutorial if you want to compile the application outside of PCL. diff --git a/doc/tutorials/content/region_growing_segmentation.rst b/doc/tutorials/content/region_growing_segmentation.rst index 9fd64853..47bdaba8 100644 --- a/doc/tutorials/content/region_growing_segmentation.rst +++ b/doc/tutorials/content/region_growing_segmentation.rst @@ -161,14 +161,14 @@ This method simply launches the segmentation algorithm. After its work it will r .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp :language: cpp - :lines: 51-60 + :lines: 51-63 These lines are simple enough, so they won't be commented. They are intended for those who are not familiar with how to work with ``pcl::PointIndices`` and how to access its elements. .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp :language: cpp - :lines: 62-67 + :lines: 65-73 The ``pcl::RegionGrowing`` class provides a method that returns the colored cloud where each cluster has its own color. So in this part of code the ``pcl::visualization::CloudViewer`` is instanciated for viewing the result of the segmentation - the same colored cloud. diff --git a/doc/tutorials/content/rops_feature.rst b/doc/tutorials/content/rops_feature.rst new file mode 100644 index 00000000..496f5a3a --- /dev/null +++ b/doc/tutorials/content/rops_feature.rst @@ -0,0 +1,114 @@ +.. _rops_feature: + +RoPs (Rotational Projection Statistics) feature +----------------------------------------------- + +In this tutorial we will learn how to use the `pcl::ROPSEstimation` class in order to extract points features. +The feature extraction method implemented in this class was proposed by Yulan Guo, Ferdous Sohel, Mohammed Bennamoun, Min Lu and +Jianwei Wanalso in their article "Rotational Projection Statistics for 3D Local Surface Description and Object Recognition" + +Theoretical Primer +------------------ + +The idea of the feature extraction method is as follows. +Having a mesh and a set of points for which feature must be computed we perform some simple steps. First of all for a given point of interest +the local surface is cropped. Local surface consists of the points and triangles that are within the given support radius. +For the given local surface LRF (Local Reference Frame) is computed. LRF is simply a triplet of vectors, +the comprehensive information about how these vectors are computed you can find in the article. +What is really important is that using these vectors we can provide the invariance to the rotation of the cloud. To do that, we simply +translate points of the local surface in such way that point of interest became the origin, after that we rotate local surface so that the +LRF vectors were aligned with the Ox, Oy and Oz axes. Having this done, we then start the feature extraction. +For every axis Ox, Oy and Oz the following steps are performed, we will refer to these axes as current axis: + + * local surface is rotated around the current axis by a given angle; + * points of the rotated local surface are projected onto three planes XY, XZ and YZ; + * for each projection distribution matrix is built, this matrix simply shows how much points fall onto each bin. Number of bins represents the matrix dimension and is the parameter of the algorithm, as well as the support radius; + * for each distribution matrix central moments are calculated: M11, M12, M21, M22, E. Here E is the Shannon entropy; + * calculated values are then concatenated to form the sub-feature. + +We iterate through these steps several times. Number of iterations depends on the given number of rotations. +Sub-features for different axes are concatenated to form the final RoPS descriptor. + +The code +-------- + +For this tutorial we will use the model from the Queen's Dataset. You can choose any other point cloud, but in order to make the +code work you will need to use the triangulation algorithm in order to obtain polygons. You can find the proposed model here: + + * `points `_ - contains the point cloud + * `indices - contains indices of the points for which RoPs must be computed + * `triangles - contains the polygons + +Next what you need to do is to create a file ``rops_feature.cpp`` in any editor you prefer and copy the following code inside of it: + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :linenos: + +The explanation +--------------- + +Now let's study out what is the purpose of this code. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 9-11 + +These lines are simply loading the cloud from the .pcd file. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 13-23 + +Here the indices of points for which RoPS feature must be computed are loaded. You can comment it and compute features for every single point in the cloud. +if you want. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 25-40 + +These lines are loading the information about the polygons. You can replace them with the code for the triangulation if you have only the point cloud +instead of the mesh. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 42-44 + +These code defines important algorithm parameters: support radius for local surface cropping, number of partition bins +used to form the distribution matrix and the number of rotations. The last parameter affects the length of the descriptor. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 46-47 + +These lines set up the search method that will be used by the algorithm. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 49-58 + +Here is the place where the instantiation of the ``pcl::ROPSEstimation`` class takes place. It has two parameters: + + * PointInT - type of the input points; + * PointOutT - type of the output points. + +Immediately after that we set the input all the necessary data neede for the feature computation. + +.. literalinclude:: sources/rops_feature/rops_feature.cpp + :language: cpp + :lines: 60-61 + +Here is the place where the computational process is launched. + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/rops_feature/CMakeLists.txt + :language: cmake + :linenos: + +After you have made the executable, you can run it. Simply do:: + + $ ./rops_feature points.pcd indices.txt triangles.txt diff --git a/doc/tutorials/content/sources/CMakeLists.txt b/doc/tutorials/content/sources/CMakeLists.txt index 99213ae3..f93e2f28 100644 --- a/doc/tutorials/content/sources/CMakeLists.txt +++ b/doc/tutorials/content/sources/CMakeLists.txt @@ -20,6 +20,7 @@ foreach(subdir iterative_closest_point kdtree_search min_cut_segmentation + moment_of_inertia #narf_descriptor_visualization narf_feature_extraction narf_keypoint_extraction @@ -45,6 +46,7 @@ foreach(subdir registration_api remove_outliers resampling + rops_feature statistical_removal stick_segmentation supervoxel_clustering diff --git a/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt b/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt index 3d62457f..5ed7e82e 100644 --- a/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt +++ b/doc/tutorials/content/sources/alignment_prerejective/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR) project(alignment_prerejective) -find_package(PCL 1.7 REQUIRED) +find_package(PCL 1.7 REQUIRED REQUIRED COMPONENTS io registration segmentation visualization) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) diff --git a/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp b/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp index c8d81f62..ca897e79 100644 --- a/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp +++ b/doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp @@ -83,16 +83,21 @@ main (int argc, char **argv) align.setSourceFeatures (object_features); align.setInputTarget (scene); align.setTargetFeatures (scene_features); + align.setMaximumIterations (10000); // Number of RANSAC iterations align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose align.setCorrespondenceRandomness (2); // Number of nearest features to use - align.setSimilarityThreshold (0.6f); // Polygonal edge length similarity threshold - align.setMaxCorrespondenceDistance (1.5f * leaf); // Set inlier threshold - align.setInlierFraction (0.25f); // Set required inlier fraction - align.align (*object_aligned); + align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold + align.setMaxCorrespondenceDistance (1.5f * leaf); // Inlier threshold + align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis + { + pcl::ScopeTime t("Alignment"); + align.align (*object_aligned); + } if (align.hasConverged ()) { // Print results + printf ("\n"); Eigen::Matrix4f transformation = align.getFinalTransformation (); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2)); diff --git a/doc/tutorials/content/sources/alignment_prerejective/chef.pcd b/doc/tutorials/content/sources/alignment_prerejective/chef.pcd index 8f9fd902..daa46f5e 100644 Binary files a/doc/tutorials/content/sources/alignment_prerejective/chef.pcd and b/doc/tutorials/content/sources/alignment_prerejective/chef.pcd differ diff --git a/doc/tutorials/content/sources/alignment_prerejective/data/alignment_prerejective.tar.gz b/doc/tutorials/content/sources/alignment_prerejective/data/alignment_prerejective.tar.gz deleted file mode 100644 index 574e109e..00000000 Binary files a/doc/tutorials/content/sources/alignment_prerejective/data/alignment_prerejective.tar.gz and /dev/null differ diff --git a/doc/tutorials/content/sources/alignment_prerejective/rs1.pcd b/doc/tutorials/content/sources/alignment_prerejective/rs1.pcd index 046fc491..39357ca0 100644 Binary files a/doc/tutorials/content/sources/alignment_prerejective/rs1.pcd and b/doc/tutorials/content/sources/alignment_prerejective/rs1.pcd differ diff --git a/doc/tutorials/content/sources/bare_earth/CMakeLists.txt b/doc/tutorials/content/sources/bare_earth/CMakeLists.txt new file mode 100644 index 00000000..2652140a --- /dev/null +++ b/doc/tutorials/content/sources/bare_earth/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(bare_earth) + +find_package(PCL 1.7.2 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (bare_earth bare_earth.cpp) +target_link_libraries (bare_earth ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/bare_earth/bare_earth.cpp b/doc/tutorials/content/sources/bare_earth/bare_earth.cpp new file mode 100644 index 00000000..c09f49b0 --- /dev/null +++ b/doc/tutorials/content/sources/bare_earth/bare_earth.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include +#include + +int +main (int argc, char** argv) +{ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + pcl::PointIndicesPtr ground (new pcl::PointIndices); + + // Fill in the cloud data + pcl::PCDReader reader; + // Replace the path below with the path where you saved your file + reader.read ("samp11-utm.pcd", *cloud); + + std::cerr << "Cloud before filtering: " << std::endl; + std::cerr << *cloud << std::endl; + + // Create the filtering object + pcl::ProgressiveMorphologicalFilter pmf; + pmf.setInputCloud (cloud); + pmf.setMaxWindowSize (20); + pmf.setSlope (1.0f); + pmf.setInitialDistance (0.5f); + pmf.setMaxDistance (3.0f); + pmf.extract (ground->indices); + + // Create the filtering object + pcl::ExtractIndices extract; + extract.setInputCloud (cloud); + extract.setIndices (ground); + extract.filter (*cloud_filtered); + + std::cerr << "Ground cloud after filtering: " << std::endl; + std::cerr << *cloud_filtered << std::endl; + + pcl::PCDWriter writer; + writer.write ("samp11-utm_ground.pcd", *cloud_filtered, false); + + // Extract non-ground returns + extract.setNegative (true); + extract.filter (*cloud_filtered); + + std::cerr << "Object cloud after filtering: " << std::endl; + std::cerr << *cloud_filtered << std::endl; + + writer.write ("samp11-utm_object.pcd", *cloud_filtered, false); + + return (0); +} + diff --git a/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt b/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt new file mode 100644 index 00000000..33251588 --- /dev/null +++ b/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(bspline_fitting) + +find_package(PCL 1.7 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (bspline_fitting bspline_fitting.cpp) +target_link_libraries (bspline_fitting ${PCL_LIBRARIES}) + diff --git a/doc/tutorials/content/sources/bspline_fitting/bspline_fitting.cpp b/doc/tutorials/content/sources/bspline_fitting/bspline_fitting.cpp new file mode 100644 index 00000000..60cf0447 --- /dev/null +++ b/doc/tutorials/content/sources/bspline_fitting/bspline_fitting.cpp @@ -0,0 +1,219 @@ +#include +#include +#include + +#include +#include +#include +#include + +typedef pcl::PointXYZ Point; + +void +PointCloud2Vector3d (pcl::PointCloud::Ptr cloud, pcl::on_nurbs::vector_vec3d &data); + +void +visualizeCurve (ON_NurbsCurve &curve, + ON_NurbsSurface &surface, + pcl::visualization::PCLVisualizer &viewer); + +int +main (int argc, char *argv[]) +{ + std::string pcd_file, file_3dm; + + if (argc < 3) + { + printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd-in-file 3dm-out-file\n\n"); + exit (0); + } + pcd_file = argv[1]; + file_3dm = argv[2]; + + pcl::visualization::PCLVisualizer viewer ("B-spline surface fitting"); + viewer.setSize (800, 600); + + // ############################################################################ + // load point cloud + + printf (" loading %s\n", pcd_file.c_str ()); + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PCLPointCloud2 cloud2; + pcl::on_nurbs::NurbsDataSurface data; + + if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1) + throw std::runtime_error (" PCD file not found."); + + fromPCLPointCloud2 (cloud2, *cloud); + PointCloud2Vector3d (cloud, data.interior); + pcl::visualization::PointCloudColorHandlerCustom handler (cloud, 0, 255, 0); + viewer.addPointCloud (cloud, handler, "cloud_cylinder"); + printf (" %lu points in data set\n", cloud->size ()); + + // ############################################################################ + // fit B-spline surface + + // parameters + unsigned order (3); + unsigned refinement (5); + unsigned iterations (10); + unsigned mesh_resolution (256); + + pcl::on_nurbs::FittingSurface::Parameter params; + params.interior_smoothness = 0.2; + params.interior_weight = 1.0; + params.boundary_smoothness = 0.2; + params.boundary_weight = 0.0; + + // initialize + printf (" surface fitting ...\n"); + ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (order, &data); + pcl::on_nurbs::FittingSurface fit (&data, nurbs); + // fit.setQuiet (false); // enable/disable debug output + + // mesh for visualization + pcl::PolygonMesh mesh; + pcl::PointCloud::Ptr mesh_cloud (new pcl::PointCloud); + std::vector mesh_vertices; + std::string mesh_id = "mesh_nurbs"; + pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (fit.m_nurbs, mesh, mesh_resolution); + viewer.addPolygonMesh (mesh, mesh_id); + + // surface refinement + for (unsigned i = 0; i < refinement; i++) + { + fit.refine (0); + fit.refine (1); + fit.assemble (params); + fit.solve (); + pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution); + viewer.updatePolygonMesh (mesh_cloud, mesh_vertices, mesh_id); + viewer.spinOnce (); + } + + // surface fitting with final refinement level + for (unsigned i = 0; i < iterations; i++) + { + fit.assemble (params); + fit.solve (); + pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution); + viewer.updatePolygonMesh (mesh_cloud, mesh_vertices, mesh_id); + viewer.spinOnce (); + } + + // ############################################################################ + // fit B-spline curve + + // parameters + pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params; + curve_params.addCPsAccuracy = 5e-2; + curve_params.addCPsIteration = 3; + curve_params.maxCPs = 200; + curve_params.accuracy = 1e-3; + curve_params.iterations = 100; + + curve_params.param.closest_point_resolution = 0; + curve_params.param.closest_point_weight = 1.0; + curve_params.param.closest_point_sigma2 = 0.1; + curve_params.param.interior_sigma2 = 0.00001; + curve_params.param.smooth_concavity = 1.0; + curve_params.param.smoothness = 1.0; + + // initialisation (circular) + printf (" curve fitting ...\n"); + pcl::on_nurbs::NurbsDataCurve2d curve_data; + curve_data.interior = data.interior_param; + curve_data.interior_weight_function.push_back (true); + ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D (order, curve_data.interior); + + // curve fitting + pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs); + // curve_fit.setQuiet (false); // enable/disable debug output + curve_fit.fitting (curve_params); + visualizeCurve (curve_fit.m_nurbs, fit.m_nurbs, viewer); + + // ############################################################################ + // triangulation of trimmed surface + + printf (" triangulate trimmed surface ...\n"); + viewer.removePolygonMesh (mesh_id); + pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (fit.m_nurbs, curve_fit.m_nurbs, mesh, + mesh_resolution); + viewer.addPolygonMesh (mesh, mesh_id); + + + // save trimmed B-spline surface + if ( fit.m_nurbs.IsValid() ) + { + ONX_Model model; + ONX_Model_Object& surf = model.m_object_table.AppendNew(); + surf.m_object = new ON_NurbsSurface(fit.m_nurbs); + surf.m_bDeleteObject = true; + surf.m_attributes.m_layer_index = 1; + surf.m_attributes.m_name = "surface"; + + ONX_Model_Object& curv = model.m_object_table.AppendNew(); + curv.m_object = new ON_NurbsCurve(curve_fit.m_nurbs); + curv.m_bDeleteObject = true; + curv.m_attributes.m_layer_index = 2; + curv.m_attributes.m_name = "trimming curve"; + + model.Write(file_3dm.c_str()); + printf(" model saved: %s\n", file_3dm.c_str()); + } + + printf (" ... done.\n"); + + viewer.spin (); + return 0; +} + +void +PointCloud2Vector3d (pcl::PointCloud::Ptr cloud, pcl::on_nurbs::vector_vec3d &data) +{ + for (unsigned i = 0; i < cloud->size (); i++) + { + Point &p = cloud->at (i); + if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z)) + data.push_back (Eigen::Vector3d (p.x, p.y, p.z)); + } +} + +void +visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer) +{ + pcl::PointCloud::Ptr curve_cloud (new pcl::PointCloud); + + pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4); + for (std::size_t i = 0; i < curve_cloud->size () - 1; i++) + { + pcl::PointXYZRGB &p1 = curve_cloud->at (i); + pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1); + std::ostringstream os; + os << "line" << i; + viewer.removeShape (os.str ()); + viewer.addLine (p1, p2, 1.0, 0.0, 0.0, os.str ()); + } + + pcl::PointCloud::Ptr curve_cps (new pcl::PointCloud); + for (int i = 0; i < curve.CVCount (); i++) + { + ON_3dPoint p1; + curve.GetCV (i, p1); + + double pnt[3]; + surface.Evaluate (p1.x, p1.y, 0, 3, pnt); + pcl::PointXYZRGB p2; + p2.x = float (pnt[0]); + p2.y = float (pnt[1]); + p2.z = float (pnt[2]); + + p2.r = 255; + p2.g = 0; + p2.b = 0; + + curve_cps->push_back (p2); + } + viewer.removePointCloud ("cloud_cps"); + viewer.addPointCloud (curve_cps, "cloud_cps"); +} diff --git a/doc/tutorials/content/sources/iccv2011/include/load_clouds.h b/doc/tutorials/content/sources/iccv2011/include/load_clouds.h index 23896844..3d5d075d 100644 --- a/doc/tutorials/content/sources/iccv2011/include/load_clouds.h +++ b/doc/tutorials/content/sources/iccv2011/include/load_clouds.h @@ -12,7 +12,7 @@ loadPointCloud (std::string filename, std::string suffix) boost::shared_ptr > output (new pcl::PointCloud); filename.append (suffix); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -22,7 +22,7 @@ loadPoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append ("_points.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -32,7 +32,7 @@ loadSurfaceNormals(std::string filename) SurfaceNormalsPtr output (new SurfaceNormals); filename.append ("_normals.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -42,7 +42,7 @@ loadKeypoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append ("_keypoints.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -52,7 +52,7 @@ loadLocalDescriptors (std::string filename) LocalDescriptorsPtr output (new LocalDescriptors); filename.append ("_localdesc.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -62,7 +62,7 @@ loadGlobalDescriptors (std::string filename) GlobalDescriptorsPtr output (new GlobalDescriptors); filename.append ("_globaldesc.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } diff --git a/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp b/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp index bfe2b431..b7b77174 100644 --- a/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/build_all_object_models.cpp @@ -198,7 +198,7 @@ main (int argc, char ** argv) filename.append(files[i]); PointCloudPtr input (new PointCloud); pcl::io::loadPCDFile (filename, *input); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str(), input->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str(), input->size ()); std::cout << files[i] << std::endl; // Construct the object model diff --git a/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp b/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp index 946321cf..ae7a0af5 100644 --- a/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/build_object_model.cpp @@ -59,7 +59,7 @@ main (int argc, char ** argv) // Load input file PointCloudPtr input (new PointCloud); pcl::io::loadPCDFile (argv[1], *input); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], input->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], input->size ()); ObjectRecognitionParameters params; ifstream params_stream; diff --git a/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp b/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp index 6a454345..d96ab5c8 100644 --- a/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/correspondence_viewer.cpp @@ -16,7 +16,7 @@ loadPoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append (".pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -26,7 +26,7 @@ loadKeypoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append ("_keypoints.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -36,7 +36,7 @@ loadLocalDescriptors (std::string filename) LocalDescriptorsPtr output (new LocalDescriptors); filename.append ("_localdesc.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } diff --git a/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp b/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp index cc9d3d54..0d98d57f 100644 --- a/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/test_feature_estimation.cpp @@ -28,7 +28,7 @@ main (int argc, char ** argv) // Load the input file PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Estimate surface normals SurfaceNormalsPtr normals; @@ -56,7 +56,7 @@ main (int argc, char ** argv) int nr_scales = atoi(tokens[2].c_str ()); float min_contrast = atof(tokens[3].c_str ()); keypoints = detectKeypoints (cloud, normals, min_scale, nr_octaves, nr_scales, min_contrast); - pcl::console::print_info ("Detected %zu keypoints\n", keypoints->size ()); + pcl::console::print_info ("Detected %lu keypoints\n", keypoints->size ()); } // Compute local descriptors diff --git a/doc/tutorials/content/sources/iccv2011/src/test_filters.cpp b/doc/tutorials/content/sources/iccv2011/src/test_filters.cpp index ce3baa11..c1f7b53d 100644 --- a/doc/tutorials/content/sources/iccv2011/src/test_filters.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/test_filters.cpp @@ -22,7 +22,7 @@ main (int argc, char ** argv) // Load the input file PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Threshold depth double min_depth, max_depth; @@ -31,7 +31,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = thresholdDepth (cloud, min_depth, max_depth); - pcl::console::print_info ("Eliminated %zu points outside depth limits\n", n - cloud->size ()); + pcl::console::print_info ("Eliminated %lu points outside depth limits\n", n - cloud->size ()); } // Downsample and threshold depth @@ -41,7 +41,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = downsample (cloud, leaf_size); - pcl::console::print_info ("Downsampled from %zu to %zu points\n", n, cloud->size ()); + pcl::console::print_info ("Downsampled from %lu to %lu points\n", n, cloud->size ()); } // Remove outliers @@ -51,7 +51,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = removeOutliers (cloud, radius, (int)min_neighbors); - pcl::console::print_info ("Removed %zu outliers\n", n - cloud->size ()); + pcl::console::print_info ("Removed %lu outliers\n", n - cloud->size ()); } // Save output diff --git a/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp b/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp index 7f2e73d6..dfc23b01 100644 --- a/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/test_object_recognition.cpp @@ -57,7 +57,7 @@ main (int argc, char ** argv) // Load input file PointCloudPtr query (new PointCloud); pcl::io::loadPCDFile (argv[1], *query); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], query->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], query->size ()); ifstream input_stream; ObjectRecognitionParameters params; diff --git a/doc/tutorials/content/sources/iccv2011/src/test_segmentation.cpp b/doc/tutorials/content/sources/iccv2011/src/test_segmentation.cpp index 9e3d9b94..df287413 100644 --- a/doc/tutorials/content/sources/iccv2011/src/test_segmentation.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/test_segmentation.cpp @@ -22,7 +22,7 @@ main (int argc, char ** argv) // Load the input file PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Subtract the dominant plane double dist_threshold, max_iters; @@ -31,7 +31,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = findAndSubtractPlane (cloud, dist_threshold, (int)max_iters); - pcl::console::print_info ("Subtracted %zu points along the detected plane\n", n - cloud->size ()); + pcl::console::print_info ("Subtracted %lu points along the detected plane\n", n - cloud->size ()); } // Cluster points @@ -41,7 +41,7 @@ main (int argc, char ** argv) if (cluster_points) { clusterObjects (cloud, tolerance, (int)min_size, (int)max_size, cluster_indices); - pcl::console::print_info ("Found %zu clusters\n", cluster_indices.size ()); + pcl::console::print_info ("Found %lu clusters\n", cluster_indices.size ()); } // Save output diff --git a/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp b/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp index 8dd0e84e..b704f655 100644 --- a/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/test_surface.cpp @@ -24,7 +24,7 @@ main (int argc, char ** argv) // Load the points PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Compute surface elements SurfaceElementsPtr surfels (new SurfaceElements); diff --git a/doc/tutorials/content/sources/interactive_icp/CMakeLists.txt b/doc/tutorials/content/sources/interactive_icp/CMakeLists.txt new file mode 100644 index 00000000..53fac37c --- /dev/null +++ b/doc/tutorials/content/sources/interactive_icp/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.6 FATAL_ERROR) + +project(pcl-interactive_icp) + +find_package(PCL 1.5 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (interactive_icp interactive_icp.cpp) +target_link_libraries (interactive_icp ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp b/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp new file mode 100644 index 00000000..4cd2cdcf --- /dev/null +++ b/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp @@ -0,0 +1,200 @@ +#include +#include + +#include +#include +#include +#include +#include // TicToc + +typedef pcl::PointXYZ PointT; +typedef pcl::PointCloud PointCloudT; + +bool next_iteration = false; + +void +print4x4Matrix (const Eigen::Matrix4d & matrix) +{ + printf ("Rotation matrix :\n"); + printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2)); + printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2)); + printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2)); + printf ("Translation vector :\n"); + printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3)); +} + +void +keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, + void* nothing) +{ + if (event.getKeySym () == "space" && event.keyDown ()) + next_iteration = true; +} + +int +main (int argc, + char* argv[]) +{ + // The point clouds we will be using + PointCloudT::Ptr cloud_in (new PointCloudT); // Original point cloud + PointCloudT::Ptr cloud_tr (new PointCloudT); // Transformed point cloud + PointCloudT::Ptr cloud_icp (new PointCloudT); // ICP output point cloud + + // Checking program arguments + if (argc < 2) + { + printf ("Usage :\n"); + printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]); + PCL_ERROR ("Provide one ply file.\n"); + return (-1); + } + + int iterations = 1; // Default number of ICP iterations + if (argc > 2) + { + // If the user passed the number of iteration as an argument + iterations = atoi (argv[2]); + if (iterations < 1) + { + PCL_ERROR ("Number of initial iterations must be >= 1\n"); + return (-1); + } + } + + pcl::console::TicToc time; + time.tic (); + if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0) + { + PCL_ERROR ("Error loading cloud %s.\n", argv[1]); + return (-1); + } + std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl; + + // Defining a rotation matrix and translation vector + Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity (); + + // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix) + double theta = M_PI / 8; // The angle of rotation in radians + transformation_matrix (0, 0) = cos (theta); + transformation_matrix (0, 1) = -sin (theta); + transformation_matrix (1, 0) = sin (theta); + transformation_matrix (1, 1) = cos (theta); + + // A translation on Z axis (0.4 meters) + transformation_matrix (2, 3) = 0.4; + + // Display in terminal the transformation matrix + std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl; + print4x4Matrix (transformation_matrix); + + // Executing the transformation + pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix); + *cloud_tr = *cloud_icp; // We backup cloud_icp into cloud_tr for later use + + // The Iterative Closest Point algorithm + time.tic (); + pcl::IterativeClosestPoint icp; + icp.setMaximumIterations (iterations); + icp.setInputSource (cloud_icp); + icp.setInputTarget (cloud_in); + icp.align (*cloud_icp); + icp.setMaximumIterations (1); // We set this variable to 1 for the next time we will call .align () function + std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl; + + if (icp.hasConverged ()) + { + std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl; + std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl; + transformation_matrix = icp.getFinalTransformation ().cast(); + print4x4Matrix (transformation_matrix); + } + else + { + PCL_ERROR ("\nICP has not converged.\n"); + return (-1); + } + + // Visualization + pcl::visualization::PCLVisualizer viewer ("ICP demo"); + // Create two verticaly separated viewports + int v1 (0); + int v2 (1); + viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1); + viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2); + + // The color we will be using + float bckgr_gray_level = 0.0; // Black + float txt_gray_lvl = 1.0 - bckgr_gray_level; + + // Original point cloud is white + pcl::visualization::PointCloudColorHandlerCustom cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl, + (int) 255 * txt_gray_lvl); + viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1); + viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2); + + // Transformed point cloud is green + pcl::visualization::PointCloudColorHandlerCustom cloud_tr_color_h (cloud_tr, 20, 180, 20); + viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1); + + // ICP aligned point cloud is red + pcl::visualization::PointCloudColorHandlerCustom cloud_icp_color_h (cloud_icp, 180, 20, 20); + viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2); + + // Adding text descriptions in each viewport + viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1); + viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2); + + std::stringstream ss; + ss << iterations; + std::string iterations_cnt = "ICP iterations = " + ss.str (); + viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2); + + // Set background color + viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1); + viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2); + + // Set camera position and orientation + viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0); + viewer.setSize (1280, 1024); // Visualiser window size + + // Register keyboard callback : + viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL); + + // Display the visualiser + while (!viewer.wasStopped ()) + { + viewer.spinOnce (); + + // The user pressed "space" : + if (next_iteration) + { + // The Iterative Closest Point algorithm + time.tic (); + icp.align (*cloud_icp); + std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl; + + if (icp.hasConverged ()) + { + printf ("\033[11A"); // Go up 11 lines in terminal output. + printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ()); + std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl; + transformation_matrix *= icp.getFinalTransformation ().cast(); // WARNING /!\ This is not accurate! For "educational" purpose only! + print4x4Matrix (transformation_matrix); // Print the transformation between original pose and current pose + + ss.str (""); + ss << iterations; + std::string iterations_cnt = "ICP iterations = " + ss.str (); + viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt"); + viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2"); + } + else + { + PCL_ERROR ("\nICP has not converged.\n"); + return (-1); + } + } + next_iteration = false; + } + return (0); +} + diff --git a/doc/tutorials/content/sources/iros2011/include/load_clouds.h b/doc/tutorials/content/sources/iros2011/include/load_clouds.h index 23896844..3d5d075d 100644 --- a/doc/tutorials/content/sources/iros2011/include/load_clouds.h +++ b/doc/tutorials/content/sources/iros2011/include/load_clouds.h @@ -12,7 +12,7 @@ loadPointCloud (std::string filename, std::string suffix) boost::shared_ptr > output (new pcl::PointCloud); filename.append (suffix); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -22,7 +22,7 @@ loadPoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append ("_points.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -32,7 +32,7 @@ loadSurfaceNormals(std::string filename) SurfaceNormalsPtr output (new SurfaceNormals); filename.append ("_normals.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -42,7 +42,7 @@ loadKeypoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append ("_keypoints.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -52,7 +52,7 @@ loadLocalDescriptors (std::string filename) LocalDescriptorsPtr output (new LocalDescriptors); filename.append ("_localdesc.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -62,7 +62,7 @@ loadGlobalDescriptors (std::string filename) GlobalDescriptorsPtr output (new GlobalDescriptors); filename.append ("_globaldesc.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } diff --git a/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp b/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp index bfe2b431..b7b77174 100644 --- a/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp +++ b/doc/tutorials/content/sources/iros2011/src/build_all_object_models.cpp @@ -198,7 +198,7 @@ main (int argc, char ** argv) filename.append(files[i]); PointCloudPtr input (new PointCloud); pcl::io::loadPCDFile (filename, *input); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str(), input->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str(), input->size ()); std::cout << files[i] << std::endl; // Construct the object model diff --git a/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp b/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp index 946321cf..ae7a0af5 100644 --- a/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp +++ b/doc/tutorials/content/sources/iros2011/src/build_object_model.cpp @@ -59,7 +59,7 @@ main (int argc, char ** argv) // Load input file PointCloudPtr input (new PointCloud); pcl::io::loadPCDFile (argv[1], *input); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], input->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], input->size ()); ObjectRecognitionParameters params; ifstream params_stream; diff --git a/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp b/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp index 6a454345..d96ab5c8 100644 --- a/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp +++ b/doc/tutorials/content/sources/iros2011/src/correspondence_viewer.cpp @@ -16,7 +16,7 @@ loadPoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append (".pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -26,7 +26,7 @@ loadKeypoints (std::string filename) PointCloudPtr output (new PointCloud); filename.append ("_keypoints.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } @@ -36,7 +36,7 @@ loadLocalDescriptors (std::string filename) LocalDescriptorsPtr output (new LocalDescriptors); filename.append ("_localdesc.pcd"); pcl::io::loadPCDFile (filename, *output); - pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str (), output->size ()); return (output); } diff --git a/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp b/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp index ebae2910..0df4ad0e 100644 --- a/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp +++ b/doc/tutorials/content/sources/iros2011/src/test_feature_estimation.cpp @@ -28,7 +28,7 @@ main (int argc, char ** argv) // Load the input file PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Estimate surface normals SurfaceNormalsPtr normals; @@ -56,7 +56,7 @@ main (int argc, char ** argv) int nr_scales = atoi(tokens[2].c_str ()); float min_contrast = atof(tokens[3].c_str ()); keypoints = detectKeypoints (cloud, normals, min_scale, nr_octaves, nr_scales, min_contrast); - pcl::console::print_info ("Detected %zu keypoints\n", keypoints->size ()); + pcl::console::print_info ("Detected %lu keypoints\n", keypoints->size ()); } // Compute local descriptors diff --git a/doc/tutorials/content/sources/iros2011/src/test_filters.cpp b/doc/tutorials/content/sources/iros2011/src/test_filters.cpp index 6bffe6b9..ae2dae13 100644 --- a/doc/tutorials/content/sources/iros2011/src/test_filters.cpp +++ b/doc/tutorials/content/sources/iros2011/src/test_filters.cpp @@ -22,7 +22,7 @@ main (int argc, char ** argv) // Load the input file PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Threshold depth double min_depth, max_depth; @@ -31,7 +31,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = thresholdDepth (cloud, min_depth, max_depth); - pcl::console::print_info ("Eliminated %zu points outside depth limits\n", n - cloud->size ()); + pcl::console::print_info ("Eliminated %lu points outside depth limits\n", n - cloud->size ()); } // Downsample and threshold depth @@ -41,7 +41,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = downsample (cloud, leaf_size); - pcl::console::print_info ("Downsampled from %zu to %zu points\n", n, cloud->size ()); + pcl::console::print_info ("Downsampled from %lu to %lu points\n", n, cloud->size ()); } // Remove outliers @@ -51,7 +51,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = removeOutliers (cloud, radius, (int)min_neighbors); - pcl::console::print_info ("Removed %zu outliers\n", n - cloud->size ()); + pcl::console::print_info ("Removed %lu outliers\n", n - cloud->size ()); } // Save output diff --git a/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp b/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp index ecc4af51..61928225 100644 --- a/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp +++ b/doc/tutorials/content/sources/iros2011/src/test_object_recognition.cpp @@ -57,7 +57,7 @@ main (int argc, char ** argv) // Load input file PointCloudPtr query (new PointCloud); pcl::io::loadPCDFile (argv[1], *query); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], query->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], query->size ()); ifstream input_stream; ObjectRecognitionParameters params; diff --git a/doc/tutorials/content/sources/iros2011/src/test_segmentation.cpp b/doc/tutorials/content/sources/iros2011/src/test_segmentation.cpp index 32027b6e..4760ebcc 100644 --- a/doc/tutorials/content/sources/iros2011/src/test_segmentation.cpp +++ b/doc/tutorials/content/sources/iros2011/src/test_segmentation.cpp @@ -22,7 +22,7 @@ main (int argc, char ** argv) // Load the input file PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Subtract the dominant plane double dist_threshold, max_iters; @@ -31,7 +31,7 @@ main (int argc, char ** argv) { size_t n = cloud->size (); cloud = findAndSubtractPlane (cloud, dist_threshold, (int)max_iters); - pcl::console::print_info ("Subtracted %zu points along the detected plane\n", n - cloud->size ()); + pcl::console::print_info ("Subtracted %lu points along the detected plane\n", n - cloud->size ()); } // Cluster points @@ -41,7 +41,7 @@ main (int argc, char ** argv) if (cluster_points) { clusterObjects (cloud, tolerance, (int)min_size, (int)max_size, cluster_indices); - pcl::console::print_info ("Found %zu clusters\n", cluster_indices.size ()); + pcl::console::print_info ("Found %lu clusters\n", cluster_indices.size ()); } // Save output diff --git a/doc/tutorials/content/sources/iros2011/src/test_surface.cpp b/doc/tutorials/content/sources/iros2011/src/test_surface.cpp index 8dd0e84e..b704f655 100644 --- a/doc/tutorials/content/sources/iros2011/src/test_surface.cpp +++ b/doc/tutorials/content/sources/iros2011/src/test_surface.cpp @@ -24,7 +24,7 @@ main (int argc, char ** argv) // Load the points PointCloudPtr cloud (new PointCloud); pcl::io::loadPCDFile (argv[1], *cloud); - pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ()); + pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ()); // Compute surface elements SurfaceElementsPtr surfels (new SurfaceElements); diff --git a/doc/tutorials/content/sources/matrix_transform/CMakeLists.txt b/doc/tutorials/content/sources/matrix_transform/CMakeLists.txt new file mode 100644 index 00000000..ab2394da --- /dev/null +++ b/doc/tutorials/content/sources/matrix_transform/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.6 FATAL_ERROR) + +project(pcl-matrix_transform) + +find_package(PCL 1.7 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (matrix_transform matrix_transform.cpp) +target_link_libraries (matrix_transform ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/matrix_transform/matrix_transform.cpp b/doc/tutorials/content/sources/matrix_transform/matrix_transform.cpp new file mode 100644 index 00000000..bea42e67 --- /dev/null +++ b/doc/tutorials/content/sources/matrix_transform/matrix_transform.cpp @@ -0,0 +1,137 @@ +#include + +#include +#include +#include +#include +#include +#include + +// This function displays the help +void +showHelp(char * program_name) +{ + std::cout << std::endl; + std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl; + std::cout << "-h: Show this help." << std::endl; +} + +// This is the main function +int +main (int argc, char** argv) +{ + + // Show help + if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) { + showHelp (argv[0]); + return 0; + } + + // Fetch point cloud filename in arguments | Works with PCD and PLY files + std::vector filenames; + bool file_is_pcd = false; + + filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply"); + + if (filenames.size () != 1) { + filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); + + if (filenames.size () != 1) { + showHelp (argv[0]); + return -1; + } else { + file_is_pcd = true; + } + } + + // Load file | Works with PCD and PLY files + pcl::PointCloud::Ptr source_cloud (new pcl::PointCloud ()); + + if (file_is_pcd) { + if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) { + std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl; + showHelp (argv[0]); + return -1; + } + } else { + if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0) { + std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl; + showHelp (argv[0]); + return -1; + } + } + + /* Reminder: how transformation matrices work : + + |-------> This column is the translation + | 1 0 0 x | \ + | 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left + | 0 0 1 z | / + | 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1) + + METHOD #1: Using a Matrix4f + This is the "manual" method, perfect to understand but error prone ! + */ + Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); + + // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix) + float theta = M_PI/4; // The angle of rotation in radians + transform_1 (0,0) = cos (theta); + transform_1 (0,1) = -sin(theta); + transform_1 (1,0) = sin (theta); + transform_1 (1,1) = cos (theta); + // (row, column) + + // Define a translation of 2.5 meters on the x axis. + transform_1 (0,3) = 2.5; + + // Print the transformation + printf ("Method #1: using a Matrix4f\n"); + std::cout << transform_1 << std::endl; + + /* METHOD #2: Using a Affine3f + This method is easier and less error prone + */ + Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); + + // Define a translation of 2.5 meters on the x axis. + transform_2.translation() << 2.5, 0.0, 0.0; + + // The same rotation matrix as before; tetha radians arround Z axis + transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ())); + + // Print the transformation + printf ("\nMethod #2: using an Affine3f\n"); + std::cout << transform_2.matrix() << std::endl; + + // Executing the transformation + pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); + // You can either apply transform_1 or transform_2; they are the same + pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2); + + // Visualization + printf( "\nPoint cloud colors : white = original point cloud\n" + " red = transformed point cloud\n"); + pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); + + // Define R,G,B colors for the point cloud + pcl::visualization::PointCloudColorHandlerCustom source_cloud_color_handler (source_cloud, 255, 255, 255); + // We add the point cloud to the viewer and pass the color handler + viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud"); + + pcl::visualization::PointCloudColorHandlerCustom transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red + viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud"); + + viewer.addCoordinateSystem (1.0, "cloud", 0); + viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud"); + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud"); + //viewer.setPosition(800, 400); // Setting visualiser window position + + while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed + viewer.spinOnce (); + } + + return 0; +} + diff --git a/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt b/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt new file mode 100644 index 00000000..de01df66 --- /dev/null +++ b/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(model_outlier_removal) + +find_package(PCL 1.7 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (model_outlier_removal model_outlier_removal.cpp) +target_link_libraries (model_outlier_removal ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp new file mode 100644 index 00000000..331c97d5 --- /dev/null +++ b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp @@ -0,0 +1,71 @@ +#include +#include +#include + +int +main () +{ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_sphere_filtered (new pcl::PointCloud); + + // 1. Generate cloud data + int noise_size = 5; + int sphere_data_size = 10; + cloud->width = noise_size + sphere_data_size; + cloud->height = 1; + cloud->points.resize (cloud->width * cloud->height); + // 1.1 Add noise + for (size_t i = 0; i < noise_size; ++i) + { + cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); + } + // 1.2 Add sphere: + double rand_x1 = 1; + double rand_x2 = 1; + for (size_t i = noise_size; i < noise_size + sphere_data_size; ++i) + { + // See: http://mathworld.wolfram.com/SpherePointPicking.html + while (pow (rand_x1, 2) + pow (rand_x2, 2) >= 1) + { + rand_x1 = (rand () % 100) / (50.0f) - 1; + rand_x2 = (rand () % 100) / (50.0f) - 1; + } + double pre_calc = sqrt (1 - pow (rand_x1, 2) - pow (rand_x2, 2)); + cloud->points[i].x = 2 * rand_x1 * pre_calc; + cloud->points[i].y = 2 * rand_x2 * pre_calc; + cloud->points[i].z = 1 - 2 * (pow (rand_x1, 2) + pow (rand_x2, 2)); + rand_x1 = 1; + rand_x2 = 1; + } + + std::cerr << "Cloud before filtering: " << std::endl; + for (size_t i = 0; i < cloud->points.size (); ++i) + std::cout << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl; + + // 2. filter sphere: + // 2.1 generate model: + // modelparameter for this sphere: + // position.x: 0, position.y: 0, position.z:0, radius: 1 + pcl::ModelCoefficients sphere_coeff; + sphere_coeff.values.resize (4); + sphere_coeff.values[0] = 0; + sphere_coeff.values[1] = 0; + sphere_coeff.values[2] = 0; + sphere_coeff.values[3] = 1; + + pcl::ModelOutlierRemoval sphere_filter; + sphere_filter.setModelCoefficients (sphere_coeff); + sphere_filter.setThreshold (0.05); + sphere_filter.setModelType (pcl::SACMODEL_SPHERE); + sphere_filter.setInputCloud (cloud); + sphere_filter.filter (*cloud_sphere_filtered); + + std::cerr << "Sphere after filtering: " << std::endl; + for (size_t i = 0; i < cloud_sphere_filtered->points.size (); ++i) + std::cout << " " << cloud_sphere_filtered->points[i].x << " " << cloud_sphere_filtered->points[i].y << " " << cloud_sphere_filtered->points[i].z + << std::endl; + + return (0); +} diff --git a/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt b/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt new file mode 100644 index 00000000..817ee201 --- /dev/null +++ b/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(moment_of_inertia) + +find_package(PCL 1.8 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (moment_of_inertia moment_of_inertia.cpp) +target_link_libraries (moment_of_inertia ${PCL_LIBRARIES}) + + diff --git a/doc/tutorials/content/sources/moment_of_inertia/moment_of_inertia.cpp b/doc/tutorials/content/sources/moment_of_inertia/moment_of_inertia.cpp new file mode 100644 index 00000000..2e48111a --- /dev/null +++ b/doc/tutorials/content/sources/moment_of_inertia/moment_of_inertia.cpp @@ -0,0 +1,107 @@ +#include +#include +#include +#include +#include +#include + +int main (int argc, char** argv) +{ + if (argc != 2) + return (0); + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + if (pcl::io::loadPCDFile (argv[1], *cloud) == -1) + return (-1); + + pcl::MomentOfInertiaEstimation feature_extractor; + feature_extractor.setInputCloud (cloud); + feature_extractor.compute (); + + std::vector moment_of_inertia; + std::vector eccentricity; + pcl::PointXYZ min_point_AABB; + pcl::PointXYZ max_point_AABB; + pcl::PointXYZ min_point_OBB; + pcl::PointXYZ max_point_OBB; + pcl::PointXYZ position_OBB; + Eigen::Matrix3f rotational_matrix_OBB; + float major_value, middle_value, minor_value; + Eigen::Vector3f major_vector, middle_vector, minor_vector; + Eigen::Vector3f mass_center; + + feature_extractor.getMomentOfInertia (moment_of_inertia); + feature_extractor.getEccentricity (eccentricity); + feature_extractor.getAABB (min_point_AABB, max_point_AABB); + feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); + feature_extractor.getEigenValues (major_value, middle_value, minor_value); + feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); + feature_extractor.getMassCenter (mass_center); + + boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + viewer->addPointCloud (cloud, "sample cloud"); + viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB"); + + Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); + Eigen::Quaternionf quat (rotational_matrix_OBB); + viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB"); + + pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2)); + pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2)); + pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2)); + pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2)); + viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector"); + viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector"); + viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector"); + + //Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z); + //Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z); + //Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z); + //Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z); + //Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z); + //Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z); + //Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z); + //Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z); + + //p1 = rotational_matrix_OBB * p1 + position; + //p2 = rotational_matrix_OBB * p2 + position; + //p3 = rotational_matrix_OBB * p3 + position; + //p4 = rotational_matrix_OBB * p4 + position; + //p5 = rotational_matrix_OBB * p5 + position; + //p6 = rotational_matrix_OBB * p6 + position; + //p7 = rotational_matrix_OBB * p7 + position; + //p8 = rotational_matrix_OBB * p8 + position; + + //pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2)); + //pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2)); + //pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2)); + //pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2)); + //pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2)); + //pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2)); + //pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2)); + //pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2)); + + //viewer->addLine (pt1, pt2, 1.0, 0.0, 0.0, "1 edge"); + //viewer->addLine (pt1, pt4, 1.0, 0.0, 0.0, "2 edge"); + //viewer->addLine (pt1, pt5, 1.0, 0.0, 0.0, "3 edge"); + //viewer->addLine (pt5, pt6, 1.0, 0.0, 0.0, "4 edge"); + //viewer->addLine (pt5, pt8, 1.0, 0.0, 0.0, "5 edge"); + //viewer->addLine (pt2, pt6, 1.0, 0.0, 0.0, "6 edge"); + //viewer->addLine (pt6, pt7, 1.0, 0.0, 0.0, "7 edge"); + //viewer->addLine (pt7, pt8, 1.0, 0.0, 0.0, "8 edge"); + //viewer->addLine (pt2, pt3, 1.0, 0.0, 0.0, "9 edge"); + //viewer->addLine (pt4, pt8, 1.0, 0.0, 0.0, "10 edge"); + //viewer->addLine (pt3, pt4, 1.0, 0.0, 0.0, "11 edge"); + //viewer->addLine (pt3, pt7, 1.0, 0.0, 0.0, "12 edge"); + + while(!viewer->wasStopped()) + { + viewer->spinOnce (100); + boost::this_thread::sleep (boost::posix_time::microseconds (100000)); + } + + return (0); +} diff --git a/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp b/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp index b3503931..70d4dbec 100644 --- a/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp +++ b/doc/tutorials/content/sources/narf_feature_extraction/narf_feature_extraction.cpp @@ -149,7 +149,7 @@ main (int argc, char** argv) pcl::visualization::PointCloudColorHandlerCustom range_image_color_handler (range_image_ptr, 0, 0, 0); viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image"); - //viewer.addCoordinateSystem (1.0f); + //viewer.addCoordinateSystem (1.0f, "global"); //PointCloudColorHandlerCustom point_cloud_color_handler (point_cloud_ptr, 150, 150, 150); //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); viewer.initCameraParameters (); diff --git a/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp b/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp index d77bf062..e6ae34f7 100644 --- a/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp +++ b/doc/tutorials/content/sources/narf_keypoint_extraction/narf_keypoint_extraction.cpp @@ -143,7 +143,7 @@ main (int argc, char** argv) pcl::visualization::PointCloudColorHandlerCustom range_image_color_handler (range_image_ptr, 0, 0, 0); viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image"); - //viewer.addCoordinateSystem (1.0f); + //viewer.addCoordinateSystem (1.0f, "global"); //PointCloudColorHandlerCustom point_cloud_color_handler (point_cloud_ptr, 150, 150, 150); //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); viewer.initCameraParameters (); diff --git a/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp b/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp index b4d2935d..ff730b99 100644 --- a/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp +++ b/doc/tutorials/content/sources/normal_distributions_transform/normal_distributions_transform.cpp @@ -95,7 +95,7 @@ main (int argc, char** argv) 1, "output cloud"); // Starting visualizer - viewer_final->addCoordinateSystem (1.0); + viewer_final->addCoordinateSystem (1.0, "global"); viewer_final->initCameraParameters (); // Wait until visualizer window is closed. diff --git a/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp b/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp index 0c26fdfe..52806c65 100644 --- a/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp +++ b/doc/tutorials/content/sources/openni_narf_keypoint_extraction/openni_narf_keypoint_extraction.cpp @@ -87,7 +87,7 @@ int main (int argc, char** argv) pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image"); pcl::visualization::PCLVisualizer viewer ("3D Viewer"); - viewer.addCoordinateSystem (1.0f); + viewer.addCoordinateSystem (1.0f, "global"); viewer.setBackgroundColor (1, 1, 1); viewer.initCameraParameters (); diff --git a/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp b/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp index 8893039b..2be935db 100644 --- a/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp +++ b/doc/tutorials/content/sources/openni_range_image_visualization/openni_range_image_visualization.cpp @@ -63,7 +63,7 @@ int main (int argc, char** argv) pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image"); pcl::visualization::PCLVisualizer viewer ("3D Viewer"); - viewer.addCoordinateSystem (1.0f); + viewer.addCoordinateSystem (1.0f, "global"); viewer.setBackgroundColor (1, 1, 1); // Set the viewing pose so that the openni cloud is visible diff --git a/doc/tutorials/content/sources/pairwise_incremental_registration/CMakeLists.txt b/doc/tutorials/content/sources/pairwise_incremental_registration/CMakeLists.txt index 9868ab23..09662178 100644 --- a/doc/tutorials/content/sources/pairwise_incremental_registration/CMakeLists.txt +++ b/doc/tutorials/content/sources/pairwise_incremental_registration/CMakeLists.txt @@ -7,7 +7,6 @@ find_package(PCL 1.4 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) -add_definitions(-Wno-deprecated -DEIGEN_DONT_VECTORIZE -DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) add_executable (pairwise_incremental_registration pairwise_incremental_registration.cpp) target_link_libraries (pairwise_incremental_registration ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp b/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp index 7bd6f0c9..35d4bc67 100644 --- a/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp +++ b/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp @@ -362,7 +362,7 @@ int main (int argc, char** argv) pcl::transformPointCloud (*temp, *result, GlobalTransform); //update the global transform - GlobalTransform = pairTransform * GlobalTransform; + GlobalTransform = GlobalTransform * pairTransform; //save aligned pair, transformed into the first cloud's frame std::stringstream ss; diff --git a/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp b/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp index f2688cbb..aa8ebbd5 100644 --- a/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp +++ b/doc/tutorials/content/sources/planar_segmentation/planar_segmentation.cpp @@ -9,31 +9,31 @@ int main (int argc, char** argv) { - pcl::PointCloud cloud; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); // Fill in the cloud data - cloud.width = 15; - cloud.height = 1; - cloud.points.resize (cloud.width * cloud.height); + cloud->width = 15; + cloud->height = 1; + cloud->points.resize (cloud->width * cloud->height); // Generate the data - for (size_t i = 0; i < cloud.points.size (); ++i) + for (size_t i = 0; i < cloud->points.size (); ++i) { - cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); - cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); - cloud.points[i].z = 1.0; + cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].z = 1.0; } // Set a few outliers - cloud.points[0].z = 2.0; - cloud.points[3].z = -2.0; - cloud.points[6].z = 4.0; + cloud->points[0].z = 2.0; + cloud->points[3].z = -2.0; + cloud->points[6].z = 4.0; - std::cerr << "Point cloud data: " << cloud.points.size () << " points" << std::endl; - for (size_t i = 0; i < cloud.points.size (); ++i) - std::cerr << " " << cloud.points[i].x << " " - << cloud.points[i].y << " " - << cloud.points[i].z << std::endl; + std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl; + for (size_t i = 0; i < cloud->points.size (); ++i) + std::cerr << " " << cloud->points[i].x << " " + << cloud->points[i].y << " " + << cloud->points[i].z << std::endl; pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); @@ -46,7 +46,7 @@ int seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); - seg.setInputCloud (cloud.makeShared ()); + seg.setInputCloud (cloud); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) @@ -62,9 +62,9 @@ int std::cerr << "Model inliers: " << inliers->indices.size () << std::endl; for (size_t i = 0; i < inliers->indices.size (); ++i) - std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " " - << cloud.points[inliers->indices[i]].y << " " - << cloud.points[inliers->indices[i]].z << std::endl; + std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " " + << cloud->points[inliers->indices[i]].y << " " + << cloud->points[inliers->indices[i]].z << std::endl; return (0); } diff --git a/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt b/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt new file mode 100644 index 00000000..914dc610 --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required (VERSION 2.6 FATAL_ERROR) + +project (pcl-visualizer) +find_package (Qt4 REQUIRED) +find_package (VTK REQUIRED) +find_package (PCL 1.7.1 REQUIRED) + +include_directories (${PCL_INCLUDE_DIRS}) +link_directories (${PCL_LIBRARY_DIRS}) +add_definitions (${PCL_DEFINITIONS}) + +set (project_SOURCES main.cpp pclviewer.cpp) +set (project_HEADERS pclviewer.h) +set (project_FORMS pclviewer.ui) +set (VTK_LIBRARIES vtkRendering vtkGraphics vtkHybrid QVTK) + +QT4_WRAP_CPP (project_HEADERS_MOC ${project_HEADERS}) +QT4_WRAP_UI (project_FORMS_HEADERS ${project_FORMS}) + +INCLUDE (${QT_USE_FILE}) +ADD_DEFINITIONS (${QT_DEFINITIONS}) + +ADD_EXECUTABLE (pcl_visualizer ${project_SOURCES} + ${project_FORMS_HEADERS} + ${project_HEADERS_MOC}) + +TARGET_LINK_LIBRARIES (pcl_visualizer ${QT_LIBRARIES} ${PCL_LIBRARIES} ${VTK_LIBRARIES}) + diff --git a/doc/tutorials/content/sources/qt_visualizer/main.cpp b/doc/tutorials/content/sources/qt_visualizer/main.cpp new file mode 100644 index 00000000..5dda9f8d --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/main.cpp @@ -0,0 +1,12 @@ +#include "pclviewer.h" +#include +#include + +int main (int argc, char *argv[]) +{ + QApplication a (argc, argv); + PCLViewer w; + w.show (); + + return a.exec (); +} diff --git a/doc/tutorials/content/sources/qt_visualizer/pcl_visualizer.pro b/doc/tutorials/content/sources/qt_visualizer/pcl_visualizer.pro new file mode 100644 index 00000000..3ca33271 --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/pcl_visualizer.pro @@ -0,0 +1,20 @@ +#------------------------------------------------- +# +# Project created by QtCreator 2014-05-01T14:24:33 +# +#------------------------------------------------- + +QT += core gui + +greaterThan(QT_MAJOR_VERSION, 4): QT += widgets + +TARGET = pcl_visualizer +TEMPLATE = app + + +SOURCES += main.cpp\ + pclviewer.cpp + +HEADERS += pclviewer.h + +FORMS += pclviewer.ui diff --git a/doc/tutorials/content/sources/qt_visualizer/pcl_visualizer.pro.user b/doc/tutorials/content/sources/qt_visualizer/pcl_visualizer.pro.user new file mode 100644 index 00000000..aa642052 --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/pcl_visualizer.pro.user @@ -0,0 +1,200 @@ + + + + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + true + 1 + true + 0 + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {7fcc8410-2f07-4a1b-8f7e-75865afc8bab} + 0 + 0 + 0 + + ../build + + + true + ../src + cmake + %{buildDir}/../build + Custom Process Step + + ProjectExplorer.ProcessStep + + + true + Make + + Qt4ProjectManager.MakeStep + + -w + -r + + false + -j2 + + + 2 + Build + + ProjectExplorer.BuildSteps.Build + + + + true + Make + + Qt4ProjectManager.MakeStep + + -w + -r + + true + clean + + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release + + Qt4ProjectManager.Qt4BuildConfiguration + 0 + true + + 1 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy locally + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + pcl_visualizer + + Qt4ProjectManager.Qt4RunConfiguration:/home/victor/Copy/qt_visualizer/src/pcl_visualizer.pro + + pcl_visualizer.pro + false + false + + 3768 + true + false + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.EnvironmentId + {8d8c9016-62ac-401a-82aa-bc1e1c77c433} + + + ProjectExplorer.Project.Updater.FileVersion + 15 + + diff --git a/doc/tutorials/content/sources/qt_visualizer/pclviewer.cpp b/doc/tutorials/content/sources/qt_visualizer/pclviewer.cpp new file mode 100644 index 00000000..beff918b --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/pclviewer.cpp @@ -0,0 +1,121 @@ +#include "pclviewer.h" +#include "../build/ui_pclviewer.h" + +PCLViewer::PCLViewer (QWidget *parent) : + QMainWindow (parent), + ui (new Ui::PCLViewer) +{ + ui->setupUi (this); + this->setWindowTitle ("PCL viewer"); + + // Setup the cloud pointer + cloud.reset (new PointCloudT); + // The number of points in the cloud + cloud->points.resize (200); + + // The default color + red = 128; + green = 128; + blue = 128; + + // Fill the cloud with some points + for (size_t i = 0; i < cloud->points.size (); ++i) + { + cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); + + cloud->points[i].r = red; + cloud->points[i].g = green; + cloud->points[i].b = blue; + } + + // Set up the QVTK window + viewer.reset (new pcl::visualization::PCLVisualizer ("viewer", false)); + ui->qvtkWidget->SetRenderWindow (viewer->getRenderWindow ()); + viewer->setupInteractor (ui->qvtkWidget->GetInteractor (), ui->qvtkWidget->GetRenderWindow ()); + ui->qvtkWidget->update (); + + // Connect "random" button and the function + connect (ui->pushButton_random, SIGNAL (clicked ()), this, SLOT (randomButtonPressed ())); + + // Connect R,G,B sliders and their functions + connect (ui->horizontalSlider_R, SIGNAL (valueChanged (int)), this, SLOT (redSliderValueChanged (int))); + connect (ui->horizontalSlider_G, SIGNAL (valueChanged (int)), this, SLOT (greenSliderValueChanged (int))); + connect (ui->horizontalSlider_B, SIGNAL (valueChanged (int)), this, SLOT (blueSliderValueChanged (int))); + connect (ui->horizontalSlider_R, SIGNAL (sliderReleased ()), this, SLOT (RGBsliderReleased ())); + connect (ui->horizontalSlider_G, SIGNAL (sliderReleased ()), this, SLOT (RGBsliderReleased ())); + connect (ui->horizontalSlider_B, SIGNAL (sliderReleased ()), this, SLOT (RGBsliderReleased ())); + + // Connect point size slider + connect (ui->horizontalSlider_p, SIGNAL (valueChanged (int)), this, SLOT (pSliderValueChanged (int))); + + viewer->addPointCloud (cloud, "cloud"); + pSliderValueChanged (2); + viewer->resetCamera (); + ui->qvtkWidget->update (); +} + +void +PCLViewer::randomButtonPressed () +{ + printf ("Random button was pressed\n"); + + // Set the new color + for (size_t i = 0; i < cloud->size(); i++) + { + cloud->points[i].r = 255 *(1024 * rand () / (RAND_MAX + 1.0f)); + cloud->points[i].g = 255 *(1024 * rand () / (RAND_MAX + 1.0f)); + cloud->points[i].b = 255 *(1024 * rand () / (RAND_MAX + 1.0f)); + } + + viewer->updatePointCloud (cloud, "cloud"); + ui->qvtkWidget->update (); +} + +void +PCLViewer::RGBsliderReleased () +{ + // Set the new color + for (size_t i = 0; i < cloud->size (); i++) + { + cloud->points[i].r = red; + cloud->points[i].g = green; + cloud->points[i].b = blue; + } + viewer->updatePointCloud (cloud, "cloud"); + ui->qvtkWidget->update (); +} + +void +PCLViewer::pSliderValueChanged (int value) +{ + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, value, "cloud"); + ui->qvtkWidget->update (); +} + +void +PCLViewer::redSliderValueChanged (int value) +{ + red = value; + printf ("redSliderValueChanged: [%d|%d|%d]\n", red, green, blue); +} + +void +PCLViewer::greenSliderValueChanged (int value) +{ + green = value; + printf ("greenSliderValueChanged: [%d|%d|%d]\n", red, green, blue); +} + +void +PCLViewer::blueSliderValueChanged (int value) +{ + blue = value; + printf("blueSliderValueChanged: [%d|%d|%d]\n", red, green, blue); +} + +PCLViewer::~PCLViewer () +{ + delete ui; +} diff --git a/doc/tutorials/content/sources/qt_visualizer/pclviewer.h b/doc/tutorials/content/sources/qt_visualizer/pclviewer.h new file mode 100644 index 00000000..2246a236 --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/pclviewer.h @@ -0,0 +1,65 @@ +#ifndef PCLVIEWER_H +#define PCLVIEWER_H + +#include + +// Qt +#include + +// Point Cloud Library +#include +#include +#include + +// Visualization Toolkit (VTK) +#include + +typedef pcl::PointXYZRGBA PointT; +typedef pcl::PointCloud PointCloudT; + +namespace Ui +{ + class PCLViewer; +} + +class PCLViewer : public QMainWindow +{ + Q_OBJECT + +public: + explicit PCLViewer (QWidget *parent = 0); + ~PCLViewer (); + +public slots: + void + randomButtonPressed (); + + void + RGBsliderReleased (); + + void + pSliderValueChanged (int value); + + void + redSliderValueChanged (int value); + + void + greenSliderValueChanged (int value); + + void + blueSliderValueChanged (int value); + +protected: + boost::shared_ptr viewer; + PointCloudT::Ptr cloud; + + unsigned int red; + unsigned int green; + unsigned int blue; + +private: + Ui::PCLViewer *ui; + +}; + +#endif // PCLVIEWER_H diff --git a/doc/tutorials/content/sources/qt_visualizer/pclviewer.ui b/doc/tutorials/content/sources/qt_visualizer/pclviewer.ui new file mode 100644 index 00000000..3f616085 --- /dev/null +++ b/doc/tutorials/content/sources/qt_visualizer/pclviewer.ui @@ -0,0 +1,367 @@ + + + PCLViewer + + + + 0 + 0 + 966 + 499 + + + + + 0 + 0 + + + + + 5000 + 5000 + + + + PCLViewer + + + + + + 300 + 10 + 640 + 480 + + + + + + + 30 + 60 + 160 + 29 + + + + 255 + + + 128 + + + Qt::Horizontal + + + + + + 30 + 140 + 160 + 29 + + + + 255 + + + 128 + + + Qt::Horizontal + + + + + + 30 + 220 + 160 + 29 + + + + 255 + + + 128 + + + Qt::Horizontal + + + + + + 200 + 50 + 81 + 41 + + + + 3 + + + QLCDNumber::Flat + + + 128 + + + + + + 200 + 130 + 81 + 41 + + + + 3 + + + QLCDNumber::Flat + + + 128 + + + + + + 200 + 210 + 81 + 41 + + + + 3 + + + QLCDNumber::Flat + + + 128 + + + + + + 30 + 320 + 160 + 29 + + + + 1 + + + 6 + + + 2 + + + Qt::Horizontal + + + + + + 200 + 310 + 81 + 41 + + + + 1 + + + QLCDNumber::Flat + + + 2 + + + + + + 30 + 20 + 191 + 31 + + + + + 16 + 50 + false + false + + + + Red component + + + + + + 30 + 100 + 191 + 31 + + + + + 16 + 50 + false + false + + + + Green component + + + + + + 30 + 190 + 191 + 31 + + + + + 16 + 50 + false + false + + + + Blue component + + + + + + 30 + 280 + 141 + 31 + + + + + 16 + 50 + false + false + + + + Point size + + + + + + 40 + 370 + 201 + 81 + + + + Random colors + + + + + + + QVTKWidget + QWidget +
QVTKWidget.h
+ + + + + + horizontalSlider_R + sliderMoved(int) + lcdNumber_R + display(int) + + + 136 + 111 + + + 222 + 115 + + + + + horizontalSlider_G + sliderMoved(int) + lcdNumber_G + display(int) + + + 166 + 193 + + + 235 + 195 + + + + + horizontalSlider_B + sliderMoved(int) + lcdNumber_B + display(int) + + + 185 + 273 + + + 224 + 275 + + + + + horizontalSlider_p + sliderMoved(int) + lcdNumber_p + display(int) + + + 136 + 352 + + + 253 + 342 + + + + + diff --git a/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp b/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp index f02b9c18..6f8eb491 100644 --- a/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp +++ b/doc/tutorials/content/sources/random_sample_consensus/random_sample_consensus.cpp @@ -19,7 +19,7 @@ simpleVis (pcl::PointCloud::ConstPtr cloud) viewer->setBackgroundColor (0, 0, 0); viewer->addPointCloud (cloud, "sample cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - //viewer->addCoordinateSystem (1.0); + //viewer->addCoordinateSystem (1.0, "global"); viewer->initCameraParameters (); return (viewer); } diff --git a/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp b/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp index abc9039e..5ad46214 100644 --- a/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp +++ b/doc/tutorials/content/sources/range_image_border_extraction/range_image_border_extraction.cpp @@ -123,7 +123,7 @@ main (int argc, char** argv) // -------------------------------------------- pcl::visualization::PCLVisualizer viewer ("3D Viewer"); viewer.setBackgroundColor (1, 1, 1); - viewer.addCoordinateSystem (1.0f); + viewer.addCoordinateSystem (1.0f, "global"); pcl::visualization::PointCloudColorHandlerCustom point_cloud_color_handler (point_cloud_ptr, 0, 0, 0); viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); //PointCloudColorHandlerCustom range_image_color_handler (range_image_ptr, 150, 150, 150); diff --git a/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp b/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp index 311bc73e..23634f3e 100644 --- a/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp +++ b/doc/tutorials/content/sources/range_image_visualization/range_image_visualization.cpp @@ -134,7 +134,7 @@ main (int argc, char** argv) pcl::visualization::PointCloudColorHandlerCustom range_image_color_handler (range_image_ptr, 0, 0, 0); viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image"); - //viewer.addCoordinateSystem (1.0f); + //viewer.addCoordinateSystem (1.0f, "global"); //PointCloudColorHandlerCustom point_cloud_color_handler (point_cloud_ptr, 150, 150, 150); //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); viewer.initCameraParameters (); diff --git a/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp b/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp index 72379d88..56a1123f 100644 --- a/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp +++ b/doc/tutorials/content/sources/region_growing_segmentation/region_growing_segmentation.cpp @@ -35,14 +35,14 @@ main (int argc, char** argv) pass.filter (*indices); pcl::RegionGrowing reg; - reg.setMinClusterSize (100); - reg.setMaxClusterSize (10000); + reg.setMinClusterSize (50); + reg.setMaxClusterSize (1000000); reg.setSearchMethod (tree); reg.setNumberOfNeighbours (30); reg.setInputCloud (cloud); //reg.setIndices (indices); reg.setInputNormals (normals); - reg.setSmoothnessThreshold (7.0 / 180.0 * M_PI); + reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI); reg.setCurvatureThreshold (1.0); std::vector clusters; @@ -53,11 +53,14 @@ main (int argc, char** argv) std::cout << "These are the indices of the points of the initial" << std::endl << "cloud that belong to the first cluster:" << std::endl; int counter = 0; - while (counter < 5 || counter > clusters[0].indices.size ()) + while (counter < clusters[0].indices.size ()) { - std::cout << clusters[0].indices[counter] << std::endl; + std::cout << clusters[0].indices[counter] << ", "; counter++; + if (counter % 10 == 0) + std::cout << std::endl; } + std::cout << std::endl; pcl::PointCloud ::Ptr colored_cloud = reg.getColoredCloud (); pcl::visualization::CloudViewer viewer ("Cluster viewer"); @@ -68,3 +71,4 @@ main (int argc, char** argv) return (0); } + diff --git a/doc/tutorials/content/sources/registration_api/example2.cpp b/doc/tutorials/content/sources/registration_api/example2.cpp index e09ee030..8a872682 100644 --- a/doc/tutorials/content/sources/registration_api/example2.cpp +++ b/doc/tutorials/content/sources/registration_api/example2.cpp @@ -142,13 +142,13 @@ computeTransformation (const PointCloud::Ptr &src, keypoints_tgt (new PointCloud); estimateKeypoints (src, tgt, *keypoints_src, *keypoints_tgt); - print_info ("Found %zu and %zu keypoints for the source and target datasets.\n", keypoints_src->points.size (), keypoints_tgt->points.size ()); + print_info ("Found %lu and %lu keypoints for the source and target datasets.\n", keypoints_src->points.size (), keypoints_tgt->points.size ()); // Compute normals for all points keypoint PointCloud::Ptr normals_src (new PointCloud), normals_tgt (new PointCloud); estimateNormals (src, tgt, *normals_src, *normals_tgt); - print_info ("Estimated %zu and %zu normals for the source and target datasets.\n", normals_src->points.size (), normals_tgt->points.size ()); + print_info ("Estimated %lu and %lu normals for the source and target datasets.\n", normals_src->points.size (), normals_tgt->points.size ()); // Compute FPFH features at each keypoint PointCloud::Ptr fpfhs_src (new PointCloud), diff --git a/doc/tutorials/content/sources/rops_feature/CMakeLists.txt b/doc/tutorials/content/sources/rops_feature/CMakeLists.txt new file mode 100644 index 00000000..47895569 --- /dev/null +++ b/doc/tutorials/content/sources/rops_feature/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(rops_feature) + +find_package(PCL 1.8 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (rops_feature rops_feature.cpp) +target_link_libraries (rops_feature ${PCL_LIBRARIES}) + + diff --git a/doc/tutorials/content/sources/rops_feature/rops_feature.cpp b/doc/tutorials/content/sources/rops_feature/rops_feature.cpp new file mode 100644 index 00000000..e68fcb1b --- /dev/null +++ b/doc/tutorials/content/sources/rops_feature/rops_feature.cpp @@ -0,0 +1,64 @@ +#include +#include + +int main (int argc, char** argv) +{ + if (argc != 4) + return (-1); + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + if (pcl::io::loadPCDFile (argv[1], *cloud) == -1) + return (-1); + + pcl::PointIndicesPtr indices = boost::shared_ptr (new pcl::PointIndices ()); + std::ifstream indices_file; + indices_file.open (argv[2], std::ifstream::in); + for (std::string line; std::getline (indices_file, line);) + { + std::istringstream in (line); + unsigned int index = 0; + in >> index; + indices->indices.push_back (index - 1); + } + indices_file.close (); + + std::vector triangles; + std::ifstream triangles_file; + triangles_file.open (argv[3], std::ifstream::in); + for (std::string line; std::getline (triangles_file, line);) + { + pcl::Vertices triangle; + std::istringstream in (line); + unsigned int vertex = 0; + in >> vertex; + triangle.vertices.push_back (vertex - 1); + in >> vertex; + triangle.vertices.push_back (vertex - 1); + in >> vertex; + triangle.vertices.push_back (vertex - 1); + triangles.push_back (triangle); + } + + float support_radius = 0.0285f; + unsigned int number_of_partition_bins = 5; + unsigned int number_of_rotations = 3; + + pcl::search::KdTree::Ptr search_method (new pcl::search::KdTree); + search_method->setInputCloud (cloud); + + pcl::ROPSEstimation > feature_estimator; + feature_estimator.setSearchMethod (search_method); + feature_estimator.setSearchSurface (cloud); + feature_estimator.setInputCloud (cloud); + feature_estimator.setIndices (indices); + feature_estimator.setTriangles (triangles); + feature_estimator.setRadiusSearch (support_radius); + feature_estimator.setNumberOfPartitionBins (number_of_partition_bins); + feature_estimator.setNumberOfRotations (number_of_rotations); + feature_estimator.setSupportRadius (support_radius); + + pcl::PointCloud >::Ptr histograms (new pcl::PointCloud > ()); + feature_estimator.compute (*histograms); + + return (0); +} diff --git a/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp b/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp index 243539f6..8fc445bd 100644 --- a/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp +++ b/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp @@ -209,7 +209,7 @@ main (int argc, char** argv) } // Display - PCL_INFO ("Found %zu inliers.\n", inliers.indices.size ()); + PCL_INFO ("Found %lu inliers.\n", inliers.indices.size ()); pcl::PointCloud::Ptr line (new pcl::PointCloud); pcl::copyPointCloud (*cloud_f, inliers, *line); diff --git a/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp b/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp index a249d0ff..1d6e961b 100644 --- a/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp +++ b/doc/tutorials/content/sources/vfh_recognition/nearest_neighbors.cpp @@ -267,7 +267,7 @@ main (int argc, char** argv) p.addText (cloud_name, 20, 10, cloud_name, viewport); } // Add coordianate systems to all viewports - p.addCoordinateSystem (0.1, 0); + p.addCoordinateSystem (0.1, "global", 0); p.spin (); return (0); diff --git a/doc/tutorials/content/supervoxel_clustering.rst b/doc/tutorials/content/supervoxel_clustering.rst index 81a71259..61887cc8 100644 --- a/doc/tutorials/content/supervoxel_clustering.rst +++ b/doc/tutorials/content/supervoxel_clustering.rst @@ -63,8 +63,8 @@ Oh, and for a more complicated example which uses Supervoxels, see ``pcl/example The code -------- -First, grab a pcd file made from a kinect or similar device - here we shall use ``milk_cartoon_all_small_clorox.pcd`` which is available in the pcl trunk :download:`here `). - +First, grab a pcd file made from a kinect or similar device - here we shall use ``milk_cartoon_all_small_clorox.pcd`` which is available in the pcl git +`here `_). Next, copy and paste the following code into your editor and save it as ``supervoxel_clustering.cpp`` (or download the source file :download:`here <./sources/supervoxel_clustering/supervoxel_clustering.cpp>`). .. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp @@ -101,6 +101,10 @@ Next we check the input arguments and set default values. You can play with the We are now ready to setup the supervoxel clustering. We use the class :pcl:`SupervoxelClustering `, which implements the clustering process and give it the parameters. +.. important:: + + You MUST set use_transform to false if you are using a cloud which doesn't have the camera at (0,0,0). The transform is specifically designed to help improve Kinect data by increasing voxel bin size as distance from the camera increases. If your data is artificial, made from combining multiple clouds from cameras at different viewpoints, or doesn't have the camera at (0,0,0), the transform MUST be set to false. + .. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp :language: cpp :lines: 73-77 diff --git a/doc/tutorials/content/template_alignment.rst b/doc/tutorials/content/template_alignment.rst index 6d49cc1c..2f445eec 100644 --- a/doc/tutorials/content/template_alignment.rst +++ b/doc/tutorials/content/template_alignment.rst @@ -15,7 +15,7 @@ We can use the code below to fit a template of a person's face (the blue points) The code -------- -First, download the datasets from `github.com/PointCloudLibrary/data/tree/master/tutorials/template_alignment/ `_ +First, download the datasets from `github.com/PointCloudLibrary/data/tree/master/tutorials/template_alignment/ `_ and extract the files. Next, copy and paste the following code into your editor and save it as ``template_alignment.cpp`` (or download the source file :download:`here <./sources/template_alignment/template_alignment.cpp>`). @@ -75,7 +75,7 @@ We start by defining a structure to store the alignment results. It contains a .. note:: - Because we are including an Eigen::Matrix4f in this struct, we need to include the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro, which will overload the struct's "operator new" so that it will generate 16-bytes-aligned pointers. If you're curious, you can find more information about this issue `here `_. + Because we are including an Eigen::Matrix4f in this struct, we need to include the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro, which will overload the struct's "operator new" so that it will generate 16-bytes-aligned pointers. If you're curious, you can find more information about this issue `here `_. .. literalinclude:: sources/template_alignment/template_alignment.cpp :language: cpp diff --git a/doc/tutorials/content/using_kinfu_large_scale.rst b/doc/tutorials/content/using_kinfu_large_scale.rst index 3d36fc55..0ff75945 100644 --- a/doc/tutorials/content/using_kinfu_large_scale.rst +++ b/doc/tutorials/content/using_kinfu_large_scale.rst @@ -42,7 +42,7 @@ As mentioned above, the TSDF cloud is a section of the TSDF volume grid; which i *Running pcl_kinfu_largeScale* -Finally, we are ready to start KinFu Large Scale. After building the trunk, we will call the application:: +Finally, we are ready to start KinFu Large Scale. After building the git master, we will call the application:: $ ./bin/pcl_kinfu_largeScale -r -et @@ -148,4 +148,4 @@ There are three executables related to this tutorial: Conclusion ---------- -In this tutorial we have shown the pipeline from scanning to final texturing using KinFu Large Scale. The - *experimental* - code is available in PCL trunk. +In this tutorial we have shown the pipeline from scanning to final texturing using KinFu Large Scale. The - *experimental* - code is available in the master branch of PCL. diff --git a/doc/tutorials/content/using_pcl_with_eclipse.rst b/doc/tutorials/content/using_pcl_with_eclipse.rst index 923b27c6..51efdbac 100644 --- a/doc/tutorials/content/using_pcl_with_eclipse.rst +++ b/doc/tutorials/content/using_pcl_with_eclipse.rst @@ -1,108 +1,221 @@ .. _using_pcl_with_eclipse: -Using PCL from trunk with Eclipse ---------------------------------- +====================== +Using PCL with Eclipse +====================== -This tutorial explains how to use Eclipse as a PCL editor +This tutorial explains how to use Eclipse as an IDE to manage your PCL projects. It was tested under Ubuntu 14.04 with Eclipse Luna; +do not hesitate to modify this tutorial by submitting a pull request on GitHub to add other configurations etc. + +.. contents:: Prerequisites -------------- +============= -We assume you have downloaded, compiled and installed PCL from trunk (see Downloads, experimental) on your machine. +We assume you have downloaded and extracted a PCL version (either PCL trunk or a stable version) on your machine. +For the example, we will use the `pcl visualizer `_ code. Creating the eclipse project files ----------------------------------- - -Open a terminal window and do:: - - $ cd /PATH/TO/MY/TRUNK/ROOT - $ cmake -G"Eclipse CDT4 - Unix Makefiles" . - -You will see something similar to:: - --- The C compiler identification is GNU --- The CXX compiler identification is GNU --- Could not determine Eclipse version, assuming at least 3.6 (Helios). Adjust CMAKE_ECLIPSE_VERSION if this is wrong. --- Check for working C compiler: /home/u0062536/bin/gcc --- Check for working C compiler: /home/u0062536/bin/gcc -- works --- Detecting C compiler ABI info --- Detecting C compiler ABI info - done --- Check for working CXX compiler: /home/u0062536/bin/c++ --- Check for working CXX compiler: /home/u0062536/bin/c++ -- works --- Detecting CXX compiler ABI info --- Detecting CXX compiler ABI info - done --- -- GCC > 4.3 found, enabling -Wabi --- Using CPU native flags for SSE optimization: -march=native --- Performing Test HAVE_MM_MALLOC --- Performing Test HAVE_MM_MALLOC - Success --- Performing Test HAVE_POSIX_MEMALIGN --- Performing Test HAVE_POSIX_MEMALIGN - Success --- Performing Test HAVE_SSE4_2_EXTENSIONS --- Performing Test HAVE_SSE4_2_EXTENSIONS - Success --- Performing Test HAVE_SSE4_1_EXTENSIONS --- Performing Test HAVE_SSE4_1_EXTENSIONS - Success --- Performing Test HAVE_SSE3_EXTENSIONS --- Performing Test HAVE_SSE3_EXTENSIONS - Success --- Performing Test HAVE_SSE2_EXTENSIONS --- Performing Test HAVE_SSE2_EXTENSIONS - Success --- Performing Test HAVE_SSE_EXTENSIONS --- Performing Test HAVE_SSE_EXTENSIONS - Success --- Found SSE4.2 extensions, using flags: -march=native -msse4.2 -mfpmath=sse --- Try OpenMP C flag = [-fopenmp] --- Performing Test OpenMP_FLAG_DETECTED --- Performing Test OpenMP_FLAG_DETECTED - Success --- Try OpenMP CXX flag = [-fopenmp] --- Performing Test OpenMP_FLAG_DETECTED --- Performing Test OpenMP_FLAG_DETECTED - Success --- Found OpenMP: -fopenmp --- Found OpenMP --- Boost version: 1.46.1 --- The following subsystems will be built: --- common --- kdtree --- octree --- io --- search --- sample_consensus --- filters --- 2d --- features --- keypoints --- geometry --- ml --- segmentation --- visualization --- outofcore --- stereo --- surface --- tracking --- registration --- people --- recognition --- global_tests --- tools --- The following subsystems will not be built: --- examples: Code examples are disabled by default. --- simulation: Disabled by default. --- apps: Disabled by default. --- Configuring done --- Generating done --- Build files have been written to: /data/git/pcl +================================== + +The files are organized like the following tree:: + + . + ├── build + └── src + ├── CMakeLists.txt + └── pcl_visualizer_demo.cpp + +Open a terminal, navigate to your project root folder and configure the project:: + + $ cd /path_to_my_project/build + $ cmake -G "Eclipse CDT4 - Unix Makefiles" ../src + +You will see something that should look like:: + + -- The C compiler identification is GNU 4.8.2 + -- The CXX compiler identification is GNU 4.8.2 + -- Could not determine Eclipse version, assuming at least 3.6 (Helios). Adjust CMAKE_ECLIPSE_VERSION if this is wrong. + -- Check for working C compiler: /usr/lib/ccache/cc + -- Check for working C compiler: /usr/lib/ccache/cc -- works + -- Detecting C compiler ABI info + -- Detecting C compiler ABI info - done + -- Check for working CXX compiler: /usr/lib/ccache/c++ + -- Check for working CXX compiler: /usr/lib/ccache/c++ -- works + -- Detecting CXX compiler ABI info + -- Detecting CXX compiler ABI info - done + -- checking for module 'eigen3' + -- found eigen3, version 3.2.0 + -- Found eigen: /usr/include/eigen3 + -- Boost version: 1.54.0 + -- Found the following Boost libraries: + -- system + -- filesystem + -- thread + -- date_time + -- iostreams + -- mpi + -- serialization + -- chrono + -- checking for module 'openni-dev' + -- package 'openni-dev' not found + -- Found openni: /usr/lib/libOpenNI.so + -- checking for module 'openni2-dev' + -- package 'openni2-dev' not found + -- Found OpenNI2: /usr/lib/libOpenNI2.so + ** WARNING ** io features related to pcap will be disabled + ** WARNING ** io features related to png will be disabled + -- Found libusb-1.0: /usr/include + -- checking for module 'flann' + -- found flann, version 1.8.4 + -- Found Flann: /usr/lib/x86_64-linux-gnu/libflann_cpp_s.a + -- Found qhull: /usr/lib/x86_64-linux-gnu/libqhull.so + -- checking for module 'openni-dev' + -- package 'openni-dev' not found + -- checking for module 'openni2-dev' + -- package 'openni2-dev' not found + -- looking for PCL_COMMON + -- Found PCL_COMMON: /usr/local/lib/libpcl_common.so + -- looking for PCL_OCTREE + -- Found PCL_OCTREE: /usr/local/lib/libpcl_octree.so + -- looking for PCL_IO + -- Found PCL_IO: /usr/local/lib/libpcl_io.so + -- looking for PCL_KDTREE + -- Found PCL_KDTREE: /usr/local/lib/libpcl_kdtree.so + -- looking for PCL_SEARCH + -- Found PCL_SEARCH: /usr/local/lib/libpcl_search.so + -- looking for PCL_SAMPLE_CONSENSUS + -- Found PCL_SAMPLE_CONSENSUS: /usr/local/lib/libpcl_sample_consensus.so + -- looking for PCL_FILTERS + -- Found PCL_FILTERS: /usr/local/lib/libpcl_filters.so + -- looking for PCL_2D + -- Found PCL_2D: /usr/local/include/pcl-1.7 + -- looking for PCL_FEATURES + -- Found PCL_FEATURES: /usr/local/lib/libpcl_features.so + -- looking for PCL_GEOMETRY + -- Found PCL_GEOMETRY: /usr/local/include/pcl-1.7 + -- looking for PCL_KEYPOINTS + -- Found PCL_KEYPOINTS: /usr/local/lib/libpcl_keypoints.so + -- looking for PCL_SURFACE + -- Found PCL_SURFACE: /usr/local/lib/libpcl_surface.so + -- looking for PCL_REGISTRATION + -- Found PCL_REGISTRATION: /usr/local/lib/libpcl_registration.so + -- looking for PCL_ML + -- Found PCL_ML: /usr/local/lib/libpcl_ml.so + -- looking for PCL_SEGMENTATION + -- Found PCL_SEGMENTATION: /usr/local/lib/libpcl_segmentation.so + -- looking for PCL_RECOGNITION + -- Found PCL_RECOGNITION: /usr/local/lib/libpcl_recognition.so + -- looking for PCL_VISUALIZATION + -- Found PCL_VISUALIZATION: /usr/local/lib/libpcl_visualization.so + -- looking for PCL_PEOPLE + -- Found PCL_PEOPLE: /usr/local/lib/libpcl_people.so + -- looking for PCL_OUTOFCORE + -- Found PCL_OUTOFCORE: /usr/local/lib/libpcl_outofcore.so + -- looking for PCL_TRACKING + -- Found PCL_TRACKING: /usr/local/lib/libpcl_tracking.so + -- looking for PCL_STEREO + -- Found PCL_STEREO: /usr/local/lib/libpcl_stereo.so + -- looking for PCL_GPU_CONTAINERS + -- Found PCL_GPU_CONTAINERS: /usr/local/lib/libpcl_gpu_containers.so + -- looking for PCL_GPU_UTILS + -- Found PCL_GPU_UTILS: /usr/local/lib/libpcl_gpu_utils.so + -- looking for PCL_GPU_OCTREE + -- Found PCL_GPU_OCTREE: /usr/local/lib/libpcl_gpu_octree.so + -- looking for PCL_GPU_FEATURES + -- Found PCL_GPU_FEATURES: /usr/local/lib/libpcl_gpu_features.so + -- looking for PCL_GPU_KINFU + -- Found PCL_GPU_KINFU: /usr/local/lib/libpcl_gpu_kinfu.so + -- looking for PCL_GPU_KINFU_LARGE_SCALE + -- Found PCL_GPU_KINFU_LARGE_SCALE: /usr/local/lib/libpcl_gpu_kinfu_large_scale.so + -- looking for PCL_GPU_SEGMENTATION + -- Found PCL_GPU_SEGMENTATION: /usr/local/lib/libpcl_gpu_segmentation.so + -- looking for PCL_CUDA_COMMON + -- Found PCL_CUDA_COMMON: /usr/local/include/pcl-1.7 + -- looking for PCL_CUDA_FEATURES + -- Found PCL_CUDA_FEATURES: /usr/local/lib/libpcl_cuda_features.so + -- looking for PCL_CUDA_SEGMENTATION + -- Found PCL_CUDA_SEGMENTATION: /usr/local/lib/libpcl_cuda_segmentation.so + -- looking for PCL_CUDA_SAMPLE_CONSENSUS + -- Found PCL_CUDA_SAMPLE_CONSENSUS: /usr/local/lib/libpcl_cuda_sample_consensus.so + -- Found PCL: /usr/lib/x86_64-linux-gnu/libboost_system.so;/usr/lib/x86_64-linux-gnu/libboost_filesystem.so;/usr/lib/x86_64-linux-gnu/libboost_thread.so;/usr/lib/x86_64-linux-gnu/libboost_date_time.so;/usr/lib/x86_64-linux-gnu/libboost_iostreams.so;/usr/lib/x86_64-linux-gnu/libboost_mpi.so;/usr/lib/x86_64-linux-gnu/libboost_serialization.so;/usr/lib/x86_64-linux-gnu/libboost_chrono.so;/usr/lib/x86_64-linux-gnu/libpthread.so;optimized;/usr/local/lib/libpcl_common.so;debug;/usr/local/lib/libpcl_common.so;optimized;/usr/local/lib/libpcl_octree.so;debug;/usr/local/lib/libpcl_octree.so;/usr/lib/libOpenNI.so;/usr/lib/libOpenNI2.so;vtkCommon;vtkFiltering;vtkImaging;vtkGraphics;vtkGenericFiltering;vtkIO;vtkRendering;vtkVolumeRendering;vtkHybrid;vtkWidgets;vtkParallel;vtkInfovis;vtkGeovis;vtkViews;vtkCharts;optimized;/usr/local/lib/libpcl_io.so;debug;/usr/local/lib/libpcl_io.so;optimized;/usr/lib/x86_64-linux-gnu/libflann_cpp_s.a;debug;/usr/lib/x86_64-linux-gnu/libflann_cpp_s.a;optimized;/usr/local/lib/libpcl_kdtree.so;debug;/usr/local/lib/libpcl_kdtree.so;optimized;/usr/local/lib/libpcl_search.so;debug;/usr/local/lib/libpcl_search.so;optimized;/usr/local/lib/libpcl_sample_consensus.so;debug;/usr/local/lib/libpcl_sample_consensus.so;optimized;/usr/local/lib/libpcl_filters.so;debug;/usr/local/lib/libpcl_filters.so;optimized;/usr/local/lib/libpcl_features.so;debug;/usr/local/lib/libpcl_features.so;optimized;/usr/local/lib/libpcl_keypoints.so;debug;/usr/local/lib/libpcl_keypoints.so;optimized;/usr/lib/x86_64-linux-gnu/libqhull.so;debug;/usr/lib/x86_64-linux-gnu/libqhull.so;optimized;/usr/local/lib/libpcl_surface.so;debug;/usr/local/lib/libpcl_surface.so;optimized;/usr/local/lib/libpcl_registration.so;debug;/usr/local/lib/libpcl_registration.so;optimized;/usr/local/lib/libpcl_ml.so;debug;/usr/local/lib/libpcl_ml.so;optimized;/usr/local/lib/libpcl_segmentation.so;debug;/usr/local/lib/libpcl_segmentation.so;optimized;/usr/local/lib/libpcl_recognition.so;debug;/usr/local/lib/libpcl_recognition.so;optimized;/usr/local/lib/libpcl_visualization.so;debug;/usr/local/lib/libpcl_visualization.so;optimized;/usr/local/lib/libpcl_people.so;debug;/usr/local/lib/libpcl_people.so;optimized;/usr/local/lib/libpcl_outofcore.so;debug;/usr/local/lib/libpcl_outofcore.so;optimized;/usr/local/lib/libpcl_tracking.so;debug;/usr/local/lib/libpcl_tracking.so;optimized;/usr/local/lib/libpcl_stereo.so;debug;/usr/local/lib/libpcl_stereo.so;optimized;/usr/local/lib/libpcl_gpu_containers.so;debug;/usr/local/lib/libpcl_gpu_containers.so;optimized;/usr/local/lib/libpcl_gpu_utils.so;debug;/usr/local/lib/libpcl_gpu_utils.so;optimized;/usr/local/lib/libpcl_gpu_octree.so;debug;/usr/local/lib/libpcl_gpu_octree.so;optimized;/usr/local/lib/libpcl_gpu_features.so;debug;/usr/local/lib/libpcl_gpu_features.so;optimized;/usr/local/lib/libpcl_gpu_kinfu.so;debug;/usr/local/lib/libpcl_gpu_kinfu.so;optimized;/usr/local/lib/libpcl_gpu_kinfu_large_scale.so;debug;/usr/local/lib/libpcl_gpu_kinfu_large_scale.so;optimized;/usr/local/lib/libpcl_gpu_segmentation.so;debug;/usr/local/lib/libpcl_gpu_segmentation.so;optimized;/usr/local/lib/libpcl_cuda_features.so;debug;/usr/local/lib/libpcl_cuda_features.so;optimized;/usr/local/lib/libpcl_cuda_segmentation.so;debug;/usr/local/lib/libpcl_cuda_segmentation.so;optimized;/usr/local/lib/libpcl_cuda_sample_consensus.so;debug;/usr/local/lib/libpcl_cuda_sample_consensus.so;/usr/lib/x86_64-linux-gnu/libboost_system.so;/usr/lib/x86_64-linux-gnu/libboost_filesystem.so;/usr/lib/x86_64-linux-gnu/libboost_thread.so;/usr/lib/x86_64-linux-gnu/libboost_date_time.so;/usr/lib/x86_64-linux-gnu/libboost_iostreams.so;/usr/lib/x86_64-linux-gnu/libboost_mpi.so;/usr/lib/x86_64-linux-gnu/libboost_serialization.so;/usr/lib/x86_64-linux-gnu/libboost_chrono.so;/usr/lib/x86_64-linux-gnu/libpthread.so;optimized;/usr/lib/x86_64-linux-gnu/libqhull.so;debug;/usr/lib/x86_64-linux-gnu/libqhull.so;/usr/lib/libOpenNI.so;/usr/lib/libOpenNI2.so;optimized;/usr/lib/x86_64-linux-gnu/libflann_cpp_s.a;debug;/usr/lib/x86_64-linux-gnu/libflann_cpp_s.a;vtkCommon;vtkFiltering;vtkImaging;vtkGraphics;vtkGenericFiltering;vtkIO;vtkRendering;vtkVolumeRendering;vtkHybrid;vtkWidgets;vtkParallel;vtkInfovis;vtkGeovis;vtkViews;vtkCharts (Required is at least version "1.7") + -- Configuring done + -- Generating done + -- Build files have been written to: /home/dell/visualizer/build Importing into Eclipse ----------------------- +====================== + +- Launch `Eclipse CDT `_ and select ``File > Import``. +- In the list select ``General > Existing Projects into Workspace`` and then next. +- Browse (``Select root directory``) to the root folder of the project and select the ``build`` folder (in the example case, ``/home/dell/visualizer/build``). +- Click ``Finish``. + +.. WARNING:: + The Eclipse indexer is going to parse the files in the project (and all the includes), this can take a lot of time and might crash Eclipse if it's not configured for big projects. + Take a look at the bottom right of Eclipse's window to see the indexer status; it is advised not to do anything until the indexer has finished it's job. + +Configuring Eclipse +------------------- + +If Eclipse fails to open your PCL project you might need to change Eclipse configuration; here are some values that should solve all problems +(but might not work on light hardware configurations):: + + $ sudo gedit /usr/lib/eclipse/eclipse.ini -Now you launch your Eclipse editor and you select File->Import... -Out of the list you select General->Existing Projects into Workspace and then next. -At the top you select Select root directory to be the root of your pcl trunk installation and press Finish. +Change the values in the last lines:: + + org.eclipse.platform + --launcher.XXMaxPermSize + 1024m + --launcher.defaultAction + openFile + --launcher.appendVmargs + -vmargs + -Dosgi.requiredJavaVersion=1.7 + -XX:MaxPermSize=512m + -Xms1024m + -Xmx1024m + +Restart Eclipse and go to ``Windows > Preferences``, then ``C/C++ > Indexer > Cache Limits``. Set the limits to [50% | 512 | 512]. Setting the PCL code style in Eclipse -------------------------------------- +===================================== + +You can find a PCL code style file for Eclipse in `PCL GitHub trunk `_ + +Global +------ +If you want to apply the PCL style guide to all projects: +``Windows > Preferences > C/C++ > Code Style > Formatter`` + +Project specific +---------------- +If you want to apply the style guide only to one project: +Go to ``Project > Properties``, then select ``Code Style`` in the left field and Enable ``project specific settings``, then ``Import`` and select where you profile file (.xml) is. + +How to format the code +---------------------- +If you want to format the whole project use ``Source > Format``. If you want to format only your selection use the shortcut ``Ctrl + Shift + F`` + +Launching the program +===================== + +To build the project, click on the build icon + +.. image:: images/pcl_with_eclipse/build_tab.gif + :height: 16 + +- Create a launch configuration, select the project on the left panel (left click on the project name); ``Run > Run Configurations..``. +- Create a new ``C/C++ Application`` click on ``Search Project`` and choose the executable to be launched. +- Go the second tab (``Arguments``) and enter your arguments; remember this is not a terminal and ``~`` won't work to get to your home folder for example ! + +Run the program by clicking on the run icon -You can find a PCL code style file for Eclipse in trunk/doc/advanced/content/files/. -In Eclipse go to Project->Properties, then select Code Style in the left field and Enable project specific settings, then Import and select where your trunk/doc/advanced/content/files/PCL_eclipse_profile.xml file is. +.. image:: images/pcl_with_eclipse/lrun_obj.gif + :height: 16 Where to get more information ------------------------------ +============================= -You can get more information here: http://www.vtk.org/Wiki/Eclipse_CDT4_Generator +You can get more information about the Eclipse CDT4 Generator `here `_. diff --git a/doc/tutorials/content/vfh_estimation.rst b/doc/tutorials/content/vfh_estimation.rst index 35ef690b..5a079c65 100644 --- a/doc/tutorials/content/vfh_estimation.rst +++ b/doc/tutorials/content/vfh_estimation.rst @@ -59,7 +59,7 @@ Estimating VFH features ----------------------- The Viewpoint Feature Histogram is implemented in PCL as part of the -`pcl_features `_ +`pcl_features `_ library. The default VFH implementation uses 45 binning subdivisions for each of the diff --git a/doc/tutorials/content/vfh_recognition.rst b/doc/tutorials/content/vfh_recognition.rst index 2a8b8d95..a248a9ea 100644 --- a/doc/tutorials/content/vfh_recognition.rst +++ b/doc/tutorials/content/vfh_recognition.rst @@ -60,7 +60,7 @@ manually. Our Kd-Tree implementation of choice for the purpose of this tutorial is of -course, `FLANN `_. +course, `FLANN `_. Training @@ -254,7 +254,7 @@ Create a new ``CMakeLists.txt`` file, and put the following content into it .. note:: - If you are running this tutorial on Windows, you have to install (`HDF5 1.8.7 Shared Library `_). If CMake is not able to find HDF5, + If you are running this tutorial on Windows, you have to install (`HDF5 1.8.7 Shared Library `_). If CMake is not able to find HDF5, you can manually supply the include directory in HDF5_INCLUDE_DIR variable and the full path of **hdf5dll.lib** in HDF5_hdf5_LIBRARY variable. Make sure that the needed dlls are in the same folder as the executables. diff --git a/doc/tutorials/content/visualization.rst b/doc/tutorials/content/visualization.rst new file mode 100644 index 00000000..19624090 --- /dev/null +++ b/doc/tutorials/content/visualization.rst @@ -0,0 +1,192 @@ +.. _visualization: + +PCL Visualization overview +-------------------------- + +The **pcl_visualization** library was built for the purpose of being able to +quickly prototype and visualize the results of algorithms operating on 3D point +cloud data. Similar to OpenCV's **highgui** routines for displaying 2D images +and for drawing basic 2D shapes on screen, the library offers: + + * methods for rendering and setting visual properties (colors, point sizes, + opacity, etc) for any n-D point cloud datasets in pcl::PointCloud format; + + .. image:: images/visualization/bunny.jpg + + * methods for drawing basic 3D shapes on screen (e.g., cylinders, spheres, + lines, polygons, etc) either from sets of points or from parametric + equations; + + .. image:: images/visualization/shapes.jpg + + * a histogram visualization module (PCLHistogramVisualizer) for 2D plots; + + .. image:: images/visualization/histogram.jpg + + * a multitude of Geometry and Color handler for pcl::PointCloud datasets; + + .. image:: images/visualization/normals.jpg + .. image:: images/visualization/pcs.jpg + + * a pcl::RangeImage visualization module. + + .. image:: images/visualization/range_image.jpg + +The package makes use of the VTK library for 3D rendering for +range image and 2D operations. + +For implementing your own visualizers, take a look at the tests and examples +accompanying the library. + +.. note:: + + Due to historical reasons, PCL 1.x stores RGB data as a packed float (to + preserve backward compatibility). To learn more about this, please see the + `PointXYZRGB + `_. + +Simple Cloud Visualization +-------------------------- + +If you just want to visualize something in your app with a few lines of code, +use a snippet like the following one: + +.. code-block:: cpp + :linenos: + + #include + //... + void + foo () + { + pcl::PointCloud cloud; + //... populate cloud + pcl_visualization::CloudViewer viewer("Simple Cloud Viewer"); + viewer.showCloud(cloud); + while (!viewer.wasStopped()) + { + } + } + +PCD Viewer +---------- + +A quick way for visualizing PCD (Point Cloud Data) files is by using +**pcl_viewer**. As of 0.2.7, pcl_viewer's help screen looks like:: + + Syntax is: pcl_viewer .pcd + where options are: + -bc r,g,b = background color + -fc r,g,b = foreground color + -ps X = point size (1..64) + -opaque X = rendered point cloud opacity (0..1) + -ax n = enable on-screen display of XYZ axes and scale them to n + -ax_pos X,Y,Z = if axes are enabled, set their X,Y,Z position in space (default 0,0,0) + + -cam (*) = use given camera settings as initial view + (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / Window Size / Window Pos] or use a that contains the same information. + + -multiview 0/1 = enable/disable auto-multi viewport rendering (default disabled) + + + -normals 0/X = disable/enable the display of every Xth point's surface normal as lines (default disabled) + -normals_scale X = resize the normal unit vector size to X (default 0.02) + + -pc 0/X = disable/enable the display of every Xth point's principal curvatures as lines (default disabled) + -pc_scale X = resize the principal curvatures vectors size to X (default 0.02) + + + (Note: for multiple .pcd files, provide multiple -{fc,ps} parameters; they will be automatically assigned to the right file) + +Usage examples +-------------- + +.. code-block:: bash + + $ pcl_viewer -multiview 1 data/partial_cup_model.pcd data/partial_cup_model.pcd data/partial_cup_model.pcd + + +The above will load the ``partial_cup_model.pcd`` file 3 times, and will create a +multi-viewport rendering (``-multiview 1``). + +.. image:: images/visualization/ex1.jpg + +Pressing ``h`` while the point clouds are being rendered will output the +following information on the console:: + + | Help: + ------- + p, P : switch to a point-based representation + w, W : switch to a wireframe-based representation (where available) + s, S : switch to a surface-based representation (where available) + + j, J : take a .PNG snapshot of the current window view + c, C : display current camera/window parameters + + + / - : increment/decrement overall point size + + g, G : display scale grid (on/off) + u, U : display lookup table (on/off) + + r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}] + + ALT + s, S : turn stereo mode on/off + ALT + f, F : switch between maximized window mode and original size + + l, L : list all available geometric and color handlers for the current actor map + ALT + 0..9 [+ CTRL] : switch between different geometric handlers (where available) + 0..9 [+ CTRL] : switch between different color handlers (where available) + + +Pressing ``l`` will show the current list of available geometry/color handlers +for the datasets that we loaded. In this example:: + + List of available geometry handlers for actor partial_cup_model.pcd-0: xyz(1) normal_xyz(2) + List of available color handlers for actor partial_cup_model.pcd-0: [random](1) x(2) y(3) z(4) normal_x(5) normal_y(6) normal_z(7) curvature(8) boundary(9) k(10) principal_curvature_x(11) principal_curvature_y(12) principal_curvature_z(13) pc1(14) pc2(15) + +Switching to a ``normal_xyz`` geometric handler using ``ALT+1`` and then +pressing ``8`` to switch to a curvature color handler, should result in the +following:: + + $ pcl_viewer -normals 100 data/partial_cup_model.pcd + +.. image:: images/visualization/ex2.jpg + + +The above will load the ``partial_cup_model.pcd`` file and render its every +``100`` th surface normal on screen. + +.. code-block:: bash + + $ pcl_viewer -pc 100 data/partial_cup_model.pcd + +.. image:: images/visualization/ex3.jpg + +The above will load the ``partial_cup_model.pcd`` file and render its every +``100`` th principal curvature (+surface normal) on screen. + +.. image:: images/visualization/ex4.jpg + +.. code-block:: bash + + $ pcl_viewer data/bun000.pcd data/bun045.pcd -ax 0.5 -ps 3 -ps 1 + +The above assumes that the ``bun000.pcd`` and ``bun045.pcd`` datasets have been +downloaded and are available. The results shown in the following picture were +obtained after pressing ``u`` and ``g`` to enable the lookup table and on-grid +display. + +.. image:: images/visualization/ex5.jpg + + +Range Image Visualizer +---------------------- + +A quick way for visualizing range images is by using the binary of the tutorial +for range_image_visualization:: + + $ tutorial_range_image_visualization data/office_scene.pcd + +The above will load the ``office_scene.pcd`` point cloud file, create a range +image from it and visualize both, the point cloud and the range image. + diff --git a/doc/tutorials/content/walkthrough.rst b/doc/tutorials/content/walkthrough.rst index e4e06c40..073c7994 100644 --- a/doc/tutorials/content/walkthrough.rst +++ b/doc/tutorials/content/walkthrough.rst @@ -69,7 +69,7 @@ Filters .. image:: images/statistical_removal_2.jpg -**Documentation:** http://docs.pointclouds.org/trunk/group__filters.html +**Documentation:** http://docs.pointclouds.org/trunk/a02945.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#filtering-tutorial @@ -104,7 +104,7 @@ Features **Background** A theoretical primer explaining how features work in PCL can be found in the `3D Features tutorial - `_. + `_. The *features* library contains data structures and mechanisms for 3D feature estimation from point cloud data. 3D features are representations at certain 3D points, or positions, in space, which describe geometrical patterns based on the information available around the point. The data space selected around the query point is usually referred to as the *k-neighborhood*. @@ -124,7 +124,7 @@ Features | -**Documentation:** http://docs.pointclouds.org/trunk/group__features.html +**Documentation:** http://docs.pointclouds.org/trunk/a02944.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#features-tutorial @@ -168,7 +168,7 @@ Keypoints | -**Documentation:** http://docs.pointclouds.org/trunk/group__keypoints.html +**Documentation:** http://docs.pointclouds.org/trunk/a02949.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#keypoints-tutorial @@ -218,7 +218,7 @@ Registration | -**Documentation:** http://docs.pointclouds.org/trunk/group__registration.html +**Documentation:** http://docs.pointclouds.org/trunk/a02953.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#registration-tutorial @@ -265,7 +265,7 @@ Kd-tree | -**Documentation:** http://docs.pointclouds.org/trunk/group__kdtree.html +**Documentation:** http://docs.pointclouds.org/trunk/a02948.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#kdtree-tutorial @@ -305,7 +305,7 @@ Octree | -**Documentation:** http://docs.pointclouds.org/trunk/group__octree.html +**Documentation:** http://docs.pointclouds.org/trunk/a02950.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#octree-tutorial @@ -346,7 +346,7 @@ Segmentation | -**Documentation:** http://docs.pointclouds.org/trunk/group__segmentation.html +**Documentation:** http://docs.pointclouds.org/trunk/a02956.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#segmentation-tutorial @@ -392,7 +392,7 @@ Sample Consensus | -**Documentation:** http://docs.pointclouds.org/trunk/group__sample__consensus.html +**Documentation:** http://docs.pointclouds.org/trunk/a02954.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#sample-consensus @@ -438,7 +438,7 @@ Surface | -**Documentation:** http://docs.pointclouds.org/trunk/group__surface.html +**Documentation:** http://docs.pointclouds.org/trunk/a02957.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#surface-tutorial @@ -479,7 +479,7 @@ Range Image | -**Documentation:** http://docs.pointclouds.org/trunk/group__range__image.html +**Documentation:** http://docs.pointclouds.org/trunk/a01344.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#range-images @@ -519,7 +519,7 @@ I/O | -**Documentation:** http://docs.pointclouds.org/trunk/group__io.html +**Documentation:** http://docs.pointclouds.org/trunk/a02947.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#i-o @@ -586,7 +586,7 @@ Visualization | -**Documentation:** http://docs.pointclouds.org/trunk/group__visualization.html +**Documentation:** http://docs.pointclouds.org/trunk/a02958.html **Tutorials:** http://pointclouds.org/documentation/tutorials/#visualization-tutorial diff --git a/examples/keypoints/CMakeLists.txt b/examples/keypoints/CMakeLists.txt index 02add23f..01646540 100644 --- a/examples/keypoints/CMakeLists.txt +++ b/examples/keypoints/CMakeLists.txt @@ -19,4 +19,7 @@ if(BUILD_visualization) PCL_ADD_EXAMPLE(pcl_example_sift_z_keypoint_estimation FILES example_sift_z_keypoint_estimation.cpp LINK_WITH pcl_common pcl_visualization pcl_keypoints pcl_io) + PCL_ADD_EXAMPLE(pcl_example_get_keypoints_indices FILES example_get_keypoints_indices.cpp + LINK_WITH pcl_common pcl_keypoints pcl_io) + endif(BUILD_visualization) diff --git a/examples/keypoints/example_get_keypoints_indices.cpp b/examples/keypoints/example_get_keypoints_indices.cpp new file mode 100644 index 00000000..d7e726c6 --- /dev/null +++ b/examples/keypoints/example_get_keypoints_indices.cpp @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include +#include +#include +#include +#include +#include + +int +main(int argc, char** argv) +{ + if (argc < 2) + { + pcl::console::print_info ("Keypoints indices example application.\n"); + pcl::console::print_info ("Syntax is: %s \n", argv[0]); + return (1); + } + + pcl::console::print_info ("Reading %s\n", argv[1]); + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + if(pcl::io::loadPCDFile (argv[1], *cloud) == -1) // load the file + { + pcl::console::print_error ("Couldn't read file %s!\n", argv[1]); + return (-1); + } + + pcl::HarrisKeypoint3D detector; + pcl::PointCloud::Ptr keypoints (new pcl::PointCloud); + detector.setNonMaxSupression (true); + detector.setInputCloud (cloud); + detector.setThreshold (1e-6); + pcl::StopWatch watch; + detector.compute (*keypoints); + pcl::console::print_highlight ("Detected %zd points in %lfs\n", keypoints->size (), watch.getTimeSeconds ()); + pcl::PointIndicesConstPtr keypoints_indices = detector.getKeypointsIndices (); + if (!keypoints_indices->indices.empty ()) + { + pcl::io::savePCDFile ("keypoints.pcd", *cloud, keypoints_indices->indices, true); + pcl::console::print_info ("Saved keypoints to keypoints.pcd\n"); + } + else + pcl::console::print_warn ("Keypoints indices are empty!\n"); +} diff --git a/examples/segmentation/example_region_growing.cpp b/examples/segmentation/example_region_growing.cpp index 65a40f2d..12e36836 100644 --- a/examples/segmentation/example_region_growing.cpp +++ b/examples/segmentation/example_region_growing.cpp @@ -72,14 +72,14 @@ main (int argc, char** av) return -1; } - pcl::console::print_highlight ("Loaded cloud %s of size %zu\n", av[1], cloud_ptr->points.size ()); + pcl::console::print_highlight ("Loaded cloud %s of size %lu\n", av[1], cloud_ptr->points.size ()); // Remove the nans cloud_ptr->is_dense = false; cloud_no_nans->is_dense = false; std::vector indices; pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices); - pcl::console::print_highlight ("Removed nans from %zu to %zu\n", cloud_ptr->points.size (), cloud_no_nans->points.size ()); + pcl::console::print_highlight ("Removed nans from %lu to %lu\n", cloud_ptr->points.size (), cloud_no_nans->points.size ()); // Estimate the normals pcl::NormalEstimation ne; @@ -88,7 +88,7 @@ main (int argc, char** av) ne.setSearchMethod (tree_n); ne.setRadiusSearch (0.03); ne.compute (*cloud_normals); - pcl::console::print_highlight ("Normals are computed and size is %zu\n", cloud_normals->points.size ()); + pcl::console::print_highlight ("Normals are computed and size is %lu\n", cloud_normals->points.size ()); // Region growing pcl::RegionGrowing rg; @@ -103,7 +103,7 @@ main (int argc, char** av) cloud_segmented = rg.getColoredCloud (); // Writing the resulting cloud into a pcd file - pcl::console::print_highlight ("Number of segments done is %zu\n", clusters.size ()); + pcl::console::print_highlight ("Number of segments done is %lu\n", clusters.size ()); writer.write ("segment_result.pcd", *cloud_segmented, false); if (pcl::console::find_switch (argc, av, "-dump")) diff --git a/examples/segmentation/example_supervoxels.cpp b/examples/segmentation/example_supervoxels.cpp index 1418c130..74d70c08 100644 --- a/examples/segmentation/example_supervoxels.cpp +++ b/examples/segmentation/example_supervoxels.cpp @@ -215,9 +215,9 @@ main (int argc, char ** argv) depth_pixel = static_cast(depth_image->GetScalarPointer (depth_dims[0]-1,depth_dims[1]-1,0)); color_pixel = static_cast (rgb_image->GetScalarPointer (depth_dims[0]-1,depth_dims[1]-1,0)); - for (int y=0; yheight; ++y) + for (size_t y=0; yheight; ++y) { - for (int x=0; xwidth; ++x, --depth_pixel, color_pixel-=3) + for (size_t x=0; xwidth; ++x, --depth_pixel, color_pixel-=3) { PointT new_point; // uint8_t* p_i = &(cloud_blob->data[y * cloud_blob->row_step + x * cloud_blob->point_step]); @@ -228,8 +228,8 @@ main (int argc, char ** argv) } else { - new_point.x = (static_cast(x - centerX)) * depth * fl_const; - new_point.y = (static_cast(centerY - y)) * depth * fl_const; // vtk seems to start at the bottom left image corner + new_point.x = (static_cast (x) - centerX) * depth * fl_const; + new_point.y = (static_cast (centerY) - y) * depth * fl_const; // vtk seems to start at the bottom left image corner new_point.z = depth; } @@ -273,6 +273,10 @@ main (int argc, char ** argv) PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters); PointLCloudT::Ptr full_labeled_cloud = super.getLabeledCloud (); + std::cout << "Getting supervoxel adjacency\n"; + std::multimap label_adjacency; + super.getSupervoxelAdjacency (label_adjacency); + std::map ::Ptr > refined_supervoxel_clusters; std::cout << "Refining supervoxels \n"; super.refineSupervoxels (3, refined_supervoxel_clusters); @@ -282,10 +286,6 @@ main (int argc, char ** argv) PointLCloudT::Ptr refined_full_labeled_cloud = super.getLabeledCloud (); PointCloudT::Ptr refined_full_colored_cloud = super.getColoredCloud (); - std::cout << "Getting supervoxel adjacency\n"; - std::multimap label_adjacency; - super.getSupervoxelAdjacency (label_adjacency); - // THESE ONLY MAKE SENSE FOR ORGANIZED CLOUDS pcl::io::savePNGFile (out_path, *full_colored_cloud, "rgb"); pcl::io::savePNGFile (refined_out_path, *refined_full_colored_cloud, "rgb"); diff --git a/examples/surface/CMakeLists.txt b/examples/surface/CMakeLists.txt index 1507f787..9d105618 100644 --- a/examples/surface/CMakeLists.txt +++ b/examples/surface/CMakeLists.txt @@ -19,6 +19,10 @@ if(BUILD_surface_on_nurbs) PCL_ADD_EXAMPLE(pcl_example_nurbs_fitting_surface FILES example_nurbs_fitting_surface.cpp LINK_WITH pcl_common pcl_io pcl_surface pcl_visualization) + + PCL_ADD_EXAMPLE(pcl_example_nurbs_viewer_surface + FILES example_nurbs_viewer_surface.cpp + LINK_WITH pcl_common pcl_io pcl_surface pcl_visualization) PCL_ADD_EXAMPLE(pcl_example_nurbs_fitting_closed_curve FILES example_nurbs_fitting_closed_curve.cpp diff --git a/examples/surface/example_nurbs_fitting_surface.cpp b/examples/surface/example_nurbs_fitting_surface.cpp index 91e61112..60cf0447 100644 --- a/examples/surface/example_nurbs_fitting_surface.cpp +++ b/examples/surface/example_nurbs_fitting_surface.cpp @@ -10,78 +10,32 @@ typedef pcl::PointXYZ Point; void -PointCloud2Vector3d (pcl::PointCloud::Ptr cloud, pcl::on_nurbs::vector_vec3d &data) -{ - for (unsigned i = 0; i < cloud->size (); i++) - { - Point &p = cloud->at (i); - if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z)) - data.push_back (Eigen::Vector3d (p.x, p.y, p.z)); - } -} +PointCloud2Vector3d (pcl::PointCloud::Ptr cloud, pcl::on_nurbs::vector_vec3d &data); void -visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer) -{ - pcl::PointCloud::Ptr curve_cloud (new pcl::PointCloud); - - pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4); - for (std::size_t i = 0; i < curve_cloud->size () - 1; i++) - { - pcl::PointXYZRGB &p1 = curve_cloud->at (i); - pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1); - std::ostringstream os; - os << "line" << i; - viewer.removeShape (os.str ()); - viewer.addLine (p1, p2, 1.0, 0.0, 0.0, os.str ()); - } - - pcl::PointCloud::Ptr curve_cps (new pcl::PointCloud); - for (int i = 0; i < curve.CVCount (); i++) - { - ON_3dPoint p1; - curve.GetCV (i, p1); - - double pnt[3]; - surface.Evaluate (p1.x, p1.y, 0, 3, pnt); - pcl::PointXYZRGB p2; - p2.x = float (pnt[0]); - p2.y = float (pnt[1]); - p2.z = float (pnt[2]); - - p2.r = 255; - p2.g = 0; - p2.b = 0; - - curve_cps->push_back (p2); - } - viewer.removePointCloud ("cloud_cps"); - viewer.addPointCloud (curve_cps, "cloud_cps"); -} +visualizeCurve (ON_NurbsCurve &curve, + ON_NurbsSurface &surface, + pcl::visualization::PCLVisualizer &viewer); int main (int argc, char *argv[]) { - std::string pcd_file; + std::string pcd_file, file_3dm; - if (argc < 2) + if (argc < 3) { - printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd-file\n\n"); + printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd-in-file 3dm-out-file\n\n"); exit (0); } - pcd_file = argv[1]; + file_3dm = argv[2]; - unsigned order (3); - unsigned refinement (6); - unsigned iterations (10); - unsigned mesh_resolution (256); - - pcl::visualization::PCLVisualizer viewer ("Test: NURBS surface fitting"); + pcl::visualization::PCLVisualizer viewer ("B-spline surface fitting"); viewer.setSize (800, 600); // ############################################################################ // load point cloud + printf (" loading %s\n", pcd_file.c_str ()); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::PCLPointCloud2 cloud2; @@ -94,30 +48,38 @@ main (int argc, char *argv[]) PointCloud2Vector3d (cloud, data.interior); pcl::visualization::PointCloudColorHandlerCustom handler (cloud, 0, 255, 0); viewer.addPointCloud (cloud, handler, "cloud_cylinder"); - printf (" %zu points in data set\n", cloud->size ()); + printf (" %lu points in data set\n", cloud->size ()); // ############################################################################ - // fit NURBS surface + // fit B-spline surface + + // parameters + unsigned order (3); + unsigned refinement (5); + unsigned iterations (10); + unsigned mesh_resolution (256); + + pcl::on_nurbs::FittingSurface::Parameter params; + params.interior_smoothness = 0.2; + params.interior_weight = 1.0; + params.boundary_smoothness = 0.2; + params.boundary_weight = 0.0; + + // initialize printf (" surface fitting ...\n"); ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (order, &data); pcl::on_nurbs::FittingSurface fit (&data, nurbs); - // fit.setQuiet (false); + // fit.setQuiet (false); // enable/disable debug output + // mesh for visualization pcl::PolygonMesh mesh; pcl::PointCloud::Ptr mesh_cloud (new pcl::PointCloud); std::vector mesh_vertices; - std::string mesh_id = "mesh_nurbs"; pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (fit.m_nurbs, mesh, mesh_resolution); viewer.addPolygonMesh (mesh, mesh_id); - pcl::on_nurbs::FittingSurface::Parameter params; - params.interior_smoothness = 0.15; - params.interior_weight = 1.0; - params.boundary_smoothness = 0.15; - params.boundary_weight = 0.0; - - // NURBS refinement + // surface refinement for (unsigned i = 0; i < refinement; i++) { fit.refine (0); @@ -129,7 +91,7 @@ main (int argc, char *argv[]) viewer.spinOnce (); } - // fitting iterations + // surface fitting with final refinement level for (unsigned i = 0; i < iterations; i++) { fit.assemble (params); @@ -140,44 +102,118 @@ main (int argc, char *argv[]) } // ############################################################################ - // fit NURBS curve + // fit B-spline curve + + // parameters pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params; - curve_params.addCPsAccuracy = 3e-2; + curve_params.addCPsAccuracy = 5e-2; curve_params.addCPsIteration = 3; curve_params.maxCPs = 200; curve_params.accuracy = 1e-3; - curve_params.iterations = 10000; + curve_params.iterations = 100; curve_params.param.closest_point_resolution = 0; curve_params.param.closest_point_weight = 1.0; curve_params.param.closest_point_sigma2 = 0.1; - curve_params.param.interior_sigma2 = 0.0001; + curve_params.param.interior_sigma2 = 0.00001; curve_params.param.smooth_concavity = 1.0; curve_params.param.smoothness = 1.0; + // initialisation (circular) + printf (" curve fitting ...\n"); pcl::on_nurbs::NurbsDataCurve2d curve_data; curve_data.interior = data.interior_param; curve_data.interior_weight_function.push_back (true); - ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D (order, curve_data.interior); + // curve fitting pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs); -// curve_fit.setQuiet (false); - - // ############################### FITTING ############################### + // curve_fit.setQuiet (false); // enable/disable debug output curve_fit.fitting (curve_params); visualizeCurve (curve_fit.m_nurbs, fit.m_nurbs, viewer); // ############################################################################ // triangulation of trimmed surface + printf (" triangulate trimmed surface ...\n"); viewer.removePolygonMesh (mesh_id); pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (fit.m_nurbs, curve_fit.m_nurbs, mesh, mesh_resolution); viewer.addPolygonMesh (mesh, mesh_id); + + // save trimmed B-spline surface + if ( fit.m_nurbs.IsValid() ) + { + ONX_Model model; + ONX_Model_Object& surf = model.m_object_table.AppendNew(); + surf.m_object = new ON_NurbsSurface(fit.m_nurbs); + surf.m_bDeleteObject = true; + surf.m_attributes.m_layer_index = 1; + surf.m_attributes.m_name = "surface"; + + ONX_Model_Object& curv = model.m_object_table.AppendNew(); + curv.m_object = new ON_NurbsCurve(curve_fit.m_nurbs); + curv.m_bDeleteObject = true; + curv.m_attributes.m_layer_index = 2; + curv.m_attributes.m_name = "trimming curve"; + + model.Write(file_3dm.c_str()); + printf(" model saved: %s\n", file_3dm.c_str()); + } + printf (" ... done.\n"); viewer.spin (); return 0; } + +void +PointCloud2Vector3d (pcl::PointCloud::Ptr cloud, pcl::on_nurbs::vector_vec3d &data) +{ + for (unsigned i = 0; i < cloud->size (); i++) + { + Point &p = cloud->at (i); + if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z)) + data.push_back (Eigen::Vector3d (p.x, p.y, p.z)); + } +} + +void +visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer) +{ + pcl::PointCloud::Ptr curve_cloud (new pcl::PointCloud); + + pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4); + for (std::size_t i = 0; i < curve_cloud->size () - 1; i++) + { + pcl::PointXYZRGB &p1 = curve_cloud->at (i); + pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1); + std::ostringstream os; + os << "line" << i; + viewer.removeShape (os.str ()); + viewer.addLine (p1, p2, 1.0, 0.0, 0.0, os.str ()); + } + + pcl::PointCloud::Ptr curve_cps (new pcl::PointCloud); + for (int i = 0; i < curve.CVCount (); i++) + { + ON_3dPoint p1; + curve.GetCV (i, p1); + + double pnt[3]; + surface.Evaluate (p1.x, p1.y, 0, 3, pnt); + pcl::PointXYZRGB p2; + p2.x = float (pnt[0]); + p2.y = float (pnt[1]); + p2.z = float (pnt[2]); + + p2.r = 255; + p2.g = 0; + p2.b = 0; + + curve_cps->push_back (p2); + } + viewer.removePointCloud ("cloud_cps"); + viewer.addPointCloud (curve_cps, "cloud_cps"); +} diff --git a/examples/surface/example_nurbs_viewer_surface.cpp b/examples/surface/example_nurbs_viewer_surface.cpp new file mode 100644 index 00000000..710cfac9 --- /dev/null +++ b/examples/surface/example_nurbs_viewer_surface.cpp @@ -0,0 +1,88 @@ +#include +#include +#include + +#include +#include +#include +#include + +typedef pcl::PointXYZ Point; + +int +main (int argc, char *argv[]) +{ + std::string file_3dm; + + if (argc < 2) + { + printf ("\nUsage: pcl_example_nurbs_viewer_surface 3dm-out-file\n\n"); + exit (0); + } + file_3dm = argv[1]; + + pcl::visualization::PCLVisualizer viewer ("B-spline surface viewer"); + viewer.setSize (800, 600); + + int mesh_resolution = 128; + + ON::Begin(); + + // load surface + ONX_Model on_model; + bool rc = on_model.Read(file_3dm.c_str()); + + // print diagnostic + if ( rc ) + std::cout << "Successfully read: " << file_3dm << std::endl; + else + std::cout << "Errors during reading: " << file_3dm << std::endl; + +// ON_TextLog out; +// on_model.Dump(out); + + if(on_model.m_object_table.Count()==0) + { + std::cout << "3dm file does not contain any objects: " << file_3dm << std::endl; + return -1; + } + + const ON_Object* on_object = on_model.m_object_table[0].m_object; + if(on_object==NULL) + { + std::cout << "object[0] not valid." << std::endl; + return -1; + } + + const ON_NurbsSurface& on_surf = *(ON_NurbsSurface*)on_object; + + pcl::PolygonMesh mesh; + std::string mesh_id = "mesh_nurbs"; + if(on_model.m_object_table.Count()==1) + { + std::cout << "3dm file does not contain a trimming curve: " << file_3dm << std::endl; + + + pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (on_surf, mesh, mesh_resolution); + } + else + { + on_object = on_model.m_object_table[1].m_object; + if(on_object==NULL) + { + std::cout << "object[1] not valid." << std::endl; + return -1; + } + + const ON_NurbsCurve& on_curv = *(ON_NurbsCurve*)on_object; + + pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (on_surf, on_curv, mesh, + mesh_resolution); + } + + viewer.addPolygonMesh (mesh, mesh_id); + + viewer.spin (); + return 0; +} + diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index 4ba063b3..f0952f9d 100644 --- a/features/CMakeLists.txt +++ b/features/CMakeLists.txt @@ -3,105 +3,112 @@ set(SUBSYS_DESC "Point cloud features library") set(SUBSYS_DEPS common search kdtree octree filters) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/eigen.h - include/pcl/${SUBSYS_NAME}/board.h - include/pcl/${SUBSYS_NAME}/cvfh.h - include/pcl/${SUBSYS_NAME}/our_cvfh.h - include/pcl/${SUBSYS_NAME}/crh.h - include/pcl/${SUBSYS_NAME}/don.h - include/pcl/${SUBSYS_NAME}/feature.h - include/pcl/${SUBSYS_NAME}/fpfh.h - include/pcl/${SUBSYS_NAME}/fpfh_omp.h - include/pcl/${SUBSYS_NAME}/gfpfh.h - include/pcl/${SUBSYS_NAME}/integral_image2D.h - include/pcl/${SUBSYS_NAME}/integral_image_normal.h - include/pcl/${SUBSYS_NAME}/intensity_gradient.h - include/pcl/${SUBSYS_NAME}/intensity_spin.h - include/pcl/${SUBSYS_NAME}/linear_least_squares_normal.h - include/pcl/${SUBSYS_NAME}/moment_invariants.h - include/pcl/${SUBSYS_NAME}/multiscale_feature_persistence.h - include/pcl/${SUBSYS_NAME}/narf.h - include/pcl/${SUBSYS_NAME}/narf_descriptor.h - include/pcl/${SUBSYS_NAME}/normal_3d.h - include/pcl/${SUBSYS_NAME}/normal_3d_omp.h - include/pcl/${SUBSYS_NAME}/normal_based_signature.h - include/pcl/${SUBSYS_NAME}/pfh.h - include/pcl/${SUBSYS_NAME}/pfh_tools.h - include/pcl/${SUBSYS_NAME}/pfhrgb.h - include/pcl/${SUBSYS_NAME}/ppf.h - include/pcl/${SUBSYS_NAME}/ppfrgb.h - include/pcl/${SUBSYS_NAME}/shot.h - include/pcl/${SUBSYS_NAME}/shot_lrf.h - include/pcl/${SUBSYS_NAME}/shot_lrf_omp.h - include/pcl/${SUBSYS_NAME}/shot_omp.h - include/pcl/${SUBSYS_NAME}/spin_image.h - include/pcl/${SUBSYS_NAME}/principal_curvatures.h - include/pcl/${SUBSYS_NAME}/rift.h - #include/pcl/${SUBSYS_NAME}/rsd.h - include/pcl/${SUBSYS_NAME}/statistical_multiscale_interest_region_extraction.h - include/pcl/${SUBSYS_NAME}/vfh.h - include/pcl/${SUBSYS_NAME}/esf.h - include/pcl/${SUBSYS_NAME}/3dsc.h - include/pcl/${SUBSYS_NAME}/usc.h - include/pcl/${SUBSYS_NAME}/boundary.h - include/pcl/${SUBSYS_NAME}/range_image_border_extractor.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/board.h" + "include/pcl/${SUBSYS_NAME}/cppf.h" + "include/pcl/${SUBSYS_NAME}/cvfh.h" + "include/pcl/${SUBSYS_NAME}/our_cvfh.h" + "include/pcl/${SUBSYS_NAME}/crh.h" + "include/pcl/${SUBSYS_NAME}/don.h" + "include/pcl/${SUBSYS_NAME}/feature.h" + "include/pcl/${SUBSYS_NAME}/fpfh.h" + "include/pcl/${SUBSYS_NAME}/fpfh_omp.h" + "include/pcl/${SUBSYS_NAME}/gfpfh.h" + "include/pcl/${SUBSYS_NAME}/integral_image2D.h" + "include/pcl/${SUBSYS_NAME}/integral_image_normal.h" + "include/pcl/${SUBSYS_NAME}/intensity_gradient.h" + "include/pcl/${SUBSYS_NAME}/intensity_spin.h" + "include/pcl/${SUBSYS_NAME}/linear_least_squares_normal.h" + "include/pcl/${SUBSYS_NAME}/moment_invariants.h" + "include/pcl/${SUBSYS_NAME}/moment_of_inertia_estimation.h" + "include/pcl/${SUBSYS_NAME}/multiscale_feature_persistence.h" + "include/pcl/${SUBSYS_NAME}/narf.h" + "include/pcl/${SUBSYS_NAME}/narf_descriptor.h" + "include/pcl/${SUBSYS_NAME}/normal_3d.h" + "include/pcl/${SUBSYS_NAME}/normal_3d_omp.h" + "include/pcl/${SUBSYS_NAME}/normal_based_signature.h" + "include/pcl/${SUBSYS_NAME}/pfh.h" + "include/pcl/${SUBSYS_NAME}/pfh_tools.h" + "include/pcl/${SUBSYS_NAME}/pfhrgb.h" + "include/pcl/${SUBSYS_NAME}/ppf.h" + "include/pcl/${SUBSYS_NAME}/ppfrgb.h" + "include/pcl/${SUBSYS_NAME}/shot.h" + "include/pcl/${SUBSYS_NAME}/shot_lrf.h" + "include/pcl/${SUBSYS_NAME}/shot_lrf_omp.h" + "include/pcl/${SUBSYS_NAME}/shot_omp.h" + "include/pcl/${SUBSYS_NAME}/spin_image.h" + "include/pcl/${SUBSYS_NAME}/principal_curvatures.h" + "include/pcl/${SUBSYS_NAME}/rift.h" + "include/pcl/${SUBSYS_NAME}/rops_estimation.h" + #"include/pcl/${SUBSYS_NAME}/rsd.h" + "include/pcl/${SUBSYS_NAME}/statistical_multiscale_interest_region_extraction.h" + "include/pcl/${SUBSYS_NAME}/vfh.h" + "include/pcl/${SUBSYS_NAME}/esf.h" + "include/pcl/${SUBSYS_NAME}/3dsc.h" + "include/pcl/${SUBSYS_NAME}/usc.h" + "include/pcl/${SUBSYS_NAME}/boundary.h" + "include/pcl/${SUBSYS_NAME}/range_image_border_extractor.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/board.hpp - include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp - include/pcl/${SUBSYS_NAME}/impl/our_cvfh.hpp - include/pcl/${SUBSYS_NAME}/impl/crh.hpp - include/pcl/${SUBSYS_NAME}/impl/don.hpp - include/pcl/${SUBSYS_NAME}/impl/feature.hpp - include/pcl/${SUBSYS_NAME}/impl/fpfh.hpp - include/pcl/${SUBSYS_NAME}/impl/fpfh_omp.hpp - include/pcl/${SUBSYS_NAME}/impl/gfpfh.hpp - include/pcl/${SUBSYS_NAME}/impl/integral_image2D.hpp - include/pcl/${SUBSYS_NAME}/impl/integral_image_normal.hpp - include/pcl/${SUBSYS_NAME}/impl/intensity_gradient.hpp - include/pcl/${SUBSYS_NAME}/impl/intensity_spin.hpp - include/pcl/${SUBSYS_NAME}/impl/linear_least_squares_normal.hpp - include/pcl/${SUBSYS_NAME}/impl/moment_invariants.hpp - include/pcl/${SUBSYS_NAME}/impl/multiscale_feature_persistence.hpp - include/pcl/${SUBSYS_NAME}/impl/narf.hpp - include/pcl/${SUBSYS_NAME}/impl/normal_3d.hpp - include/pcl/${SUBSYS_NAME}/impl/normal_3d_omp.hpp - include/pcl/${SUBSYS_NAME}/impl/normal_based_signature.hpp - include/pcl/${SUBSYS_NAME}/impl/pfh.hpp - include/pcl/${SUBSYS_NAME}/impl/pfhrgb.hpp - include/pcl/${SUBSYS_NAME}/impl/ppf.hpp - include/pcl/${SUBSYS_NAME}/impl/ppfrgb.hpp - include/pcl/${SUBSYS_NAME}/impl/shot.hpp - include/pcl/${SUBSYS_NAME}/impl/shot_lrf.hpp - include/pcl/${SUBSYS_NAME}/impl/shot_lrf_omp.hpp - include/pcl/${SUBSYS_NAME}/impl/shot_omp.hpp - include/pcl/${SUBSYS_NAME}/impl/spin_image.hpp - include/pcl/${SUBSYS_NAME}/impl/principal_curvatures.hpp - include/pcl/${SUBSYS_NAME}/impl/rift.hpp - #include/pcl/${SUBSYS_NAME}/impl/rsd.hpp - include/pcl/${SUBSYS_NAME}/impl/statistical_multiscale_interest_region_extraction.hpp - include/pcl/${SUBSYS_NAME}/impl/vfh.hpp - include/pcl/${SUBSYS_NAME}/impl/esf.hpp - include/pcl/${SUBSYS_NAME}/impl/3dsc.hpp - include/pcl/${SUBSYS_NAME}/impl/usc.hpp - include/pcl/${SUBSYS_NAME}/impl/boundary.hpp - include/pcl/${SUBSYS_NAME}/impl/range_image_border_extractor.hpp + "include/pcl/${SUBSYS_NAME}/impl/board.hpp" + "include/pcl/${SUBSYS_NAME}/impl/cppf.hpp" + "include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/our_cvfh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/crh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/don.hpp" + "include/pcl/${SUBSYS_NAME}/impl/feature.hpp" + "include/pcl/${SUBSYS_NAME}/impl/fpfh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/fpfh_omp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/gfpfh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/integral_image2D.hpp" + "include/pcl/${SUBSYS_NAME}/impl/integral_image_normal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/intensity_gradient.hpp" + "include/pcl/${SUBSYS_NAME}/impl/intensity_spin.hpp" + "include/pcl/${SUBSYS_NAME}/impl/linear_least_squares_normal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/moment_invariants.hpp" + "include/pcl/${SUBSYS_NAME}/impl/moment_of_inertia_estimation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/multiscale_feature_persistence.hpp" + "include/pcl/${SUBSYS_NAME}/impl/narf.hpp" + "include/pcl/${SUBSYS_NAME}/impl/normal_3d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/normal_3d_omp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/normal_based_signature.hpp" + "include/pcl/${SUBSYS_NAME}/impl/pfh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/pfhrgb.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ppf.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ppfrgb.hpp" + "include/pcl/${SUBSYS_NAME}/impl/shot.hpp" + "include/pcl/${SUBSYS_NAME}/impl/shot_lrf.hpp" + "include/pcl/${SUBSYS_NAME}/impl/shot_lrf_omp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/shot_omp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/spin_image.hpp" + "include/pcl/${SUBSYS_NAME}/impl/principal_curvatures.hpp" + "include/pcl/${SUBSYS_NAME}/impl/rift.hpp" + "include/pcl/${SUBSYS_NAME}/impl/rops_estimation.hpp" + #"include/pcl/${SUBSYS_NAME}/impl/rsd.hpp" + "include/pcl/${SUBSYS_NAME}/impl/statistical_multiscale_interest_region_extraction.hpp" + "include/pcl/${SUBSYS_NAME}/impl/vfh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/esf.hpp" + "include/pcl/${SUBSYS_NAME}/impl/3dsc.hpp" + "include/pcl/${SUBSYS_NAME}/impl/usc.hpp" + "include/pcl/${SUBSYS_NAME}/impl/boundary.hpp" + "include/pcl/${SUBSYS_NAME}/impl/range_image_border_extractor.hpp" ) set(srcs src/board.cpp src/boundary.cpp + src/cppf.cpp src/cvfh.cpp src/our_cvfh.cpp src/crh.cpp @@ -113,6 +120,7 @@ if(build) src/intensity_spin.cpp src/linear_least_squares_normal.cpp src/moment_invariants.cpp + src/moment_of_inertia_estimation.cpp src/multiscale_feature_persistence.cpp src/narf.cpp src/normal_3d.cpp @@ -124,6 +132,7 @@ if(build) src/spin_image.cpp src/principal_curvatures.cpp src/rift.cpp + src/rops_estimation.cpp #src/rsd.cpp src/statistical_multiscale_interest_region_extraction.cpp src/vfh.cpp @@ -132,13 +141,20 @@ if(build) src/usc.cpp src/range_image_border_extractor.cpp ) - - set(LIB_NAME pcl_${SUBSYS_NAME}) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_common pcl_search pcl_kdtree pcl_octree pcl_filters) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + + if(MSVC) + # Workaround to aviod hitting the MSVC 4GB linker memory limit when building pcl_features. + # Disable whole program optimization (/GL) and link-time code generation (/LTCG). + string(REPLACE "/GL" "" CMAKE_CXX_FLAGS_RELEASE ${CMAKE_CXX_FLAGS_RELEASE}) + string(REPLACE "/LTCG" "" CMAKE_SHARED_LINKER_FLAGS_RELEASE ${CMAKE_SHARED_LINKER_FLAGS_RELEASE}) + endif(MSVC) + + set(LIB_NAME "pcl_${SUBSYS_NAME}") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_common pcl_search pcl_kdtree pcl_octree pcl_filters) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install headers - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/features/features.doxy b/features/features.doxy index 28441018..14ad2368 100644 --- a/features/features.doxy +++ b/features/features.doxy @@ -19,11 +19,11 @@ An example of two of the most widely used geometric point features are the underlying surface's estimated curvature and normal at a query point p. Both of them are considered local features, as they characterize a point using the information provided by its k closest point neighbors. For determining -these neighbors efficienctly, the input dataset is usually split into smaller +these neighbors efficiently, the input dataset is usually split into smaller chunks using spatial decomposition techniques such as octrees or kD-trees (see the figure below - left: kD-tree, right: octree), and then closest point searches are performed in that space. Depending on the application one can opt -for either determining a fixed number of k points in the vecinity of p, or all +for either determining a fixed number of k points in the vicinity of p, or all points which are found inside of a sphere of radius r centered at p. Unarguably, one the easiest methods for estimating the surface normals and curvature changes at a point p is to perform an eigendecomposition (i.e. @@ -43,6 +43,6 @@ Please visit http://www.pointclouds.org for more information. - \ref search "search" - \ref kdtree "kdtree" - \ref octree "octree" - - \ref range_image "range_image" + - \ref pcl::RangeImage "range_image" */ diff --git a/features/include/pcl/features/3dsc.h b/features/include/pcl/features/3dsc.h index 467a7ffb..c9fcde45 100644 --- a/features/include/pcl/features/3dsc.h +++ b/features/include/pcl/features/3dsc.h @@ -218,11 +218,11 @@ namespace pcl /** \brief Boost-based random number generator distribution. */ boost::shared_ptr > rng_; - /** \brief Shift computed descriptor "L" times along the azimuthal direction - * \param[in] block_size the size of each azimuthal block - * \param[in] desc at input desc == original descriptor and on output it contains - * shifted descriptor resized descriptor_length_ * azimuth_bins_ - */ + /* \brief Shift computed descriptor "L" times along the azimuthal direction + * \param[in] block_size the size of each azimuthal block + * \param[in] desc at input desc == original descriptor and on output it contains + * shifted descriptor resized descriptor_length_ * azimuth_bins_ + */ //void //shiftAlongAzimuth (size_t block_size, std::vector& desc); diff --git a/features/include/pcl/features/cppf.h b/features/include/pcl/features/cppf.h new file mode 100755 index 00000000..4f44aaf2 --- /dev/null +++ b/features/include/pcl/features/cppf.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2013, Martin Szarski + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_CPPF_H_ +#define PCL_CPPF_H_ + +#include +#include + +namespace pcl +{ + /** \brief + * \param[in] p1 + * \param[in] n1 + * \param[in] p2 + * \param[in] n2 + * \param[in] c1 + * \param[in] c2 + * \param[out] f1 + * \param[out] f2 + * \param[out] f3 + * \param[out] f4 + * \param[out] f5 + * \param[out] f6 + * \param[out] f7 + * \param[out] f8 + * \param[out] f9 + * \param[out] f10 + */ + PCL_EXPORTS bool + computeCPPFPairFeature (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &c1, + const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &c2, + float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7, float &f8, float &f9, float &f10); + + + + /** \brief Class that calculates the "surflet" features for each pair in the given + * pointcloud. Please refer to the following publication for more details: + * C. Choi, Henrik Christensen + * 3D Pose Estimation of Daily Objects Using an RGB-D Camera + * Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) + * 2012 + * + * PointOutT is meant to be pcl::CPPFSignature - contains the 10 values of the Surflet + * feature and in addition, alpha_m for the respective pair - optimization proposed by + * the authors (see above) + * + * \author Martin Szarski, Alexandru-Eugen Ichim + */ + + template + class CPPFEstimation : public FeatureFromNormals + { + public: + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + using PCLBase::indices_; + using Feature::input_; + using Feature::feature_name_; + using Feature::getClassName; + using FeatureFromNormals::normals_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Empty Constructor. */ + CPPFEstimation (); + + + private: + /** \brief The method called for actually doing the computations + * \param[out] output the resulting point cloud (which should be of type pcl::CPPFSignature); + * its size is the size of the input cloud, squared (i.e., one point for each pair in + * the input cloud); + */ + void + computeFeature (PointCloudOut &output); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif // PCL_CPPF_H_ diff --git a/features/include/pcl/features/cvfh.h b/features/include/pcl/features/cvfh.h index 127c0bab..95334dec 100644 --- a/features/include/pcl/features/cvfh.h +++ b/features/include/pcl/features/cvfh.h @@ -100,6 +100,7 @@ namespace pcl /** \brief Removes normals with high curvature caused by real edges or noisy data * \param[in] cloud pointcloud to be filtered + * \param[in] indices_to_use the indices to use * \param[out] indices_out the indices of the points with higher curvature than threshold * \param[out] indices_in the indices of the remaining points after filtering * \param[in] threshold threshold value for curvature diff --git a/features/include/pcl/features/fpfh.h b/features/include/pcl/features/fpfh.h index f0c1dce4..b53103bf 100644 --- a/features/include/pcl/features/fpfh.h +++ b/features/include/pcl/features/fpfh.h @@ -182,7 +182,7 @@ namespace pcl protected: /** \brief Estimate the set of all SPFH (Simple Point Feature Histograms) signatures for the input cloud - * \param[out] spfh_hist_lookup a lookup table for all the SPF feature indices + * \param[out] spf_hist_lookup a lookup table for all the SPF feature indices * \param[out] hist_f1 the resultant SPFH histogram for feature f1 * \param[out] hist_f2 the resultant SPFH histogram for feature f2 * \param[out] hist_f3 the resultant SPFH histogram for feature f3 diff --git a/features/include/pcl/features/impl/cppf.hpp b/features/include/pcl/features/impl/cppf.hpp new file mode 100755 index 00000000..5e2174c2 --- /dev/null +++ b/features/include/pcl/features/impl/cppf.hpp @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2013, Martin Szarski + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_CPPF_H_ +#define PCL_FEATURES_IMPL_CPPF_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::CPPFEstimation::CPPFEstimation () + : FeatureFromNormals () +{ + feature_name_ = "CPPFEstimation"; + // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () + Feature::tree_.reset (new pcl::search::KdTree ()); + Feature::search_radius_ = 1.0f; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CPPFEstimation::computeFeature (PointCloudOut &output) +{ + // Initialize output container - overwrite the sizes done by Feature::initCompute () + output.points.resize (indices_->size () * input_->points.size ()); + output.height = 1; + output.width = static_cast (output.points.size ()); + output.is_dense = true; + + // Compute point pair features for every pair of points in the cloud + for (size_t index_i = 0; index_i < indices_->size (); ++index_i) + { + size_t i = (*indices_)[index_i]; + for (size_t j = 0 ; j < input_->points.size (); ++j) + { + PointOutT p; + if (i != j) + { + if ( + pcl::computeCPPFPairFeature (input_->points[i].getVector4fMap (), + normals_->points[i].getNormalVector4fMap (), + input_->points[i].getRGBVector4i (), + input_->points[j].getVector4fMap (), + normals_->points[j].getNormalVector4fMap (), + input_->points[j].getRGBVector4i (), + p.f1, p.f2, p.f3, p.f4, p.f5, p.f6, p.f7, p.f8, p.f9, p.f10)) + { + // Calculate alpha_m angle + Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), + model_reference_normal = normals_->points[i].getNormalVector3fMap (), + model_point = input_->points[j].getVector3fMap (); + Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), + model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); + Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + + Eigen::Vector3f model_point_transformed = transform_mg * model_point; + float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); + if (sin (angle) * model_point_transformed(2) < 0.0f) + angle *= (-1); + p.alpha_m = -angle; + } + else + { + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j); + p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + } + } + // Do not calculate the feature for identity pairs (i, i) as they are not used + // in the following computations + else + { + p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + } + + output.points[index_i*input_->points.size () + j] = p; + } + } +} + +#define PCL_INSTANTIATE_CPPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CPPFEstimation; + + +#endif // PCL_FEATURES_IMPL_CPPF_H_ diff --git a/features/include/pcl/features/impl/cvfh.hpp b/features/include/pcl/features/impl/cvfh.hpp index d88a8cd5..45fa76fd 100644 --- a/features/include/pcl/features/impl/cvfh.hpp +++ b/features/include/pcl/features/impl/cvfh.hpp @@ -83,12 +83,12 @@ pcl::CVFHEstimation::extractEuclideanClustersSmoot { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (cloud.points.size () != normals.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); return; } diff --git a/features/include/pcl/features/impl/esf.hpp b/features/include/pcl/features/impl/esf.hpp index 109a632b..f36ab4bc 100644 --- a/features/include/pcl/features/impl/esf.hpp +++ b/features/include/pcl/features/impl/esf.hpp @@ -74,7 +74,6 @@ pcl::ESFEstimation::computeESF ( float h_a3_in[binsize] = {0}; float h_a3_out[binsize] = {0}; float h_a3_mix[binsize] = {0}; - float h_d1[binsize] = {0}; float h_d3_in[binsize] = {0}; float h_d3_out[binsize] = {0}; diff --git a/features/include/pcl/features/impl/feature.hpp b/features/include/pcl/features/impl/feature.hpp index de96afa8..62409b69 100644 --- a/features/include/pcl/features/impl/feature.hpp +++ b/features/include/pcl/features/impl/feature.hpp @@ -201,10 +201,12 @@ pcl::Feature::compute (PointCloudOut &output) // Resize the output dataset if (output.points.size () != indices_->size ()) output.points.resize (indices_->size ()); + // Check if the output will be computed for all points or only a subset - if (indices_->size () != input_->points.size ()) + // If the input width or height are not set, set output width as size + if (indices_->size () != input_->points.size () || input_->width * input_->height == 0) { - output.width = static_cast (indices_->size ()); + output.width = static_cast (indices_->size ()); output.height = 1; } else diff --git a/features/include/pcl/features/impl/integral_image_normal.hpp b/features/include/pcl/features/impl/integral_image_normal.hpp index 1e9634fa..9b23e5c6 100644 --- a/features/include/pcl/features/impl/integral_image_normal.hpp +++ b/features/include/pcl/features/impl/integral_image_normal.hpp @@ -816,6 +816,22 @@ pcl::IntegralImageNormalEstimation::computeFeature (PointCl current_row -= input_->width; } + if (indices_->size () < input_->size ()) + computeFeaturePart (distanceMap, bad_point, output); + else + computeFeatureFull (distanceMap, bad_point, output); + + delete[] depthChangeMap; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computeFeatureFull (const float *distanceMap, + const float &bad_point, + PointCloudOut &output) +{ + unsigned index = 0; + if (border_policy_ == BORDER_POLICY_IGNORE) { // Set all normals that we do not touch to NaN @@ -993,9 +1009,173 @@ pcl::IntegralImageNormalEstimation::computeFeature (PointCl } } } +} - delete[] depthChangeMap; - //delete[] distanceMap; +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computeFeaturePart (const float *distanceMap, + const float &bad_point, + PointCloudOut &output) +{ + if (border_policy_ == BORDER_POLICY_IGNORE) + { + output.is_dense = false; + unsigned border = int(normal_smoothing_size_); + unsigned bottom = input_->height > border ? input_->height - border : 0; + unsigned right = input_->width > border ? input_->width - border : 0; + if (use_depth_dependent_smoothing_) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + if (v < border || v > bottom) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + if (u < border || v > right) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + const float depth = input_->points[pt_index].z; + if (!pcl_isfinite (depth)) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast(depth)/10.0f); + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormal (u, v, pt_index, output [idx]); + } + else + { + output[idx].getNormalVector3fMap ().setConstant (bad_point); + output[idx].curvature = bad_point; + } + } + } + else + { + float smoothing_constant = normal_smoothing_size_; + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + if (v < border || v > bottom) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + if (u < border || v > right) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + if (!pcl_isfinite (input_->points[pt_index].z)) + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormal (u, v, pt_index, output [idx]); + } + else + { + output [pt_index].getNormalVector3fMap ().setConstant (bad_point); + output [pt_index].curvature = bad_point; + } + } + } + }// border_policy_ == BORDER_POLICY_IGNORE + else if (border_policy_ == BORDER_POLICY_MIRROR) + { + output.is_dense = false; + + if (use_depth_dependent_smoothing_) + { + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + + const float depth = input_->points[pt_index].z; + if (!pcl_isfinite (depth)) + { + output[idx].getNormalVector3fMap ().setConstant (bad_point); + output[idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast(depth)/10.0f); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormalMirror (u, v, pt_index, output [idx]); + } + else + { + output[idx].getNormalVector3fMap ().setConstant (bad_point); + output[idx].curvature = bad_point; + } + } + } + else + { + float smoothing_constant = normal_smoothing_size_; + for (size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + + if (!pcl_isfinite (input_->points[pt_index].z)) + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormalMirror (u, v, pt_index, output [idx]); + } + else + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + } + } + } + } // border_policy_ == BORDER_POLICY_MIRROR } ////////////////////////////////////////////////////////////////////////////////////////// diff --git a/features/include/pcl/features/impl/intensity_spin.hpp b/features/include/pcl/features/impl/intensity_spin.hpp index 58590626..669c1011 100644 --- a/features/include/pcl/features/impl/intensity_spin.hpp +++ b/features/include/pcl/features/impl/intensity_spin.hpp @@ -58,7 +58,7 @@ pcl::IntensitySpinEstimation::computeIntensitySpinImage ( // Find the min and max intensity values in the given neighborhood float min_intensity = std::numeric_limits::max (); - float max_intensity = std::numeric_limits::min (); + float max_intensity = -std::numeric_limits::max (); for (int idx = 0; idx < k; ++idx) { min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity); diff --git a/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp b/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp new file mode 100644 index 00000000..c00670cd --- /dev/null +++ b/features/include/pcl/features/impl/moment_of_inertia_estimation.hpp @@ -0,0 +1,649 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#ifndef PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ +#define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MomentOfInertiaEstimation::MomentOfInertiaEstimation () : + is_valid_ (false), + step_ (10.0f), + point_mass_ (0.0001f), + normalize_ (true), + mean_value_ (0.0f, 0.0f, 0.0f), + major_axis_ (0.0f, 0.0f, 0.0f), + middle_axis_ (0.0f, 0.0f, 0.0f), + minor_axis_ (0.0f, 0.0f, 0.0f), + major_value_ (0.0f), + middle_value_ (0.0f), + minor_value_ (0.0f), + moment_of_inertia_ (), + eccentricity_ (), + aabb_min_point_ (), + aabb_max_point_ (), + obb_min_point_ (), + obb_max_point_ (), + obb_position_ (0.0f, 0.0f, 0.0f), + obb_rotational_matrix_ () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MomentOfInertiaEstimation::~MomentOfInertiaEstimation () +{ + moment_of_inertia_.clear (); + eccentricity_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setAngleStep (const float step) +{ + if (step <= 0.0f) + return; + + step_ = step; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::getAngleStep () const +{ + return (step_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setNormalizePointMassFlag (bool need_to_normalize) +{ + normalize_ = need_to_normalize; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getNormalizePointMassFlag () const +{ + return (normalize_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setPointMass (const float point_mass) +{ + if (point_mass <= 0.0f) + return; + + point_mass_ = point_mass; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::getPointMass () const +{ + return (point_mass_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::compute () +{ + moment_of_inertia_.clear (); + eccentricity_.clear (); + + if (!initCompute ()) + { + deinitCompute (); + return; + } + + if (normalize_) + { + if (indices_->size () > 0) + point_mass_ = 1.0f / static_cast (indices_->size () * indices_->size ()); + else + point_mass_ = 1.0f; + } + + computeMeanValue (); + + Eigen::Matrix covariance_matrix; + covariance_matrix.setZero (); + computeCovarianceMatrix (covariance_matrix); + + computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_); + + float theta = 0.0f; + while (theta <= 90.0f) + { + float phi = 0.0f; + Eigen::Vector3f rotated_vector; + rotateVector (major_axis_, middle_axis_, theta, rotated_vector); + while (phi <= 360.0f) + { + Eigen::Vector3f current_axis; + rotateVector (rotated_vector, minor_axis_, phi, current_axis); + current_axis.normalize (); + + //compute moment of inertia for the current axis + float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_); + moment_of_inertia_.push_back (current_moment_of_inertia); + + //compute eccentricity for the current plane + typename pcl::PointCloud::Ptr projected_cloud (new pcl::PointCloud ()); + getProjectedCloud (current_axis, mean_value_, projected_cloud); + Eigen::Matrix covariance_matrix; + covariance_matrix.setZero (); + computeCovarianceMatrix (projected_cloud, covariance_matrix); + projected_cloud.reset (); + float current_eccentricity = computeEccentricity (covariance_matrix, current_axis); + eccentricity_.push_back (current_eccentricity); + + phi += step_; + } + theta += step_; + } + + computeOBB (); + + is_valid_ = true; + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getAABB (PointT& min_point, PointT& max_point) const +{ + min_point = aabb_min_point_; + max_point = aabb_max_point_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const +{ + min_point = obb_min_point_; + max_point = obb_max_point_; + position.x = obb_position_ (0); + position.y = obb_position_ (1); + position.z = obb_position_ (2); + rotational_matrix = obb_rotational_matrix_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeOBB () +{ + obb_min_point_.x = std::numeric_limits ::max (); + obb_min_point_.y = std::numeric_limits ::max (); + obb_min_point_.z = std::numeric_limits ::max (); + + obb_max_point_.x = std::numeric_limits ::min (); + obb_max_point_.y = std::numeric_limits ::min (); + obb_max_point_.z = std::numeric_limits ::min (); + + unsigned int number_of_points = static_cast (indices_->size ()); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + float x = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) + + (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) + + (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2); + float y = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) + + (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) + + (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2); + float z = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) + + (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) + + (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2); + + if (x <= obb_min_point_.x) obb_min_point_.x = x; + if (y <= obb_min_point_.y) obb_min_point_.y = y; + if (z <= obb_min_point_.z) obb_min_point_.z = z; + + if (x >= obb_max_point_.x) obb_max_point_.x = x; + if (y >= obb_max_point_.y) obb_max_point_.y = y; + if (z >= obb_max_point_.z) obb_max_point_.z = z; + } + + obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0), + major_axis_ (1), middle_axis_ (1), minor_axis_ (1), + major_axis_ (2), middle_axis_ (2), minor_axis_ (2); + + Eigen::Vector3f shift ( + (obb_max_point_.x + obb_min_point_.x) / 2.0f, + (obb_max_point_.y + obb_min_point_.y) / 2.0f, + (obb_max_point_.z + obb_min_point_.z) / 2.0f); + + obb_min_point_.x -= shift (0); + obb_min_point_.y -= shift (1); + obb_min_point_.z -= shift (2); + + obb_max_point_.x -= shift (0); + obb_max_point_.y -= shift (1); + obb_max_point_.z -= shift (2); + + obb_position_ = mean_value_ + obb_rotational_matrix_ * shift; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getEigenValues (float& major, float& middle, float& minor) const +{ + major = major_value_; + middle = middle_value_; + minor = minor_value_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const +{ + major = major_axis_; + middle = middle_axis_; + minor = minor_axis_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getMomentOfInertia (std::vector & moment_of_inertia) const +{ + moment_of_inertia.resize (moment_of_inertia_.size (), 0.0f); + std::copy (moment_of_inertia_.begin (), moment_of_inertia_.end (), moment_of_inertia.begin ()); + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getEccentricity (std::vector & eccentricity) const +{ + eccentricity.resize (eccentricity_.size (), 0.0f); + std::copy (eccentricity_.begin (), eccentricity_.end (), eccentricity.begin ()); + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeMeanValue () +{ + mean_value_ (0) = 0.0f; + mean_value_ (1) = 0.0f; + mean_value_ (2) = 0.0f; + + aabb_min_point_.x = std::numeric_limits ::max (); + aabb_min_point_.y = std::numeric_limits ::max (); + aabb_min_point_.z = std::numeric_limits ::max (); + + aabb_max_point_.x = -std::numeric_limits ::max (); + aabb_max_point_.y = -std::numeric_limits ::max (); + aabb_max_point_.z = -std::numeric_limits ::max (); + + unsigned int number_of_points = static_cast (indices_->size ()); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + mean_value_ (0) += input_->points[(*indices_)[i_point]].x; + mean_value_ (1) += input_->points[(*indices_)[i_point]].y; + mean_value_ (2) += input_->points[(*indices_)[i_point]].z; + + if (input_->points[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = input_->points[(*indices_)[i_point]].x; + if (input_->points[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = input_->points[(*indices_)[i_point]].y; + if (input_->points[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = input_->points[(*indices_)[i_point]].z; + + if (input_->points[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = input_->points[(*indices_)[i_point]].x; + if (input_->points[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = input_->points[(*indices_)[i_point]].y; + if (input_->points[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = input_->points[(*indices_)[i_point]].z; + } + + if (number_of_points == 0) + number_of_points = 1; + + mean_value_ (0) /= number_of_points; + mean_value_ (1) /= number_of_points; + mean_value_ (2) /= number_of_points; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeCovarianceMatrix (Eigen::Matrix & covariance_matrix) const +{ + covariance_matrix.setZero (); + + unsigned int number_of_points = static_cast (indices_->size ()); + float factor = 1.0f / static_cast ((number_of_points - 1 > 0)?(number_of_points - 1):1); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f); + current_point (0) = input_->points[(*indices_)[i_point]].x - mean_value_ (0); + current_point (1) = input_->points[(*indices_)[i_point]].y - mean_value_ (1); + current_point (2) = input_->points[(*indices_)[i_point]].z - mean_value_ (2); + + covariance_matrix += current_point * current_point.transpose (); + } + + covariance_matrix *= factor; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix & covariance_matrix) const +{ + covariance_matrix.setZero (); + + unsigned int number_of_points = static_cast (cloud->points.size ()); + float factor = 1.0f / static_cast ((number_of_points - 1 > 0)?(number_of_points - 1):1); + Eigen::Vector3f current_point; + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + current_point (0) = cloud->points[i_point].x - mean_value_ (0); + current_point (1) = cloud->points[i_point].y - mean_value_ (1); + current_point (2) = cloud->points[i_point].z - mean_value_ (2); + + covariance_matrix += current_point * current_point.transpose (); + } + + covariance_matrix *= factor; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeEigenVectors (const Eigen::Matrix & covariance_matrix, + Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value, + float& middle_value, float& minor_value) +{ + Eigen::EigenSolver > eigen_solver; + eigen_solver.compute (covariance_matrix); + + Eigen::EigenSolver >::EigenvectorsType eigen_vectors; + Eigen::EigenSolver >::EigenvalueType eigen_values; + eigen_vectors = eigen_solver.eigenvectors (); + eigen_values = eigen_solver.eigenvalues (); + + unsigned int temp = 0; + unsigned int major_index = 0; + unsigned int middle_index = 1; + unsigned int minor_index = 2; + + if (eigen_values.real () (major_index) < eigen_values.real () (middle_index)) + { + temp = major_index; + major_index = middle_index; + middle_index = temp; + } + + if (eigen_values.real () (major_index) < eigen_values.real () (minor_index)) + { + temp = major_index; + major_index = minor_index; + minor_index = temp; + } + + if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index)) + { + temp = minor_index; + minor_index = middle_index; + middle_index = temp; + } + + major_value = eigen_values.real () (major_index); + middle_value = eigen_values.real () (middle_index); + minor_value = eigen_values.real () (minor_index); + + major_axis = eigen_vectors.col (major_index).real (); + middle_axis = eigen_vectors.col (middle_index).real (); + minor_axis = eigen_vectors.col (minor_index).real (); + + major_axis.normalize (); + middle_axis.normalize (); + minor_axis.normalize (); + + float det = major_axis.dot (middle_axis.cross (minor_axis)); + if (det <= 0.0f) + { + major_axis (0) = -major_axis (0); + major_axis (1) = -major_axis (1); + major_axis (2) = -major_axis (2); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const +{ + Eigen::Matrix rotation_matrix; + const float x = axis (0); + const float y = axis (1); + const float z = axis (2); + const float rad = M_PI / 180.0f; + const float cosine = cos (angle * rad); + const float sine = sin (angle * rad); + rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y, + (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x, + (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z; + + rotated_vector = rotation_matrix * vector; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const +{ + float moment_of_inertia = 0.0f; + unsigned int number_of_points = static_cast (indices_->size ()); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::Vector3f vector; + vector (0) = mean_value (0) - input_->points[(*indices_)[i_point]].x; + vector (1) = mean_value (1) - input_->points[(*indices_)[i_point]].y; + vector (2) = mean_value (2) - input_->points[(*indices_)[i_point]].z; + + Eigen::Vector3f product = vector.cross (current_axis); + + float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2); + + moment_of_inertia += distance; + } + + return (point_mass_ * moment_of_inertia); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud ::Ptr projected_cloud) const +{ + const float D = - normal_vector.dot (point); + + unsigned int number_of_points = static_cast (indices_->size ()); + projected_cloud->points.resize (number_of_points, PointT ()); + + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + const unsigned int index = (*indices_)[i_point]; + float K = - (D + normal_vector (0) * input_->points[index].x + normal_vector (1) * input_->points[index].y + normal_vector (2) * input_->points[index].z); + PointT projected_point; + projected_point.x = input_->points[index].x + K * normal_vector (0); + projected_point.y = input_->points[index].y + K * normal_vector (1); + projected_point.z = input_->points[index].z + K * normal_vector (2); + projected_cloud->points[i_point] = projected_point; + } + projected_cloud->width = number_of_points; + projected_cloud->height = 1; + projected_cloud->header = input_->header; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::computeEccentricity (const Eigen::Matrix & covariance_matrix, const Eigen::Vector3f& normal_vector) +{ + Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f); + Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f); + Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f); + float major_value = 0.0f; + float middle_value = 0.0f; + float minor_value = 0.0f; + computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value); + + float major = abs (major_axis.dot (normal_vector)); + float middle = abs (middle_axis.dot (normal_vector)); + float minor = abs (minor_axis.dot (normal_vector)); + + float eccentricity = 0.0f; + + if (major >= middle && major >= minor && middle_value != 0.0f) + eccentricity = pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f); + + if (middle >= major && middle >= minor && major_value != 0.0f) + eccentricity = pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f); + + if (minor >= major && minor >= middle && major_value != 0.0f) + eccentricity = pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f); + + return (eccentricity); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getMassCenter (Eigen::Vector3f& mass_center) const +{ + mass_center = mean_value_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setInputCloud (const PointCloudConstPtr& cloud) +{ + input_ = cloud; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (const IndicesPtr& indices) +{ + indices_ = indices; + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (const IndicesConstPtr& indices) +{ + indices_.reset (new std::vector (*indices)); + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (const PointIndicesConstPtr& indices) +{ + indices_.reset (new std::vector (indices->indices)); + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols) +{ + if ((nb_rows > input_->height) || (row_start > input_->height)) + { + PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height); + return; + } + + if ((nb_cols > input_->width) || (col_start > input_->width)) + { + PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width); + return; + } + + size_t row_end = row_start + nb_rows; + if (row_end > input_->height) + { + PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height); + return; + } + + size_t col_end = col_start + nb_cols; + if (col_end > input_->width) + { + PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width); + return; + } + + indices_.reset (new std::vector); + indices_->reserve (nb_cols * nb_rows); + for(size_t i = row_start; i < row_end; i++) + for(size_t j = col_start; j < col_end; j++) + indices_->push_back (static_cast ((i * input_->width) + j)); + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +#endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ diff --git a/features/include/pcl/features/impl/our_cvfh.hpp b/features/include/pcl/features/impl/our_cvfh.hpp index 87e37afd..7ddd84c6 100644 --- a/features/include/pcl/features/impl/our_cvfh.hpp +++ b/features/include/pcl/features/impl/our_cvfh.hpp @@ -82,12 +82,12 @@ pcl::OURCVFHEstimation::extractEuclideanClustersSm { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (cloud.points.size () != normals.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); return; } @@ -310,7 +310,7 @@ pcl::OURCVFHEstimation::sgurf (Eigen::Vector3f & c if ((min_axis / max_axis) > axis_ratio_) { - PCL_WARN("Both axis are equally easy/difficult to disambiguate\n"); + PCL_WARN ("Both axes are equally easy/difficult to disambiguate\n"); Eigen::Vector3f evy_copy = evy; Eigen::Vector3f evxminus = evx * -1; @@ -376,6 +376,10 @@ pcl::OURCVFHEstimation::computeRFAndShapeDistribut std::vector & cluster_indices) { PointCloudOut ourcvfh_output; + + cluster_axes_.clear (); + cluster_axes_.resize (centroids_dominant_orientations_.size ()); + for (size_t i = 0; i < centroids_dominant_orientations_.size (); i++) { @@ -383,6 +387,9 @@ pcl::OURCVFHEstimation::computeRFAndShapeDistribut PointInTPtr grid (new pcl::PointCloud); sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]); + // Make a note of how many transformations correspond to each cluster + cluster_axes_[i] = transformations.size (); + for (size_t t = 0; t < transformations.size (); t++) { @@ -489,7 +496,7 @@ pcl::OURCVFHEstimation::computeRFAndShapeDistribut weights[ii] *= 0.5f - wz * 0.5f; } - int h_index = static_cast (std::floor (size_hists * (d / distance_normalization_factor))); + int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1; for (int j = 0; j < num_hists; j++) quadrants[j][h_index] += hist_incr * weights[j]; @@ -512,11 +519,15 @@ pcl::OURCVFHEstimation::computeRFAndShapeDistribut } ourcvfh_output.points.push_back (vfh_signature.points[0]); - + ourcvfh_output.width = ourcvfh_output.points.size (); delete[] weights; } } + if (ourcvfh_output.points.size ()) + { + ourcvfh_output.height = 1; + } output = ourcvfh_output; } @@ -633,8 +644,8 @@ pcl::OURCVFHEstimation::computeFeature (PointCloud //remove last cluster if no points found... if (clusters_[cluster_filtered_idx].indices.size () == 0) { - clusters_.erase (clusters_.end ()); - clusters_filtered.erase (clusters_filtered.end ()); + clusters_.pop_back (); + clusters_filtered.pop_back (); } else cluster_filtered_idx++; diff --git a/features/include/pcl/features/impl/ppf.hpp b/features/include/pcl/features/impl/ppf.hpp index 31eb7455..e1a58720 100644 --- a/features/include/pcl/features/impl/ppf.hpp +++ b/features/include/pcl/features/impl/ppf.hpp @@ -85,9 +85,11 @@ pcl::PPFEstimation::computeFeature (PointCloudOut Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), model_reference_normal = normals_->points[i].getNormalVector3fMap (), model_point = input_->points[j].getVector3fMap (); - Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), - model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); - Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + float rotation_angle = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis = (parallel_to_x)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis); + Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); Eigen::Vector3f model_point_transformed = transform_mg * model_point; float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); @@ -97,7 +99,7 @@ pcl::PPFEstimation::computeFeature (PointCloudOut } else { - PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %u and %u went wrong.\n", getClassName ().c_str (), i, j); p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits::quiet_NaN (); output.is_dense = false; } diff --git a/features/include/pcl/features/impl/ppfrgb.hpp b/features/include/pcl/features/impl/ppfrgb.hpp index 4ffabbc1..d351c100 100644 --- a/features/include/pcl/features/impl/ppfrgb.hpp +++ b/features/include/pcl/features/impl/ppfrgb.hpp @@ -92,7 +92,7 @@ pcl::PPFRGBEstimation::computeFeature (PointCloudO } else { - PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j); p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; } } @@ -156,7 +156,7 @@ pcl::PPFRGBRegionEstimation::computeFeature (Point } else { - PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j); } } } diff --git a/features/include/pcl/features/impl/rops_estimation.hpp b/features/include/pcl/features/impl/rops_estimation.hpp new file mode 100644 index 00000000..5befcdb3 --- /dev/null +++ b/features/include/pcl/features/impl/rops_estimation.hpp @@ -0,0 +1,537 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#ifndef PCL_ROPS_ESTIMATION_HPP_ +#define PCL_ROPS_ESTIMATION_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ROPSEstimation ::ROPSEstimation () : + number_of_bins_ (5), + number_of_rotations_ (3), + support_radius_ (1.0f), + sqr_support_radius_ (1.0f), + step_ (30.0f), + triangles_ (0), + triangles_of_the_point_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ROPSEstimation ::~ROPSEstimation () +{ + triangles_.clear (); + triangles_of_the_point_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setNumberOfPartitionBins (unsigned int number_of_bins) +{ + if (number_of_bins != 0) + number_of_bins_ = number_of_bins; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::ROPSEstimation ::getNumberOfPartitionBins () const +{ + return (number_of_bins_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setNumberOfRotations (unsigned int number_of_rotations) +{ + if (number_of_rotations != 0) + { + number_of_rotations_ = number_of_rotations; + step_ = 90.0f / static_cast (number_of_rotations_ + 1); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::ROPSEstimation ::getNumberOfRotations () const +{ + return (number_of_rotations_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setSupportRadius (float support_radius) +{ + if (support_radius > 0.0f) + { + support_radius_ = support_radius; + sqr_support_radius_ = support_radius * support_radius; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::ROPSEstimation ::getSupportRadius () const +{ + return (support_radius_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setTriangles (const std::vector & triangles) +{ + triangles_ = triangles; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::getTriangles (std::vector & triangles) const +{ + triangles = triangles_; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeFeature (PointCloudOut &output) +{ + if (triangles_.size () == 0) + { + output.points.clear (); + return; + } + + buildListOfPointsTriangles (); + + //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments + unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5; + unsigned int number_of_points = static_cast (indices_->size ()); + output.points.resize (number_of_points, PointOutT ()); + + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + std::set local_triangles; + std::vector local_points; + getLocalSurface (input_->points[(*indices_)[i_point]], local_triangles, local_points); + + Eigen::Matrix3f lrf_matrix; + computeLRF (input_->points[(*indices_)[i_point]], local_triangles, lrf_matrix); + + PointCloudIn transformed_cloud; + transformCloud (input_->points[(*indices_)[i_point]], lrf_matrix, local_points, transformed_cloud); + + PointInT axis[3]; + axis[0].x = 1.0f; axis[0].y = 0.0f; axis[0].z = 0.0f; + axis[1].x = 0.0f; axis[1].y = 1.0f; axis[1].z = 0.0f; + axis[2].x = 0.0f; axis[2].y = 0.0f; axis[2].z = 1.0f; + std::vector feature; + for (unsigned int i_axis = 0; i_axis < 3; i_axis++) + { + float theta = step_; + do + { + //rotate local surface and get bounding box + PointCloudIn rotated_cloud; + Eigen::Vector3f min, max; + rotateCloud (axis[i_axis], theta, transformed_cloud, rotated_cloud, min, max); + + //for each projection (XY, XZ and YZ) compute distribution matrix and central moments + for (unsigned int i_proj = 0; i_proj < 3; i_proj++) + { + Eigen::MatrixXf distribution_matrix; + distribution_matrix.resize (number_of_bins_, number_of_bins_); + getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix); + + std::vector moments; + computeCentralMoments (distribution_matrix, moments); + + feature.insert (feature.end (), moments.begin (), moments.end ()); + } + + theta += step_; + } while (theta < 90.0f); + } + + float norm = 0.0f; + for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++) + norm += feature[i_dim]; + if (abs (norm) < std::numeric_limits ::epsilon ()) + norm = 1.0f / norm; + else + norm = 1.0f; + + for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++) + output.points[i_point].histogram[i_dim] = feature[i_dim] * norm; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::buildListOfPointsTriangles () +{ + triangles_of_the_point_.clear (); + + const unsigned int number_of_triangles = static_cast (triangles_.size ()); + + std::vector dummy; + dummy.reserve (100); + triangles_of_the_point_.resize (surface_->points. size (), dummy); + + for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++) + for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) + triangles_of_the_point_[triangles_[i_triangle].vertices[i_vertex]].push_back (i_triangle); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::getLocalSurface (const PointInT& point, std::set & local_triangles, std::vector & local_points) const +{ + std::vector distances; + tree_->radiusSearch (point, support_radius_, local_points, distances); + + const unsigned int number_of_indices = static_cast (local_points.size ()); + for (unsigned int i = 0; i < number_of_indices; i++) + local_triangles.insert (triangles_of_the_point_[local_points[i]].begin (), triangles_of_the_point_[local_points[i]].end ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeLRF (const PointInT& point, const std::set & local_triangles, Eigen::Matrix3f& lrf_matrix) const +{ + const unsigned int number_of_triangles = static_cast (local_triangles.size ()); + + std::vector scatter_matrices (number_of_triangles); + std::vector triangle_area (number_of_triangles); + std::vector distance_weight (number_of_triangles); + + float total_area = 0.0f; + const float coeff = 1.0f / 12.0f; + const float coeff_1_div_3 = 1.0f / 3.0f; + + Eigen::Vector3f feature_point (point.x, point.y, point.z); + + std::set ::const_iterator it; + unsigned int i_triangle = 0; + for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++) + { + Eigen::Vector3f pt[3]; + for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) + { + const unsigned int index = triangles_[*it].vertices[i_vertex]; + pt[i_vertex] (0) = surface_->points[index].x; + pt[i_vertex] (1) = surface_->points[index].y; + pt[i_vertex] (2) = surface_->points[index].z; + } + + const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm (); + triangle_area[i_triangle] = curr_area; + total_area += curr_area; + + distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f); + + Eigen::Matrix3f curr_scatter_matrix; + curr_scatter_matrix.setZero (); + for (unsigned int i_pt = 0; i_pt < 3; i_pt++) + { + Eigen::Vector3f vec = pt[i_pt] - feature_point; + curr_scatter_matrix += vec * (vec.transpose ()); + for (unsigned int j_pt = 0; j_pt < 3; j_pt++) + curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ()); + } + scatter_matrices[i_triangle] = coeff * curr_scatter_matrix; + } + + if (abs (total_area) < std::numeric_limits ::epsilon ()) + total_area = 1.0f / total_area; + else + total_area = 1.0f; + + Eigen::Matrix3f overall_scatter_matrix; + overall_scatter_matrix.setZero (); + std::vector total_weight (number_of_triangles); + const float denominator = 1.0f / 6.0f; + for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++) + { + float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area; + overall_scatter_matrix += factor * scatter_matrices[i_triangle]; + total_weight[i_triangle] = factor * denominator; + } + + Eigen::Vector3f v1, v2, v3; + computeEigenVectors (overall_scatter_matrix, v1, v2, v3); + + float h1 = 0.0f; + float h3 = 0.0f; + for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++) + { + Eigen::Vector3f pt[3]; + for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) + { + const unsigned int index = triangles_[*it].vertices[i_vertex]; + pt[i_vertex] (0) = surface_->points[index].x; + pt[i_vertex] (1) = surface_->points[index].y; + pt[i_vertex] (2) = surface_->points[index].z; + } + + float factor1 = 0.0f; + float factor3 = 0.0f; + for (unsigned int i_pt = 0; i_pt < 3; i_pt++) + { + Eigen::Vector3f vec = pt[i_pt] - feature_point; + factor1 += vec.dot (v1); + factor3 += vec.dot (v3); + } + h1 += total_weight[i_triangle] * factor1; + h3 += total_weight[i_triangle] * factor3; + } + + if (h1 < 0.0f) v1 = -v1; + if (h3 < 0.0f) v3 = -v3; + + v2 = v3.cross (v1); + + lrf_matrix.row (0) = v1; + lrf_matrix.row (1) = v2; + lrf_matrix.row (2) = v3; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeEigenVectors (const Eigen::Matrix3f& matrix, + Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const +{ + Eigen::EigenSolver eigen_solver; + eigen_solver.compute (matrix); + + Eigen::EigenSolver ::EigenvectorsType eigen_vectors; + Eigen::EigenSolver ::EigenvalueType eigen_values; + eigen_vectors = eigen_solver.eigenvectors (); + eigen_values = eigen_solver.eigenvalues (); + + unsigned int temp = 0; + unsigned int major_index = 0; + unsigned int middle_index = 1; + unsigned int minor_index = 2; + + if (eigen_values.real () (major_index) < eigen_values.real () (middle_index)) + { + temp = major_index; + major_index = middle_index; + middle_index = temp; + } + + if (eigen_values.real () (major_index) < eigen_values.real () (minor_index)) + { + temp = major_index; + major_index = minor_index; + minor_index = temp; + } + + if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index)) + { + temp = minor_index; + minor_index = middle_index; + middle_index = temp; + } + + major_axis = eigen_vectors.col (major_index).real (); + middle_axis = eigen_vectors.col (middle_index).real (); + minor_axis = eigen_vectors.col (minor_index).real (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector & local_points, PointCloudIn& transformed_cloud) const +{ + const unsigned int number_of_points = static_cast (local_points.size ()); + transformed_cloud.points.resize (number_of_points, PointInT ()); + + for (unsigned int i = 0; i < number_of_points; i++) + { + Eigen::Vector3f transformed_point ( + surface_->points[local_points[i]].x - point.x, + surface_->points[local_points[i]].y - point.y, + surface_->points[local_points[i]].z - point.z); + + transformed_point = matrix * transformed_point; + + PointInT new_point; + new_point.x = transformed_point (0); + new_point.y = transformed_point (1); + new_point.z = transformed_point (2); + transformed_cloud.points[i] = new_point; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const +{ + Eigen::Matrix3f rotation_matrix; + const float x = axis.x; + const float y = axis.y; + const float z = axis.z; + const float rad = M_PI / 180.0f; + const float cosine = cos (angle * rad); + const float sine = sin (angle * rad); + rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y, + (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x, + (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z; + + const unsigned int number_of_points = static_cast (cloud.points.size ()); + + rotated_cloud.header = cloud.header; + rotated_cloud.width = number_of_points; + rotated_cloud.height = 1; + rotated_cloud.points.resize (number_of_points, PointInT ()); + + min (0) = std::numeric_limits ::max (); + min (1) = std::numeric_limits ::max (); + min (2) = std::numeric_limits ::max (); + max (0) = -std::numeric_limits ::max (); + max (1) = -std::numeric_limits ::max (); + max (2) = -std::numeric_limits ::max (); + + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::Vector3f point ( + cloud.points[i_point].x, + cloud.points[i_point].y, + cloud.points[i_point].z); + + point = rotation_matrix * point; + PointInT rotated_point; + rotated_point.x = point (0); + rotated_point.y = point (1); + rotated_point.z = point (2); + rotated_cloud.points[i_point] = rotated_point; + + if (min (0) > rotated_point.x) min (0) = rotated_point.x; + if (min (1) > rotated_point.y) min (1) = rotated_point.y; + if (min (2) > rotated_point.z) min (2) = rotated_point.z; + + if (max (0) < rotated_point.x) max (0) = rotated_point.x; + if (max (1) < rotated_point.y) max (1) = rotated_point.y; + if (max (2) < rotated_point.z) max (2) = rotated_point.z; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const +{ + matrix.setZero (); + + const unsigned int number_of_points = static_cast (cloud.points.size ()); + + const unsigned int coord[3][2] = { + {0, 1}, + {0, 2}, + {1, 2}}; + + const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_; + const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_; + + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::Vector3f point ( + cloud.points[i_point].x, + cloud.points[i_point].y, + cloud.points[i_point].z); + + const float u_length = point (coord[projection][0]) - min[coord[projection][0]]; + const float v_length = point (coord[projection][1]) - min[coord[projection][1]]; + + const float u_ratio = u_length / u_bin_length; + unsigned int row = static_cast (u_ratio); + if (row == number_of_bins_) row--; + + const float v_ratio = v_length / v_bin_length; + unsigned int col = static_cast (v_ratio); + if (col == number_of_bins_) col--; + + matrix (row, col) += 1.0f; + } + + matrix /= static_cast (number_of_points); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector & moments) const +{ + float mean_i = 0.0f; + float mean_j = 0.0f; + + for (unsigned int i = 0; i < number_of_bins_; i++) + for (unsigned int j = 0; j < number_of_bins_; j++) + { + const float m = matrix (i, j); + mean_i += static_cast (i + 1) * m; + mean_j += static_cast (j + 1) * m; + } + + const unsigned int number_of_moments_to_compute = 4; + const float power[number_of_moments_to_compute][2] = { + {1.0f, 1.0f}, + {2.0f, 1.0f}, + {1.0f, 2.0f}, + {2.0f, 2.0f}}; + + float entropy = 0.0f; + moments.resize (number_of_moments_to_compute + 1, 0.0f); + for (unsigned int i = 0; i < number_of_bins_; i++) + { + const float i_factor = static_cast (i + 1) - mean_i; + for (unsigned int j = 0; j < number_of_bins_; j++) + { + const float j_factor = static_cast (j + 1) - mean_j; + const float m = matrix (i, j); + if (m > 0.0f) + entropy -= m * log (m); + for (unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++) + moments[i_moment] += pow (i_factor, power[i_moment][0]) * pow (j_factor, power[i_moment][1]) * m; + } + } + + moments[number_of_moments_to_compute] = entropy; +} + +#endif // PCL_ROPS_ESTIMATION_HPP_ diff --git a/features/include/pcl/features/impl/shot.hpp b/features/include/pcl/features/impl/shot.hpp index fe0e260f..8dae2d91 100644 --- a/features/include/pcl/features/impl/shot.hpp +++ b/features/include/pcl/features/impl/shot.hpp @@ -895,6 +895,7 @@ pcl::SHOTColorEstimation::computeFeature } } +#define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase; #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation; #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation; diff --git a/features/include/pcl/features/integral_image_normal.h b/features/include/pcl/features/integral_image_normal.h index 4422ce05..4404f29f 100644 --- a/features/include/pcl/features/integral_image_normal.h +++ b/features/include/pcl/features/integral_image_normal.h @@ -50,6 +50,18 @@ namespace pcl { /** \brief Surface normal estimation on organized data using integral images. + * + * For detailed information about this method see: + * + * S. Holzer and R. B. Rusu and M. Dixon and S. Gedikli and N. Navab, + * Adaptive Neighborhood Selection for Real-Time Surface Normal Estimation + * from Organized Point Cloud Data Using Integral Images, IROS 2012. + * + * D. Holz, S. Holzer, R. B. Rusu, and S. Behnke (2011, July). + * Real-Time Plane Segmentation using RGB-D Cameras. In Proceedings of + * the 15th RoboCup International Symposium, Istanbul, Turkey. + * http://www.ais.uni-bonn.de/~holz/papers/holz_2011_robocup.pdf + * * \author Stefan Holzer */ template @@ -59,6 +71,7 @@ namespace pcl using Feature::feature_name_; using Feature::tree_; using Feature::k_; + using Feature::indices_; public: typedef boost::shared_ptr > Ptr; @@ -303,12 +316,28 @@ namespace pcl protected: - /** \brief Computes the normal for the complete cloud. + /** \brief Computes the normal for the complete cloud or only \a indices_ if provided. * \param[out] output the resultant normals */ void computeFeature (PointCloudOut &output); + /** \brief Computes the normal for the complete cloud. + * \param[in] distance_map distance map + * \param[in] bad_point constant given to invalid normal components + * \param[out] output the resultant normals + */ + void + computeFeatureFull (const float* distance_map, const float& bad_point, PointCloudOut& output); + + /** \brief Computes the normal for part of the cloud specified by \a indices_ + * \param[in] distance_map distance map + * \param[in] bad_point constant given to invalid normal components + * \param[out] output the resultant normals + */ + void + computeFeaturePart (const float* distance_map, const float& bad_point, PointCloudOut& output); + /** \brief Initialize the data structures, based on the normal estimation method chosen. */ void initData (); diff --git a/features/include/pcl/features/intensity_gradient.h b/features/include/pcl/features/intensity_gradient.h index 0959c01d..7f14ea8e 100644 --- a/features/include/pcl/features/intensity_gradient.h +++ b/features/include/pcl/features/intensity_gradient.h @@ -92,6 +92,7 @@ namespace pcl * \param cloud a point cloud dataset containing XYZI coordinates (Cartesian coordinates + intensity) * \param indices the indices of the neighoring points in the dataset * \param point the 3D Cartesian coordinates of the point at which to estimate the gradient + * \param mean_intensity * \param normal the 3D surface normal of the given point * \param gradient the resultant 3D gradient vector */ diff --git a/features/include/pcl/features/moment_of_inertia_estimation.h b/features/include/pcl/features/moment_of_inertia_estimation.h new file mode 100644 index 00000000..e36c1286 --- /dev/null +++ b/features/include/pcl/features/moment_of_inertia_estimation.h @@ -0,0 +1,362 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#ifndef PCL_MOMENT_OF_INERTIA_ESIMATION_H_ +#define PCL_MOMENT_OF_INERTIA_ESIMATION_H_ + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the method for extracting features based on moment of inertia. It also + * calculates AABB, OBB and eccentricity of the projected cloud. + */ + template + class PCL_EXPORTS MomentOfInertiaEstimation : public pcl::PCLBase + { + public: + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::fake_indices_; + using PCLBase ::use_indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + typedef typename pcl::PCLBase ::PointCloudConstPtr PointCloudConstPtr; + typedef typename pcl::PCLBase ::PointIndicesConstPtr PointIndicesConstPtr; + + public: + + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + virtual void + setInputCloud (const PointCloudConstPtr& cloud); + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + virtual void + setIndices (const IndicesPtr& indices); + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + virtual void + setIndices (const IndicesConstPtr& indices); + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + virtual void + setIndices (const PointIndicesConstPtr& indices); + + /** \brief Set the indices for the points laying within an interest region of + * the point cloud. + * \note you shouldn't call this method on unorganized point clouds! + * \param[in] row_start the offset on rows + * \param[in] col_start the offset on columns + * \param[in] nb_rows the number of rows to be considered row_start included + * \param[in] nb_cols the number of columns to be considered col_start included + */ + virtual void + setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols); + + /** \brief Constructor that sets default values for member variables. */ + MomentOfInertiaEstimation (); + + /** \brief Virtual destructor which frees the memory. */ + virtual + ~MomentOfInertiaEstimation (); + + /** \brief This method allows to set the angle step. It is used for the rotation + * of the axis which is used for moment of inertia/eccentricity calculation. + * \param[in] step angle step + */ + void + setAngleStep (const float step); + + /** \brief Returns the angle step. */ + float + getAngleStep () const; + + /** \brief This method allows to set the normalize_ flag. If set to false, then + * point_mass_ will be used to scale the moment of inertia values. Otherwise, + * point_mass_ will be set to 1 / number_of_points. Default value is true. + * \param[in] need_to_normalize desired value + */ + void + setNormalizePointMassFlag (bool need_to_normalize); + + /** \brief Returns the normalize_ flag. */ + bool + getNormalizePointMassFlag () const; + + /** \brief This method allows to set point mass that will be used for + * moment of inertia calculation. It is needed to scale moment of inertia values. + * default value is 0.0001. + * \param[in] point_mass point mass + */ + void + setPointMass (const float point_mass); + + /** \brief Returns the mass of point. */ + float + getPointMass () const; + + /** \brief This method launches the computation of all features. After execution + * it sets is_valid_ flag to true and each feature can be accessed with the + * corresponding get method. + */ + void + compute (); + + /** \brief This method gives access to the computed axis aligned bounding box. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] min_point min point of the AABB + * \param[out] max_point max point of the AABB + */ + bool + getAABB (PointT& min_point, PointT& max_point) const; + + /** \brief This method gives access to the computed oriented bounding box. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * Note that in order to get the OBB, each vertex of the given AABB (specified with min_point and max_point) + * must be rotated with the given rotational matrix (rotation transform) and then positioned. + * Also pay attention to the fact that this is not the minimal possible bounding box. This is the bounding box + * which is oriented in accordance with the eigen vectors. + * \param[out] min_point min point of the OBB + * \param[out] max_point max point of the OBB + * \param[out] position position of the OBB + * \param[out] rotational_matrix this matrix represents the rotation transform + */ + bool + getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const; + + /** \brief This method gives access to the computed eigen values. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] major major eigen value + * \param[out] middle middle eigen value + * \param[out] minor minor eigen value + */ + bool + getEigenValues (float& major, float& middle, float& minor) const; + + /** \brief This method gives access to the computed eigen vectors. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] major axis which corresponds to the eigen vector with the major eigen value + * \param[out] middle axis which corresponds to the eigen vector with the middle eigen value + * \param[out] minor axis which corresponds to the eigen vector with the minor eigen value + */ + bool + getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const; + + /** \brief This method gives access to the computed moments of inertia. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] moment_of_inertia computed moments of inertia + */ + bool + getMomentOfInertia (std::vector & moment_of_inertia) const; + + /** \brief This method gives access to the computed ecentricities. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] eccentricity computed eccentricities + */ + bool + getEccentricity (std::vector & eccentricity) const; + + /** \brief This method gives access to the computed mass center. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * Note that when mass center of a cloud is computed, mass point is always considered equal 1. + * \param[out] mass_center computed mass center + */ + bool + getMassCenter (Eigen::Vector3f& mass_center) const; + + private: + + /** \brief This method rotates the given vector around the given axis. + * \param[in] vector vector that must be rotated + * \param[in] axis axis around which vector must be rotated + * \param[in] angle angle in degrees + * \param[out] rotated_vector resultant vector + */ + void + rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const; + + /** \brief This method computes center of mass and axis aligned bounding box. */ + void + computeMeanValue (); + + /** \brief This method computes the oriented bounding box. */ + void + computeOBB (); + + /** \brief This method computes the covariance matrix for the input_ cloud. + * \param[out] covariance_matrix stores the computed covariance matrix + */ + void + computeCovarianceMatrix (Eigen::Matrix & covariance_matrix) const; + + /** \brief This method computes the covariance matrix for the given cloud. + * It uses all points in the cloud, unlike the previous method that uses indices. + * \param[in] cloud cloud for which covariance matrix will be computed + * \param[out] covariance_matrix stores the computed covariance matrix + */ + void + computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix & covariance_matrix) const; + + /** \brief This method calculates the eigen values and eigen vectors + * for the given covariance matrix. Note that it returns normalized eigen + * vectors that always form the right-handed coordinate system. + * \param[in] covariance_matrix covariance matrix + * \param[out] major_axis eigen vector which corresponds to a major eigen value + * \param[out] middle_axis eigen vector which corresponds to a middle eigen value + * \param[out] minor_axis eigen vector which corresponds to a minor eigen value + * \param[out] major_value major eigen value + * \param[out] middle_value middle eigen value + * \param[out] minor_value minor eigen value + */ + void + computeEigenVectors (const Eigen::Matrix & covariance_matrix, Eigen::Vector3f& major_axis, + Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value, float& middle_value, + float& minor_value); + + /** \brief This method returns the moment of inertia of a given input_ cloud. + * Note that when moment of inertia is computed it is multiplied by the point mass. + * Point mass can be accessed with the corresponding get/set methods. + * \param[in] current_axis axis that will be used in moment of inertia computation + * \param[in] mean_value mean value(center of mass) of the cloud + */ + float + calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const; + + /** \brief This method simply projects the given input_ cloud on the plane specified with + * the normal vector. + * \param[in] normal_vector nrmal vector of the plane + * \param[in] point point belonging to the plane + * \param[out] projected_cloud projected cloud + */ + void + getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud ::Ptr projected_cloud) const; + + /** \brief This method returns the eccentricity of the projected cloud. + * \param[in] covariance_matrix covariance matrix of the projected cloud + * \param[in] normal_vector normal vector of the plane, it is used to discard the + * third eigen vector and eigen value*/ + float + computeEccentricity (const Eigen::Matrix & covariance_matrix, const Eigen::Vector3f& normal_vector); + + private: + + /** \brief Indicates if the stored values (eccentricity, moment of inertia, AABB etc.) + * are valid when accessed with the get methods. */ + bool is_valid_; + + /** \brief Stores the angle step */ + float step_; + + /** \brief Stores the mass of point in the cloud */ + float point_mass_; + + /** \brief Stores the flag for mass normalization */ + bool normalize_; + + /** \brief Stores the mean value (center of mass) of the cloud */ + Eigen::Vector3f mean_value_; + + /** \brief Major eigen vector */ + Eigen::Vector3f major_axis_; + + /** \brief Middle eigen vector */ + Eigen::Vector3f middle_axis_; + + /** \brief Minor eigen vector */ + Eigen::Vector3f minor_axis_; + + /** \brief Major eigen value */ + float major_value_; + + /** \brief Middle eigen value */ + float middle_value_; + + /** \brief Minor eigen value */ + float minor_value_; + + /** \brief Stores calculated moments of inertia */ + std::vector moment_of_inertia_; + + /** \brief Stores calculated eccentricities */ + std::vector eccentricity_; + + /** \brief Min point of the axis aligned bounding box */ + PointT aabb_min_point_; + + /** \brief Max point of the axis aligned bounding box */ + PointT aabb_max_point_; + + /** \brief Min point of the oriented bounding box */ + PointT obb_min_point_; + + /** \brief Max point of the oriented bounding box */ + PointT obb_max_point_; + + /** \brief Stores position of the oriented bounding box */ + Eigen::Vector3f obb_position_; + + /** \brief Stores the rotational matrix of the oriented bounding box */ + Eigen::Matrix3f obb_rotational_matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#define PCL_INSTANTIATE_MomentOfInertiaEstimation(T) template class pcl::MomentOfInertiaEstimation; + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif diff --git a/features/include/pcl/features/our_cvfh.h b/features/include/pcl/features/our_cvfh.h index cc46835d..5d5bc150 100644 --- a/features/include/pcl/features/our_cvfh.h +++ b/features/include/pcl/features/our_cvfh.h @@ -92,7 +92,7 @@ namespace pcl /** \brief Creates an affine transformation from the RF axes * \param[in] evx the x-axis - * \param[in] evy the z-axis + * \param[in] evy the y-axis * \param[in] evz the z-axis * \param[out] transformPC the resulting transformation * \param[in] center_mat 4x4 matrix concatenated to the resulting transformation @@ -144,6 +144,7 @@ namespace pcl /** \brief Removes normals with high curvature caused by real edges or noisy data * \param[in] cloud pointcloud to be filtered + * \param[in] indices_to_use * \param[out] indices_out the indices of the points with higher curvature than threshold * \param[out] indices_in the indices of the remaining points after filtering * \param[in] threshold threshold value for curvature @@ -261,6 +262,15 @@ namespace pcl { indices = clusters_; } + + /** \brief Gets the number of non-disambiguable axes that correspond to each centroid + * \param[out] cluster_axes vector mapping each centroid to the number of signatures + */ + inline void + getClusterAxes (std::vector & cluster_axes) + { + cluster_axes = cluster_axes_; + } /** \brief Sets the refinement factor for the clusters * \param[in] rc the factor used to decide if a point is used to estimate a stable cluster @@ -390,6 +400,8 @@ namespace pcl std::vector dominant_normals_; /** \brief Indices to the points representing the stable clusters */ std::vector clusters_; + /** \brief Mapping from clusters to OUR-CVFH descriptors */ + std::vector cluster_axes_; }; } diff --git a/features/include/pcl/features/rops_estimation.h b/features/include/pcl/features/rops_estimation.h new file mode 100644 index 00000000..4e270958 --- /dev/null +++ b/features/include/pcl/features/rops_estimation.h @@ -0,0 +1,236 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#ifndef PCL_ROPS_ESIMATION_H_ +#define PCL_ROPS_ESIMATION_H_ + +#include +#include +#include + +namespace pcl +{ + /** \brief + * This class implements the method for extracting RoPS features presented in the article + * "Rotational Projection Statistics for 3D Local Surface Description and Object Recognition" by + * Yulan Guo, Ferdous Sohel, Mohammed Bennamoun, Min Lu and Jianwei Wan. + */ + template + class PCL_EXPORTS ROPSEstimation : public pcl::Feature + { + public: + + using Feature ::input_; + using Feature ::indices_; + using Feature ::surface_; + using Feature ::tree_; + + typedef typename pcl::Feature ::PointCloudOut PointCloudOut; + typedef typename pcl::Feature ::PointCloudIn PointCloudIn; + + public: + + /** \brief Simple constructor. */ + ROPSEstimation (); + + /** \brief Virtual destructor. */ + virtual + ~ROPSEstimation (); + + /** \brief Allows to set the number of partition bins that is used for distribution matrix calculation. + * \param[in] number_of_bins number of partition bins + */ + void + setNumberOfPartitionBins (unsigned int number_of_bins); + + /** \brief Returns the nmber of partition bins. */ + unsigned int + getNumberOfPartitionBins () const; + + /** \brief This method sets the number of rotations. + * \param[in] number_of_rotations number of rotations + */ + void + setNumberOfRotations (unsigned int number_of_rotations); + + /** \brief returns the number of rotations. */ + unsigned int + getNumberOfRotations () const; + + /** \brief Allows to set the support radius that is used to crop the local surface of the point. + * \param[in] support_radius support radius + */ + void + setSupportRadius (float support_radius); + + /** \brief Returns the support radius. */ + float + getSupportRadius () const; + + /** \brief This method sets the triangles of the mesh. + * \param[in] triangles list of triangles of the mesh + */ + void + setTriangles (const std::vector & triangles); + + /** \brief Returns the triangles of the mesh. + * \param[out] triangles triangles of tthe mesh + */ + void + getTriangles (std::vector & triangles) const; + + private: + + /** \brief Abstract feature estimation method. + * \param[out] output the resultant features + */ + virtual void + computeFeature (PointCloudOut& output); + + /** \brief This method simply builds the list of triangles for every point. + * The list of triangles for each point consists of indices of triangles it belongs to. + * The only purpose of this method is to improve perfomance of the algorithm. + */ + void + buildListOfPointsTriangles (); + + /** \brief This method crops all the triangles within the given radius of the given point. + * \param[in] point point for which the local surface is computed + * \param[out] local_triangles strores the indices of the triangles that belong to the local surface + * \param[out] local_points stores the indices of the points that belong to the local surface + */ + void + getLocalSurface (const PointInT& point, std::set & local_triangles, std::vector & local_points) const; + + /** \brief This method computes LRF (Local Reference Frame) matrix for the given point. + * \param[in] point point for which the LRF is computed + * \param[in] local_triangles list of triangles that represents the local surface of the point + * \paran[out] lrf_matrix strores computed LRF matrix for the given point + */ + void + computeLRF (const PointInT& point, const std::set & local_triangles, Eigen::Matrix3f& lrf_matrix) const; + + /** \brief This method calculates the eigen values and eigen vectors + * for the given covariance matrix. Note that it returns normalized eigen + * vectors that always form the right-handed coordinate system. + * \param[in] matrix covariance matrix of the cloud + * \param[out] major_axis eigen vector which corresponds to a major eigen value + * \param[out] middle_axis eigen vector which corresponds to a middle eigen value + * \param[out] minor_axis eigen vector which corresponds to a minor eigen value + */ + void + computeEigenVectors (const Eigen::Matrix3f& matrix, Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, + Eigen::Vector3f& minor_axis) const; + + /** \brief This method translates the cloud so that the given point becomes the origin. + * After that the cloud is rotated with the help of the given matrix. + * \param[in] point point which stores the translation information + * \param[in] matrix rotation matrix + * \param[in] local_points point to transform + * \param[out] transformed_cloud stores the transformed cloud + */ + void + transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector & local_points, PointCloudIn& transformed_cloud) const; + + /** \brief This method rotates the cloud around the given axis and computes AABB of the rotated cloud. + * \param[in] axis axis around which cloud must be rotated + * \param[in] angle angle in degrees + * \param[in] cloud cloud to rotate + * \param[out] rotated_cloud stores the rotated cloud + * \param[out] min stores the min point of the AABB + * \param[out] max stores the max point of the AABB + */ + void + rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, + Eigen::Vector3f& min, Eigen::Vector3f& max) const; + + /** \brief This method projects the local surface onto the XY, XZ or YZ plane + * and computes the distribution matrix. + * \param[in] projection represents the case of projection. 1 - XY, 2 - XZ, 3 - YZ + * \param[in] min min point of the AABB + * \param[in] max max point of the AABB + * \param[in] cloud cloud containing the points of the local surface + * \param[out] matrix stores computed distribution matrix + */ + void + getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const; + + /** \brief This method computes the set ofcentral moments for the given matrix. + * \param[in] matrix input matrix + * \param[out] moments set of computed moments + */ + void + computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector & moments) const; + + private: + + /** \brief Stores the number of partition bins that is used for distribution matrix calculation. */ + unsigned int number_of_bins_; + + /** \brief Stores number of rotations. Central moments are calculated for every rotation. */ + unsigned int number_of_rotations_; + + /** \brief Support radius that is used to crop the local surface of the point. */ + float support_radius_; + + /** \brief Stores the squared support radius. Used to improve performance. */ + float sqr_support_radius_; + + /** \brief Stores the angle step. Step is calculated with respect to number of rotations. */ + float step_; + + /** \brief Stores the set of triangles reprsenting the mesh. */ + std::vector triangles_; + + /** \brief Stores the set of triangles for each point. Its purpose is to improve perfomance. */ + std::vector > triangles_of_the_point_; + + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#define PCL_INSTANTIATE_ROPSEstimation(InT, OutT) template class pcl::ROPSEstimation; + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif diff --git a/features/include/pcl/features/shot.h b/features/include/pcl/features/shot.h index 9828c7b6..c6a71f05 100644 --- a/features/include/pcl/features/shot.h +++ b/features/include/pcl/features/shot.h @@ -160,7 +160,6 @@ namespace pcl /** \brief Create a binned distance shape histogram * \param[in] index the index of the point in indices_ * \param[in] indices the k-neighborhood point indices in surface_ - * \param[in] sqr_dists the k-neighborhood point distances in surface_ * \param[out] bin_distance_shape the resultant histogram */ void diff --git a/features/include/pcl/features/shot_lrf.h b/features/include/pcl/features/shot_lrf.h index dfcaa91d..ecc1f2ce 100644 --- a/features/include/pcl/features/shot_lrf.h +++ b/features/include/pcl/features/shot_lrf.h @@ -91,11 +91,7 @@ namespace pcl typedef typename Feature::PointCloudOut PointCloudOut; /** \brief Computes disambiguated local RF for a point index - * \param[in] cloud input point cloud - * \param[in] search_radius the neighborhood radius - * \param[in] central_point the point from the input_ cloud at which the local RF is computed - * \param[in] indices the neighbours indices - * \param[in] dists the squared distances to the neighbours + * \param[in] index the index * \param[out] rf reference frame to compute */ float diff --git a/features/src/cppf.cpp b/features/src/cppf.cpp new file mode 100755 index 00000000..ddac9966 --- /dev/null +++ b/features/src/cppf.cpp @@ -0,0 +1,128 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2013, Martin Szarski + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// + + inline void + RGBtoHSV (const Eigen::Vector4i &in, + Eigen::Vector4f &out) + { + const unsigned char max = std::max (in[0], std::max (in[1], in[2])); + const unsigned char min = std::min (in[0], std::min (in[1], in[2])); + + out[2] = static_cast (max) / 255.f; + + if (max == 0) // division by zero + { + out[1] = 0.f; + out[0] = 0.f; // h = -1.f; + return; + } + + const float diff = static_cast (max - min); + out[1] = diff / static_cast (max); + + if (min == max) // diff == 0 -> division by zero + { + out[0] = 0; + return; + } + + if (max == in[0]) out[0] = 60.f * ( static_cast (in[1] - in[2]) / diff); + else if (max == in[1]) out[0] = 60.f * (2.f + static_cast (in[2] - in[0]) / diff); + else out[0] = 60.f * (4.f + static_cast (in[0] - in[1]) / diff); // max == b + + if (out[0] < 0.f) out[0] += 360.f; + } + +bool +pcl::computeCPPFPairFeature (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &c1, + const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &c2, + float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7, float &f8, float &f9, float &f10) +{ + Eigen::Vector4f delta = p2 - p1; + delta[3] = 0.0f; + // f4 = ||delta|| + f4 = delta.norm (); + + delta /= f4; + + // f1 = n1 dot delta + f1 = n1[0] * delta[0] + n1[1] * delta[1] + n1[2] * delta[2]; + // f2 = n2 dot delta + f2 = n2[0] * delta[0] + n2[1] * delta[1] + n2[2] * delta[2]; + // f3 = n1 dot n2 + f3 = n1[0] * n2[0] + n1[1] * n2[1] + n1[2] * n2[2]; + + // f5-f7 is hsv component of p1 + // f8-f10 is hsv component of p2 + Eigen::Vector4f hsv1; + Eigen::Vector4f hsv2; + + RGBtoHSV (c1,hsv1); + RGBtoHSV (c2,hsv2); + + f5 = hsv1[0] / 360.0; //normalise to [0-1] + f6 = hsv1[1]; + f7 = hsv1[2]; + + f8 = hsv2[0] / 360.0; //normalise to [0-1] + f9 = hsv2[1]; + f10 = hsv2[2]; + + return (true); +} + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE_PRODUCT(CPPFEstimation, ((pcl::PointXYZRGBA) (pcl::PointXYZRGBNormal)) + ((pcl::Normal) (pcl::PointNormal) (pcl::PointXYZRGBNormal)) + ((pcl::CPPFSignature))) +#else + PCL_INSTANTIATE_PRODUCT(CPPFEstimation, ((pcl::PointXYZRGBA) (pcl::PointXYZRGBNormal)) + ((pcl::Normal) (pcl::PointNormal) (pcl::PointXYZRGBNormal)) + ((pcl::CPPFSignature))) +#endif +#endif // PCL_NO_PRECOMPILE + diff --git a/features/src/moment_of_inertia_estimation.cpp b/features/src/moment_of_inertia_estimation.cpp new file mode 100644 index 00000000..2d174fc2 --- /dev/null +++ b/features/src/moment_of_inertia_estimation.cpp @@ -0,0 +1,55 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#include +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE_PRODUCT(MomentOfInertiaEstimation, ((pcl::PointXYZ))) + PCL_INSTANTIATE_PRODUCT(MomentOfInertiaEstimation, ((pcl::PointXYZI))) + PCL_INSTANTIATE_PRODUCT(MomentOfInertiaEstimation, ((pcl::PointXYZRGBA))) + PCL_INSTANTIATE_PRODUCT(MomentOfInertiaEstimation, ((pcl::PointNormal))) +#else + PCL_INSTANTIATE_PRODUCT(MomentOfInertiaEstimation, (PCL_XYZ_POINT_TYPES)) +#endif +#endif // PCL_NO_PRECOMPILE diff --git a/features/src/pfh.cpp b/features/src/pfh.cpp index 3175e650..e7c87489 100644 --- a/features/src/pfh.cpp +++ b/features/src/pfh.cpp @@ -151,9 +151,9 @@ pcl::computeRGBPairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n // everything before was standard 4D-Darboux frame feature pair // now, for the experimental color stuff - f5 = (colors2[0] != 0) ? static_cast (colors1[0] / colors2[0]) : 1.0f; - f6 = (colors2[1] != 0) ? static_cast (colors1[1] / colors2[1]) : 1.0f; - f7 = (colors2[2] != 0) ? static_cast (colors1[2] / colors2[2]) : 1.0f; + f5 = (colors2[0] != 0) ? static_cast (colors1[0]) / colors2[0] : 1.0f; + f6 = (colors2[1] != 0) ? static_cast (colors1[1]) / colors2[1] : 1.0f; + f7 = (colors2[2] != 0) ? static_cast (colors1[2]) / colors2[2] : 1.0f; // make sure the ratios are in the [-1, 1] interval if (f5 > 1.0f) f5 = - 1.0f / f5; diff --git a/features/src/rops_estimation.cpp b/features/src/rops_estimation.cpp new file mode 100644 index 00000000..edfbfff1 --- /dev/null +++ b/features/src/rops_estimation.cpp @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#include +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE_PRODUCT(ROPSEstimation, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointNormal))((pcl::Histogram<135>))) +#else + PCL_INSTANTIATE_PRODUCT(ROPSEstimation, (PCL_XYZ_POINT_TYPES)((pcl::Histogram<135>))) +#endif +#endif // PCL_NO_PRECOMPILE diff --git a/features/src/shot.cpp b/features/src/shot.cpp index fd0620bf..d7f2d7e8 100644 --- a/features/src/shot.cpp +++ b/features/src/shot.cpp @@ -44,11 +44,13 @@ #include // Instantiations of specific point types #ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE_PRODUCT(SHOTEstimationBase, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::SHOT352)(pcl::SHOT1344))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTEstimation, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::SHOT352))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTColorEstimation, ((pcl::PointXYZRGB)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::SHOT1344))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTEstimationOMP, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::SHOT352))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTColorEstimationOMP, ((pcl::PointXYZRGB)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::SHOT1344))((pcl::ReferenceFrame))) #else + PCL_INSTANTIATE_PRODUCT(SHOTEstimationBase, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::SHOT352)(pcl::SHOT1344))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::SHOT352))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTColorEstimation, ((pcl::PointXYZRGBA)(pcl::PointXYZRGB)(pcl::PointXYZRGBL)(pcl::PointXYZRGBNormal))(PCL_NORMAL_POINT_TYPES)((pcl::SHOT1344))((pcl::ReferenceFrame))) PCL_INSTANTIATE_PRODUCT(SHOTEstimationOMP, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::SHOT352))((pcl::ReferenceFrame))) diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt index a9cd25ee..731ed72a 100644 --- a/filters/CMakeLists.txt +++ b/filters/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud filters library") set(SUBSYS_DEPS common sample_consensus search kdtree octree) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -36,83 +36,95 @@ if(build) src/median_filter.cpp src/voxel_grid_occlusion_estimation.cpp src/normal_refinement.cpp + src/grid_minimum.cpp + src/morphological_filter.cpp + src/local_maximum.cpp + src/model_outlier_removal.cpp ) set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/conditional_removal.h - include/pcl/${SUBSYS_NAME}/crop_box.h - include/pcl/${SUBSYS_NAME}/clipper3D.h - include/pcl/${SUBSYS_NAME}/plane_clipper3D.h - include/pcl/${SUBSYS_NAME}/box_clipper3D.h - include/pcl/${SUBSYS_NAME}/crop_hull.h - include/pcl/${SUBSYS_NAME}/extract_indices.h - include/pcl/${SUBSYS_NAME}/filter.h - include/pcl/${SUBSYS_NAME}/filter_indices.h - include/pcl/${SUBSYS_NAME}/passthrough.h - include/pcl/${SUBSYS_NAME}/shadowpoints.h - include/pcl/${SUBSYS_NAME}/project_inliers.h - include/pcl/${SUBSYS_NAME}/radius_outlier_removal.h - include/pcl/${SUBSYS_NAME}/random_sample.h - include/pcl/${SUBSYS_NAME}/normal_space.h - include/pcl/${SUBSYS_NAME}/sampling_surface_normal.h - include/pcl/${SUBSYS_NAME}/statistical_outlier_removal.h - include/pcl/${SUBSYS_NAME}/voxel_grid.h - include/pcl/${SUBSYS_NAME}/approximate_voxel_grid.h - include/pcl/${SUBSYS_NAME}/bilateral.h - include/pcl/${SUBSYS_NAME}/fast_bilateral.h - include/pcl/${SUBSYS_NAME}/fast_bilateral_omp.h - include/pcl/${SUBSYS_NAME}/voxel_grid_covariance.h - include/pcl/${SUBSYS_NAME}/convolution.h - include/pcl/${SUBSYS_NAME}/convolution_3d.h - include/pcl/${SUBSYS_NAME}/voxel_grid_label.h - include/pcl/${SUBSYS_NAME}/voxel_grid_occlusion_estimation.h - include/pcl/${SUBSYS_NAME}/frustum_culling.h - include/pcl/${SUBSYS_NAME}/covariance_sampling.h - include/pcl/${SUBSYS_NAME}/median_filter.h - include/pcl/${SUBSYS_NAME}/normal_refinement.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/conditional_removal.h" + "include/pcl/${SUBSYS_NAME}/crop_box.h" + "include/pcl/${SUBSYS_NAME}/clipper3D.h" + "include/pcl/${SUBSYS_NAME}/plane_clipper3D.h" + "include/pcl/${SUBSYS_NAME}/box_clipper3D.h" + "include/pcl/${SUBSYS_NAME}/crop_hull.h" + "include/pcl/${SUBSYS_NAME}/extract_indices.h" + "include/pcl/${SUBSYS_NAME}/filter.h" + "include/pcl/${SUBSYS_NAME}/filter_indices.h" + "include/pcl/${SUBSYS_NAME}/passthrough.h" + "include/pcl/${SUBSYS_NAME}/shadowpoints.h" + "include/pcl/${SUBSYS_NAME}/project_inliers.h" + "include/pcl/${SUBSYS_NAME}/radius_outlier_removal.h" + "include/pcl/${SUBSYS_NAME}/random_sample.h" + "include/pcl/${SUBSYS_NAME}/normal_space.h" + "include/pcl/${SUBSYS_NAME}/sampling_surface_normal.h" + "include/pcl/${SUBSYS_NAME}/statistical_outlier_removal.h" + "include/pcl/${SUBSYS_NAME}/voxel_grid.h" + "include/pcl/${SUBSYS_NAME}/approximate_voxel_grid.h" + "include/pcl/${SUBSYS_NAME}/bilateral.h" + "include/pcl/${SUBSYS_NAME}/fast_bilateral.h" + "include/pcl/${SUBSYS_NAME}/fast_bilateral_omp.h" + "include/pcl/${SUBSYS_NAME}/voxel_grid_covariance.h" + "include/pcl/${SUBSYS_NAME}/convolution.h" + "include/pcl/${SUBSYS_NAME}/convolution_3d.h" + "include/pcl/${SUBSYS_NAME}/voxel_grid_label.h" + "include/pcl/${SUBSYS_NAME}/voxel_grid_occlusion_estimation.h" + "include/pcl/${SUBSYS_NAME}/frustum_culling.h" + "include/pcl/${SUBSYS_NAME}/covariance_sampling.h" + "include/pcl/${SUBSYS_NAME}/median_filter.h" + "include/pcl/${SUBSYS_NAME}/normal_refinement.h" + "include/pcl/${SUBSYS_NAME}/grid_minimum.h" + "include/pcl/${SUBSYS_NAME}/morphological_filter.h" + "include/pcl/${SUBSYS_NAME}/local_maximum.h" + "include/pcl/${SUBSYS_NAME}/model_outlier_removal.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/conditional_removal.hpp - include/pcl/${SUBSYS_NAME}/impl/crop_box.hpp - include/pcl/${SUBSYS_NAME}/impl/crop_hull.hpp - include/pcl/${SUBSYS_NAME}/impl/plane_clipper3D.hpp - include/pcl/${SUBSYS_NAME}/impl/box_clipper3D.hpp - include/pcl/${SUBSYS_NAME}/impl/extract_indices.hpp - include/pcl/${SUBSYS_NAME}/impl/filter.hpp - include/pcl/${SUBSYS_NAME}/impl/filter_indices.hpp - include/pcl/${SUBSYS_NAME}/impl/passthrough.hpp - include/pcl/${SUBSYS_NAME}/impl/shadowpoints.hpp - include/pcl/${SUBSYS_NAME}/impl/project_inliers.hpp - include/pcl/${SUBSYS_NAME}/impl/radius_outlier_removal.hpp - include/pcl/${SUBSYS_NAME}/impl/random_sample.hpp - include/pcl/${SUBSYS_NAME}/impl/normal_space.hpp - include/pcl/${SUBSYS_NAME}/impl/sampling_surface_normal.hpp - include/pcl/${SUBSYS_NAME}/impl/statistical_outlier_removal.hpp - include/pcl/${SUBSYS_NAME}/impl/voxel_grid.hpp - include/pcl/${SUBSYS_NAME}/impl/approximate_voxel_grid.hpp - include/pcl/${SUBSYS_NAME}/impl/bilateral.hpp - include/pcl/${SUBSYS_NAME}/impl/fast_bilateral.hpp - include/pcl/${SUBSYS_NAME}/impl/fast_bilateral_omp.hpp - include/pcl/${SUBSYS_NAME}/impl/voxel_grid_covariance.hpp - include/pcl/${SUBSYS_NAME}/impl/convolution.hpp - include/pcl/${SUBSYS_NAME}/impl/convolution_3d.hpp - include/pcl/${SUBSYS_NAME}/impl/voxel_grid_occlusion_estimation.hpp - include/pcl/${SUBSYS_NAME}/impl/frustum_culling.hpp - include/pcl/${SUBSYS_NAME}/impl/covariance_sampling.hpp - include/pcl/${SUBSYS_NAME}/impl/median_filter.hpp - include/pcl/${SUBSYS_NAME}/impl/normal_refinement.hpp + "include/pcl/${SUBSYS_NAME}/impl/conditional_removal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/crop_box.hpp" + "include/pcl/${SUBSYS_NAME}/impl/crop_hull.hpp" + "include/pcl/${SUBSYS_NAME}/impl/plane_clipper3D.hpp" + "include/pcl/${SUBSYS_NAME}/impl/box_clipper3D.hpp" + "include/pcl/${SUBSYS_NAME}/impl/extract_indices.hpp" + "include/pcl/${SUBSYS_NAME}/impl/filter.hpp" + "include/pcl/${SUBSYS_NAME}/impl/filter_indices.hpp" + "include/pcl/${SUBSYS_NAME}/impl/passthrough.hpp" + "include/pcl/${SUBSYS_NAME}/impl/shadowpoints.hpp" + "include/pcl/${SUBSYS_NAME}/impl/project_inliers.hpp" + "include/pcl/${SUBSYS_NAME}/impl/radius_outlier_removal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/random_sample.hpp" + "include/pcl/${SUBSYS_NAME}/impl/normal_space.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sampling_surface_normal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/statistical_outlier_removal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/voxel_grid.hpp" + "include/pcl/${SUBSYS_NAME}/impl/approximate_voxel_grid.hpp" + "include/pcl/${SUBSYS_NAME}/impl/bilateral.hpp" + "include/pcl/${SUBSYS_NAME}/impl/fast_bilateral.hpp" + "include/pcl/${SUBSYS_NAME}/impl/fast_bilateral_omp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/voxel_grid_covariance.hpp" + "include/pcl/${SUBSYS_NAME}/impl/convolution.hpp" + "include/pcl/${SUBSYS_NAME}/impl/convolution_3d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/voxel_grid_occlusion_estimation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/frustum_culling.hpp" + "include/pcl/${SUBSYS_NAME}/impl/covariance_sampling.hpp" + "include/pcl/${SUBSYS_NAME}/impl/median_filter.hpp" + "include/pcl/${SUBSYS_NAME}/impl/normal_refinement.hpp" + "include/pcl/${SUBSYS_NAME}/impl/grid_minimum.hpp" + "include/pcl/${SUBSYS_NAME}/impl/morphological_filter.hpp" + "include/pcl/${SUBSYS_NAME}/impl/local_maximum.hpp" + "include/pcl/${SUBSYS_NAME}/impl/model_outlier_removal.hpp" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_common pcl_sample_consensus pcl_search pcl_kdtree pcl_octree) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_common pcl_sample_consensus pcl_search pcl_kdtree pcl_octree) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/filters/include/pcl/filters/box_clipper3D.h b/filters/include/pcl/filters/box_clipper3D.h index 58429f2c..b133d36e 100644 --- a/filters/include/pcl/filters/box_clipper3D.h +++ b/filters/include/pcl/filters/box_clipper3D.h @@ -97,10 +97,10 @@ namespace pcl clipLineSegment3D (PointT& from, PointT& to) const; virtual void - clipPlanarPolygon3D (std::vector& polygon) const; + clipPlanarPolygon3D (std::vector >& polygon) const; virtual void - clipPlanarPolygon3D (const std::vector& polygon, std::vector& clipped_polygon) const; + clipPlanarPolygon3D (const std::vector >& polygon, std::vector& clipped_polygon) const; virtual void clipPointCloud3D (const pcl::PointCloud &cloud_in, std::vector& clipped, const std::vector& indices = std::vector ()) const; @@ -122,8 +122,6 @@ namespace pcl }; } -#ifdef PCL_NO_PRECOMPILE #include -#endif #endif // PCL_BOX_CLIPPER3D_H_ diff --git a/filters/include/pcl/filters/clipper3D.h b/filters/include/pcl/filters/clipper3D.h index 79dc8127..ddd9ab08 100644 --- a/filters/include/pcl/filters/clipper3D.h +++ b/filters/include/pcl/filters/clipper3D.h @@ -39,6 +39,7 @@ #define PCL_CLIPPER3D_H_ #include #include +#include namespace pcl { @@ -82,7 +83,7 @@ namespace pcl * \param[in,out] polygon the polygon in any direction (ccw or cw) but ordered, thus two neighboring points define an edge of the polygon */ virtual void - clipPlanarPolygon3D (std::vector& polygon) const = 0; + clipPlanarPolygon3D (std::vector >& polygon) const = 0; /** * \brief interface to clip a planar polygon given by an ordered list of points @@ -90,7 +91,7 @@ namespace pcl * \param[out] clipped_polygon the clipped polygon */ virtual void - clipPlanarPolygon3D (const std::vector& polygon, std::vector& clipped_polygon) const = 0; + clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const = 0; /** * \brief interface to clip a point cloud @@ -111,8 +112,4 @@ namespace pcl }; } -#ifdef PCL_NO_PRECOMPILE -#include -#endif - #endif // PCL_CLIPPER3D_H_ diff --git a/filters/include/pcl/filters/conditional_removal.h b/filters/include/pcl/filters/conditional_removal.h index fc32a74b..50f8a780 100644 --- a/filters/include/pcl/filters/conditional_removal.h +++ b/filters/include/pcl/filters/conditional_removal.h @@ -627,6 +627,8 @@ namespace pcl * being removed by the filter * \param extract_removed_indices extract filtered indices from indices vector */ + PCL_DEPRECATED ("ConditionalRemoval(ConditionBasePtr condition, bool extract_removed_indices = false) is deprecated, " + "please use the setCondition (ConditionBasePtr condition) function instead.") ConditionalRemoval (ConditionBasePtr condition, bool extract_removed_indices = false) : Filter::Filter (extract_removed_indices), capable_ (false), keep_organized_ (false), condition_ (), user_filter_value_ (std::numeric_limits::quiet_NaN ()) @@ -682,8 +684,6 @@ namespace pcl void applyFilter (PointCloud &output); - typedef typename pcl::traits::fieldList::type FieldList; - /** \brief True if capable. */ bool capable_; diff --git a/filters/include/pcl/filters/convolution.h b/filters/include/pcl/filters/convolution.h index ddb830e1..d96dea68 100644 --- a/filters/include/pcl/filters/convolution.h +++ b/filters/include/pcl/filters/convolution.h @@ -117,7 +117,7 @@ namespace pcl * In 3D the next point in (u,v) coordinate can be really far so a distance * threshold is used to keep us from ghost points. * The value you set here is strongly related to the sensor. A good value for - * kinect data is 0.001 \default is std::numeric::infinity () + * kinect data is 0.001. Default is std::numeric::infinity () * \param[in] threshold maximum allowed distance between 2 juxtaposed points */ inline void @@ -131,7 +131,6 @@ namespace pcl inline void setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } /** Convolve a float image rows by a given kernel. - * \param[in] kernel convolution kernel * \param[out] output the convolved cloud * \note if output doesn't fit in input i.e. output.rows () < input.rows () or * output.cols () < input.cols () then output is resized to input sizes. @@ -139,7 +138,6 @@ namespace pcl inline void convolveRows (PointCloudOut& output); /** Convolve a float image columns by a given kernel. - * \param[in] kernel convolution kernel * \param[out] output the convolved image * \note if output doesn't fit in input i.e. output.rows () < input.rows () or * output.cols () < input.cols () then output is resized to input sizes. @@ -184,7 +182,7 @@ namespace pcl void convolve_cols_duplicate (PointCloudOut& output); /** init compute is an internal method called before computation - * \param[in] kernel convolution kernel to be used + * \param[in] output * \throw pcl::InitFailedException */ void diff --git a/filters/include/pcl/filters/convolution_3d.h b/filters/include/pcl/filters/convolution_3d.h index 65392489..1931673d 100644 --- a/filters/include/pcl/filters/convolution_3d.h +++ b/filters/include/pcl/filters/convolution_3d.h @@ -94,7 +94,7 @@ namespace pcl initCompute () { return false; } /** \brief Utility function that annihilates a point making it fail the \ref pcl::isFinite test - * \param[in/out] p point to annihilate + * \param p point to annihilate */ static void makeInfinite (PointOutT& p) diff --git a/filters/include/pcl/filters/covariance_sampling.h b/filters/include/pcl/filters/covariance_sampling.h index 4f8e5633..6d41efc5 100644 --- a/filters/include/pcl/filters/covariance_sampling.h +++ b/filters/include/pcl/filters/covariance_sampling.h @@ -81,7 +81,7 @@ namespace pcl { filter_name_ = "CovarianceSampling"; } /** \brief Set number of indices to be sampled. - * \param[in] sample the number of sample indices + * \param[in] samples the number of sample indices */ inline void setNumberOfSamples (unsigned int samples) diff --git a/filters/include/pcl/filters/filter_indices.h b/filters/include/pcl/filters/filter_indices.h index efcd219c..5bba9e8f 100644 --- a/filters/include/pcl/filters/filter_indices.h +++ b/filters/include/pcl/filters/filter_indices.h @@ -169,6 +169,7 @@ namespace pcl } protected: + using Filter::initCompute; using Filter::deinitCompute; @@ -184,6 +185,10 @@ namespace pcl /** \brief Abstract filter method for point cloud indices. */ virtual void applyFilter (std::vector &indices) = 0; + + /** \brief Abstract filter method for point cloud. */ + virtual void + applyFilter (PointCloud &output) = 0; }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -280,6 +285,7 @@ namespace pcl } protected: + /** \brief False = normal filter behavior (default), true = inverted behavior. */ bool negative_; @@ -292,6 +298,10 @@ namespace pcl /** \brief Abstract filter method for point cloud indices. */ virtual void applyFilter (std::vector &indices) = 0; + + /** \brief Abstract filter method for point cloud. */ + virtual void + applyFilter (PCLPointCloud2 &output) = 0; }; } diff --git a/filters/include/pcl/filters/frustum_culling.h b/filters/include/pcl/filters/frustum_culling.h index 8616f40c..9069a6d0 100644 --- a/filters/include/pcl/filters/frustum_culling.h +++ b/filters/include/pcl/filters/frustum_culling.h @@ -235,4 +235,8 @@ namespace pcl }; } +#ifdef PCL_NO_PRECOMPILE +#include +#endif + #endif diff --git a/filters/include/pcl/filters/grid_minimum.h b/filters/include/pcl/filters/grid_minimum.h new file mode 100644 index 00000000..5d4a5995 --- /dev/null +++ b/filters/include/pcl/filters/grid_minimum.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_VOXEL_GRID_MINIMUM_H_ +#define PCL_FILTERS_VOXEL_GRID_MINIMUM_H_ + +#include +#include +#include + +namespace pcl +{ + /** \brief GridMinimum assembles a local 2D grid over a given PointCloud, and downsamples the data. + * + * The GridMinimum class creates a *2D grid* over the input point cloud + * data. Then, in each *cell* (i.e., 2D grid element), all the points + * present will be *downsampled* with the minimum z value. This grid minimum + * can be useful in a number of topographic processing tasks such as crudely + * estimating ground returns, especially under foliage. + * + * \author Bradley J Chambers + * \ingroup filters + */ + template + class GridMinimum: public FilterIndices + { + protected: + using Filter::filter_name_; + using Filter::getClassName; + using Filter::input_; + using Filter::indices_; + + typedef typename FilterIndices::PointCloud PointCloud; + + public: + /** \brief Empty constructor. */ + GridMinimum (const float resolution) + { + setResolution (resolution); + filter_name_ = "GridMinimum"; + } + + /** \brief Destructor. */ + virtual ~GridMinimum () + { + } + + /** \brief Set the grid resolution. + * \param[in] resolution the grid resolution + */ + inline void + setResolution (const float resolution) + { + resolution_ = resolution; + // Use multiplications instead of divisions + inverse_resolution_ = 1.0f / resolution_; + } + + /** \brief Get the grid resolution. */ + inline float + getResolution () { return (resolution_); } + + protected: + /** \brief The resolution. */ + float resolution_; + + /** \brief Internal resolution stored as 1/resolution_ for efficiency reasons. */ + float inverse_resolution_; + + /** \brief Downsample a Point Cloud using a 2D grid approach + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output); + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_FILTERS_VOXEL_GRID_MINIMUM_H_ + diff --git a/filters/include/pcl/filters/impl/box_clipper3D.hpp b/filters/include/pcl/filters/impl/box_clipper3D.hpp index cb8be328..9f0bb24e 100644 --- a/filters/include/pcl/filters/impl/box_clipper3D.hpp +++ b/filters/include/pcl/filters/impl/box_clipper3D.hpp @@ -183,7 +183,7 @@ pcl::BoxClipper3D::clipLineSegment3D (PointT& point1, PointT& point2) co * @attention untested code */ template void -pcl::BoxClipper3D::clipPlanarPolygon3D (const std::vector& polygon, std::vector& clipped_polygon) const +pcl::BoxClipper3D::clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const { // not implemented -> clip everything clipped_polygon.clear (); @@ -194,7 +194,7 @@ pcl::BoxClipper3D::clipPlanarPolygon3D (const std::vector& polyg * @attention untested code */ template void -pcl::BoxClipper3D::clipPlanarPolygon3D (std::vector& polygon) const +pcl::BoxClipper3D::clipPlanarPolygon3D (std::vector >& polygon) const { // not implemented -> clip everything polygon.clear (); diff --git a/filters/include/pcl/filters/impl/conditional_removal.hpp b/filters/include/pcl/filters/impl/conditional_removal.hpp index cae746b1..9a20beff 100644 --- a/filters/include/pcl/filters/impl/conditional_removal.hpp +++ b/filters/include/pcl/filters/impl/conditional_removal.hpp @@ -39,6 +39,7 @@ #define PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_ #include +#include #include ////////////////////////////////////////////////////////////////////////// @@ -725,10 +726,7 @@ pcl::ConditionalRemoval::applyFilter (PointCloud &output) if (condition_->evaluate (input_->points[(*Filter < PointT > ::indices_)[cp]])) { - pcl::for_each_type ( - pcl::NdConcatenateFunctor ( - input_->points[(*Filter < PointT > ::indices_)[cp]], - output.points[nr_p])); + copyPoint (input_->points[(*Filter < PointT > ::indices_)[cp]], output.points[nr_p]); nr_p++; } else @@ -762,8 +760,8 @@ pcl::ConditionalRemoval::applyFilter (PointCloud &output) } // copy all the fields - pcl::for_each_type (pcl::NdConcatenateFunctor (input_->points[cp], - output.points[cp])); + copyPoint (input_->points[cp], output.points[cp]); + if (!condition_->evaluate (input_->points[cp])) { output.points[cp].getVector4fMap ().setConstant (user_filter_value_); diff --git a/filters/include/pcl/filters/impl/convolution.hpp b/filters/include/pcl/filters/impl/convolution.hpp index 509d30ff..bc13b3a4 100644 --- a/filters/include/pcl/filters/impl/convolution.hpp +++ b/filters/include/pcl/filters/impl/convolution.hpp @@ -412,7 +412,7 @@ pcl::filters::Convolution::convolve_rows (PointCloudOut& outp if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int j = 0; j < height; ++j) { @@ -429,7 +429,7 @@ pcl::filters::Convolution::convolve_rows (PointCloudOut& outp else { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int j = 0; j < height; ++j) { @@ -457,7 +457,7 @@ pcl::filters::Convolution::convolve_rows_duplicate (PointClou if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int j = 0; j < height; ++j) { @@ -474,7 +474,7 @@ pcl::filters::Convolution::convolve_rows_duplicate (PointClou else { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int j = 0; j < height; ++j) { @@ -502,7 +502,7 @@ pcl::filters::Convolution::convolve_rows_mirror (PointCloudOu if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int j = 0; j < height; ++j) { @@ -519,7 +519,7 @@ pcl::filters::Convolution::convolve_rows_mirror (PointCloudOu else { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int j = 0; j < height; ++j) { @@ -546,7 +546,7 @@ pcl::filters::Convolution::convolve_cols (PointCloudOut& outp if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int i = 0; i < width; ++i) { @@ -563,7 +563,7 @@ pcl::filters::Convolution::convolve_cols (PointCloudOut& outp else { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int i = 0; i < width; ++i) { @@ -591,7 +591,7 @@ pcl::filters::Convolution::convolve_cols_duplicate (PointClou if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int i = 0; i < width; ++i) { @@ -608,7 +608,7 @@ pcl::filters::Convolution::convolve_cols_duplicate (PointClou else { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int i = 0; i < width; ++i) { @@ -636,7 +636,7 @@ pcl::filters::Convolution::convolve_cols_mirror (PointCloudOu if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int i = 0; i < width; ++i) { @@ -653,7 +653,7 @@ pcl::filters::Convolution::convolve_cols_mirror (PointCloudOu else { #ifdef _OPENMP -#pragma omp parallel for shared (output, last) num_threads (threads_) +#pragma omp parallel for shared (output) num_threads (threads_) #endif for(int i = 0; i < width; ++i) { diff --git a/filters/include/pcl/filters/impl/covariance_sampling.hpp b/filters/include/pcl/filters/impl/covariance_sampling.hpp index 89449ca6..a5285d6d 100644 --- a/filters/include/pcl/filters/impl/covariance_sampling.hpp +++ b/filters/include/pcl/filters/impl/covariance_sampling.hpp @@ -54,7 +54,7 @@ pcl::CovarianceSampling::initCompute () if (num_samples_ > indices_->size ()) { - PCL_ERROR ("[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%zu)\n", + PCL_ERROR ("[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%lu)\n", num_samples_, indices_->size ()); return false; } @@ -93,7 +93,7 @@ pcl::CovarianceSampling::computeConditionNumber () Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues (); - double max_ev = std::numeric_limits::min (), + double max_ev = -std::numeric_limits::max (), min_ev = std::numeric_limits::max (); for (size_t i = 0; i < 6; ++i) { @@ -117,7 +117,7 @@ pcl::CovarianceSampling::computeConditionNumber (const Eigen::M Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues (); - double max_ev = std::numeric_limits::min (), + double max_ev = -std::numeric_limits::max (), min_ev = std::numeric_limits::max (); for (size_t i = 0; i < 6; ++i) { diff --git a/filters/include/pcl/filters/impl/grid_minimum.hpp b/filters/include/pcl/filters/impl/grid_minimum.hpp new file mode 100644 index 00000000..a8b8bf59 --- /dev/null +++ b/filters/include/pcl/filters/impl/grid_minimum.hpp @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ +#define PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ + +#include +#include +#include + +struct point_index_idx +{ + unsigned int idx; + unsigned int cloud_point_index; + + point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} + bool operator < (const point_index_idx &p) const { return (idx < p.idx); } +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridMinimum::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + std::vector indices; + + output.is_dense = true; + applyFilterIndices (indices); + pcl::copyPointCloud (*input_, indices, output); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridMinimum::applyFilterIndices (std::vector &indices) +{ + indices.resize (indices_->size ()); + int oii = 0; + + // Get the minimum and maximum dimensions + Eigen::Vector4f min_p, max_p; + getMinMax3D (*input_, *indices_, min_p, max_p); + + // Check that the resolution is not too small, given the size of the data + int64_t dx = static_cast ((max_p[0] - min_p[0]) * inverse_resolution_)+1; + int64_t dy = static_cast ((max_p[1] - min_p[1]) * inverse_resolution_)+1; + + if ((dx*dy) > static_cast (std::numeric_limits::max ())) + { + PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName ().c_str ()); + return; + } + + Eigen::Vector4i min_b, max_b, div_b, divb_mul; + + // Compute the minimum and maximum bounding box values + min_b[0] = static_cast (floor (min_p[0] * inverse_resolution_)); + max_b[0] = static_cast (floor (max_p[0] * inverse_resolution_)); + min_b[1] = static_cast (floor (min_p[1] * inverse_resolution_)); + max_b[1] = static_cast (floor (max_p[1] * inverse_resolution_)); + + // Compute the number of divisions needed along all axis + div_b = max_b - min_b + Eigen::Vector4i::Ones (); + div_b[3] = 0; + + // Set up the division multiplier + divb_mul = Eigen::Vector4i (1, div_b[0], 0, 0); + + std::vector index_vector; + index_vector.reserve (indices_->size ()); + + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!pcl_isfinite (input_->points[*it].x) || + !pcl_isfinite (input_->points[*it].y) || + !pcl_isfinite (input_->points[*it].z)) + continue; + + int ijk0 = static_cast (floor (input_->points[*it].x * inverse_resolution_) - static_cast (min_b[0])); + int ijk1 = static_cast (floor (input_->points[*it].y * inverse_resolution_) - static_cast (min_b[1])); + + // Compute the grid cell index + int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1]; + index_vector.push_back (point_index_idx (static_cast (idx), *it)); + } + + // Second pass: sort the index_vector vector using value representing target cell as index + // in effect all points belonging to the same output cell will be next to each other + std::sort (index_vector.begin (), index_vector.end (), std::less ()); + + // Third pass: count output cells + // we need to skip all the same, adjacenent idx values + unsigned int total = 0; + unsigned int index = 0; + + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector > first_and_last_indices_vector; + + // Worst case size + first_and_last_indices_vector.reserve (index_vector.size ()); + while (index < index_vector.size ()) + { + unsigned int i = index + 1; + while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) + ++i; + ++total; + first_and_last_indices_vector.push_back (std::pair (index, i)); + index = i; + } + + // Fourth pass: locate grid minimums + indices.resize (total); + + index = 0; + + for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp) + { + unsigned int first_index = first_and_last_indices_vector[cp].first; + unsigned int last_index = first_and_last_indices_vector[cp].second; + unsigned int min_index = index_vector[first_index].cloud_point_index; + float min_z = input_->points[index_vector[first_index].cloud_point_index].z; + + for (unsigned int i = first_index + 1; i < last_index; ++i) + { + if (input_->points[index_vector[i].cloud_point_index].z < min_z) + { + min_z = input_->points[index_vector[i].cloud_point_index].z; + min_index = index_vector[i].cloud_point_index; + } + } + + indices[index] = min_index; + + ++index; + } + + oii = indices.size (); + + // Resize the output arrays + indices.resize (oii); +} + +#define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum; + +#endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ + diff --git a/filters/include/pcl/filters/impl/local_maximum.hpp b/filters/include/pcl/filters/impl/local_maximum.hpp new file mode 100644 index 00000000..c58953dc --- /dev/null +++ b/filters/include/pcl/filters/impl/local_maximum.hpp @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ +#define PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LocalMaximum::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + std::vector indices; + + output.is_dense = true; + applyFilterIndices (indices); + pcl::copyPointCloud (*input_, indices, output); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LocalMaximum::applyFilterIndices (std::vector &indices) +{ + typename PointCloud::Ptr cloud_projected (new PointCloud); + + // Create a set of planar coefficients with X=Y=0,Z=1 + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); + coefficients->values.resize (4); + coefficients->values[0] = coefficients->values[1] = 0; + coefficients->values[2] = 1.0; + coefficients->values[3] = 0; + + // Create the filtering object and project input into xy plane + pcl::ProjectInliers proj; + proj.setModelType (pcl::SACMODEL_PLANE); + proj.setInputCloud (input_); + proj.setModelCoefficients (coefficients); + proj.filter (*cloud_projected); + + // Initialize the search class + if (!searcher_) + { + if (input_->isOrganized ()) + searcher_.reset (new pcl::search::OrganizedNeighbor ()); + else + searcher_.reset (new pcl::search::KdTree (false)); + } + searcher_->setInputCloud (cloud_projected); + + // The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + + std::vector point_is_max (indices_->size (), false); + std::vector point_is_visited (indices_->size (), false); + + // Find all points within xy radius (i.e., a vertical cylinder) of the query + // point, removing those that are locally maximal (i.e., highest z within the + // cylinder) + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) + { + if (!isFinite (input_->points[(*indices_)[iii]])) + { + continue; + } + + // Points in the neighborhood of a previously identified local max, will + // not be maximal in their own neighborhood + if (point_is_visited[(*indices_)[iii]] && !point_is_max[(*indices_)[iii]]) + { + continue; + } + + // Assume the current query point is the maximum, mark as visited + point_is_max[(*indices_)[iii]] = true; + point_is_visited[(*indices_)[iii]] = true; + + // Perform the radius search in the projected cloud + std::vector radius_indices; + std::vector radius_dists; + PointT p = cloud_projected->points[(*indices_)[iii]]; + if (searcher_->radiusSearch (p, radius_, radius_indices, radius_dists) == 0) + { + PCL_WARN ("[pcl::%s::applyFilter] Searching for neighbors within radius %f failed.\n", getClassName ().c_str (), radius_); + continue; + } + + // If query point is alone, we retain it regardless + if (radius_indices.size () == 1) + { + point_is_max[(*indices_)[iii]] = false; + } + + // Check to see if a neighbor is higher than the query point + float query_z = input_->points[(*indices_)[iii]].z; + for (size_t k = 1; k < radius_indices.size (); ++k) // k = 1 is the first neighbor + { + if (input_->points[radius_indices[k]].z > query_z) + { + // Query point is not the local max, no need to check others + point_is_max[(*indices_)[iii]] = false; + break; + } + } + + // If the query point was a local max, all neighbors can be marked as + // visited, excluding them from future consideration as local maxima + if (point_is_max[(*indices_)[iii]]) + { + for (size_t k = 1; k < radius_indices.size (); ++k) // k = 1 is the first neighbor + { + point_is_visited[radius_indices[k]] = true; + } + } + + // Points that are local maxima are passed to removed indices + // Unless negative was set, then it's the opposite condition + if ((!negative_ && point_is_max[(*indices_)[iii]]) || (negative_ && !point_is_max[(*indices_)[iii]])) + { + if (extract_removed_indices_) + { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); +} + +#define PCL_INSTANTIATE_LocalMaximum(T) template class PCL_EXPORTS pcl::LocalMaximum; + +#endif // PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ + diff --git a/filters/include/pcl/filters/impl/median_filter.hpp b/filters/include/pcl/filters/impl/median_filter.hpp index 65dcfa57..4d8a27ce 100644 --- a/filters/include/pcl/filters/impl/median_filter.hpp +++ b/filters/include/pcl/filters/impl/median_filter.hpp @@ -55,8 +55,10 @@ pcl::MedianFilter::applyFilter (PointCloud &output) // Copy everything from the input cloud to the output cloud (takes care of all the fields) copyPointCloud (*input_, output); - for (int y = 0; y < output.height; ++y) - for (int x = 0; x < output.width; ++x) + int height = static_cast (output.height); + int width = static_cast (output.width); + for (int y = 0; y < height; ++y) + for (int x = 0; x < width; ++x) if (pcl::isFinite ((*input_)(x, y))) { std::vector vals; @@ -65,8 +67,8 @@ pcl::MedianFilter::applyFilter (PointCloud &output) for (int y_dev = -window_size_/2; y_dev <= window_size_/2; ++y_dev) for (int x_dev = -window_size_/2; x_dev <= window_size_/2; ++x_dev) { - if (x + x_dev >= 0 && x + x_dev < output.width && - y + y_dev >= 0 && y + y_dev < output.height && + if (x + x_dev >= 0 && x + x_dev < width && + y + y_dev >= 0 && y + y_dev < height && pcl::isFinite ((*input_)(x+x_dev, y+y_dev))) vals.push_back ((*input_)(x+x_dev, y+y_dev).z); } diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp new file mode 100644 index 00000000..e7e9ff69 --- /dev/null +++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp @@ -0,0 +1,265 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ +#define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ModelOutlierRemoval::initSACModel (pcl::SacModel model_type) +{ + // Build the model + switch (model_type) + { + case SACMODEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPlane (input_)); + break; + } + case SACMODEL_LINE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelLine (input_)); + break; + } + case SACMODEL_CIRCLE2D: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCircle2D (input_)); + break; + } + case SACMODEL_SPHERE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelSphere (input_)); + break; + } + case SACMODEL_PARALLEL_LINE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelLine (input_)); + break; + } + case SACMODEL_PERPENDICULAR_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPerpendicularPlane (input_)); + break; + } + case SACMODEL_CYLINDER: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCylinder (input_)); + break; + } + case SACMODEL_NORMAL_PLANE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalPlane (input_)); + break; + } + case SACMODEL_CONE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCone (input_)); + break; + } + case SACMODEL_NORMAL_SPHERE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalSphere (input_)); + break; + } + case SACMODEL_NORMAL_PARALLEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalParallelPlane (input_)); + break; + } + case SACMODEL_PARALLEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelPlane (input_)); + break; + } + default: + { + PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); + return (false); + } + } + return (true); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ModelOutlierRemoval::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator + output.points[ (*removed_indices_)[rii]].x = output.points[ (*removed_indices_)[rii]].y = output.points[ (*removed_indices_)[rii]].z = user_filter_value_; + if (!pcl_isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) +{ + //The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + //is the filtersetup correct? + bool valid_setup = true; + + valid_setup &= initSACModel (model_type_); + + typedef SampleConsensusModelFromNormals SACModelFromNormals; + // Returns NULL if cast isn't possible + SACModelFromNormals *model_from_normals = dynamic_cast (& (*model_)); + + if (model_from_normals) + { + if (!cloud_normals_) + { + valid_setup = false; + PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n"); + } + else + { + model_from_normals->setNormalDistanceWeight (normals_distance_weight_); + model_from_normals->setInputNormals (cloud_normals_); + } + } + + //if the filter setup is invalid filter for nan and return; + if (!valid_setup) + { + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if (!isFinite (input_->points[ (*indices_)[iii]])) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + indices[oii++] = (*indices_)[iii]; + } + return; + } + // check distance of pointcloud to model + std::vector distances; + //TODO: get signed distances ! + model_->getDistancesToModel (model_coefficients_, distances); + bool thresh_result; + + // Filter for non-finite entries and the specified field limits + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if (!isFinite (input_->points[ (*indices_)[iii]])) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // use threshold function to seperate outliers from inliers: + thresh_result = threshold_function_ (distances[iii]); + + // in normal mode: define outliers as false thresh_result + if (!negative_ && !thresh_result) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // in negative_ mode: define outliers as true thresh_result + if (negative_ && thresh_result) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); + +} + +#define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval; + +#endif // PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ diff --git a/filters/include/pcl/filters/impl/morphological_filter.hpp b/filters/include/pcl/filters/impl/morphological_filter.hpp new file mode 100644 index 00000000..59a0a965 --- /dev/null +++ b/filters/include/pcl/filters/impl/morphological_filter.hpp @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ +#define PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out) +{ + if (cloud_in->empty ()) + return; + + pcl::copyPointCloud (*cloud_in, cloud_out); + + pcl::octree::OctreePointCloudSearch tree (resolution); + + tree.setInputCloud (cloud_in); + tree.addPointsFromInputCloud (); + + float half_res = resolution / 2.0f; + + switch (morphological_operator) + { + case MORPH_DILATE: + case MORPH_ERODE: + { + for (size_t p_idx = 0; p_idx < cloud_in->points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_in->points[p_idx].x - half_res; + float miny = cloud_in->points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_in->points[p_idx].x + half_res; + float maxy = cloud_in->points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (pt_indices.size () > 0) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (*cloud_in, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_DILATE: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + case MORPH_ERODE: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + } + } + } + break; + } + case MORPH_OPEN: + case MORPH_CLOSE: + { + pcl::PointCloud cloud_temp; + + pcl::copyPointCloud (*cloud_in, cloud_temp); + + for (size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_temp.points[p_idx].x - half_res; + float miny = cloud_temp.points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_temp.points[p_idx].x + half_res; + float maxy = cloud_temp.points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (pt_indices.size () > 0) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (cloud_temp, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_OPEN: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + case MORPH_CLOSE: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + } + } + } + + cloud_temp.swap (cloud_out); + + for (size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_temp.points[p_idx].x - half_res; + float miny = cloud_temp.points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_temp.points[p_idx].x + half_res; + float maxy = cloud_temp.points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (pt_indices.size () > 0) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (cloud_temp, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_OPEN: + default: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + case MORPH_CLOSE: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + } + } + } + break; + } + default: + { + PCL_ERROR ("Morphological operator is not supported!\n"); + break; + } + } + + return; +} + +#define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator (const pcl::PointCloud::ConstPtr &, float, const int, pcl::PointCloud &); + +#endif //#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ + diff --git a/filters/include/pcl/filters/impl/normal_space.hpp b/filters/include/pcl/filters/impl/normal_space.hpp index d623f24f..e3809d70 100644 --- a/filters/include/pcl/filters/impl/normal_space.hpp +++ b/filters/include/pcl/filters/impl/normal_space.hpp @@ -54,7 +54,7 @@ pcl::NormalSpaceSampling::initCompute () // If sample size is 0 or if the sample size is greater then input cloud size then return entire copy of cloud if (sample_ >= input_->size ()) { - PCL_ERROR ("[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %zu\n", + PCL_ERROR ("[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %lu\n", sample_, input_->size ()); return false; } diff --git a/filters/include/pcl/filters/impl/passthrough.hpp b/filters/include/pcl/filters/impl/passthrough.hpp index df8a5b52..e2e0a41f 100644 --- a/filters/include/pcl/filters/impl/passthrough.hpp +++ b/filters/include/pcl/filters/impl/passthrough.hpp @@ -144,7 +144,7 @@ pcl::PassThrough::applyFilterIndices (std::vector &indices) } // Inside of the field limits are passed to removed indices if negative was set - if (negative_ && field_value > filter_limit_min_ && field_value < filter_limit_max_) + if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[iii]; diff --git a/filters/include/pcl/filters/impl/plane_clipper3D.hpp b/filters/include/pcl/filters/impl/plane_clipper3D.hpp index a467b7f8..a242ade4 100644 --- a/filters/include/pcl/filters/impl/plane_clipper3D.hpp +++ b/filters/include/pcl/filters/impl/plane_clipper3D.hpp @@ -111,7 +111,7 @@ pcl::PlaneClipper3D::clipLineSegment3D (PointT& point1, PointT& point2) * @attention untested code */ template void -pcl::PlaneClipper3D::clipPlanarPolygon3D (const std::vector& polygon, std::vector& clipped_polygon) const +pcl::PlaneClipper3D::clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const { clipped_polygon.clear (); clipped_polygon.reserve (polygon.size ()); @@ -140,9 +140,9 @@ pcl::PlaneClipper3D::clipPlanarPolygon3D (const std::vector& pol if (previous_distance > 0) clipped_polygon.push_back (polygon [0]); - typename std::vector::const_iterator prev_it = polygon.begin (); + typename std::vector >::const_iterator prev_it = polygon.begin (); - for (typename std::vector::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++) + for (typename std::vector >::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++) { // if we intersect plane float distance = getDistance (*pIt); @@ -168,9 +168,9 @@ pcl::PlaneClipper3D::clipPlanarPolygon3D (const std::vector& pol * @attention untested code */ template void -pcl::PlaneClipper3D::clipPlanarPolygon3D (std::vector& polygon) const +pcl::PlaneClipper3D::clipPlanarPolygon3D (std::vector > &polygon) const { - std::vector clipped; + std::vector > clipped; clipPlanarPolygon3D (polygon, clipped); polygon = clipped; } @@ -225,4 +225,4 @@ pcl::PlaneClipper3D::clipPointCloud3D (const pcl::PointCloud& cl clipped.push_back (*iIt); } } -#endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP \ No newline at end of file +#endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP diff --git a/filters/include/pcl/filters/impl/sampling_surface_normal.hpp b/filters/include/pcl/filters/impl/sampling_surface_normal.hpp index 1d0a256b..6ea6fbd7 100644 --- a/filters/include/pcl/filters/impl/sampling_surface_normal.hpp +++ b/filters/include/pcl/filters/impl/sampling_surface_normal.hpp @@ -48,7 +48,7 @@ template void pcl::SamplingSurfaceNormal::applyFilter (PointCloud &output) { std::vector indices; - int npts = int (input_->points.size ()); + size_t npts = input_->points.size (); for (unsigned int i = 0; i < npts; i++) indices.push_back (i); @@ -125,7 +125,7 @@ pcl::SamplingSurfaceNormal::partition ( std::vector& indices, PointCloud& output) { const int count (last - first); - if (count <= sample_) + if (count <= static_cast (sample_)) { samplePartition (cloud, first, last, indices, output); return; @@ -164,7 +164,7 @@ pcl::SamplingSurfaceNormal::samplePartition ( { pcl::PointCloud cloud; - for (unsigned int i = first; i < last; i++) + for (int i = first; i < last; i++) { PointT pt; pt.x = data.points[indices[i]].x; diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index 1b2cd041..00677aa3 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -237,7 +237,7 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; - if( (dx*dy*dz) > static_cast(std::numeric_limits::max()) ) + if ((dx*dy*dz) > static_cast(std::numeric_limits::max())) { PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); output = *input_; @@ -359,12 +359,22 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) // we need to skip all the same, adjacenent idx values unsigned int total = 0; unsigned int index = 0; + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector > first_and_last_indices_vector; + // Worst case size + first_and_last_indices_vector.reserve (index_vector.size ()); while (index < index_vector.size ()) { unsigned int i = index + 1; while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) ++i; - ++total; + if (i - index >= min_points_per_voxel_) + { + ++total; + first_and_last_indices_vector.push_back (std::pair (index, i)); + } index = i; } @@ -400,14 +410,16 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size); - for (unsigned int cp = 0; cp < index_vector.size ();) + for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp) { // calculate centroid - sum values from all input points, that have the same idx value in index_vector array + unsigned int first_index = first_and_last_indices_vector[cp].first; + unsigned int last_index = first_and_last_indices_vector[cp].second; if (!downsample_all_data_) { - centroid[0] = input_->points[index_vector[cp].cloud_point_index].x; - centroid[1] = input_->points[index_vector[cp].cloud_point_index].y; - centroid[2] = input_->points[index_vector[cp].cloud_point_index].z; + centroid[0] = input_->points[index_vector[first_index].cloud_point_index].x; + centroid[1] = input_->points[index_vector[first_index].cloud_point_index].y; + centroid[2] = input_->points[index_vector[first_index].cloud_point_index].z; } else { @@ -416,16 +428,15 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) { // Fill r/g/b data, assuming that the order is BGRA pcl::RGB rgb; - memcpy (&rgb, reinterpret_cast (&input_->points[index_vector[cp].cloud_point_index]) + rgba_index, sizeof (RGB)); + memcpy (&rgb, reinterpret_cast (&input_->points[index_vector[first_index].cloud_point_index]) + rgba_index, sizeof (RGB)); centroid[centroid_size-3] = rgb.r; centroid[centroid_size-2] = rgb.g; centroid[centroid_size-1] = rgb.b; } - pcl::for_each_type (NdCopyPointEigenFunctor (input_->points[index_vector[cp].cloud_point_index], centroid)); + pcl::for_each_type (NdCopyPointEigenFunctor (input_->points[index_vector[first_index].cloud_point_index], centroid)); } - unsigned int i = cp + 1; - while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx) + for (unsigned int i = first_index + 1; i < last_index; ++i) { if (!downsample_all_data_) { @@ -448,14 +459,13 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) pcl::for_each_type (NdCopyPointEigenFunctor (input_->points[index_vector[i].cloud_point_index], temporary)); centroid += temporary; } - ++i; } // index is centroid final position in resulting PointCloud if (save_leaf_layout_) - leaf_layout_[index_vector[cp].idx] = index; + leaf_layout_[index_vector[first_index].idx] = index; - centroid /= static_cast (i - cp); + centroid /= static_cast (last_index - first_index); // store centroid // Do we need to process all the fields? @@ -477,7 +487,6 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) memcpy (reinterpret_cast (&output.points[index]) + rgba_index, &rgb, sizeof (float)); } } - cp = i; ++index; } output.width = static_cast (output.points.size ()); diff --git a/filters/include/pcl/filters/local_maximum.h b/filters/include/pcl/filters/local_maximum.h new file mode 100644 index 00000000..8a8cf28f --- /dev/null +++ b/filters/include/pcl/filters/local_maximum.h @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_LOCAL_MAXIMUM_H_ +#define PCL_FILTERS_LOCAL_MAXIMUM_H_ + +#include +#include + +namespace pcl +{ + /** \brief LocalMaximum downsamples the cloud, by eliminating points that are locally maximal. + * + * The LocalMaximum class analyzes each point and removes those that are + * found to be locally maximal with respect to their neighbors (found via + * radius search). The comparison is made in the z dimension only at this + * time. + * + * \author Bradley J Chambers + * \ingroup filters + */ + template + class LocalMaximum: public FilterIndices + { + protected: + typedef typename FilterIndices::PointCloud PointCloud; + typedef typename pcl::search::Search::Ptr SearcherPtr; + + public: + /** \brief Empty constructor. */ + LocalMaximum (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + searcher_ (), + radius_ (1) + { + filter_name_ = "LocalMaximum"; + } + + /** \brief Set the radius to use to determine if a point is the local max. + * \param[in] radius The radius to use to determine if a point is the local max. + */ + inline void + setRadius (float radius) { radius_ = radius; } + + /** \brief Get the radius to use to determine if a point is the local max. + * \return The radius to use to determine if a point is the local max. + */ + inline float + getRadius () const { return (radius_); } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Downsample a Point Cloud by eliminating points that are locally maximal in z + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output); + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + private: + /** \brief A pointer to the spatial search object. */ + SearcherPtr searcher_; + + /** \brief The radius to use to determine if a point is the local max. */ + float radius_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_FILTERS_LOCAL_MAXIMUM_H_ + diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h new file mode 100644 index 00000000..b513ffc0 --- /dev/null +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -0,0 +1,257 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_FILTERS_MODEL_OUTLIER_REMOVAL_H_ +#define PCL_FILTERS_MODEL_OUTLIER_REMOVAL_H_ + +#include +#include + +// Sample Consensus models +#include +#include + +namespace pcl +{ + /** \brief @b ModelOutlierRemoval filters points in a cloud based on the distance between model and point. + * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside + * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). + *

+ * Usage example: + * \code + * pcl::ModelCoefficients model_coeff; + * model_coeff.values.resize(4); + * model_coeff.values[0] = 0; model_coeff.values[1] = 0; model_coeff.values[2] = 1.5; model_coeff.values[3] = 0.5; + * pcl::ModelOutlierRemoval filter; + * filter.setModelCoefficients (model_coeff); + * filter.setThreshold (0.1); + * filter.setModelType (pcl::SACMODEL_PLANE); + * filter.setInputCloud (*cloud_in); + * filter.setFilterLimitsNegative (false); + * filter.filter (*cloud_out); + * \endcode + */ + template + class ModelOutlierRemoval : public FilterIndices + { + protected: + typedef typename FilterIndices::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + typedef typename SampleConsensusModel::Ptr SampleConsensusModelPtr; + + public: + typedef typename pcl::PointCloud::Ptr PointCloudNPtr; + typedef typename pcl::PointCloud::ConstPtr PointCloudNConstPtr; + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + inline + ModelOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices) + { + thresh_ = 0; + normals_distance_weight_ = 0; + filter_name_ = "ModelOutlierRemoval"; + setThresholdFunction (&pcl::ModelOutlierRemoval::checkSingleThreshold, *this); + } + + /** \brief sets the models coefficients */ + inline void + setModelCoefficients (const pcl::ModelCoefficients model_coefficients) + { + model_coefficients_.resize (model_coefficients.values.size ()); + for (unsigned int i = 0; i < model_coefficients.values.size (); i++) + { + model_coefficients_[i] = model_coefficients.values[i]; + } + } + + /** \brief returns the models coefficients + */ + inline pcl::ModelCoefficients + getModelCoefficients () const + { + pcl::ModelCoefficients mc; + mc.values.resize (model_coefficients_.size ()); + for (unsigned int i = 0; i < mc.values.size (); i++) + mc.values[i] = model_coefficients_[i]; + return (mc); + } + + /** \brief Set the type of SAC model used. */ + inline void + setModelType (pcl::SacModel model) + { + model_type_ = model; + } + + /** \brief Get the type of SAC model used. */ + inline pcl::SacModel + getModelType () const + { + return (model_type_); + } + + /** \brief Set the thresholdfunction*/ + inline void + setThreshold (float thresh) + { + thresh_ = thresh; + } + + /** \brief Get the thresholdfunction*/ + inline float + getThreshold () const + { + return (thresh_); + } + + /** \brief Set the normals cloud*/ + inline void + setInputNormals (const PointCloudNConstPtr normals_ptr) + { + cloud_normals_ = normals_ptr; + } + + /** \brief Get the normals cloud*/ + inline PointCloudNConstPtr + getInputNormals () const + { + return (cloud_normals_); + } + + /** \brief Set the normals distance weight*/ + inline void + setNormalDistanceWeight (const double weight) + { + normals_distance_weight_ = weight; + } + + /** \brief get the normal distance weight*/ + inline double + getNormalDistanceWeight () const + { + return (normals_distance_weight_); + } + + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + */ + void + setThresholdFunction (boost::function thresh) + { + threshold_function_ = thresh; + } + + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + * \param[in] instance + */ + template void + setThresholdFunction (bool (T::*thresh_function) (double), T& instance) + { + setThresholdFunction (boost::bind (thresh_function, boost::ref (instance), _1)); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output); + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + protected: + double normals_distance_weight_; + PointCloudNConstPtr cloud_normals_; + + /** \brief The model used to calculate distances */ + SampleConsensusModelPtr model_; + + /** \brief The threshold used to seperate outliers (removed_indices) from inliers (indices) */ + float thresh_; + + /** \brief The model coefficients */ + Eigen::VectorXf model_coefficients_; + + /** \brief The type of model to use (user given parameter). */ + pcl::SacModel model_type_; + boost::function threshold_function_; + + inline bool + checkSingleThreshold (double value) + { + return (value < thresh_); + } + + private: + virtual bool + initSACModel (pcl::SacModel model_type); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif // PCL_FILTERS_MODEL_OUTLIER_REMOVAL_H_ diff --git a/filters/include/pcl/filters/morphological_filter.h b/filters/include/pcl/filters/morphological_filter.h new file mode 100644 index 00000000..4ea8dda3 --- /dev/null +++ b/filters/include/pcl/filters/morphological_filter.h @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_MORPHOLOGICAL_FILTER_H_ +#define PCL_FILTERS_MORPHOLOGICAL_FILTER_H_ + +#include +#include +#include +#include +#include + +namespace pcl +{ + enum MorphologicalOperators + { + MORPH_OPEN, + MORPH_CLOSE, + MORPH_DILATE, + MORPH_ERODE + }; +} + +namespace pcl +{ + /** \brief Apply morphological operator to the z dimension of the input point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] resolution the window size to be used for the morphological operation + * \param[in] morphological_operator the morphological operator to apply (open, close, dilate, erode) + * \param[out] cloud_out the resultant output point cloud dataset + * \ingroup filters + */ + template PCL_EXPORTS void + applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out); +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_FILTERS_MORPHOLOGICAL_FILTER_H_ + diff --git a/filters/include/pcl/filters/plane_clipper3D.h b/filters/include/pcl/filters/plane_clipper3D.h index aee40515..3f16c634 100644 --- a/filters/include/pcl/filters/plane_clipper3D.h +++ b/filters/include/pcl/filters/plane_clipper3D.h @@ -82,10 +82,10 @@ namespace pcl clipLineSegment3D (PointT& from, PointT& to) const; virtual void - clipPlanarPolygon3D (std::vector& polygon) const; + clipPlanarPolygon3D (std::vector >& polygon) const; virtual void - clipPlanarPolygon3D (const std::vector& polygon, std::vector& clipped_polygon) const; + clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const; virtual void clipPointCloud3D (const pcl::PointCloud &cloud_in, std::vector& clipped, const std::vector& indices = std::vector ()) const; @@ -102,8 +102,6 @@ namespace pcl }; } -#ifdef PCL_NO_PRECOMPILE #include -#endif #endif // PCL_PLANE_CLIPPER3D_H_ diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index 7ee207ce..1f1024b5 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/filters/include/pcl/filters/radius_outlier_removal.h @@ -131,7 +131,7 @@ namespace pcl /** \brief Get the number of neighbors that need to be present in order to be classified as an inlier. * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater * than this number in order to be classified as an inlier point (i.e. will not be filtered). - * \param min_pts The minimum number of neighbors (default = 1). + * \return The minimum number of neighbors (default = 1). */ inline int getMinNeighborsInRadius () diff --git a/filters/include/pcl/filters/shadowpoints.h b/filters/include/pcl/filters/shadowpoints.h index 8a72d56a..1f2733ec 100644 --- a/filters/include/pcl/filters/shadowpoints.h +++ b/filters/include/pcl/filters/shadowpoints.h @@ -92,7 +92,7 @@ namespace pcl getNormals () const { return (input_normals_); } /** \brief Set the threshold for shadow points rejection - * \param[in] thresold the threshold + * \param[in] threshold the threshold */ inline void setThreshold (float threshold) { threshold_ = threshold; } diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 2b670902..2aeb4153 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -136,7 +136,6 @@ namespace pcl /** \brief Get the standard deviation multiplier for the distance threshold calculation. * \details The distance threshold will be equal to: mean + stddev_mult * stddev. * Points will be classified as inlier or outlier if their average neighbor distance is below or above this threshold respectively. - * \param[in] stddev_mult The standard deviation multiplier. */ inline double getStddevMulThresh () diff --git a/filters/include/pcl/filters/voxel_grid.h b/filters/include/pcl/filters/voxel_grid.h index 6a271663..2d2c464f 100644 --- a/filters/include/pcl/filters/voxel_grid.h +++ b/filters/include/pcl/filters/voxel_grid.h @@ -205,7 +205,8 @@ namespace pcl filter_field_name_ (""), filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX), - filter_limit_negative_ (false) + filter_limit_negative_ (false), + min_points_per_voxel_ (0) { filter_name_ = "VoxelGrid"; } @@ -261,6 +262,17 @@ namespace pcl inline bool getDownsampleAllData () { return (downsample_all_data_); } + /** \brief Set the minimum number of points required for a voxel to be used. + * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used + */ + inline void + setMinimumPointsNumberPerVoxel (unsigned int min_points_per_voxel) { min_points_per_voxel_ = min_points_per_voxel; } + + /** \brief Return the minimum number of points required for a voxel to be used. + */ + inline unsigned int + getMinimumPointsNumberPerVoxel () { return min_points_per_voxel_; } + /** \brief Set to true if leaf layout information needs to be saved for later access. * \param[in] save_leaf_layout the new value (true/false) */ @@ -471,6 +483,9 @@ namespace pcl /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ bool filter_limit_negative_; + /** \brief Minimum number of points per voxel for the centroid to be computed */ + unsigned int min_points_per_voxel_; + typedef typename pcl::traits::fieldList::type FieldList; /** \brief Downsample a Point Cloud using a voxelized grid approach @@ -517,7 +532,8 @@ namespace pcl filter_field_name_ (""), filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX), - filter_limit_negative_ (false) + filter_limit_negative_ (false), + min_points_per_voxel_ (0) { filter_name_ = "VoxelGrid"; } @@ -573,6 +589,17 @@ namespace pcl inline bool getDownsampleAllData () { return (downsample_all_data_); } + /** \brief Set the minimum number of points required for a voxel to be used. + * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used + */ + inline void + setMinimumPointsNumberPerVoxel (unsigned int min_points_per_voxel) { min_points_per_voxel_ = min_points_per_voxel; } + + /** \brief Return the minimum number of points required for a voxel to be used. + */ + inline unsigned int + getMinimumPointsNumberPerVoxel () { return min_points_per_voxel_; } + /** \brief Set to true if leaf layout information needs to be saved for later access. * \param[in] save_leaf_layout the new value (true/false) */ @@ -811,6 +838,9 @@ namespace pcl /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ bool filter_limit_negative_; + /** \brief Minimum number of points per voxel for the centroid to be computed */ + unsigned int min_points_per_voxel_; + /** \brief Downsample a Point Cloud using a voxelized grid approach * \param[out] output the resultant point cloud */ diff --git a/filters/include/pcl/filters/voxel_grid_covariance.h b/filters/include/pcl/filters/voxel_grid_covariance.h index 8278a3b3..d4e6a094 100644 --- a/filters/include/pcl/filters/voxel_grid_covariance.h +++ b/filters/include/pcl/filters/voxel_grid_covariance.h @@ -242,7 +242,7 @@ namespace pcl } /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. - * \param[in] min_points_per_voxel the minimum allowable ratio between eigenvalues + * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues */ inline void setCovEigValueInflationRatio (double min_covar_eigvalue_mult) @@ -397,7 +397,7 @@ namespace pcl /** \brief Get a cloud to visualize each voxels normal distribution. - * \param[out] a cloud created by sampling the normal distributions of each voxel + * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel */ void getDisplayCloud (pcl::PointCloud& cell_cloud); @@ -438,7 +438,8 @@ namespace pcl /** \brief Search for the k-nearest occupied voxels for the given query point. * \note Only voxels containing a sufficient number of points are used. - * \param[in] point the given query point + * \param[in] cloud the given query point + * \param[in] index the index * \param[in] k the number of neighbors to search for * \param[out] k_leaves the resultant leaves of the neighboring points * \param[out] k_sqr_distances the resultant squared distances to the neighboring points @@ -460,6 +461,7 @@ namespace pcl * \param[in] radius the radius of the sphere bounding all of p_q's neighbors * \param[out] k_leaves the resultant leaves of the neighboring points * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn * \return number of neighbors found */ int @@ -495,6 +497,7 @@ namespace pcl * \param[in] radius the radius of the sphere bounding all of p_q's neighbors * \param[out] k_leaves the resultant leaves of the neighboring points * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn * \return number of neighbors found */ inline int diff --git a/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h b/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h index d06f9a56..90e46b06 100644 --- a/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h +++ b/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h @@ -89,8 +89,9 @@ namespace pcl /** \brief Returns the state (free = 0, occluded = 1) of the voxel * after utilizing a ray traversal algorithm to a target voxel * in (i, j, k) coordinates. - * \param[out] The state of the voxel. - * \param[in] The target voxel coordinate (i, j, k) of the voxel. + * \param[out] out_state The state of the voxel. + * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel. + * \return the state (free = 0, occluded = 1) of the voxel */ int occlusionEstimation (int& out_state, @@ -101,9 +102,10 @@ namespace pcl * in (i, j, k) coordinates. Additionally, this function returns * the voxels penetrated of the ray-traversal algorithm till reaching * the target voxel. - * \param[out] The state of the voxel. - * \param[out] The voxels penetrated of the ray-traversal algorithm. - * \param[in] The target voxel coordinate (i, j, k) of the voxel. + * \param[out] out_state The state of the voxel. + * \param[out] out_ray The voxels penetrated of the ray-traversal algorithm. + * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel. + * \return the state (free = 0, occluded = 1) of the voxel */ int occlusionEstimation (int& out_state, @@ -112,13 +114,14 @@ namespace pcl /** \brief Returns the voxel coordinates (i, j, k) of all occluded * voxels in the voxel gird. - * \param[out] the coordinates (i, j, k) of all occluded voxels + * \param[out] occluded_voxels the coordinates (i, j, k) of all occluded voxels + * \return the voxel coordinates (i, j, k) */ int occlusionEstimationAll (std::vector& occluded_voxels); /** \brief Returns the voxel grid filtered point cloud - * \param[out] The voxel grid filtered point cloud + * \return The voxel grid filtered point cloud */ inline PointCloud getFilteredPointCloud () { return filtered_cloud_; } @@ -138,7 +141,7 @@ namespace pcl /** \brief Returns the corresponding centroid (x,y,z) coordinates * in the grid of voxel (i,j,k). - * \param[in] the coordinate (i, j, k) of the voxel + * \param[in] ijk the coordinate (i, j, k) of the voxel * \return the (x,y,z) coordinate of the voxel centroid */ inline Eigen::Vector4f @@ -167,8 +170,8 @@ namespace pcl /** \brief Returns the scaling value (tmin) were the ray intersects with the * voxel grid bounding box. (p_entry = origin + tmin * orientation) - * \param[in] The sensor origin - * \param[in] The sensor orientation + * \param[in] origin The sensor origin + * \param[in] direction The sensor orientation * \return the scaling value */ float @@ -177,10 +180,10 @@ namespace pcl /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) * unsing a ray traversal algorithm. - * \param[in] The target voxel in the voxel grid with coordinate (i, j, k). - * \param[in] The sensor origin. - * \param[in] The sensor orientation - * \param[in] The scaling value (tmin). + * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k). + * \param[in] origin The sensor origin. + * \param[in] direction The sensor orientation + * \param[in] t_min The scaling value (tmin). * \return The estimated voxel state. */ int @@ -191,11 +194,11 @@ namespace pcl /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) and * the voxels penetrated by the ray unsing a ray traversal algorithm. - * \param[out] The voxels penetrated by the ray in (i, j, k) coordinates - * \param[in] The target voxel in the voxel grid with coordinate (i, j, k). - * \param[in] The sensor origin. - * \param[in] The sensor orientation - * \param[in] The scaling value (tmin). + * \param[out] out_ray The voxels penetrated by the ray in (i, j, k) coordinates + * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k). + * \param[in] origin The sensor origin. + * \param[in] direction The sensor orientation + * \param[in] t_min The scaling value (tmin). * \return The estimated voxel state. */ int @@ -206,7 +209,7 @@ namespace pcl const float t_min); /** \brief Returns a rounded value. - * \param[in] value + * \param[in] d * \return rounded value */ inline float @@ -243,4 +246,8 @@ namespace pcl }; } +#ifdef PCL_NO_PRECOMPILE +#include +#endif + #endif //#ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ diff --git a/filters/src/grid_minimum.cpp b/filters/src/grid_minimum.cpp new file mode 100644 index 00000000..6eb88a6d --- /dev/null +++ b/filters/src/grid_minimum.cpp @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(GridMinimum, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE + diff --git a/filters/src/local_maximum.cpp b/filters/src/local_maximum.cpp new file mode 100644 index 00000000..edf8957f --- /dev/null +++ b/filters/src/local_maximum.cpp @@ -0,0 +1,56 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE(LocalMaximum, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)) +#else + PCL_INSTANTIATE(LocalMaximum, PCL_XYZ_POINT_TYPES) +#endif + +#endif // PCL_NO_PRECOMPILE + diff --git a/filters/src/median_filter.cpp b/filters/src/median_filter.cpp index 5ec56bba..18773d0b 100644 --- a/filters/src/median_filter.cpp +++ b/filters/src/median_filter.cpp @@ -43,7 +43,7 @@ #include #include -PCL_INSTANTIATE (MedianFilter, (pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)) +PCL_INSTANTIATE (MedianFilter, PCL_XYZ_POINT_TYPES) #endif // PCL_NO_PRECOMPILE diff --git a/filters/src/model_outlier_removal.cpp b/filters/src/model_outlier_removal.cpp new file mode 100644 index 00000000..67ffc75e --- /dev/null +++ b/filters/src/model_outlier_removal.cpp @@ -0,0 +1,51 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE (ModelOutlierRemoval, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)) +#else + PCL_INSTANTIATE (ModelOutlierRemoval, PCL_XYZ_POINT_TYPES) +#endif + +#endif // PCL_NO_PRECOMPILE diff --git a/filters/src/morphological_filter.cpp b/filters/src/morphological_filter.cpp new file mode 100644 index 00000000..8985881d --- /dev/null +++ b/filters/src/morphological_filter.cpp @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(applyMorphologicalOperator, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE + diff --git a/geometry/CMakeLists.txt b/geometry/CMakeLists.txt index ec8d9d34..8ee4c9ef 100644 --- a/geometry/CMakeLists.txt +++ b/geometry/CMakeLists.txt @@ -3,48 +3,49 @@ set(SUBSYS_DESC "Point cloud geometry library") set(SUBSYS_DEPS common) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/eigen.h - include/pcl/${SUBSYS_NAME}/line_iterator.h - include/pcl/${SUBSYS_NAME}/mesh_base.h - include/pcl/${SUBSYS_NAME}/mesh_circulators.h - include/pcl/${SUBSYS_NAME}/mesh_conversion.h - include/pcl/${SUBSYS_NAME}/mesh_elements.h - include/pcl/${SUBSYS_NAME}/mesh_indices.h - include/pcl/${SUBSYS_NAME}/mesh_io.h - include/pcl/${SUBSYS_NAME}/mesh_traits.h - include/pcl/${SUBSYS_NAME}/organized_index_iterator.h - include/pcl/${SUBSYS_NAME}/planar_polygon.h - include/pcl/${SUBSYS_NAME}/polygon_operations.h - include/pcl/${SUBSYS_NAME}/polygon_mesh.h - include/pcl/${SUBSYS_NAME}/triangle_mesh.h - include/pcl/${SUBSYS_NAME}/quad_mesh.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/get_boundary.h" + "include/pcl/${SUBSYS_NAME}/line_iterator.h" + "include/pcl/${SUBSYS_NAME}/mesh_base.h" + "include/pcl/${SUBSYS_NAME}/mesh_circulators.h" + "include/pcl/${SUBSYS_NAME}/mesh_conversion.h" + "include/pcl/${SUBSYS_NAME}/mesh_elements.h" + "include/pcl/${SUBSYS_NAME}/mesh_indices.h" + "include/pcl/${SUBSYS_NAME}/mesh_io.h" + "include/pcl/${SUBSYS_NAME}/mesh_traits.h" + "include/pcl/${SUBSYS_NAME}/organized_index_iterator.h" + "include/pcl/${SUBSYS_NAME}/planar_polygon.h" + "include/pcl/${SUBSYS_NAME}/polygon_mesh.h" + "include/pcl/${SUBSYS_NAME}/polygon_operations.h" + "include/pcl/${SUBSYS_NAME}/quad_mesh.h" + "include/pcl/${SUBSYS_NAME}/triangle_mesh.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/polygon_operations.hpp + "include/pcl/${SUBSYS_NAME}/impl/polygon_operations.hpp" ) # set(srcs # src/geometry.cpp # ) -set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) -# PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) -# target_link_libraries(${LIB_NAME} pcl_common) -PCL_MAKE_PKGCONFIG_HEADER_ONLY(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "") +set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") +# PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) +# target_link_libraries("${LIB_NAME}" pcl_common) + PCL_MAKE_PKGCONFIG_HEADER_ONLY("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/geometry/include/pcl/geometry/mesh_base.h b/geometry/include/pcl/geometry/mesh_base.h index 85bb971f..a50eb505 100644 --- a/geometry/include/pcl/geometry/mesh_base.h +++ b/geometry/include/pcl/geometry/mesh_base.h @@ -726,7 +726,7 @@ namespace pcl return (this->isBoundary (idx_face, boost::integral_constant ())); } - /** \brief Check if the given face lies on the boundary. This method uses isBoundary which checks if any vertex lies on the boundary. */ + /** \brief Check if the given face lies on the boundary. This method uses isBoundary \c true which checks if any vertex lies on the boundary. */ inline bool isBoundary (const FaceIndex& idx_face) const { @@ -1306,7 +1306,7 @@ namespace pcl /** \brief Check if the half-edge bc is the next half-edge of ab. * \param[in] idx_he_ab Index to the half-edge between the vertices a and b. - * \param[in] idx_ha_bc Index to the half-edge between the vertices b and c. + * \param[in] idx_he_bc Index to the half-edge between the vertices b and c. * \param[in] is_new_ab Half-edge ab is new. * \param[in] is_new_bc Half-edge bc is new. * \param[out] make_adjacent_ab_bc Half-edges ab and bc need to be made adjacent. @@ -1350,7 +1350,7 @@ namespace pcl /** \brief Make the half-edges bc the next half-edge of ab. * \param[in] idx_he_ab Index to the half-edge between the vertices a and b. - * \param[in] idx_ha_bc Index to the half-edge between the vertices b and c. + * \param[in] idx_he_bc Index to the half-edge between the vertices b and c. * \param[in, out] idx_free_half_edge Free half-edge needed to re-connect the half-edges around vertex b. */ void @@ -1743,10 +1743,10 @@ namespace pcl //////////////////////////////////////////////////////////////////////// /** \brief Removes mesh elements and data that are marked as deleted from the container. - * \IndexContainerT e.g. std::vector - * \ElementContainerT e.g. std::vector - * \DataContainerT e.g. std::vector - * \HasDataT Integral constant specifying if the mesh has data associated with the elements. + * \param IndexContainerT e.g. std::vector \ + * \param ElementContainerT e.g. std::vector \ + * \param DataContainerT e.g. std::vector \ + * \param HasDataT Integral constant specifying if the mesh has data associated with the elements. * \param[in, out] elements Container for the mesh elements. Resized to the new size. * \param[in, out] data_cloud Container for the mesh data. Resized to the new size. * \return Container with the same size as the old input data. Holds the indices to the new elements for each non-deleted element and an invalid index if it is deleted. @@ -1983,7 +1983,7 @@ namespace pcl /** \brief Resize the mesh data. */ template inline void - resizeData (DataCloudT& data_cloud, const size_t n, const typename DataCloudT::value_type& data, boost::true_type /*has_data*/) const + resizeData (DataCloudT& /*data_cloud*/, const size_t n, const typename DataCloudT::value_type& data, boost::true_type /*has_data*/) const { data.resize (n, data); } diff --git a/geometry/include/pcl/geometry/mesh_indices.h b/geometry/include/pcl/geometry/mesh_indices.h index 83b6d9a4..36f82b75 100644 --- a/geometry/include/pcl/geometry/mesh_indices.h +++ b/geometry/include/pcl/geometry/mesh_indices.h @@ -619,6 +619,7 @@ namespace pcl } /** \brief Convert the given edge index to a half-edge index. + * \param index * \param[in] get_first The first half-edge of the edge is returned if this variable is true; elsewise the second. */ inline pcl::geometry::HalfEdgeIndex diff --git a/geometry/include/pcl/geometry/mesh_traits.h b/geometry/include/pcl/geometry/mesh_traits.h index ba57ce70..0c23243a 100644 --- a/geometry/include/pcl/geometry/mesh_traits.h +++ b/geometry/include/pcl/geometry/mesh_traits.h @@ -48,7 +48,15 @@ namespace pcl namespace geometry { /** \brief No data is associated with the vertices / half-edges / edges / faces. */ - struct NoData {}; + struct NoData + { +#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 + operator unsigned char() const + { + return 0; + } +#endif + }; /** \brief The mesh traits are used to set up compile time settings for the mesh. * \tparam VertexDataT Data stored for each vertex. Defaults to pcl::NoData. diff --git a/geometry/include/pcl/geometry/polygon_operations.h b/geometry/include/pcl/geometry/polygon_operations.h index 8ac5d557..ec6c1a76 100644 --- a/geometry/include/pcl/geometry/polygon_operations.h +++ b/geometry/include/pcl/geometry/polygon_operations.h @@ -57,6 +57,7 @@ namespace pcl * \param [in] polygon input polygon * \param [out] approx_polygon approximate polygon * \param [in] threshold maximum allowed distance of an input vertex to an output edge + * \param refine * \param [in] closed whether it is a closed polygon or a polyline * \author Suat Gedikli */ diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index f66b81ea..347f6581 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -3,22 +3,82 @@ set(SUBSYS_DESC "Point cloud IO library") set(SUBSYS_DEPS common octree) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) if(WIN32) - PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS openni vtk) + PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 pcap png vtk) else(WIN32) - PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS openni vtk libusb-1.0) + PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 pcap png vtk libusb-1.0) endif(WIN32) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) + set(IMAGE_INCLUDES + include/pcl/io/image_metadata_wrapper.h + include/pcl/io/image.h + include/pcl/io/image_rgb24.h + include/pcl/io/image_yuv422.h + include/pcl/io/image_ir.h + include/pcl/io/image_depth.h + ) + + set(IMAGE_SOURCES + src/image_rgb24.cpp + src/image_yuv422.cpp + src/image_ir.cpp + src/image_depth.cpp + ) + + ## OpenNI 2.x + OPTION(BUILD_OPENNI2 "Build the OpenNI 2 Grabber." OFF) + MARK_AS_ADVANCED(BUILD_OPENNI2) + if(NOT BUILD_OPENNI2) + # Set OPENNI2_FOUND to false locally to avoid building anything OpenNI 2 related + # Remember that other modules (libraries) need to check explicitly for BUILD_OPENNI2 + set(OPENNI2_FOUND FALSE) + endif() + if(OPENNI2_FOUND) + set(OPENNI2_GRABBER_INCLUDES + include/pcl/io/openni2_grabber.h + ) + set(OPENNI2_INCLUDES + include/pcl/io/openni2/openni.h + include/pcl/io/openni2/openni2_metadata_wrapper.h + include/pcl/io/openni2/openni2_frame_listener.h + include/pcl/io/openni2/openni2_timer_filter.h + include/pcl/io/openni2/openni2_video_mode.h + include/pcl/io/openni2/openni2_convert.h + include/pcl/io/openni2/openni2_device.h + include/pcl/io/openni2/openni2_device_info.h + include/pcl/io/openni2/openni2_device_manager.h + ) + + set(OPENNI2_GRABBER_SOURCES + src/openni2_grabber.cpp + src/openni2/openni2_timer_filter.cpp + src/openni2/openni2_video_mode.cpp + src/openni2/openni2_convert.cpp + src/openni2/openni2_device.cpp + src/openni2/openni2_device_info.cpp + src/openni2/openni2_device_manager.cpp + ) + + source_group("OpenNI 2\\Header Files" FILES ${OPENNI2_GRABBER_INCLUDES} ${OPENNI2_INCLUDES} ${IMAGE_INCLUDES}) + source_group("OpenNI 2\\Source Files" FILES ${OPENNI2_GRABBER_SOURCES} ${IMAGE_SOURCES}) + + # Copy OpenNI2 redist directory to bin. Needed for driver modules. Only tested on Windows. + if(MSVC) + file(COPY ${OPENNI2_REDIST_DIR} DESTINATION ${CMAKE_BINARY_DIR}/bin PATTERN *.*) + endif(MSVC) + endif(OPENNI2_FOUND) + + ## OpenNI 1.x OPTION(BUILD_OPENNI "Build the OpenNI Grabber." ON) MARK_AS_ADVANCED(BUILD_OPENNI) if(NOT BUILD_OPENNI) # Set OPENNI_FOUND to false locally to avoid building anything OpenNI related # Remember that other modules (libraries) need to check explicitly for BUILD_OPENNI - set(OPENNI_FOUND FALSE) + set(OPENNI_FOUND FALSE) endif() if(OPENNI_FOUND) set(OPENNI_GRABBER_INCLUDES @@ -41,8 +101,10 @@ if(build) include/pcl/io/openni_camera/openni_image_yuv_422.h include/pcl/io/openni_camera/openni_image_rgb24.h include/pcl/io/openni_camera/openni_ir_image.h - ) - set(OPENNI_GRABBER_SOURCES src/openni_camera/openni_device.cpp + ${IMAGE_INCLUDES} + ) + set(OPENNI_GRABBER_SOURCES + src/openni_camera/openni_device.cpp src/openni_camera/openni_device_primesense.cpp src/openni_camera/openni_image_bayer_grbg.cpp src/openni_camera/openni_depth_image.cpp @@ -56,9 +118,13 @@ if(build) src/openni_camera/openni_image_rgb24.cpp src/openni_grabber.cpp src/oni_grabber.cpp - ) + ${IMAGE_SOURCES} + ) endif(OPENNI_FOUND) + source_group("Image Headers" FILES ${IMAGE_INCLUDES}) + source_group("Image Sources" FILES ${IMAGE_SOURCES}) + if(FZAPI_FOUND) set(FZAPI_GRABBER_INCLUDES include/pcl/io/fotonic_grabber.h @@ -66,11 +132,11 @@ if(build) # set(FZAPI_INCLUDES # include/pcl/io/openni_camera/openni.h # ) - set(FZAPI_GRABBER_SOURCES + set(FZAPI_GRABBER_SOURCES src/fotonic_grabber.cpp ) endif(FZAPI_FOUND) - + if(PXCAPI_FOUND) set(PXC_GRABBER_INCLUDES include/pcl/io/pxc_grabber.h @@ -79,7 +145,7 @@ if(build) src/pxc_grabber.cpp ) endif(PXCAPI_FOUND) - + if(LIBUSB_1_FOUND) set(DINAST_GRABBER_INCLUDES include/pcl/io/dinast_grabber.h @@ -90,16 +156,16 @@ if(build) endif(LIBUSB_1_FOUND) if (VTK_FOUND AND NOT ANDROID) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) - set(VTK_IO_INCLUDES - include/pcl/${SUBSYS_NAME}/vtk_lib_io.h - include/pcl/${SUBSYS_NAME}/png_io.h + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") + set(VTK_IO_INCLUDES + "include/pcl/${SUBSYS_NAME}/vtk_lib_io.h" + "include/pcl/${SUBSYS_NAME}/png_io.h" ) set(VTK_IO_INCLUDES_IMPL - include/pcl/${SUBSYS_NAME}/impl/vtk_lib_io.hpp + "include/pcl/${SUBSYS_NAME}/impl/vtk_lib_io.hpp" ) - set(VTK_IO_SOURCE + set(VTK_IO_SOURCE src/vtk_lib_io.cpp src/png_io.cpp ) @@ -109,16 +175,16 @@ if(build) endif () set(PLY_SOURCES src/ply/ply_parser.cpp) - set(PLY_INCLUDES - include/pcl/${SUBSYS_NAME}/ply/byte_order.h - include/pcl/${SUBSYS_NAME}/ply/io_operators.h - include/pcl/${SUBSYS_NAME}/ply/ply.h - include/pcl/${SUBSYS_NAME}/ply/ply_parser.h + set(PLY_INCLUDES + "include/pcl/${SUBSYS_NAME}/ply/byte_order.h" + "include/pcl/${SUBSYS_NAME}/ply/io_operators.h" + "include/pcl/${SUBSYS_NAME}/ply/ply.h" + "include/pcl/${SUBSYS_NAME}/ply/ply_parser.h" ) - PCL_ADD_LIBRARY(pcl_io_ply ${SUBSYS_NAME} ${PLY_SOURCES} ${PLY_INCLUDES}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/ply ${PLY_INCLUDES}) + PCL_ADD_LIBRARY(pcl_io_ply "${SUBSYS_NAME}" ${PLY_SOURCES} ${PLY_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/ply" ${PLY_INCLUDES}) - set(srcs + set(srcs src/debayer.cpp src/pcd_grabber.cpp src/pcd_io.cpp @@ -129,18 +195,22 @@ if(build) src/lzf.cpp src/lzf_image_io.cpp src/obj_io.cpp + src/ifs_io.cpp src/image_grabber.cpp src/hdl_grabber.cpp src/robot_eye_grabber.cpp + src/file_io.cpp + src/io_exception.cpp ${VTK_IO_SOURCE} ${OPENNI_GRABBER_SOURCES} + ${OPENNI2_GRABBER_SOURCES} + ${IMAGE_SOURCES} ${DINAST_GRABBER_SOURCES} ${FZAPI_GRABBER_SOURCES} - ${PXC_GRABBER_SOURCES} + ${PXC_GRABBER_SOURCES} ) if(PNG_FOUND) - set(srcs - ${srcs} + list(APPEND srcs src/libpng_wrapper.cpp ) endif(PNG_FOUND) @@ -151,32 +221,36 @@ if(build) add_definitions(${PCAP_DEFINES}) endif(PCAP_FOUND) - set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/eigen.h - include/pcl/${SUBSYS_NAME}/debayer.h - include/pcl/${SUBSYS_NAME}/file_io.h - include/pcl/${SUBSYS_NAME}/lzf.h - include/pcl/${SUBSYS_NAME}/lzf_image_io.h - include/pcl/${SUBSYS_NAME}/io.h - include/pcl/${SUBSYS_NAME}/grabber.h - include/pcl/${SUBSYS_NAME}/file_grabber.h - include/pcl/${SUBSYS_NAME}/pcd_grabber.h - include/pcl/${SUBSYS_NAME}/pcd_io.h - include/pcl/${SUBSYS_NAME}/vtk_io.h - include/pcl/${SUBSYS_NAME}/ply_io.h - include/pcl/${SUBSYS_NAME}/tar.h - include/pcl/${SUBSYS_NAME}/obj_io.h - include/pcl/${SUBSYS_NAME}/ascii_io.h - include/pcl/${SUBSYS_NAME}/image_grabber.h - include/pcl/${SUBSYS_NAME}/hdl_grabber.h - include/pcl/${SUBSYS_NAME}/robot_eye_grabber.h - include/pcl/${SUBSYS_NAME}/point_cloud_image_extractors.h + set(incs + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/debayer.h" + "include/pcl/${SUBSYS_NAME}/file_io.h" + "include/pcl/${SUBSYS_NAME}/lzf.h" + "include/pcl/${SUBSYS_NAME}/lzf_image_io.h" + "include/pcl/${SUBSYS_NAME}/io.h" + "include/pcl/${SUBSYS_NAME}/grabber.h" + "include/pcl/${SUBSYS_NAME}/file_grabber.h" + "include/pcl/${SUBSYS_NAME}/pcd_grabber.h" + "include/pcl/${SUBSYS_NAME}/pcd_io.h" + "include/pcl/${SUBSYS_NAME}/vtk_io.h" + "include/pcl/${SUBSYS_NAME}/ply_io.h" + "include/pcl/${SUBSYS_NAME}/tar.h" + "include/pcl/${SUBSYS_NAME}/obj_io.h" + "include/pcl/${SUBSYS_NAME}/ascii_io.h" + "include/pcl/${SUBSYS_NAME}/ifs_io.h" + "include/pcl/${SUBSYS_NAME}/image_grabber.h" + "include/pcl/${SUBSYS_NAME}/hdl_grabber.h" + "include/pcl/${SUBSYS_NAME}/robot_eye_grabber.h" + "include/pcl/${SUBSYS_NAME}/point_cloud_image_extractors.h" + "include/pcl/${SUBSYS_NAME}/io_exception.h" ${VTK_IO_INCLUDES} ${OPENNI_GRABBER_INCLUDES} + ${OPENNI2_GRABBER_INCLUDES} + ${IMAGE_INCLUDES} ${DINAST_GRABBER_INCLUDES} ${FZAPI_GRABBER_INCLUDES} - ${PXC_GRABBER_INCLUDES} + ${PXC_GRABBER_INCLUDES} ) set(compression_incs @@ -187,80 +261,92 @@ if(build) include/pcl/compression/point_coding.h ) if(PNG_FOUND) - set(compression_incs - ${compression_incs} + list(APPEND compression_incs include/pcl/compression/organized_pointcloud_conversion.h include/pcl/compression/libpng_wrapper.h ) - if(OPENNI_FOUND) - set(compression_incs - ${compression_incs} + if(OPENNI_FOUND OR OPENNI2_FOUND) + list(APPEND compression_incs include/pcl/compression/organized_pointcloud_compression.h ) - endif(OPENNI_FOUND) + endif(OPENNI_FOUND OR OPENNI2_FOUND) endif(PNG_FOUND) - set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/pcd_io.hpp - include/pcl/${SUBSYS_NAME}/impl/lzf_image_io.hpp - include/pcl/${SUBSYS_NAME}/impl/synchronized_queue.hpp - include/pcl/${SUBSYS_NAME}/impl/point_cloud_image_extractors.hpp + set(impl_incs + "include/pcl/${SUBSYS_NAME}/impl/pcd_io.hpp" + "include/pcl/${SUBSYS_NAME}/impl/lzf_image_io.hpp" + "include/pcl/${SUBSYS_NAME}/impl/synchronized_queue.hpp" + "include/pcl/${SUBSYS_NAME}/impl/point_cloud_image_extractors.hpp" include/pcl/compression/impl/entropy_range_coder.hpp include/pcl/compression/impl/octree_pointcloud_compression.hpp ${VTK_IO_INCLUDES_IMPL} ) - if(PNG_FOUND AND OPENNI_FOUND) - set(impl_incs - ${impl_incs} + if(PNG_FOUND AND (OPENNI_FOUND OR OPENNI2_FOUND) ) + list(APPEND impl_incs include/pcl/compression/impl/organized_pointcloud_compression.hpp ) - endif(PNG_FOUND AND OPENNI_FOUND) + endif(PNG_FOUND AND (OPENNI_FOUND OR OPENNI2_FOUND) ) - set(LIB_NAME pcl_${SUBSYS_NAME}) + set(LIB_NAME "pcl_${SUBSYS_NAME}") - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${VTK_INCLUDE_DIRECTORIES}) + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" ${VTK_INCLUDE_DIRECTORIES}) add_definitions(${VTK_DEFINES}) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${compression_incs} ${impl_incs} ${OPENNI_INCLUDES}) + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${compression_incs} ${impl_incs} ${OPENNI_INCLUDES} ${OPENNI2_INCLUDES}) link_directories(${VTK_LINK_DIRECTORIES}) - target_link_libraries(${LIB_NAME} pcl_common pcl_io_ply ${VTK_IO_TARGET_LINK_LIBRARIES} ) + target_link_libraries("${LIB_NAME}" pcl_common pcl_io_ply ${VTK_LIBRARIES} ) if(PNG_FOUND) - target_link_libraries(${LIB_NAME} ${PNG_LIBRARY}) + target_link_libraries("${LIB_NAME}" "${PNG_LIBRARY}") endif(PNG_FOUND) if(LIBUSB_1_FOUND) - target_link_libraries(${LIB_NAME} ${LIBUSB_1_LIBRARIES}) + target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES}) endif(LIBUSB_1_FOUND) + if(OPENNI2_FOUND) + target_link_libraries(${LIB_NAME} ${OPENNI2_LIBRARIES}) + endif(OPENNI2_FOUND) + if(OPENNI_FOUND) - target_link_libraries(${LIB_NAME} ${OPENNI_LIBRARIES}) + target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES}) endif(OPENNI_FOUND) - + if(FZAPI_FOUND) - target_link_libraries(${LIB_NAME} ${FZAPI_LIBS}) + target_link_libraries("${LIB_NAME}" ${FZAPI_LIBS}) if(WIN32) - target_link_libraries(${LIB_NAME} Version.lib) + target_link_libraries("${LIB_NAME}" Version.lib) endif(WIN32) endif(FZAPI_FOUND) - + if(PXCAPI_FOUND) link_directories(${PXCAPI_LIB_DIRS}) - target_link_libraries(${LIB_NAME} ${PXCAPI_LIBS}) + target_link_libraries("${LIB_NAME}" ${PXCAPI_LIBS}) endif(PXCAPI_FOUND) - + if (PCAP_FOUND) - target_link_libraries(${LIB_NAME} ${PCAP_LIBRARIES}) + target_link_libraries("${LIB_NAME}" ${PCAP_LIBRARIES}) endif(PCAP_FOUND) set(EXT_DEPS eigen3) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" + + if(OPENNI_FOUND) + list(APPEND EXT_DEPS openni-dev) + endif(OPENNI_FOUND) + if(OPENNI2_FOUND) + list(APPEND EXT_DEPS openni2-dev) + endif(OPENNI2_FOUND) + + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} compression ${compression_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/openni_camera ${OPENNI_INCLUDES}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) - - add_subdirectory(tools) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" compression ${compression_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni_camera" ${OPENNI_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni2" ${OPENNI2_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) + + if(BUILD_tools) + add_subdirectory(tools) + endif(BUILD_tools) endif(build) diff --git a/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp b/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp index e495349a..8849aa36 100644 --- a/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp +++ b/io/include/pcl/compression/impl/organized_pointcloud_compression.hpp @@ -280,7 +280,10 @@ namespace pcl { uint32_t cloud_width; uint32_t cloud_height; - float maxDepth, focalLength, disparityShift, disparityScale; + float maxDepth; + float focalLength; + float disparityShift = 0.0f; + float disparityScale; // disparity and rgb image data std::vector disparityData; diff --git a/io/include/pcl/compression/libpng_wrapper.h b/io/include/pcl/compression/libpng_wrapper.h index 6d562b36..f56cc9d6 100644 --- a/io/include/pcl/compression/libpng_wrapper.h +++ b/io/include/pcl/compression/libpng_wrapper.h @@ -49,7 +49,7 @@ namespace pcl * \param[in] image_arg input image data * \param[in] width_arg image width * \param[in] height_arg image height - * \param[out] pngData PNG compressed image data + * \param[out] pngData_arg PNG compressed image data * \param[in] png_level_arg zLib compression level (default level: -1) * \ingroup io */ @@ -64,7 +64,7 @@ namespace pcl * \param[in] image_arg input image data * \param[in] width_arg image width * \param[in] height_arg image height - * \param[out] pngData PNG compressed image data + * \param[out] pngData_arg PNG compressed image data * \param[in] png_level_arg zLib compression level (default level: -1) * \ingroup io */ @@ -79,7 +79,7 @@ namespace pcl * \param[in] image_arg input image data * \param[in] width_arg image width * \param[in] height_arg image height - * \param[out] pngData PNG compressed image data + * \param[out] pngData_arg PNG compressed image data * \param[in] png_level_arg zLib compression level (default level: -1) * \ingroup io */ @@ -94,7 +94,7 @@ namespace pcl * \param[in] image_arg input image data * \param[in] width_arg image width * \param[in] height_arg image height - * \param[out] pngData PNG compressed image data + * \param[out] pngData_arg PNG compressed image data * \param[in] png_level_arg zLib compression level (default level: -1) * \ingroup io */ @@ -106,11 +106,11 @@ namespace pcl int png_level_arg = -1); /** \brief Decode compressed PNG to 8-bit image - * \param[in] pngData PNG compressed input data - * \param[in] imageData image output data - * \param[out] width image width - * \param[out] height image height - * \param[out] channels number of channels + * \param[in] pngData_arg PNG compressed input data + * \param[in] imageData_arg image output data + * \param[out] width_arg image width + * \param[out] heigh_argt image height + * \param[out] channels_arg number of channels * \ingroup io */ PCL_EXPORTS void @@ -121,11 +121,11 @@ namespace pcl unsigned int& channels_arg); /** \brief Decode compressed PNG to 16-bit image - * \param[in] pngData PNG compressed input data - * \param[in] imageData image output data - * \param[out] width image width - * \param[out] height image height - * \param[out] channels number of channels + * \param[in] pngData_arg PNG compressed input data + * \param[in] imageData_arg image output data + * \param[out] width_arg image width + * \param[out] height_arg image height + * \param[out] channels_arg number of channels * \ingroup io */ PCL_EXPORTS void diff --git a/io/include/pcl/compression/octree_pointcloud_compression.h b/io/include/pcl/compression/octree_pointcloud_compression.h index bca7ae04..de169048 100644 --- a/io/include/pcl/compression/octree_pointcloud_compression.h +++ b/io/include/pcl/compression/octree_pointcloud_compression.h @@ -252,9 +252,9 @@ namespace pcl serializeTreeCallback (LeafT &leaf_arg, const OctreeKey& key_arg); /** \brief Decode leaf nodes information during deserialization - * \param leaf_arg: reference to new leaf node - * \param key_arg: octree key of new leaf node + * \param key_arg octree key of new leaf node */ + // param leaf_arg reference to new leaf node virtual void deserializeTreeCallback (LeafT&, const OctreeKey& key_arg); diff --git a/io/include/pcl/compression/organized_pointcloud_compression.h b/io/include/pcl/compression/organized_pointcloud_compression.h index 4f7624db..5485ae31 100644 --- a/io/include/pcl/compression/organized_pointcloud_compression.h +++ b/io/include/pcl/compression/organized_pointcloud_compression.h @@ -93,7 +93,7 @@ namespace pcl /** \brief Encode raw disparity map and color image. * \note Default values are configured according to the kinect/asus device specifications * \param[in] disparityMap_arg: pointer to raw 16-bit disparity map - * \param[in] disparityMap_arg: pointer to raw 8-bit rgb color image + * \param[in] colorImage_arg: pointer to raw 8-bit rgb color image * \param[in] width_arg: width of disparity map/color image * \param[in] height_arg: height of disparity map/color image * \param[out] compressedDataOut_arg: binary output stream containing compressed data diff --git a/io/include/pcl/io/boost.h b/io/include/pcl/io/boost.h index 329b765f..6b3dd035 100644 --- a/io/include/pcl/io/boost.h +++ b/io/include/pcl/io/boost.h @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,7 @@ #if BOOST_VERSION >= 104900 #include #endif +#include #define BOOST_PARAMETER_MAX_ARITY 7 #include #include diff --git a/io/include/pcl/io/dinast_grabber.h b/io/include/pcl/io/dinast_grabber.h index 631eb4aa..429a2287 100644 --- a/io/include/pcl/io/dinast_grabber.h +++ b/io/include/pcl/io/dinast_grabber.h @@ -118,6 +118,7 @@ namespace pcl /** \brief Send a RX data packet request * \param[in] req_code the request to send (the request field for the setup packet) + * \param buffer * \param[in] length the length field for the setup packet. The data buffer should be at least this size. */ bool @@ -127,6 +128,7 @@ namespace pcl /** \brief Send a TX data packet request * \param[in] req_code the request to send (the request field for the setup packet) + * \param buffer * \param[in] length the length field for the setup packet. The data buffer should be at least this size. */ bool @@ -148,7 +150,7 @@ namespace pcl readImage (); /** \brief Obtains XYZI Point Cloud from the image of the camera - * \param[out] the point cloud from the image data + * \return the point cloud from the image data */ pcl::PointCloud::Ptr getXYZIPointCloud (); diff --git a/io/include/pcl/io/file_io.h b/io/include/pcl/io/file_io.h index ad69f400..481a8210 100644 --- a/io/include/pcl/io/file_io.h +++ b/io/include/pcl/io/file_io.h @@ -43,6 +43,8 @@ #include #include #include +#include +#include namespace pcl { @@ -387,6 +389,77 @@ namespace pcl cloud.fields[field_idx].offset + fields_count * sizeof (uint8_t)], reinterpret_cast (&value), sizeof (uint8_t)); } + + namespace io + { + /** \brief Load a file into a PointCloud2 according to extension. + * \param[in] file_name the name of the file to load + * \param[out] blob the resultant pcl::PointCloud2 blob + * \ingroup io + */ + PCL_EXPORTS int + load (const std::string& file_name, pcl::PCLPointCloud2& blob); + + /** \brief Load a file into a template PointCloud type according to extension. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + template int + load (const std::string& file_name, pcl::PointCloud& cloud); + + /** \brief Load a file into a PolygonMesh according to extension. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant pcl::PolygonMesh + * \ingroup io + */ + PCL_EXPORTS int + load (const std::string& file_name, pcl::PolygonMesh& mesh); + + /** \brief Load a file into a TextureMesh according to extension. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant pcl::TextureMesh + * \ingroup io + */ + PCL_EXPORTS int + load (const std::string& file_name, pcl::TextureMesh& mesh); + + /** \brief Save point cloud data to a binary file when available else to ASCII. + * \param[in] file_name the output file name + * \param[in] blob the point cloud data message + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + PCL_EXPORTS int + save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision = 5); + + /** \brief Save point cloud to a binary file when available else to ASCII. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + template int + save (const std::string& file_name, const pcl::PointCloud& cloud, unsigned precision = 5); + + /** \brief Saves a TextureMesh to a binary file when available else to ASCII. + * \param[in] file_name the name of the file to write to disk + * \param[in] tex_mesh the texture mesh to save + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + PCL_EXPORTS int + save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision = 5); + + /** \brief Saves a PolygonMesh to a binary file when available else to ASCII. + * \param[in] file_name the name of the file to write to disk + * \param[in] mesh the polygonal mesh to save + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + PCL_EXPORTS int + save (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); + } } #endif //#ifndef PCL_IO_FILE_IO_H_ diff --git a/io/include/pcl/io/ifs_io.h b/io/include/pcl/io/ifs_io.h new file mode 100644 index 00000000..80eecaa9 --- /dev/null +++ b/io/include/pcl/io/ifs_io.h @@ -0,0 +1,258 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_IO_IFS_IO_H_ +#define PCL_IO_IFS_IO_H_ + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Indexed Face set (IFS) file format reader. This file format is used for + * the Brown Mesh Set for instance. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS IFSReader + { + public: + /** Empty constructor */ + IFSReader () {} + /** Empty destructor */ + ~IFSReader () {} + + /** \brief we support two versions + * 1.0 classic + * 1.1 with texture coordinates addon + */ + enum + { + IFS_V1_0 = 0, + IFS_V1_1 = 1 + }; + + /** \brief Read a point cloud data header from an IFS file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given IFS file. Useful for fast + * evaluation of the underlying data structure. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only header will be filled) + * \param[out] ifs_version the IFS version of the file (IFS_V1_0 or IFS_V1_1) + * \param[out] data_idx the offset of cloud data within the file + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + int &ifs_version, unsigned int &data_idx); + + /** \brief Read a point cloud data from an IFS file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PCLPointCloud2 blob read from disk + * \param[out] ifs_version the IFS version of the file (either IFS_V1_0 or IFS_V1_1) + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, int &ifs_version); + + /** \brief Read a point cloud data from an IFS file and store it into a PolygonMesh. + * \param[in] file_name the name of the file containing the mesh data + * \param[out] mesh the resultant PolygonMesh + * \param[out] ifs_version the IFS version of the file (either IFS_V1_0 or IFS_V1_1) + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, int &ifs_version); + + /** \brief Read a point cloud data from an IFS file, and convert it to the + * given template pcl::PointCloud format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + template int + read (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::PCLPointCloud2 blob; + int ifs_version; + cloud.sensor_origin_ = Eigen::Vector4f::Zero (); + cloud.sensor_orientation_ = Eigen::Quaternionf::Identity (); + int res = read (file_name, blob, ifs_version); + + // If no error, convert the data + if (res == 0) + pcl::fromPCLPointCloud2 (blob, cloud); + return (res); + } + }; + + /** \brief Point Cloud Data (IFS) file format writer. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS IFSWriter + { + public: + IFSWriter() {} + ~IFSWriter() {} + + /** \brief Save point cloud data to an IFS file containing 3D points. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data + * \param[in] cloud_name the point cloud name to be stored inside the IFS file. + * + * \return + * * 0 on success + * * < 0 on error + */ + int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const std::string &cloud_name = "cloud"); + + /** \brief Save point cloud data to an IFS file containing 3D points. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \param[in] cloud_name the point cloud name to be stored inside the IFS file. + * + * \return + * * 0 on success + * * < 0 on error + */ + template int + write (const std::string &file_name, const pcl::PointCloud &cloud, + const std::string &cloud_name = "cloud") + { + pcl::PCLPointCloud2 blob; + pcl::fromPCLPointCloud2 (blob, cloud); + return (write (file_name, blob, cloud_name)); + } + }; + + namespace io + { + /** \brief Load an IFS file into a PCLPointCloud2 blob type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadIFSFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::IFSReader p; + int ifs_version; + return (p.read (file_name, cloud, ifs_version)); + } + + /** \brief Load any IFS file into a templated PointCloud type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + template inline int + loadIFSFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::IFSReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any IFS file into a PolygonMesh type. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant mesh + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadIFSFile (const std::string &file_name, pcl::PolygonMesh &mesh) + { + pcl::IFSReader p; + int ifs_version; + return (p.read (file_name, mesh, ifs_version)); + } + + /** \brief Save point cloud data to an IFS file containing 3D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + saveIFSFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud) + { + pcl::IFSWriter w; + return (w.write (file_name, cloud)); + } + + /** \brief Save point cloud data to an IFS file containing 3D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + template int + saveIFSFile (const std::string &file_name, const pcl::PointCloud &cloud) + { + pcl::IFSWriter w; + return (w.write (file_name, cloud)); + } + } +} + +#endif //#ifndef PCL_IO_IFS_IO_H_ diff --git a/io/include/pcl/io/image.h b/io/include/pcl/io/image.h new file mode 100644 index 00000000..bc1459c5 --- /dev/null +++ b/io/include/pcl/io/image.h @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include +#ifndef PCL_IO_IMAGE_H_ +#define PCL_IO_IMAGE_H_ + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Image interface class providing an interface to fill a RGB or Grayscale image buffer. + * @param[in] image_metadata + * @ingroup io + */ + class PCL_EXPORTS Image + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef boost::chrono::high_resolution_clock Clock; + typedef boost::chrono::high_resolution_clock::time_point Timestamp; + + typedef enum + { + BAYER_GRBG, + YUV422, + RGB + } Encoding; + + Image (FrameWrapper::Ptr image_metadata) + : wrapper_ (image_metadata) + , timestamp_ (Clock::now ()) + {} + + Image (FrameWrapper::Ptr image_metadata, Timestamp time) + : wrapper_ (image_metadata) + , timestamp_ (time) + {} + + /** + * @brief virtual Destructor that never throws an exception. + */ + inline virtual ~Image () + {} + + /** + * @param[in] input_width width of input image + * @param[in] input_height height of input image + * @param[in] output_width width of desired output image + * @param[in] output_height height of desired output image + * @return wheter the resizing is supported or not. + */ + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, + unsigned output_width, unsigned output_height) const = 0; + + /** + * @brief fills a user given buffer with the RGB values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] rgb_buffer the output RGB buffer. + * @param[in] rgb_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const = 0; + + /** + * @brief returns the encoding of the native data. + * @return encoding + */ + virtual Encoding + getEncoding () const = 0; + + /** + * @brief fills a user given buffer with the raw values. + * @param[in,out] rgb_buffer + */ + virtual void + fillRaw (unsigned char* rgb_buffer) const + { + memcpy (rgb_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + } + + /** + * @brief fills a user given buffer with the gray values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] gray_buffer the output gray buffer. + * @param[in] gray_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, + unsigned gray_line_step = 0) const = 0; + + /** + * @return width of the image + */ + unsigned + getWidth () const + { + return (wrapper_->getWidth ()); + } + + /** + * @return height of the image + */ + unsigned + getHeight () const + { + return (wrapper_->getHeight ()); + } + + /** + * @return frame id of the image. + * @note frame ids are ascending, but not necessarily synchronized with other streams + */ + unsigned + getFrameID () const + { + return (wrapper_->getFrameID ()); + } + + /** + * @return the timestamp of the image + * @note the time value is not synchronized with the system time + */ + pcl::uint64_t + getTimestamp () const + { + return (wrapper_->getTimestamp ()); + } + + + /** + * @return the timestamp of the image + * @note the time value *is* synchronized with the system time. + */ + Timestamp + getSystemTimestamp () const + { + return (timestamp_); + } + + // Get a const pointer to the raw depth buffer + const void* + getData () + { + return (wrapper_->getData ()); + } + + // Data buffer size in bytes + int + getDataSize () const + { + return (wrapper_->getDataSize ()); + } + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} + +#endif //PCL_IO_IMAGE_H_ diff --git a/io/include/pcl/io/image_depth.h b/io/include/pcl/io/image_depth.h new file mode 100644 index 00000000..56867bc3 --- /dev/null +++ b/io/include/pcl/io/image_depth.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#pragma once +#include + +#ifndef PCL_IO_IMAGE_DEPTH_H_ +#define PCL_IO_IMAGE_DEPTH_H_ + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** \brief This class provides methods to fill a depth or disparity image. + */ + class PCL_EXPORTS DepthImage + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef boost::chrono::high_resolution_clock Clock; + typedef boost::chrono::high_resolution_clock::time_point Timestamp; + + /** \brief Constructor + * \param[in] depth_meta_data the actual data from the OpenNI library + * \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for + * Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design. + * \param[in] focal_length focal length of the "stereo" frame. + * \param[in] shadow_value defines which values in the depth data are indicating shadow (resulting from the parallax between projector and IR camera) + * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . + * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. + */ + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, pcl::uint64_t shadow_value, pcl::uint64_t no_sample_value); + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, pcl::uint64_t shadow_value, pcl::uint64_t no_sample_value, Timestamp time); + + /** \brief Destructor. Never throws an exception. */ + ~DepthImage (); + + /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method + * \return the actual depth data of type openni::VideoFrameRef. + */ + const FrameWrapper::Ptr + getMetaData () const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width the width of the desired disparity image. + * \param[in] height the height of the desired disparity image. + * \param[in,out] disparity_buffer the float pointer to the actual memory buffer to be filled with the disparity values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired depth image. + * \param[in] height height the height of the desired depth image. + * \param[in,out] depth_buffer the float pointer to the actual memory buffer to be filled with the depth values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the raw values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired raw image. + * \param[in] height height the height of the desired raw image. + * \param[in,out] depth_buffer the unsigned short pointer to the actual memory buffer to be filled with the raw values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step = 0) const; + + /** \brief method to access the baseline of the "stereo" frame that was used to retrieve the depth image. + * \return baseline in meters + */ + float + getBaseline () const; + + /** \brief method to access the focal length of the "stereo" frame that was used to retrieve the depth image. + * \return focal length in pixels + */ + float + getFocalLength () const; + + /** \brief method to access the shadow value, that indicates pixels lying in shadow in the depth image. + * \return shadow value + */ + pcl::uint64_t + getShadowValue () const; + + /** \brief method to access the no-sample value, that indicates pixels where no disparity could be determined for the depth image. + * \return no-sample value + */ + pcl::uint64_t + getNoSampleValue () const; + + /** \return the width of the depth image */ + unsigned + getWidth () const; + + /** \return the height of the depth image */ + unsigned + getHeight () const; + + /** \return an ascending id for the depth frame + * \attention not necessarily synchronized with other streams + */ + unsigned + getFrameID () const; + + /** \return a ascending timestamp for the depth frame + * \attention its not the system time, thus can not be used directly to synchronize different sensors. + * But definitely synchronized with other streams + */ + pcl::uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + pcl::io::FrameWrapper::Ptr wrapper_; + + float baseline_; + float focal_length_; + pcl::uint64_t shadow_value_; + pcl::uint64_t no_sample_value_; + Timestamp timestamp_; + }; + +}} // namespace + +#endif // PCL_IO_IMAGE_DEPTH_H_ diff --git a/io/include/pcl/io/image_grabber.h b/io/include/pcl/io/image_grabber.h index 7437c85f..52251475 100644 --- a/io/include/pcl/io/image_grabber.h +++ b/io/include/pcl/io/image_grabber.h @@ -62,6 +62,7 @@ namespace pcl * \param[in] directory Directory which contains an ordered set of images corresponding to an [RGB]D video, stored as TIFF, PNG, JPG, or PPM files. The naming convention is: frame_[timestamp]_["depth"/"rgb"].[extension] * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. * \param[in] repeat whether to play PCD file in an endless loop or not. + * \param pclzf_mode */ ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode); @@ -147,7 +148,7 @@ namespace pcl /** \brief Query only the timestamp of an index, if it exists */ bool - getTimestampAtIndex (size_t idx, uint64_t ×tamp) const; + getTimestampAtIndex (size_t idx, pcl::uint64_t ×tamp) const; /** \brief Manually set RGB image files. * \param[in] rgb_image_files A vector of [tiff/png/jpg/ppm] files to use as input. There must be a 1-to-1 correspondence between these and the depth images you set @@ -167,7 +168,7 @@ namespace pcl const double principal_point_x, const double principal_point_y); - /** \brief Get the current focal length and center pixel. If the intrinsics have been manually set with @setCameraIntrinsics@, this will return those values. Else, if start () has been called and the grabber has found a frame_[timestamp].xml file, this will return the most recent values read. Else, returns factory defaults. + /** \brief Get the current focal length and center pixel. If the intrinsics have been manually set with setCameraIntrinsics, this will return those values. Else, if start () has been called and the grabber has found a frame_[timestamp].xml file, this will return the most recent values read. Else, returns factory defaults. * \param[out] focal_length_x Horizontal focal length (fx) * \param[out] focal_length_y Vertical focal length (fy) * \param[out] principal_point_x Horizontal coordinates of the principal point (cx) @@ -190,7 +191,8 @@ namespace pcl setNumberOfThreads (unsigned int nr_threads = 0); protected: - /** \brief Convenience function to see how many frames this consists of */ + /** \brief Convenience function to see how many frames this consists of + */ size_t numFrames () const; diff --git a/io/include/pcl/io/image_ir.h b/io/include/pcl/io/image_ir.h new file mode 100644 index 00000000..ce6a14ea --- /dev/null +++ b/io/include/pcl/io/image_ir.h @@ -0,0 +1,114 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ +#ifndef PCL_IO_IMAGE_IR_H_ +#define PCL_IO_IMAGE_IR_H_ + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Class containing just a reference to IR meta data. + */ + class PCL_EXPORTS IRImage + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef boost::chrono::high_resolution_clock Clock; + typedef boost::chrono::high_resolution_clock::time_point Timestamp; + + IRImage (FrameWrapper::Ptr ir_metadata); + IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time); + + ~IRImage () throw () + {} + + void + fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const; + + unsigned + getWidth () const; + + unsigned + getHeight () const; + + unsigned + getFrameID () const; + + pcl::uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer. If the data is accessed just read-only, then this method is faster than a fillXXX method + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + /** \brief method to access the internal data structure wrapper, which needs to be cast to an + * approperate subclass before the getMetadata(..) function is available to access the native data type. + */ + const FrameWrapper::Ptr + getMetaData () const; + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_IR_H_ diff --git a/io/include/pcl/io/image_metadata_wrapper.h b/io/include/pcl/io/image_metadata_wrapper.h new file mode 100644 index 00000000..19dcb827 --- /dev/null +++ b/io/include/pcl/io/image_metadata_wrapper.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#ifndef PCL_IO_IMAGE_METADATA_WRAPPER_H_ +#define PCL_IO_IMAGE_METADATA_WRAPPER_H_ + +#include +#include + +namespace pcl +{ + namespace io + { + + /** + * Pure abstract interface to wrap native frame data types. + */ + class FrameWrapper + { + public: + typedef boost::shared_ptr Ptr; + + virtual const void* + getData () const = 0; + + virtual unsigned + getDataSize () const = 0; + + virtual unsigned + getWidth () const = 0; + + virtual unsigned + getHeight () const = 0; + + virtual unsigned + getFrameID () const = 0; + + // Microseconds from some arbitrary start point + virtual pcl::uint64_t + getTimestamp () const = 0; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_METADATA_WRAPPER_H_ diff --git a/io/include/pcl/io/image_rgb24.h b/io/include/pcl/io/image_rgb24.h new file mode 100644 index 00000000..41c155df --- /dev/null +++ b/io/include/pcl/io/image_rgb24.h @@ -0,0 +1,91 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include + +#ifndef PCL_IO_IMAGE_RGB_H_ +#define PCL_IO_IMAGE_RGB_H_ + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image. + * @ingroup io + */ + class PCL_EXPORTS ImageRGB24 : public pcl::io::Image + { + public: + + ImageRGB24 (FrameWrapper::Ptr image_metadata); + ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + virtual ~ImageRGB24 () throw (); + + inline virtual Encoding + getEncoding () const + { + return (RGB); + } + + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const; + + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const; + + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const; + + private: + + // Struct used for type conversion + typedef struct + { + uint8_t r; + uint8_t g; + uint8_t b; + } RGB888Pixel; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_RGB_H_ diff --git a/io/include/pcl/io/image_yuv422.h b/io/include/pcl/io/image_yuv422.h new file mode 100644 index 00000000..61ecdd74 --- /dev/null +++ b/io/include/pcl/io/image_yuv422.h @@ -0,0 +1,79 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ +#include + +#ifndef PCL_IO_IMAGE_YUV422_H_ +#define PCL_IO_IMAGE_YUV422_H_ +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief Concrete implementation of the interface Image for a YUV 422 image used by Primesense devices. + * @ingroup io + */ + class PCL_EXPORTS ImageYUV422 : public pcl::io::Image + { + public: + ImageYUV422 (FrameWrapper::Ptr image_metadata); + ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + + virtual ~ImageYUV422 () throw (); + + inline virtual Encoding + getEncoding () const + { + return (YUV422); + } + + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const; + + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const; + + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_YUV22_H_ diff --git a/io/include/pcl/io/impl/lzf_image_io.hpp b/io/include/pcl/io/impl/lzf_image_io.hpp index b3323c3f..c6ddb44b 100644 --- a/io/include/pcl/io/impl/lzf_image_io.hpp +++ b/io/include/pcl/io/impl/lzf_image_io.hpp @@ -79,9 +79,9 @@ pcl::io::LZFDepth16ImageReader::read ( register int depth_idx = 0, point_idx = 0; double constant_x = 1.0 / parameters_.focal_length_x, constant_y = 1.0 / parameters_.focal_length_y; - for (int v = 0; v < cloud.height; ++v) + for (uint32_t v = 0; v < cloud.height; ++v) { - for (register int u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2) + for (register uint32_t u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2) { PointT &pt = cloud.points[point_idx]; unsigned short val; @@ -326,7 +326,7 @@ pcl::io::LZFYUV422ImageReader::read ( unsigned char *color_v = reinterpret_cast (&uncompressed_data[wh2 + getWidth () * getHeight ()]); register int y_idx = 0; - for (size_t i = 0; i < wh2; ++i, y_idx += 2) + for (int i = 0; i < wh2; ++i, y_idx += 2) { int v = color_v[i] - 128; int u = color_u[i] - 128; diff --git a/io/include/pcl/io/impl/point_cloud_image_extractors.hpp b/io/include/pcl/io/impl/point_cloud_image_extractors.hpp index a5d543bf..bb37f19a 100644 --- a/io/include/pcl/io/impl/point_cloud_image_extractors.hpp +++ b/io/include/pcl/io/impl/point_cloud_image_extractors.hpp @@ -38,6 +38,7 @@ #ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ #define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ +#include #include #include #include @@ -46,11 +47,28 @@ /////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::io::PointCloudImageExtractorFromNormalField::extract (const PointCloud& cloud, pcl::PCLImage& img) const +pcl::io::PointCloudImageExtractor::extract (const PointCloud& cloud, pcl::PCLImage& img) const { if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height) return (false); + bool result = this->extractImpl (cloud, img); + + if (paint_nans_with_black_ && result) + { + size_t size = img.encoding == "mono16" ? 2 : 3; + for (size_t i = 0; i < cloud.points.size (); ++i) + if (!pcl::isFinite (cloud[i])) + std::memset (&img.data[i * size], 0, size); + } + + return (result); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::PointCloudImageExtractorFromNormalField::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const +{ std::vector fields; int field_x_idx = pcl::getFieldIndex (cloud, "normal_x", fields); int field_y_idx = pcl::getFieldIndex (cloud, "normal_y", fields); @@ -85,11 +103,8 @@ pcl::io::PointCloudImageExtractorFromNormalField::extract (const PointCl /////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::io::PointCloudImageExtractorFromRGBField::extract (const PointCloud& cloud, pcl::PCLImage& img) const +pcl::io::PointCloudImageExtractorFromRGBField::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const { - if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height) - return (false); - std::vector fields; int field_idx = pcl::getFieldIndex (cloud, "rgb", fields); if (field_idx == -1) @@ -118,13 +133,273 @@ pcl::io::PointCloudImageExtractorFromRGBField::extract (const PointCloud return (true); } +// Lookup table is copied from (excluding the first entry): +// https://github.com/fiji/fiji/blob/master/luts/glasbey.lut +const uint8_t GLASBEY_LUT[] = +{ + 0, 0, 255, + 255, 0, 0, + 0, 255, 0, + 0, 0, 51, + 255, 0, 182, + 0, 83, 0, + 255, 211, 0, + 0, 159, 255, + 154, 77, 66, + 0, 255, 190, + 120, 63, 193, + 31, 150, 152, + 255, 172, 253, + 177, 204, 113, + 241, 8, 92, + 254, 143, 66, + 221, 0, 255, + 32, 26, 1, + 114, 0, 85, + 118, 108, 149, + 2, 173, 36, + 200, 255, 0, + 136, 108, 0, + 255, 183, 159, + 133, 133, 103, + 161, 3, 0, + 20, 249, 255, + 0, 71, 158, + 220, 94, 147, + 147, 212, 255, + 0, 76, 255, + 0, 66, 80, + 57, 167, 106, + 238, 112, 254, + 0, 0, 100, + 171, 245, 204, + 161, 146, 255, + 164, 255, 115, + 255, 206, 113, + 71, 0, 21, + 212, 173, 197, + 251, 118, 111, + 171, 188, 0, + 117, 0, 215, + 166, 0, 154, + 0, 115, 254, + 165, 93, 174, + 98, 132, 2, + 0, 121, 168, + 0, 255, 131, + 86, 53, 0, + 159, 0, 63, + 66, 45, 66, + 255, 242, 187, + 0, 93, 67, + 252, 255, 124, + 159, 191, 186, + 167, 84, 19, + 74, 39, 108, + 0, 16, 166, + 145, 78, 109, + 207, 149, 0, + 195, 187, 255, + 253, 68, 64, + 66, 78, 32, + 106, 1, 0, + 181, 131, 84, + 132, 233, 147, + 96, 217, 0, + 255, 111, 211, + 102, 75, 63, + 254, 100, 0, + 228, 3, 127, + 17, 199, 174, + 210, 129, 139, + 91, 118, 124, + 32, 59, 106, + 180, 84, 255, + 226, 8, 210, + 0, 1, 20, + 93, 132, 68, + 166, 250, 255, + 97, 123, 201, + 98, 0, 122, + 126, 190, 58, + 0, 60, 183, + 255, 253, 0, + 7, 197, 226, + 180, 167, 57, + 148, 186, 138, + 204, 187, 160, + 55, 0, 49, + 0, 40, 1, + 150, 122, 129, + 39, 136, 38, + 206, 130, 180, + 150, 164, 196, + 180, 32, 128, + 110, 86, 180, + 147, 0, 185, + 199, 48, 61, + 115, 102, 255, + 15, 187, 253, + 172, 164, 100, + 182, 117, 250, + 216, 220, 254, + 87, 141, 113, + 216, 85, 34, + 0, 196, 103, + 243, 165, 105, + 216, 255, 182, + 1, 24, 219, + 52, 66, 54, + 255, 154, 0, + 87, 95, 1, + 198, 241, 79, + 255, 95, 133, + 123, 172, 240, + 120, 100, 49, + 162, 133, 204, + 105, 255, 220, + 198, 82, 100, + 121, 26, 64, + 0, 238, 70, + 231, 207, 69, + 217, 128, 233, + 255, 211, 209, + 209, 255, 141, + 36, 0, 3, + 87, 163, 193, + 211, 231, 201, + 203, 111, 79 , + 62, 24, 0, + 0, 117, 223, + 112, 176, 88 , + 209, 24, 0, + 0, 30, 107, + 105, 200, 197, + 255, 203, 255, + 233, 194, 137, + 191, 129, 46, + 69, 42, 145, + 171, 76, 194, + 14, 117, 61, + 0, 30, 25, + 118, 73, 127, + 255, 169, 200, + 94, 55, 217, + 238, 230, 138, + 159, 54, 33, + 80, 0, 148, + 189, 144, 128, + 0, 109, 126, + 88, 223, 96, + 71, 80, 103, + 1, 93, 159, + 99, 48, 60, + 2, 206, 148, + 139, 83, 37, + 171, 0, 255, + 141, 42, 135, + 85, 83, 148, + 150, 255, 0, + 0, 152, 123, + 255, 138, 203, + 222, 69, 200, + 107, 109, 230, + 30, 0, 68, + 173, 76, 138, + 255, 134, 161, + 0, 35, 60, + 138, 205, 0, + 111, 202, 157, + 225, 75, 253, + 255, 176, 77, + 229, 232, 57, + 114, 16, 255, + 111, 82, 101, + 134, 137, 48, + 99, 38, 80, + 105, 38, 32, + 200, 110, 0, + 209, 164, 255, + 198, 210, 86, + 79, 103, 77, + 174, 165, 166, + 170, 45, 101, + 199, 81, 175, + 255, 89, 172, + 146, 102, 78, + 102, 134, 184, + 111, 152, 255, + 92, 255, 159, + 172, 137, 178, + 210, 34, 98, + 199, 207, 147, + 255, 185, 30, + 250, 148, 141, + 49, 34, 78, + 254, 81, 97, + 254, 141, 100, + 68, 54, 23, + 201, 162, 84, + 199, 232, 240, + 68, 152, 0, + 147, 172, 58, + 22, 75, 28, + 8, 84, 121, + 116, 45, 0, + 104, 60, 255, + 64, 41, 38, + 164, 113, 215, + 207, 0, 155, + 118, 1, 35, + 83, 0, 88, + 0, 82, 232, + 43, 92, 87, + 160, 217, 146, + 176, 26, 229, + 29, 3, 36, + 122, 58, 159, + 214, 209, 207, + 160, 100, 105, + 106, 157, 160, + 153, 219, 113, + 192, 56, 207, + 125, 255, 89, + 149, 0, 34, + 213, 162, 223, + 22, 131, 204, + 166, 249, 69, + 109, 105, 97, + 86, 188, 78, + 255, 109, 81, + 255, 3, 248, + 255, 0, 73, + 202, 0, 35, + 67, 109, 18, + 234, 170, 173, + 191, 165, 0, + 38, 44, 51, + 85, 185, 2, + 121, 182, 158, + 254, 236, 212, + 139, 165, 89, + 141, 254, 193, + 0, 60, 43, + 63, 17, 40, + 255, 221, 246, + 17, 26, 146, + 154, 66, 84, + 149, 157, 238, + 126, 130, 72, + 58, 6, 101, + 189, 117, 101, +}; + +const size_t GLASBEY_LUT_SIZE = sizeof (GLASBEY_LUT) / sizeof (GLASBEY_LUT[0]); + /////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::io::PointCloudImageExtractorFromLabelField::extract (const PointCloud& cloud, pcl::PCLImage& img) const +pcl::io::PointCloudImageExtractorFromLabelField::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const { - if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height) - return (false); - std::vector fields; int field_idx = pcl::getFieldIndex (cloud, "label", fields); if (field_idx == -1) @@ -178,6 +453,44 @@ pcl::io::PointCloudImageExtractorFromLabelField::extract (const PointClo } break; } + case COLORS_RGB_GLASBEY: + { + img.encoding = "rgb8"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned char) * 3; + img.data.resize (img.step * img.height); + + std::srand(std::time(0)); + std::set labels; + std::map colormap; + + // First pass: find unique labels + for (size_t i = 0; i < cloud.points.size (); ++i) + { + uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + labels.insert (val); + } + + // Assign Glasbey colors in ascending order of labels + size_t color = 0; + for (std::set::iterator iter = labels.begin (); iter != labels.end (); ++iter) + { + colormap[*iter] = color % GLASBEY_LUT_SIZE; + ++color; + } + + // Second pass: copy colors from the LUT + for (size_t i = 0; i < cloud.points.size (); ++i) + { + uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + memcpy (&img.data[i * 3], &GLASBEY_LUT[colormap[val] * 3], 3); + } + + break; + } } return (true); @@ -185,11 +498,8 @@ pcl::io::PointCloudImageExtractorFromLabelField::extract (const PointClo /////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::io::PointCloudImageExtractorWithScaling::extract (const PointCloud& cloud, pcl::PCLImage& img) const +pcl::io::PointCloudImageExtractorWithScaling::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const { - if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height) - return (false); - std::vector fields; int field_idx = pcl::getFieldIndex (cloud, field_name_, fields); if (field_idx == -1) diff --git a/io/include/pcl/io/impl/vtk_lib_io.hpp b/io/include/pcl/io/impl/vtk_lib_io.hpp index fcd52f95..695e3464 100644 --- a/io/include/pcl/io/impl/vtk_lib_io.hpp +++ b/io/include/pcl/io/impl/vtk_lib_io.hpp @@ -51,6 +51,7 @@ #ifdef __GNUC__ #pragma GCC system_header #endif +#include #include #include #include @@ -377,7 +378,7 @@ pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud& cloud, vtkPolyD // Add 0D topology to every point vtkSmartPointer vertex_glyph_filter = vtkSmartPointer::New (); - #if VTK_MAJOR_VERSION <= 5 + #if VTK_MAJOR_VERSION < 6 vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ()); #else vertex_glyph_filter->SetInputData (temp_polydata); diff --git a/io/include/pcl/io/io_exception.h b/io/include/pcl/io/io_exception.h new file mode 100644 index 00000000..0586cf48 --- /dev/null +++ b/io/include/pcl/io/io_exception.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#ifndef PCL_IO_EXCEPTION_H_ +#define PCL_IO_EXCEPTION_H_ + +#include +#include +#include +#include + + +//fom +#if defined _WIN32 && defined _MSC_VER && !defined __PRETTY_FUNCTION__ + #define __PRETTY_FUNCTION__ __FUNCTION__ +#endif + + +#define THROW_IO_EXCEPTION(format,...) throwIOException( __PRETTY_FUNCTION__, __FILE__, __LINE__, format , ##__VA_ARGS__ ) + + +namespace pcl +{ + namespace io + { + /** + * @brief General IO exception class + */ + class IOException : public std::exception + { + public: + IOException (const std::string& function_name, + const std::string& file_name, + unsigned line_number, + const std::string& message); + + virtual ~IOException () throw (); + + IOException& + operator= (const IOException& exception); + + virtual const char* + what () const throw (); + + const std::string& + getFunctionName () const; + + const std::string& + getFileName () const; + + unsigned + getLineNumber () const; + + protected: + std::string function_name_; + std::string file_name_; + unsigned line_number_; + std::string message_; + std::string message_long_; + }; + + inline void + throwIOException (const char* function, const char* file, unsigned line, const char* format, ...) + { + static char msg[1024]; + va_list args; + va_start (args, format); + vsnprintf (msg, 1024, format, args); + throw IOException (function, file, line, msg); + } + } // namespace +} +#endif // PCL_IO_EXCEPTION_H_ diff --git a/io/include/pcl/io/lzf_image_io.h b/io/include/pcl/io/lzf_image_io.h index f24e8b34..bdee4377 100644 --- a/io/include/pcl/io/lzf_image_io.h +++ b/io/include/pcl/io/lzf_image_io.h @@ -143,7 +143,8 @@ namespace pcl /** \brief Load a compressed image array from disk * \param[in] filename the file name to load the data from - * \param[out] data_size the size of the data + * \param[out] data the size of the data + * \param uncompressed_size * \return an array filled with the data loaded from disk, NULL if error */ bool @@ -214,7 +215,7 @@ namespace pcl unsigned int num_threads=0); /** \brief Read camera parameters from a given stream and store them internally. - * The parameters will be read from the ... tag. + * The parameters will be read from the \ ... \ tag. * \return true if operation successful, false otherwise */ virtual bool @@ -265,7 +266,7 @@ namespace pcl unsigned int num_threads=0); /** \brief Read camera parameters from a given stream and store them internally. - * The parameters will be read from the ... tag. + * The parameters will be read from the \ ... \ tag. * \return true if operation successful, false otherwise */ virtual bool @@ -506,12 +507,12 @@ namespace pcl * \param[in] filename the file name to write * \return true if operation successful, false otherwise * This overwrites the following parameters in the xml file, under the - * tag: - * ... - * ... - * ... - * ... - * ... + * \ tag: + * \...\ + * \...\ + * \...\ + * \...\ + * \...\ */ virtual bool writeParameters (const CameraParameters ¶meters, diff --git a/io/include/pcl/io/obj_io.h b/io/include/pcl/io/obj_io.h index beab8fd2..29c371cf 100644 --- a/io/include/pcl/io/obj_io.h +++ b/io/include/pcl/io/obj_io.h @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2013, Open Perception, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,8 +32,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id: obj_io.h 1001 2011-07-13 13:07:00 ktran $ - * */ #ifndef OBJ_IO_H_ @@ -40,11 +39,286 @@ #include #include #include +#include namespace pcl { + class PCL_EXPORTS MTLReader + { + public: + /** \brief empty constructor */ + MTLReader (); + + /** \brief empty destructor */ + virtual ~MTLReader() {} + + /** \brief Read a MTL file given its full path. + * \param[in] filename full path to MTL file + * \return 0 on success < 0 else. + */ + int + read (const std::string& filename); + + /** \brief Read a MTL file given an OBJ file full path and the MTL file name. + * \param[in] obj_file_name full path to OBJ file + * \param[in] mtl_file_name MTL file name + * \return 0 on success < 0 else. + */ + int + read (const std::string& obj_file_name, const std::string& mtl_file_name); + + std::vector::const_iterator + getMaterial (const std::string& material_name) const; + + /// materials array + std::vector materials_; + + private: + /// converts CIE XYZ to RGB + inline void + cie2rgb (const Eigen::Vector3f& xyz, pcl::TexMaterial::RGB& rgb) const; + /// fill a pcl::TexMaterial::RGB from a split line containing CIE x y z values + int + fillRGBfromXYZ (const std::vector& split_line, pcl::TexMaterial::RGB& rgb); + /// fill a pcl::TexMaterial::RGB from a split line containing r g b values + int + fillRGBfromRGB (const std::vector& split_line, pcl::TexMaterial::RGB& rgb); + /// matrix to convert CIE to RGB + Eigen::Matrix3f xyz_to_rgb_matrix_; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + class PCL_EXPORTS OBJReader : public FileReader + { + public: + /** \brief empty constructor */ + OBJReader() {} + /** \brief empty destructor */ + virtual ~OBJReader() {} + /** \brief Read a point cloud data header from a FILE file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given FILE file. Useful for fast + * evaluation of the underlying data structure. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin always null + * \param[out] orientation the sensor acquisition orientation always identity + * \param[out] file_version always 0 + * \param data_type + * \param data_idx + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + * + * \return 0 on success. + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, int &data_type, unsigned int &data_idx, + const int offset); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin always null + * \param[out] orientation the sensor acquisition orientation always identity + * \param[out] file_version always 0 + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset = 0); + + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/TextureMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant TextureMesh read from disk + * \param[out] origin the sensor origin always null + * \param[out] orientation the sensor orientation always identity + * \param[out] file_version always 0 + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::TextureMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/TextureMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant TextureMesh read from disk + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::TextureMesh &mesh, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PolygonMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant PolygonMesh read from disk + * \param[out] origin the sensor origin always null + * \param[out] orientation the sensor orientation always identity + * \param[out] file_version always 0 + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PolygonMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant PolygonMesh read from disk + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset = 0); + + /** \brief Read a point cloud data from any FILE file, and convert it to the given + * template format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + template inline int + read (const std::string &file_name, pcl::PointCloud &cloud, + const int offset =0) + { + pcl::PCLPointCloud2 blob; + int file_version; + int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, + file_version, offset); + if (res < 0) + return (res); + + pcl::fromPCLPointCloud2 (blob, cloud); + return (0); + } + + private: + /// Usually OBJ files come MTL files where texture materials are stored + std::vector companions_; + }; + namespace io { + /** \brief Load any OBJ file into a templated PointCloud type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \param[out] origin the sensor acquisition origin, null + * \param[out] orientation the sensor acquisition orientation, identity + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) + { + pcl::OBJReader p; + int obj_version; + return (p.read (file_name, cloud, origin, orientation, obj_version)); + } + + /** \brief Load an OBJ file into a PCLPointCloud2 blob type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::OBJReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any OBJ file into a templated PointCloud type + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + template inline int + loadOBJFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::OBJReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any OBJ file into a PolygonMesh type. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant mesh + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::PolygonMesh &mesh) + { + pcl::OBJReader p; + return (p.read (file_name, mesh)); + } + + /** \brief Load any OBJ file into a TextureMesh type. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant mesh + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::TextureMesh &mesh) + { + pcl::OBJReader p; + return (p.read (file_name, mesh)); + } + /** \brief Saves a TextureMesh in ascii OBJ format. * \param[in] file_name the name of the file to write to disk * \param[in] tex_mesh the texture mesh to save @@ -52,8 +326,8 @@ namespace pcl * \ingroup io */ PCL_EXPORTS int - saveOBJFile (const std::string &file_name, - const pcl::TextureMesh &tex_mesh, + saveOBJFile (const std::string &file_name, + const pcl::TextureMesh &tex_mesh, unsigned precision = 5); /** \brief Saves a PolygonMesh in ascii PLY format. @@ -63,8 +337,8 @@ namespace pcl * \ingroup io */ PCL_EXPORTS int - saveOBJFile (const std::string &file_name, - const pcl::PolygonMesh &mesh, + saveOBJFile (const std::string &file_name, + const pcl::PolygonMesh &mesh, unsigned precision = 5); } diff --git a/io/include/pcl/io/openni2/openni.h b/io/include/pcl/io/openni2/openni.h new file mode 100644 index 00000000..b9f43dea --- /dev/null +++ b/io/include/pcl/io/openni2/openni.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include +#ifdef HAVE_OPENNI2 + +#ifndef PCL_IO_OPENNI2_OPENNI_H_ +#define PCL_IO_OPENNI2_OPENNI_H_ + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +// Standard resolutions, ported from OpenNI 1.x. To be removed later. +#define XN_QQVGA_X_RES 160 +#define XN_QQVGA_Y_RES 120 +#define XN_CGA_X_RES 320 +#define XN_CGA_Y_RES 200 +#define XN_QVGA_X_RES 320 +#define XN_QVGA_Y_RES 240 +#define XN_VGA_X_RES 640 +#define XN_VGA_Y_RES 480 +#define XN_SVGA_X_RES 800 +#define XN_SVGA_Y_RES 600 +#define XN_XGA_X_RES 1024 +#define XN_XGA_Y_RES 768 +#define XN_720P_X_RES 1280 +#define XN_720P_Y_RES 720 +#define XN_SXGA_X_RES 1280 +#define XN_SXGA_Y_RES 1024 +#define XN_UXGA_X_RES 1600 +#define XN_UXGA_Y_RES 1200 +#define XN_1080P_X_RES 1920 +#define XN_1080P_Y_RES 1080 +#define XN_QCIF_X_RES 176 +#define XN_QCIF_Y_RES 144 +#define XN_240P_X_RES 423 +#define XN_240P_Y_RES 240 +#define XN_CIF_X_RES 352 +#define XN_CIF_Y_RES 288 +#define XN_WVGA_X_RES 640 +#define XN_WVGA_Y_RES 360 +#define XN_480P_X_RES 864 +#define XN_480P_Y_RES 480 +#define XN_576P_X_RES 1024 +#define XN_576P_Y_RES 576 +#define XN_DV_X_RES 960 +#define XN_DV_Y_RES 720 + +#endif // PCL_IO_OPENNI2_OPENNI_H_ +#endif // HAVE_OPENNI2 diff --git a/io/include/pcl/io/openni2/openni2_convert.h b/io/include/pcl/io/openni2/openni2_convert.h new file mode 100644 index 00000000..bfc439e3 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_convert.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + + +#ifndef PCL_IO_OPENNI2_CONVERT_H_ +#define PCL_IO_OPENNI2_CONVERT_H_ + +#include "pcl/io/openni2/openni2_device_info.h" +#include "pcl/io/openni2/openni2_video_mode.h" + +#include "OpenNI.h" + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + const OpenNI2DeviceInfo + openni2_convert (const openni::DeviceInfo* pInfo); + + const openni::VideoMode + grabberModeToOpenniMode (const OpenNI2VideoMode& input); + + const OpenNI2VideoMode + openniModeToGrabberMode (const openni::VideoMode& input); + + const std::vector + openniModeToGrabberMode (const openni::Array& input); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_CONVERT_H_ diff --git a/io/include/pcl/io/openni2/openni2_device.h b/io/include/pcl/io/openni2/openni2_device.h new file mode 100644 index 00000000..354de9f5 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_device.h @@ -0,0 +1,334 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_IO_OPENNI2_DEVICE_H_ +#define PCL_IO_OPENNI2_DEVICE_H_ + +#include +#include "openni.h" +#include "pcl/io/openni2/openni2_video_mode.h" +#include "pcl/io/io_exception.h" + +#include +#include +#include +#include +#include +#include + +// Template frame wrappers +#include +#include +#include + + + +namespace openni +{ + class Device; + class DeviceInfo; + class VideoStream; + class SensorInfo; +} + +namespace pcl +{ + namespace io + { + namespace openni2 + { + typedef pcl::io::DepthImage DepthImage; + typedef pcl::io::IRImage IRImage; + typedef pcl::io::Image Image; + + class OpenNI2FrameListener; + + class PCL_EXPORTS OpenNI2Device + { + public: + + typedef boost::function, void* cookie) > ImageCallbackFunction; + typedef boost::function, void* cookie) > DepthImageCallbackFunction; + typedef boost::function, void* cookie) > IRImageCallbackFunction; + typedef unsigned CallbackHandle; + + typedef boost::function StreamCallbackFunction; + + OpenNI2Device (const std::string& device_URI); + virtual ~OpenNI2Device (); + + const std::string + getUri () const; + const std::string + getVendor () const; + const std::string + getName () const; + uint16_t + getUsbVendorId () const; + uint16_t + getUsbProductId () const; + + const std::string + getStringID () const; + + bool + isValid () const; + + bool + hasIRSensor () const; + bool + hasColorSensor () const; + bool + hasDepthSensor () const; + + void + startIRStream (); + void + startColorStream (); + void + startDepthStream (); + + void + stopAllStreams (); + + void + stopIRStream (); + void + stopColorStream (); + void + stopDepthStream (); + + bool + isIRStreamStarted (); + bool + isColorStreamStarted (); + bool + isDepthStreamStarted (); + + bool + isImageRegistrationModeSupported () const; + void + setImageRegistrationMode (bool enabled); + bool + isDepthRegistered () const; + + const OpenNI2VideoMode + getIRVideoMode (); + const OpenNI2VideoMode + getColorVideoMode (); + const OpenNI2VideoMode + getDepthVideoMode (); + + const std::vector& + getSupportedIRVideoModes () const; + const std::vector& + getSupportedColorVideoModes () const; + const std::vector& + getSupportedDepthVideoModes () const; + + bool + isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + bool + isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + bool + isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + + bool + findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + bool + findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + bool + findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + + void + setIRVideoMode (const OpenNI2VideoMode& video_mode); + void + setColorVideoMode (const OpenNI2VideoMode& video_mode); + void + setDepthVideoMode (const OpenNI2VideoMode& video_mode); + + OpenNI2VideoMode + getDefaultIRMode () const; + OpenNI2VideoMode + getDefaultColorMode () const; + OpenNI2VideoMode + getDefaultDepthMode () const; + + float + getIRFocalLength () const; + float + getColorFocalLength () const; + float + getDepthFocalLength () const; + + // Baseline between sensors. Returns 0 if this value does not exist. + float + getBaseline(); + + // Value of pixels in shadow or that have no valid measurement + uint64_t + getShadowValue(); + + void + setAutoExposure (bool enable); + void + setAutoWhiteBalance (bool enable); + + inline bool + isSynchronized () + { + return (openni_device_->getDepthColorSyncEnabled ()); + } + + inline bool + isSynchronizationSupported () + { + return (true); // Not sure how to query this from the hardware + } + + inline bool + isFile() + { + return (openni_device_->isFile()); + } + + void + setSynchronization (bool enableSync); + + bool + getAutoExposure () const; + bool + getAutoWhiteBalance () const; + + void + setUseDeviceTimer (bool enable); + + /** \brief Get absolut number of depth frames in the current stream. + * This function returns 0 if the current device is not a file stream or + * if the current mode has no depth stream. + */ + int + getDepthFrameCount (); + + /** \brief Get absolut number of color frames in the current stream. + * This function returns 0 if the current device is not a file stream or + * if the current mode has no color stream. + */ + int + getColorFrameCount (); + + /** \brief Get absolut number of ir frames in the current stream. + * This function returns 0 if the current device is not a file stream or + * if the current mode has no ir stream. + */ + int + getIRFrameCount (); + + /** \brief Set the playback speed if the device is an recorded stream. + * If setting the device playback speed fails, because the device is no recorded stream or + * any other reason this function returns false. Otherwise true is returned. + * \param[in] speed The playback speed factor 1.0 means the same speed as recorded, + * 0.5 half the speed, 2.0 double speed and so on. + * \return True on success, false otherwise. + */ + bool + setPlaybackSpeed (double speed); + + /************************************************************************************/ + // Callbacks from openni::VideoStream to grabber. Internal interface + void + setColorCallback (StreamCallbackFunction color_callback); + void + setDepthCallback (StreamCallbackFunction depth_callback); + void + setIRCallback (StreamCallbackFunction ir_callback); + + protected: + void shutdown (); + + boost::shared_ptr + getIRVideoStream () const; + boost::shared_ptr + getColorVideoStream () const; + boost::shared_ptr + getDepthVideoStream () const; + + + void + processColorFrame (openni::VideoStream& stream); + void + processDepthFrame (openni::VideoStream& stream); + void + processIRFrame (openni::VideoStream& stream); + + + bool + findCompatibleVideoMode (const std::vector supportedModes, + const OpenNI2VideoMode& output_mode, OpenNI2VideoMode& mode) const; + + bool + resizingSupported (size_t input_width, size_t input_height, size_t output_width, size_t output_height) const; + + // Members + + boost::shared_ptr openni_device_; + boost::shared_ptr device_info_; + + boost::shared_ptr ir_frame_listener; + boost::shared_ptr color_frame_listener; + boost::shared_ptr depth_frame_listener; + + mutable boost::shared_ptr ir_video_stream_; + mutable boost::shared_ptr color_video_stream_; + mutable boost::shared_ptr depth_video_stream_; + + mutable std::vector ir_video_modes_; + mutable std::vector color_video_modes_; + mutable std::vector depth_video_modes_; + + bool ir_video_started_; + bool color_video_started_; + bool depth_video_started_; + + /** \brief distance between the projector and the IR camera in meters*/ + float baseline_; + /** the value for shadow (occluded pixels) */ + uint64_t shadow_value_; + /** the value for pixels without a valid disparity measurement */ + uint64_t no_sample_value_; + }; + + PCL_EXPORTS std::ostream& operator<< (std::ostream& stream, const OpenNI2Device& device); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_DEVICE_H_ diff --git a/io/include/pcl/io/openni2/openni2_device_info.h b/io/include/pcl/io/openni2/openni2_device_info.h new file mode 100644 index 00000000..2954e45e --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_device_info.h @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_DEVICE_INFO_H_ +#define PCL_IO_OPENNI2_DEVICE_INFO_H_ + +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + struct OpenNI2DeviceInfo + { + std::string uri_; + std::string vendor_; + std::string name_; + uint16_t vendor_id_; + uint16_t product_id_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2DeviceInfo& device_info); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_DEVICE_INFO_H_ diff --git a/io/include/pcl/io/openni2/openni2_device_manager.h b/io/include/pcl/io/openni2/openni2_device_manager.h new file mode 100644 index 00000000..5e6d623a --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_device_manager.h @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_DEVICE_MANAGER_H_ +#define PCL_IO_OPENNI2_DEVICE_MANAGER_H_ + +#include +#include "pcl/io/openni2/openni2_device_info.h" + +#include +#include +#include + +#include +#include +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2DeviceListener; + class OpenNI2Device; + + class PCL_EXPORTS OpenNI2DeviceManager + { + public: + OpenNI2DeviceManager (); + virtual ~OpenNI2DeviceManager (); + + // This may not actually be a sigleton yet. Need to work out cross-dll incerface. + // Based on http://stackoverflow.com/a/13431981/1789618 + static boost::shared_ptr getInstance () + { + static boost::shared_ptr instance = boost::make_shared(); + return (instance); + } + + boost::shared_ptr > + getConnectedDeviceInfos () const; + + boost::shared_ptr > + getConnectedDeviceURIs () const; + + std::size_t + getNumOfConnectedDevices () const; + + boost::shared_ptr + getAnyDevice (); + + boost::shared_ptr + getDevice (const std::string& device_URI); + + boost::shared_ptr + getDeviceByIndex (int index); + + boost::shared_ptr + getFileDevice (const std::string& path); + + protected: + boost::shared_ptr device_listener_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2DeviceManager& device_manager); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_DEVICE_MANAGER_H_ diff --git a/io/include/pcl/io/openni2/openni2_frame_listener.h b/io/include/pcl/io/openni2/openni2_frame_listener.h new file mode 100644 index 00000000..6dfcb886 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_frame_listener.h @@ -0,0 +1,89 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_IO_OPENNI2_FRAME_LISTENER_H_ +#define PCL_IO_OPENNI2_FRAME_LISTENER_H_ + +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + typedef boost::function StreamCallbackFunction; + + /* Each NewFrameListener may only listen to one VideoStream at a time. + **/ + class OpenNI2FrameListener : public openni::VideoStream::NewFrameListener + { + public: + + OpenNI2FrameListener () + : callback_(0) {} + OpenNI2FrameListener (StreamCallbackFunction cb) + : callback_(cb) {} + + virtual ~OpenNI2FrameListener () + { }; + + inline void + onNewFrame (openni::VideoStream& stream) + { + if (callback_) + callback_(stream); + } + + void + setCallback (StreamCallbackFunction cb) + { + callback_ = cb; + } + + private: + StreamCallbackFunction callback_; + }; + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_FRAME_LISTENER_H_ diff --git a/io/include/pcl/io/openni2/openni2_metadata_wrapper.h b/io/include/pcl/io/openni2/openni2_metadata_wrapper.h new file mode 100644 index 00000000..4e1cc6c5 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_metadata_wrapper.h @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#ifndef PCL_IO_OPENNI2_METADATA_WRAPPER_H_ +#define PCL_IO_OPENNI2_METADATA_WRAPPER_H_ + +#include + +#if defined(HAVE_OPENNI2) + +#include +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + class Openni2FrameWrapper : public pcl::io::FrameWrapper + { + public: + Openni2FrameWrapper (openni::VideoFrameRef metadata) + : metadata_(metadata) + {} + + virtual inline const void* + getData () const + { + return (metadata_.getData ()); + } + + virtual inline unsigned + getDataSize () const + { + return (metadata_.getDataSize ()); + } + + virtual inline unsigned + getWidth () const + { + return (metadata_.getWidth ()); + } + + virtual inline unsigned + getHeight () const + { + return (metadata_.getHeight ()); + } + + virtual inline unsigned + getFrameID () const + { + return (metadata_.getFrameIndex ()); + } + + virtual inline uint64_t + getTimestamp () const + { + return (metadata_.getTimestamp ()); + } + + + const inline openni::VideoFrameRef& + getMetaData () const + { + return (metadata_); + } + + private: + openni::VideoFrameRef metadata_; // Internally reference counted + }; + + } // namespace + } +} +#endif // HAVE_OPENNI2 + +#endif // PCL_IO_OPENNI2_METADATA_WRAPPER_H_ diff --git a/io/include/pcl/io/openni2/openni2_timer_filter.h b/io/include/pcl/io/openni2/openni2_timer_filter.h new file mode 100644 index 00000000..c2d78c0f --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_timer_filter.h @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_TIME_FILTER_H_ +#define PCL_IO_OPENNI2_TIME_FILTER_H_ + +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2TimerFilter + { + public: + OpenNI2TimerFilter (std::size_t filter_len); + virtual ~OpenNI2TimerFilter (); + + void + addSample (double sample); + + double + getMedian (); + + double + getMovingAvg (); + + void + clear (); + + private: + std::size_t filter_len_; + + std::deque buffer_; + }; + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_TIME_FILTER_H_ diff --git a/io/include/pcl/io/openni2/openni2_video_mode.h b/io/include/pcl/io/openni2/openni2_video_mode.h new file mode 100644 index 00000000..7c6fdd8c --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_video_mode.h @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_VIDEO_MODE_H_ +#define PCL_IO_OPENNI2_VIDEO_MODE_H_ + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + // copied from OniEnums.h + typedef enum + { + // Depth + PIXEL_FORMAT_DEPTH_1_MM = 100, + PIXEL_FORMAT_DEPTH_100_UM = 101, + PIXEL_FORMAT_SHIFT_9_2 = 102, + PIXEL_FORMAT_SHIFT_9_3 = 103, + + // Color + PIXEL_FORMAT_RGB888 = 200, + PIXEL_FORMAT_YUV422 = 201, + PIXEL_FORMAT_GRAY8 = 202, + PIXEL_FORMAT_GRAY16 = 203, + PIXEL_FORMAT_JPEG = 204, + PIXEL_FORMAT_YUYV = 205, + } PixelFormat; + + struct OpenNI2VideoMode + { + OpenNI2VideoMode () + :x_resolution_(0), y_resolution_(0), frame_rate_(0) + {} + + OpenNI2VideoMode (int xResolution, int yResolution, int frameRate) + :x_resolution_(xResolution), y_resolution_(yResolution), frame_rate_(frameRate) + {} + + int x_resolution_; + int y_resolution_; + int frame_rate_; + PixelFormat pixel_format_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2VideoMode& video_mode); + + bool + operator== (const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); + + bool + operator!= (const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); + + } // namespace + } +} + +#endif diff --git a/io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h b/io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h new file mode 100644 index 00000000..75d429c8 --- /dev/null +++ b/io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include +#ifdef HAVE_OPENNI2 + +#ifndef __OPENNI_SHIFT_TO_DEPTH_CONVERSION +#define __OPENNI_SHIFT_TO_DEPTH_CONVERSION + +#include +#include + +namespace openni_wrapper +{ + /** \brief This class provides conversion of the openni 11-bit shift data to depth; + */ + class PCL_EXPORTS ShiftToDepthConverter + { + public: + /** \brief Constructor. */ + ShiftToDepthConverter () : init_(false) {} + + /** \brief Destructor. */ + virtual ~ShiftToDepthConverter () {}; + + /** \brief This method generates a look-up table to convert openni shift values to depth + */ + void + generateLookupTable () + { + // lookup of 11 bit shift values + const std::size_t table_size = 1<<10; + + lookupTable_.clear(); + lookupTable_.resize(table_size); + + // constants taken from openni driver + static const int16_t nConstShift = 800; + static const double nParamCoeff = 4.000000; + static const double dPlanePixelSize = 0.104200; + static const double nShiftScale = 10.000000; + static const double dPlaneDsr = 120.000000; + static const double dPlaneDcl = 7.500000; + + std::size_t i; + double dFixedRefX; + double dMetric; + + for (i=0; i(i - nConstShift) / nParamCoeff)-0.375; + dMetric = dFixedRefX * dPlanePixelSize; + lookupTable_[i] = static_cast((nShiftScale * ((dMetric * dPlaneDsr / (dPlaneDcl - dMetric)) + dPlaneDsr) ) / 1000.0f); + } + + init_ = true; + } + + /** \brief Generate a look-up table for converting openni shift values to depth + */ + inline float + shiftToDepth (uint16_t shift_val) + { + assert (init_); + + static const float bad_point = std::numeric_limits::quiet_NaN (); + + float ret = bad_point; + + // lookup depth value in shift lookup table + if (shift_val lookupTable_; + bool init_; + } ; +} + +#endif +#endif //__OPENNI_SHIFT_TO_DEPTH_CONVERSION diff --git a/io/include/pcl/io/openni2_grabber.h b/io/include/pcl/io/openni2_grabber.h new file mode 100644 index 00000000..2a113d5d --- /dev/null +++ b/io/include/pcl/io/openni2_grabber.h @@ -0,0 +1,508 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#ifdef HAVE_OPENNI2 + +#ifndef PCL_IO_OPENNI2_GRABBER_H_ +#define PCL_IO_OPENNI2_GRABBER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace pcl +{ + struct PointXYZ; + struct PointXYZRGB; + struct PointXYZRGBA; + struct PointXYZI; + template class PointCloud; + + namespace io + { + + /** \brief Grabber for OpenNI 2 devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) + * \ingroup io + */ + class PCL_EXPORTS OpenNI2Grabber : public Grabber + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + // Templated images + typedef pcl::io::DepthImage DepthImage; + typedef pcl::io::IRImage IRImage; + typedef pcl::io::Image Image; + + /** \brief Basic camera parameters placeholder. */ + struct CameraParameters + { + /** fx */ + double focal_length_x; + /** fy */ + double focal_length_y; + /** cx */ + double principal_point_x; + /** cy */ + double principal_point_y; + + CameraParameters (double initValue) + : focal_length_x (initValue), focal_length_y (initValue), + principal_point_x (initValue), principal_point_y (initValue) + {} + + CameraParameters (double fx, double fy, double cx, double cy) + : focal_length_x (fx), focal_length_y (fy), principal_point_x (cx), principal_point_y (cy) + { } + }; + + typedef enum + { + OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz + OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect + OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect + OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion + OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion + OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect + OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion + OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) + } Mode; + + //define callback signature typedefs + typedef void (sig_cb_openni_image) (const boost::shared_ptr&); + typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr&); + typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr&); + typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr&, const boost::shared_ptr&, float reciprocalFocalLength) ; + typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr&, const boost::shared_ptr&, float reciprocalFocalLength) ; + typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr >&); + typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr >&); + typedef void (sig_cb_openni_point_cloud_rgba) (const boost::shared_ptr >&); + typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr >&); + + public: + /** \brief Constructor + * \param[in] device_id ID of the device, which might be a serial number, bus@address or the index of the device. + * \param[in] depth_mode the mode of the depth stream + * \param[in] image_mode the mode of the image stream + */ + OpenNI2Grabber (const std::string& device_id = "", + const Mode& depth_mode = OpenNI_Default_Mode, + const Mode& image_mode = OpenNI_Default_Mode); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + virtual ~OpenNI2Grabber () throw (); + + /** \brief Start the data acquisition. */ + virtual void + start (); + + /** \brief Stop the data acquisition. */ + virtual void + stop (); + + /** \brief Check if the data acquisition is still running. */ + virtual bool + isRunning () const; + + virtual std::string + getName () const; + + /** \brief Obtain the number of frames per second (FPS). */ + virtual float + getFramesPerSecond () const; + + /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */ + inline boost::shared_ptr + getDevice () const; + + /** \brief Obtain a list of the available depth modes that this device supports. */ + std::vector > + getAvailableDepthModes () const; + + /** \brief Obtain a list of the available image modes that this device supports. */ + std::vector > + getAvailableImageModes () const; + + /** \brief Set the RGB camera parameters (fx, fy, cx, cy) + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_length_y the RGB focal length (fy) + * \param[in] rgb_principal_point_x the RGB principal point (cx) + * \param[in] rgb_principal_point_y the RGB principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setRGBCameraIntrinsics (const double rgb_focal_length_x, + const double rgb_focal_length_y, + const double rgb_principal_point_x, + const double rgb_principal_point_y) + { + rgb_parameters_ = CameraParameters ( + rgb_focal_length_x, rgb_focal_length_y, + rgb_principal_point_x, rgb_principal_point_y); + } + + /** \brief Get the RGB camera parameters (fx, fy, cx, cy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + * \param[out] rgb_principal_point_x the RGB principal point (cx) + * \param[out] rgb_principal_point_y the RGB principal point (cy) + */ + inline void + getRGBCameraIntrinsics (double &rgb_focal_length_x, + double &rgb_focal_length_y, + double &rgb_principal_point_x, + double &rgb_principal_point_y) const + { + rgb_focal_length_x = rgb_parameters_.focal_length_x; + rgb_focal_length_y = rgb_parameters_.focal_length_y; + rgb_principal_point_x = rgb_parameters_.principal_point_x; + rgb_principal_point_y = rgb_parameters_.principal_point_y; + } + + + /** \brief Set the RGB image focal length (fx = fy). + * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length) + { + rgb_parameters_.focal_length_x = rgb_focal_length; + rgb_parameters_.focal_length_y = rgb_focal_length; + } + + /** \brief Set the RGB image focal length + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_ulength_y the RGB focal length (fy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y) + { + rgb_parameters_.focal_length_x = rgb_focal_length_x; + rgb_parameters_.focal_length_y = rgb_focal_length_y; + } + + /** \brief Return the RGB focal length parameters (fx, fy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + */ + inline void + getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const + { + rgb_focal_length_x = rgb_parameters_.focal_length_x; + rgb_focal_length_y = rgb_parameters_.focal_length_y; + } + + /** \brief Set the Depth camera parameters (fx, fy, cx, cy) + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * \param[in] depth_principal_point_x the Depth principal point (cx) + * \param[in] depth_principal_point_y the Depth principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthCameraIntrinsics (const double depth_focal_length_x, + const double depth_focal_length_y, + const double depth_principal_point_x, + const double depth_principal_point_y) + { + depth_parameters_ = CameraParameters ( + depth_focal_length_x, depth_focal_length_y, + depth_principal_point_x, depth_principal_point_y); + } + + /** \brief Get the Depth camera parameters (fx, fy, cx, cy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + * \param[out] depth_principal_point_x the Depth principal point (cx) + * \param[out] depth_principal_point_y the Depth principal point (cy) + */ + inline void + getDepthCameraIntrinsics (double &depth_focal_length_x, + double &depth_focal_length_y, + double &depth_principal_point_x, + double &depth_principal_point_y) const + { + depth_focal_length_x = depth_parameters_.focal_length_x; + depth_focal_length_y = depth_parameters_.focal_length_y; + depth_principal_point_x = depth_parameters_.principal_point_x; + depth_principal_point_y = depth_parameters_.principal_point_y; + } + + /** \brief Set the Depth image focal length (fx = fy). + * \param[in] depth_focal_length the Depth focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length) + { + depth_parameters_.focal_length_x = depth_focal_length; + depth_parameters_.focal_length_y = depth_focal_length; + } + + + /** \brief Set the Depth image focal length + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y) + { + depth_parameters_.focal_length_x = depth_focal_length_x; + depth_parameters_.focal_length_y = depth_focal_length_y; + } + + /** \brief Return the Depth focal length parameters (fx, fy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + */ + inline void + getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const + { + depth_focal_length_x = depth_parameters_.focal_length_x; + depth_focal_length_y = depth_parameters_.focal_length_y; + } + + protected: + + /** \brief Sets up an OpenNI device. */ + void + setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); + + /** \brief Update mode maps. */ + void + updateModeMaps (); + + /** \brief Start synchronization. */ + void + startSynchronization (); + + /** \brief Stop synchronization. */ + void + stopSynchronization (); + + // TODO: rename to mapMode2OniMode + /** \brief Map config modes. */ + bool + mapMode2XnMode (int mode, pcl::io::openni2::OpenNI2VideoMode& videoMode) const; + + // callback methods + /** \brief RGB image callback. */ + virtual void + imageCallback (pcl::io::openni2::Image::Ptr image, void* cookie); + + /** \brief Depth image callback. */ + virtual void + depthCallback (pcl::io::openni2::DepthImage::Ptr depth_image, void* cookie); + + /** \brief IR image callback. */ + virtual void + irCallback (pcl::io::openni2::IRImage::Ptr ir_image, void* cookie); + + /** \brief RGB + Depth image callback. */ + virtual void + imageDepthImageCallback (const pcl::io::openni2::Image::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief IR + Depth image callback. */ + virtual void + irDepthImageCallback (const pcl::io::openni2::IRImage::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief Process changed signals. */ + virtual void + signalsChanged (); + + // helper methods + + /** \brief Check if the RGB and Depth images are required to be synchronized or not. */ + virtual void + checkImageAndDepthSynchronizationRequired (); + + /** \brief Check if the RGB image stream is required or not. */ + virtual void + checkImageStreamRequired (); + + /** \brief Check if the depth stream is required or not. */ + virtual void + checkDepthStreamRequired (); + + /** \brief Check if the IR image stream is required or not. */ + virtual void + checkIRStreamRequired (); + + + // Point cloud conversion /////////////////////////////////////////////// + + /** \brief Convert a Depth image to a pcl::PointCloud + * \param[in] depth the depth image to convert + */ + boost::shared_ptr > + convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth); + + /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud + * \param[in] image the RGB image to convert + * \param[in] depth_image the depth image to convert + */ + template typename pcl::PointCloud::Ptr + convertToXYZRGBPointCloud (const pcl::io::openni2::Image::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud + * \param[in] image the IR image to convert + * \param[in] depth_image the depth image to convert + */ + boost::shared_ptr > + convertToXYZIPointCloud (const pcl::io::openni2::IRImage::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + std::vector color_resize_buffer_; + std::vector depth_resize_buffer_; + std::vector ir_resize_buffer_; + + // Stream callbacks ///////////////////////////////////////////////////// + void + processColorFrame (openni::VideoStream& stream); + + void + processDepthFrame (openni::VideoStream& stream); + + void + processIRFrame (openni::VideoStream& stream); + + + Synchronizer rgb_sync_; + Synchronizer ir_sync_; + + /** \brief The actual openni device. */ + boost::shared_ptr device_; + + std::string rgb_frame_id_; + std::string depth_frame_id_; + unsigned image_width_; + unsigned image_height_; + unsigned depth_width_; + unsigned depth_height_; + + bool image_required_; + bool depth_required_; + bool ir_required_; + bool sync_required_; + + boost::signals2::signal* image_signal_; + boost::signals2::signal* depth_image_signal_; + boost::signals2::signal* ir_image_signal_; + boost::signals2::signal* image_depth_image_signal_; + boost::signals2::signal* ir_depth_image_signal_; + boost::signals2::signal* point_cloud_signal_; + boost::signals2::signal* point_cloud_i_signal_; + boost::signals2::signal* point_cloud_rgb_signal_; + boost::signals2::signal* point_cloud_rgba_signal_; + + struct modeComp + { + bool operator () (const openni::VideoMode& mode1, const openni::VideoMode & mode2) const + { + if (mode1.getResolutionX () < mode2.getResolutionX ()) + return true; + else if (mode1.getResolutionX () > mode2.getResolutionX ()) + return false; + else if (mode1.getResolutionY () < mode2.getResolutionY ()) + return true; + else if (mode1.getResolutionY () > mode2.getResolutionY ()) + return false; + else if (mode1.getFps () < mode2.getFps () ) + return true; + else + return false; + } + }; + + // Mapping from config (enum) modes to native OpenNI modes + std::map config2oni_map_; + + pcl::io::openni2::OpenNI2Device::CallbackHandle depth_callback_handle_; + pcl::io::openni2::OpenNI2Device::CallbackHandle image_callback_handle_; + pcl::io::openni2::OpenNI2Device::CallbackHandle ir_callback_handle_; + bool running_; + + + CameraParameters rgb_parameters_; + CameraParameters depth_parameters_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + boost::shared_ptr + OpenNI2Grabber::getDevice () const + { + return device_; + } + + } // namespace +} + +#endif // PCL_IO_OPENNI2_GRABBER_H_ +#endif // HAVE_OPENNI2 diff --git a/io/include/pcl/io/openni_camera/openni_device.h b/io/include/pcl/io/openni_camera/openni_device.h index 6821e958..b4607b33 100644 --- a/io/include/pcl/io/openni_camera/openni_device.h +++ b/io/include/pcl/io/openni_camera/openni_device.h @@ -289,6 +289,7 @@ namespace openni_wrapper * This version is used to register a member function of any class. * The callback will always be called with a new image and the user data "cookie". * \param[in] callback the user callback to be called if a new image is available + * \param instance * \param[in] cookie the cookie that needs to be passed to the callback together with the new image. * \return a callback handler that can be used to remove the user callback from list of image-stream callbacks. */ @@ -316,6 +317,7 @@ namespace openni_wrapper * This version is used to register a member function of any class. * The callback will always be called with a new depth image and the user data "cookie". * \param[in] callback the user callback to be called if a new depth image is available + * \param instance * \param[in] cookie the cookie that needs to be passed to the callback together with the new depth image. * \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks. */ @@ -342,6 +344,7 @@ namespace openni_wrapper * This version is used to register a member function of any class. * The callback will always be called with a new IR image and the user data "cookie". * \param[in] callback the user callback to be called if a new IR image is available + * \param instance * \param[in] cookie the cookie that needs to be passed to the callback together with the new IR image. * \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks. */ diff --git a/io/include/pcl/io/openni_grabber.h b/io/include/pcl/io/openni_grabber.h index eedb2d2c..03ba27ae 100644 --- a/io/include/pcl/io/openni_grabber.h +++ b/io/include/pcl/io/openni_grabber.h @@ -99,7 +99,7 @@ namespace pcl public: /** \brief Constructor - * \param[in] device_id ID of the device, which might be a serial number, bus@address or the index of the device. + * \param[in] device_id ID of the device, which might be a serial number, bus\@address or the index of the device. * \param[in] depth_mode the mode of the depth stream * \param[in] image_mode the mode of the image stream */ @@ -129,7 +129,7 @@ namespace pcl virtual float getFramesPerSecond () const; - /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */ + /** \brief Get a boost shared pointer to the \ref pcl::openni_wrapper::OpenNIDevice object. */ inline boost::shared_ptr getDevice () const; @@ -194,7 +194,7 @@ namespace pcl /** \brief Set the RGB image focal length * \param[in] rgb_focal_length_x the RGB focal length (fx) - * \param[in] rgb_focal_ulength_y the RGB focal length (fy) + * \param[in] rgb_focal_length_y the RGB focal length (fy) * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them * and the grabber will use the default values from the camera instead. * These parameters will be used for XYZRGBA clouds. @@ -469,6 +469,12 @@ namespace pcl openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle; bool running_; + mutable unsigned rgb_array_size_; + mutable unsigned depth_buffer_size_; + mutable boost::shared_array rgb_array_; + mutable boost::shared_array depth_buffer_; + mutable boost::shared_array ir_buffer_; + /** \brief The RGB image focal length (fx). */ double rgb_focal_length_x_; /** \brief The RGB image focal length (fy). */ diff --git a/io/include/pcl/io/ply/ply_parser.h b/io/include/pcl/io/ply/ply_parser.h index 92198d8c..6fef2574 100644 --- a/io/include/pcl/io/ply/ply_parser.h +++ b/io/include/pcl/io/ply/ply_parser.h @@ -157,16 +157,16 @@ namespace pcl at (const scalar_property_definition_callbacks_type& scalar_property_definition_callbacks); }; - template - friend typename scalar_property_definition_callback_type::type& + template static + typename scalar_property_definition_callback_type::type& at (scalar_property_definition_callbacks_type& scalar_property_definition_callbacks) { return (scalar_property_definition_callbacks.get ()); } - template - friend const typename scalar_property_definition_callback_type::type& + template static + const typename scalar_property_definition_callback_type::type& at (const scalar_property_definition_callbacks_type& scalar_property_definition_callbacks) { return (scalar_property_definition_callbacks.get ()); @@ -253,15 +253,15 @@ namespace pcl at (const list_property_definition_callbacks_type& list_property_definition_callbacks); }; - template - friend typename list_property_definition_callback_type::type& + template static + typename list_property_definition_callback_type::type& at (list_property_definition_callbacks_type& list_property_definition_callbacks) { return (list_property_definition_callbacks.get ()); } - template - friend const typename list_property_definition_callback_type::type& + template static + const typename list_property_definition_callback_type::type& at (const list_property_definition_callbacks_type& list_property_definition_callbacks) { return (list_property_definition_callbacks.get ()); @@ -521,8 +521,8 @@ inline void pcl::io::ply::ply_parser::parse_list_property_definition (const std: { typedef SizeType size_type; typedef ScalarType scalar_type; - typename list_property_definition_callback_type::type& list_property_definition_callback = - list_property_definition_callbacks_.get (); + typedef typename list_property_definition_callback_type::type list_property_definition_callback_type; + list_property_definition_callback_type& list_property_definition_callback = list_property_definition_callbacks_.get (); typedef typename list_property_begin_callback_type::type list_property_begin_callback_type; typedef typename list_property_element_callback_type::type list_property_element_callback_type; typedef typename list_property_end_callback_type::type list_property_end_callback_type; diff --git a/io/include/pcl/io/ply_io.h b/io/include/pcl/io/ply_io.h index f0bba19e..7f339981 100644 --- a/io/include/pcl/io/ply_io.h +++ b/io/include/pcl/io/ply_io.h @@ -90,13 +90,11 @@ namespace pcl , orientation_ (Eigen::Matrix3f::Zero ()) , cloud_ () , vertex_count_ (0) - , vertex_properties_counter_ (0) , vertex_offset_before_ (0) , range_grid_ (0) - , range_count_ (0) - , range_grid_vertex_indices_element_index_ (0) , rgb_offset_before_ (0) , do_resize_ (false) + , polygons_ (0) {} PLYReader (const PLYReader &p) @@ -106,13 +104,11 @@ namespace pcl , orientation_ (Eigen::Matrix3f::Zero ()) , cloud_ () , vertex_count_ (0) - , vertex_properties_counter_ (0) , vertex_offset_before_ (0) , range_grid_ (0) - , range_count_ (0) - , range_grid_vertex_indices_element_index_ (0) , rgb_offset_before_ (0) , do_resize_ (false) + , polygons_ (0) { *this = p; } @@ -123,6 +119,7 @@ namespace pcl origin_ = p.origin_; orientation_ = p.orientation_; range_grid_ = p.range_grid_; + polygons_ = p.polygons_; return (*this); } @@ -170,13 +167,8 @@ namespace pcl read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version, const int offset = 0); - /** \brief Read a point cloud data from a PLY file (PLY_V6 only!) and store it into a pcl/PCLPointCloud2. - * - * \note This function is provided for backwards compatibility only and - * it can only read PLY_V6 files correctly, as pcl::PCLPointCloud2 - * does not contain a sensor origin/orientation. Reading any file - * > PLY_V6 will generate a warning. - * + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2. + * \note This function is provided for backwards compatibility only * \param[in] file_name the name of the file containing the actual PointCloud data * \param[out] cloud the resultant PointCloud message read from disk * \param[in] offset the offset in the file where to expect the true header to begin. @@ -218,6 +210,37 @@ namespace pcl return (0); } + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] mesh the resultant PolygonMesh message read from disk + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[out] ply_version the PLY version read from the file + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int& ply_version, const int offset = 0); + + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] mesh the resultant PolygonMesh message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset = 0); + private: ::pcl::io::ply::ply_parser parser_; @@ -298,61 +321,12 @@ namespace pcl inline void vertexListPropertyEndCallback (); - /** Callback function for an anonymous vertex double property. + /** Callback function for an anonymous vertex scalar property. * Writes down a double value in cloud data. * param[in] value double value parsed */ - inline void - vertexDoublePropertyCallback (pcl::io::ply::float64 value); - - /** Callback function for an anonymous vertex float property. - * Writes down a float value in cloud data. - * param[in] value float value parsed - */ - inline void - vertexFloatPropertyCallback (pcl::io::ply::float32 value); - - /** Callback function for an anonymous vertex int property. - * Writes down a int value in cloud data. - * param[in] value int value parsed - */ - inline void - vertexIntPropertyCallback (pcl::io::ply::int32 value); - - /** Callback function for an anonymous vertex uint property. - * Writes down a uint value in cloud data. - * param[in] value uint value parsed - */ - inline void - vertexUnsignedIntPropertyCallback (pcl::io::ply::uint32 value); - - /** Callback function for an anonymous vertex short property. - * Writes down a short value in cloud data. - * param[in] value short value parsed - */ - inline void - vertexShortPropertyCallback (pcl::io::ply::int16 value); - - /** Callback function for an anonymous vertex ushort property. - * Writes down a ushort value in cloud data. - * param[in] value ushort value parsed - */ - inline void - vertexUnsignedShortPropertyCallback (pcl::io::ply::uint16 value); - - /** Callback function for an anonymous vertex char property. - * Writes down a char value in cloud data. - * param[in] value char value parsed - */ - inline void - vertexCharPropertyCallback (pcl::io::ply::int8 value); - - /** Callback function for an anonymous vertex uchar property. - * Writes down a uchar value in cloud data. - * param[in] value uchar value parsed - */ - inline void - vertexUnsignedCharPropertyCallback (pcl::io::ply::uint8 value); + template void + vertexScalarPropertyCallback (Scalar value); /** Callback function for vertex RGB color. * This callback is in charge of packing red green and blue in a single int @@ -461,69 +435,13 @@ namespace pcl inline void cloudWidthCallback (const int &width) { cloud_->width = width; } - /** Append a double property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendDoubleProperty (const std::string& name, const size_t& count = 1); - - /** Append a float property to the cloud fields. + /** Append a scalar property to the cloud fields. * param[in] name property name * param[in] count property count: 1 for scalar properties and higher for a * list property. */ - void - appendFloatProperty (const std::string& name, const size_t& count = 1); - - /** Append an unsigned int property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendIntProperty (const std::string& name, const size_t& count = 1); - - /** Append an unsigned int property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendUnsignedIntProperty (const std::string& name, const size_t& count = 1); - - /** Append a short property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendShortProperty (const std::string& name, const size_t& count = 1); - - /** Append a short property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendUnsignedShortProperty (const std::string& name, const size_t& count = 1); - - /** Append a char property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendCharProperty (const std::string& name, const size_t& count = 1); - - /** Append a char property to the cloud fields. - * param[in] name property name - * param[in] count property count: 1 for scalar properties and higher for a - * list property. - */ - void - appendUnsignedCharProperty (const std::string& name, const size_t& count = 1); + template void + appendScalarProperty (const std::string& name, const size_t& count = 1); /** Amend property from cloud fields identified by \a old_name renaming * it \a new_name. @@ -569,6 +487,30 @@ namespace pcl void objInfoCallback (const std::string& line); + /** Callback function for the begin of face line */ + void + faceBeginCallback (); + + /** Callback function for the begin of face vertex_indices property + * param[in] size vertex_indices list size + */ + void + faceVertexIndicesBeginCallback (pcl::io::ply::uint8 size); + + /** Callback function for each face vertex_indices element + * param[in] vertex_index index of the vertex in vertex_indices + */ + void + faceVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index); + + /** Callback function for the end of a face vertex_indices property */ + void + faceVertexIndicesEndCallback (); + + /** Callback function for the end of a face element end */ + void + faceEndCallback (); + /// origin Eigen::Vector4f origin_; @@ -577,13 +519,14 @@ namespace pcl //vertex element artifacts pcl::PCLPointCloud2 *cloud_; - size_t vertex_count_, vertex_properties_counter_; + size_t vertex_count_; int vertex_offset_before_; //range element artifacts std::vector > *range_grid_; - size_t range_count_, range_grid_vertex_indices_element_index_; size_t rgb_offset_before_; bool do_resize_; + //face element artifact + std::vector *polygons_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -832,12 +775,29 @@ namespace pcl return (p.read (file_name, cloud)); } + /** \brief Load a PLY file into a PolygonMesh type. + * + * Any PLY files containg sensor data will generate a warning as a + * pcl/PolygonMesh message cannot hold the sensor origin. + * + * \param[in] file_name the name of the file to load + * \param[in] mesh the resultant polygon mesh + * \ingroup io + */ + inline int + loadPLYFile (const std::string &file_name, pcl::PolygonMesh &mesh) + { + pcl::PLYReader p; + return (p.read (file_name, mesh)); + } + /** \brief Save point cloud data to a PLY file containing n-D points * \param[in] file_name the output file name * \param[in] cloud the point cloud data message * \param[in] origin the sensor data acquisition origin (translation) * \param[in] orientation the sensor data acquisition origin (rotation) * \param[in] binary_mode true for binary mode, false (default) for ASCII + * \param[in] use_camera * \ingroup io */ inline int diff --git a/io/include/pcl/io/png_io.h b/io/include/pcl/io/png_io.h index c82c32c7..a30b140f 100644 --- a/io/include/pcl/io/png_io.h +++ b/io/include/pcl/io/png_io.h @@ -82,32 +82,23 @@ namespace pcl * \ingroup io */ PCL_EXPORTS void - saveRgbPNGFile (const std::string& file_name, const unsigned char *rgb_image, int width, int height) - { - saveCharPNGFile(file_name, rgb_image, width, height, 3); - } + saveRgbPNGFile (const std::string& file_name, const unsigned char *rgb_image, int width, int height); /** \brief Saves 8-bit grayscale cloud as image to PNG file. * \param[in] file_name the name of the file to write to disk * \param[in] cloud point cloud to save * \ingroup io */ - void - savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) - { - saveCharPNGFile(file_name, &cloud.points[0], cloud.width, cloud.height, 1); - } + PCL_EXPORTS void + savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud); /** \brief Saves 16-bit grayscale cloud as image to PNG file. * \param[in] file_name the name of the file to write to disk * \param[in] cloud point cloud to save * \ingroup io */ - void - savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) - { - saveShortPNGFile(file_name, &cloud.points[0], cloud.width, cloud.height, 1); - } + PCL_EXPORTS void + savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud); /** \brief Saves a PCLImage (formely ROS sensor_msgs::Image) to PNG file. * \param[in] file_name the name of the file to write to disk @@ -115,37 +106,20 @@ namespace pcl * \ingroup io * \note Currently only "rgb8", "mono8", and "mono16" image encodings are supported. */ - void - savePNGFile (const std::string& file_name, const pcl::PCLImage& image) - { - if (image.encoding == "rgb8") - { - saveRgbPNGFile(file_name, &image.data[0], image.width, image.height); - } - else if (image.encoding == "mono8") - { - saveCharPNGFile(file_name, &image.data[0], image.width, image.height, 1); - } - else if (image.encoding == "mono16") - { - saveShortPNGFile(file_name, reinterpret_cast(&image.data[0]), image.width, image.height, 1); - } - else - { - PCL_ERROR ("[pcl::io::savePNGFile] Unsupported image encoding \"%s\".\n", image.encoding.c_str ()); - } - } + PCL_EXPORTS void + savePNGFile (const std::string& file_name, const pcl::PCLImage& image); /** \brief Saves RGB fields of cloud as image to PNG file. * \param[in] file_name the name of the file to write to disk * \param[in] cloud point cloud to save * \ingroup io */ - PCL_DEPRECATED (template void savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud), + template + PCL_DEPRECATED ( "pcl::io::savePNGFile (file_name, cloud) is deprecated, please use a new generic " "function pcl::io::savePNGFile (file_name, cloud, field_name) with \"rgb\" as the field name." - ); - template void + ) + void savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) { std::vector data(cloud.width * cloud.height * 3); @@ -165,20 +139,12 @@ namespace pcl * \ingroup io * Warning: Converts to 16 bit (for png), labels using more than 16 bits will cause problems */ - PCL_DEPRECATED (void savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud), - "pcl::io::savePNGFile (file_name, cloud) is deprecated, please use a new generic function " - "pcl::io::savePNGFile (file_name, cloud, field_name) with \"label\" as the field name." - ); + PCL_EXPORTS PCL_DEPRECATED ( + "savePNGFile (file_name, cloud) is deprecated, please use a new generic function " + "savePNGFile (file_name, cloud, field_name) with \"label\" as the field name." + ) void - savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) - { - std::vector data(cloud.width * cloud.height); - for (size_t i = 0; i < cloud.points.size (); ++i) - { - data[i] = static_cast (cloud.points[i].label); - } - saveShortPNGFile(file_name, &data[0], cloud.width, cloud.height,1); - } + savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud); /** \brief Saves the data from the specified field of the point cloud as image to PNG file. * \param[in] file_name the name of the file to write to disk diff --git a/io/include/pcl/io/point_cloud_image_extractors.h b/io/include/pcl/io/point_cloud_image_extractors.h index 347ca0ad..e7213b11 100644 --- a/io/include/pcl/io/point_cloud_image_extractors.h +++ b/io/include/pcl/io/point_cloud_image_extractors.h @@ -84,7 +84,9 @@ namespace pcl typedef boost::shared_ptr > ConstPtr; /** \brief Constructor. */ - PointCloudImageExtractor () {} + PointCloudImageExtractor () + : paint_nans_with_black_ (false) + {} /** \brief Destructor. */ virtual ~PointCloudImageExtractor () {} @@ -94,8 +96,29 @@ namespace pcl * \param[out] image the output image * \return true if the operation was successful, false otherwise */ + bool + extract (const PointCloud& cloud, pcl::PCLImage& image) const; + + /** \brief Set a flag that controls if image pixels corresponding to + * NaN (infinite) points should be painted black. + */ + inline void + setPaintNaNsWithBlack (bool flag) + { + paint_nans_with_black_ = flag; + } + + protected: + + /** \brief Implementation of the extract() function, has to be + * implemented in deriving classes. + */ virtual bool - extract (const PointCloud& cloud, pcl::PCLImage& image) const = 0; + extractImpl (const PointCloud& cloud, pcl::PCLImage& image) const = 0; + + /// A flag that controls if image pixels corresponding to NaN (infinite) + /// points should be painted black. + bool paint_nans_with_black_; }; ////////////////////////////////////////////////////////////////////////////////////// @@ -146,14 +169,6 @@ namespace pcl /** \brief Destructor. */ virtual ~PointCloudImageExtractorWithScaling () {} - /** \brief Obtain the image from the given cloud. - * \param[in] cloud organized point cloud to extract image from - * \param[out] image the output image - * \return true if the operation was successful, false otherwise - */ - virtual bool - extract (const PointCloud& cloud, pcl::PCLImage& image) const; - /** \brief Set scaling method. */ inline void setScalingMethod (const ScalingMethod scaling_method) @@ -170,6 +185,9 @@ namespace pcl protected: + virtual bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& image) const; + std::string field_name_; ScalingMethod scaling_method_; float scaling_factor_; @@ -196,14 +214,10 @@ namespace pcl /** \brief Destructor. */ virtual ~PointCloudImageExtractorFromNormalField () {} - /** \brief Obtain the color image from the given cloud. - * The cloud should contain "normal" field. - * \param[in] cloud organized point cloud to extract image from - * \param[out] image the output image - * \return true if the operation was successful, false otherwise - */ + protected: + virtual bool - extract (const PointCloud& cloud, pcl::PCLImage& img) const; + extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const; }; ////////////////////////////////////////////////////////////////////////////////////// @@ -227,21 +241,17 @@ namespace pcl /** \brief Destructor. */ virtual ~PointCloudImageExtractorFromRGBField () {} - /** \brief Obtain the color image from the given cloud. - * The cloud should contain either "rgb" or "rgba" field. - * \param[in] cloud organized point cloud to extract image from - * \param[out] image the output image - * \return true if the operation was successful, false otherwise - */ + protected: + virtual bool - extract (const PointCloud& cloud, pcl::PCLImage& img) const; + extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const; }; ////////////////////////////////////////////////////////////////////////////////////// /** \brief Image Extractor which uses the data present in the "label" field to produce * either monochrome or RGB image where different labels correspond to different - * colors. In the monochrome case colors are shades of gray, in the RGB case the - * colors are generated randomly. + * colors. + * See the documentation for ColorMode to learn about available coloring options. * \author Sergey Alexandrov * \ingroup io */ @@ -254,16 +264,17 @@ namespace pcl typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr > ConstPtr; - /** \brief Different modes for color mapping. - *
    - *
  • COLORS_MONO - shades of gray (according to label id).
    • - *
  • COLORS_RGB_RANDOM - randomly generated RGB colors.
    • - *
- */ + /** \brief Different modes for color mapping. */ enum ColorMode { + /// Shades of gray (according to label id) + /// \note Labels using more than 16 bits will cause problems COLORS_MONO, + /// Randomly generated RGB colors COLORS_RGB_RANDOM, + /// Fixed RGB colors from the [Glasbey lookup table](http://fiji.sc/Glasbey), + /// assigned in the ascending order of label id + COLORS_RGB_GLASBEY, }; /** \brief Constructor. */ @@ -275,16 +286,6 @@ namespace pcl /** \brief Destructor. */ virtual ~PointCloudImageExtractorFromLabelField () {} - /** \brief Obtain the label image from the given cloud. - * The cloud should contain "label" field. - * \note Labels using more than 16 bits will cause problems in COLORS_MONO mode. - * \param[in] cloud organized point cloud to extract image from - * \param[out] image the output image - * \return true if the operation was successful, false otherwise - */ - virtual bool - extract (const PointCloud& cloud, pcl::PCLImage& img) const; - /** \brief Set color mapping mode. */ inline void setColorMode (const ColorMode color_mode) @@ -292,6 +293,11 @@ namespace pcl color_mode_ = color_mode; } + protected: + + virtual bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const; + private: ColorMode color_mode_; @@ -314,7 +320,7 @@ namespace pcl typedef boost::shared_ptr > ConstPtr; /** \brief Constructor. - * \param[i] scaling_factor a scaling factor to apply to each depth value (default 10000) + * \param[in] scaling_factor a scaling factor to apply to each depth value (default 10000) */ PointCloudImageExtractorFromZField (const float scaling_factor = 10000) : PointCloudImageExtractorWithScaling ("z", scaling_factor) @@ -322,7 +328,7 @@ namespace pcl } /** \brief Constructor. - * \param[i] scaling_method a scaling method to use + * \param[in] scaling_method a scaling method to use */ PointCloudImageExtractorFromZField (const ScalingMethod scaling_method) : PointCloudImageExtractorWithScaling ("z", scaling_method) @@ -356,7 +362,7 @@ namespace pcl typedef boost::shared_ptr > ConstPtr; /** \brief Constructor. - * \param[i] scaling_method a scaling method to use (default SCALING_FULL_RANGE) + * \param[in] scaling_method a scaling method to use (default SCALING_FULL_RANGE) */ PointCloudImageExtractorFromCurvatureField (const ScalingMethod scaling_method = PointCloudImageExtractorWithScaling::SCALING_FULL_RANGE) : PointCloudImageExtractorWithScaling ("curvature", scaling_method) @@ -364,7 +370,7 @@ namespace pcl } /** \brief Constructor. - * \param[i] scaling_factor a scaling factor to apply to each curvature value + * \param[in] scaling_factor a scaling factor to apply to each curvature value */ PointCloudImageExtractorFromCurvatureField (const float scaling_factor) : PointCloudImageExtractorWithScaling ("curvature", scaling_factor) @@ -398,7 +404,7 @@ namespace pcl typedef boost::shared_ptr > ConstPtr; /** \brief Constructor. - * \param[i] scaling_method a scaling method to use (default SCALING_NO) + * \param[in] scaling_method a scaling method to use (default SCALING_NO) */ PointCloudImageExtractorFromIntensityField (const ScalingMethod scaling_method = PointCloudImageExtractorWithScaling::SCALING_NO) : PointCloudImageExtractorWithScaling ("intensity", scaling_method) @@ -406,7 +412,7 @@ namespace pcl } /** \brief Constructor. - * \param[i] scaling_factor a scaling factor to apply to each intensity value + * \param[in] scaling_factor a scaling factor to apply to each intensity value */ PointCloudImageExtractorFromIntensityField (const float scaling_factor) : PointCloudImageExtractorWithScaling ("intensity", scaling_factor) diff --git a/io/include/pcl/io/tar.h b/io/include/pcl/io/tar.h index d872aaec..18e95fdf 100644 --- a/io/include/pcl/io/tar.h +++ b/io/include/pcl/io/tar.h @@ -67,7 +67,7 @@ namespace pcl char file_name_prefix[155]; char _padding[12]; - /** \brief . */ + /** \brief get file size */ unsigned int getFileSize () { @@ -84,16 +84,15 @@ namespace pcl /** \brief Save a PointCloud dataset into a TAR file. * Append if the file exists, or create a new one if not. - * - * \param[in] tar_filename the name of the TAR file to save the cloud to - * \param[in] cloud the point cloud dataset to save - * \param[in] pcd_filename the internal name of the PCD file that should be stored in the TAR header * \remark till implemented will return FALSE */ + // param[in] tar_filename the name of the TAR file to save the cloud to + // param[in] cloud the point cloud dataset to save + // param[in] pcd_filename the internal name of the PCD file that should be stored in the TAR header template bool - saveTARPointCloud (const std::string &tar_filename, - const PointCloud &cloud, - const std::string &pcd_filename) + saveTARPointCloud (const std::string& /*tar_filename*/, + const PointCloud& /*cloud*/, + const std::string& /*pcd_filename*/) { return (false); } diff --git a/io/include/pcl/io/vtk_lib_io.h b/io/include/pcl/io/vtk_lib_io.h index 275a151c..f5cbd23c 100644 --- a/io/include/pcl/io/vtk_lib_io.h +++ b/io/include/pcl/io/vtk_lib_io.h @@ -54,6 +54,7 @@ #ifdef __GNUC__ #pragma GCC system_header #endif +#include #include #include #include diff --git a/io/src/ascii_io.cpp b/io/src/ascii_io.cpp index bcad052f..c567eb5e 100644 --- a/io/src/ascii_io.cpp +++ b/io/src/ascii_io.cpp @@ -106,7 +106,7 @@ pcl::ASCIIReader::readHeader (const std::string& file_name, cloud.fields = fields_; cloud.point_step = 0; - for (int i = 0; i < fields_.size (); i++) + for (size_t i = 0; i < fields_.size (); i++) cloud.point_step += typeSize (cloud.fields[i].datatype); std::fstream ifile (file_name.c_str (), std::fstream::in); @@ -162,7 +162,7 @@ pcl::ASCIIReader::read ( uint32_t offset = 0; try { - for (int i = 0; i < fields_.size (); i++) + for (size_t i = 0; i < fields_.size (); i++) offset += parse (tokens[i], fields_[i], data + offset); } catch (std::exception& /*e*/) diff --git a/io/src/dinast_grabber.cpp b/io/src/dinast_grabber.cpp index 3acfcc99..d621bae9 100644 --- a/io/src/dinast_grabber.cpp +++ b/io/src/dinast_grabber.cpp @@ -277,7 +277,7 @@ pcl::DinastGrabber::readImage () // Is there enough data in the buffer to return one image, and did we find the header? // If not, go back and read some more data off the USB port // Else, read the data, clean the g_buffer_, return to the user - if (!first_image && (g_buffer_.size () >= image_size_ + data_adr) && data_adr != -1) + if (!first_image && (g_buffer_.size () >= static_cast::size_type> (image_size_ + data_adr)) && data_adr != -1) { // An image found. Copy it from the buffer into the user given buffer @@ -289,7 +289,7 @@ pcl::DinastGrabber::readImage () first_image = true; } - if (first_image && g_buffer_.size () >= image_size_) + if (first_image && g_buffer_.size () >= static_cast::size_type> (image_size_)) break; } } @@ -307,9 +307,9 @@ pcl::DinastGrabber::getXYZIPointCloud () int depth_idx = 0; - for (int x = 0; x < cloud->width; ++x) + for (int x = 0; x < static_cast (cloud->width); ++x) { - for (int y = 0; y < cloud->height; ++y, ++depth_idx) + for (int y = 0; y < static_cast (cloud->height); ++y, ++depth_idx) { double pixel = image_[x + image_width_ * y]; @@ -342,7 +342,7 @@ pcl::DinastGrabber::getXYZIPointCloud () // Get rid of the noise - if(cloud->points[depth_idx].z > 0.8f or cloud->points[depth_idx].z < 0.02f) + if(cloud->points[depth_idx].z > 0.8f || cloud->points[depth_idx].z < 0.02f) { cloud->points[depth_idx].x = std::numeric_limits::quiet_NaN (); cloud->points[depth_idx].y = std::numeric_limits::quiet_NaN (); @@ -412,7 +412,7 @@ pcl::DinastGrabber::checkHeader () { // We need at least 2 full sync packets, in case the header starts at the end of the first sync packet to // guarantee that the index returned actually exists in g_buffer_ (we perform no checking in the rest of the code) - if (g_buffer_.size () < 2 * sync_packet_size_) + if (g_buffer_.size () < static_cast::size_type> (2 * sync_packet_size_)) return (-1); int data_ptr = -1; diff --git a/io/src/file_io.cpp b/io/src/file_io.cpp new file mode 100644 index 00000000..c67e5b0a --- /dev/null +++ b/io/src/file_io.cpp @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace io + { + template int + load (const std::string& file_name, pcl::PointCloud& cloud) + { + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".pcd") + result = pcl::io::loadPCDFile (file_name, cloud); + else if (extension == ".ply") + result = pcl::io::loadPLYFile (file_name, cloud); + else if (extension == ".ifs") + result = pcl::io::loadIFSFile (file_name, cloud); + else + { + PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); + } + + template int + save (const std::string& file_name, const pcl::PointCloud& cloud) + { + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".pcd") + result = pcl::io::savePCDFile (file_name, cloud, true); + else if (extension == ".ply") + result = pcl::io::savePLYFile (file_name, cloud, true); + else if (extension == ".ifs") + result = pcl::io::saveIFSFile (file_name, cloud); + else + { + PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); + } + } +} + +int +pcl::io::load (const std::string& file_name, pcl::PCLPointCloud2& blob) +{ + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".pcd") + result = pcl::io::loadPCDFile (file_name, blob); + else if (extension == ".ply") + result = pcl::io::loadPLYFile (file_name, blob); + else if (extension == ".ifs") + result = pcl::io::loadIFSFile (file_name, blob); + else if (extension == ".obj") + result = pcl::io::loadOBJFile (file_name, blob); + else + { + PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); +} + +int +pcl::io::load (const std::string& file_name, pcl::PolygonMesh& mesh) +{ + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".ply") + result = pcl::io::loadPLYFile (file_name, mesh); + else if (extension == ".ifs") + result = pcl::io::loadIFSFile (file_name, mesh); + else if (extension == ".obj") + result = pcl::io::loadOBJFile (file_name, mesh); + else + { + PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); +} + +int +pcl::io::load (const std::string& file_name, pcl::TextureMesh& mesh) +{ + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".obj") + result = pcl::io::loadOBJFile (file_name, mesh); + else + { + PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); +} + +int +pcl::io::save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision) +{ + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".pcd") + { + Eigen::Vector4f origin = Eigen::Vector4f::Zero (); + Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity (); + result = pcl::io::savePCDFile (file_name, blob, origin, orientation, true); + } + else if (extension == ".ply") + { + Eigen::Vector4f origin = Eigen::Vector4f::Zero (); + Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity (); + result = pcl::io::savePLYFile (file_name, blob, origin, orientation, true); + } + else if (extension == ".ifs") + result = pcl::io::saveIFSFile (file_name, blob); + else if (extension == ".vtk") + result = pcl::io::saveVTKFile (file_name, blob, precision); + else + { + PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); +} + +int +pcl::io::save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision) +{ + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".obj") + result = pcl::io::saveOBJFile (file_name, tex_mesh, precision); + else + { + PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); +} + +int +pcl::io::save (const std::string &file_name, const pcl::PolygonMesh &poly_mesh, unsigned precision) +{ + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".ply") + result = pcl::io::savePLYFileBinary (file_name, poly_mesh); + else if (extension == ".obj") + result = pcl::io::saveOBJFile (file_name, poly_mesh, precision); + else if (extension == ".vtk") + result = pcl::io::saveVTKFile (file_name, poly_mesh, precision); + else + { + PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); +} diff --git a/io/src/ifs_io.cpp b/io/src/ifs_io.cpp new file mode 100644 index 00000000..247fca81 --- /dev/null +++ b/io/src/ifs_io.cpp @@ -0,0 +1,403 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSReader::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + int &ifs_version, unsigned int &data_idx) +{ + // Default values + data_idx = 0; + ifs_version = IFS_V1_0; + cloud.width = cloud.height = cloud.point_step = cloud.row_step = 0; + cloud.data.clear (); + + // By default, assume that there are _no_ invalid (e.g., NaN) points + //cloud.is_dense = true; + + uint32_t nr_points = 0; + std::ifstream fs; + std::string line; + + if (file_name == "" || !boost::filesystem::exists (file_name)) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Could not find file '%s'.\n", file_name.c_str ()); + return (-1); + } + + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + + //Read the magic + uint32_t length_of_magic; + fs.read ((char*)&length_of_magic, sizeof (uint32_t)); + char *magic = new char [length_of_magic]; + fs.read (magic, sizeof (char) * length_of_magic); + if (strcmp (magic, "IFS")) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] File %s is not an IFS file!\n", file_name.c_str ()); + fs.close (); + return (-1); + } + delete[] magic; + + //Read IFS version + float version; + fs.read ((char*)&version, sizeof (float)); + if (version == 1.0f) + ifs_version = IFS_V1_0; + else + if (version == 1.1f) + ifs_version = IFS_V1_1; + else + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Bad IFS file %f!\n", version); + fs.close (); + return (-1); + } + + //Read the name + uint32_t length_of_name; + fs.read ((char*)&length_of_name, sizeof (uint32_t)); + char *name = new char [length_of_name]; + fs.read (name, sizeof (char) * length_of_name); + delete[] name; + int offset = 0; + + // Read the header and fill it in with wonderful values + try + { + while (!fs.eof ()) + { + //Read the keyword + uint32_t length_of_keyword; + fs.read ((char*)&length_of_keyword, sizeof (uint32_t)); + char *keyword = new char [length_of_keyword]; + fs.read (keyword, sizeof (char) * length_of_keyword); + + if (strcmp (keyword, "VERTICES") == 0) + { + fs.read ((char*)&nr_points, sizeof (uint32_t)); + if ((nr_points == 0) || (nr_points > 10000000)) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Bad number of vertices %lu!\n", nr_points); + fs.close (); + return (-1); + } + + cloud.fields.resize (3); + cloud.fields[0].name = "x"; + cloud.fields[1].name = "y"; + cloud.fields[2].name = "z"; + + for (int i = 0; i < 3; ++i, offset += 4) + { + cloud.fields[i].offset = offset; + cloud.fields[i].datatype = pcl::PCLPointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.point_step = offset; + cloud.data.resize (nr_points * cloud.point_step); + data_idx = fs.tellg (); + break; + } + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] %s\n", exception); + fs.close (); + return (-1); + } + + // Set width and height + cloud.width = nr_points; + cloud.height = 1; + cloud.row_step = cloud.point_step * cloud.width; + + // Close file + fs.close (); + + return (0); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSReader::read (const std::string &file_name, + pcl::PCLPointCloud2 &cloud, int &ifs_version) +{ + pcl::console::TicToc tt; + tt.tic (); + + unsigned int data_idx; + + int res = readHeader (file_name, cloud, ifs_version, data_idx); + + if (res < 0) + return (res); + + // Setting the is_dense property to true by default + cloud.is_dense = true; + + boost::iostreams::mapped_file_source mapped_file; + + size_t data_size = data_idx + cloud.data.size (); + + try + { + mapped_file.open (file_name, data_size, 0); + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSReader::read] Error : %s!\n", file_name.c_str (), exception); + mapped_file.close (); + return (-1); + } + + if(!mapped_file.is_open ()) + { + PCL_ERROR ("[pcl::IFSReader::read] File mapping failure\n"); + mapped_file.close (); + return (-1); + } + + // Copy the data + memcpy (&cloud.data[0], mapped_file.data () + data_idx, cloud.data.size ()); + + mapped_file.close (); + + double total_time = tt.toc (); + PCL_DEBUG ("[pcl::IFSReader::read] Loaded %s as a %s cloud in %g ms with %d points. Available dimensions: %s.\n", + file_name.c_str (), cloud.is_dense ? "dense" : "non-dense", total_time, + cloud.width * cloud.height, pcl::getFieldsList (cloud).c_str ()); + return (0); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, int &ifs_version) +{ + pcl::console::TicToc tt; + tt.tic (); + + unsigned int data_idx; + + int res = readHeader (file_name, mesh.cloud, ifs_version, data_idx); + + if (res < 0) + return (res); + + // Setting the is_dense property to true by default + mesh.cloud.is_dense = true; + + boost::iostreams::mapped_file_source mapped_file; + + size_t data_size = data_idx + mesh.cloud.data.size (); + + try + { + mapped_file.open (file_name, data_size, 0); + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSReader::read] Error : %s!\n", file_name.c_str (), exception); + mapped_file.close (); + return (-1); + } + + if(!mapped_file.is_open ()) + { + PCL_ERROR ("[pcl::IFSReader::read] File mapping failure\n"); + mapped_file.close (); + return (-1); + } + + // Copy the data + memcpy (&mesh.cloud.data[0], mapped_file.data () + data_idx, mesh.cloud.data.size ()); + + mapped_file.close (); + + // Reopen the file to load the facets + std::ifstream fs; + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::IFSReader::read] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + // Jump to the end of cloud data + fs.seekg (data_size); + // Read the TRIANGLES keyword + uint32_t length_of_keyword; + fs.read ((char*)&length_of_keyword, sizeof (uint32_t)); + char *keyword = new char [length_of_keyword]; + fs.read (keyword, sizeof (char) * length_of_keyword); + if (strcmp (keyword, "TRIANGLES")) + { + PCL_ERROR ("[pcl::IFSReader::read] File %s is does not contain facets!\n", file_name.c_str ()); + fs.close (); + return (-1); + } + delete[] keyword; + // Read the number of facets + uint32_t nr_facets; + fs.read ((char*)&nr_facets, sizeof (uint32_t)); + if ((nr_facets == 0) || (nr_facets > 10000000)) + { + PCL_ERROR ("[pcl::IFSReader::read] Bad number of facets %lu!\n", nr_facets); + fs.close (); + return (-1); + } + // Resize the mesh polygons + mesh.polygons.resize (nr_facets); + // Fill each polygon + for (uint32_t i = 0; i < nr_facets; ++i) + { + pcl::Vertices &facet = mesh.polygons[i]; + facet.vertices.resize (3); + fs.read ((char*)&(facet.vertices[0]), sizeof (uint32_t)); + fs.read ((char*)&(facet.vertices[1]), sizeof (uint32_t)); + fs.read ((char*)&(facet.vertices[2]), sizeof (uint32_t)); + } + // We are done, close the file + fs.close (); + // Display statistics + double total_time = tt.toc (); + PCL_DEBUG ("[pcl::IFSReader::read] Loaded %s as a polygon mesh in %g ms with %d points and %d facets.\n", + file_name.c_str (), total_time, mesh.cloud.width * mesh.cloud.height, mesh.polygons.size ()); + return (0); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSWriter::write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const std::string& cloud_name) +{ + if (cloud.data.empty ()) + { + PCL_ERROR ("[pcl::IFSWriter::write] Input point cloud has no data!\n"); + return (-1); + } + + if (!cloud.is_dense) + { + PCL_ERROR ("[pcl::IFSWriter::write] Non dense cloud are not alowed by IFS format!\n"); + return (-1); + } + + const std::string magic = "IFS"; + const float version = 1.0f; + const std::string vertices = "VERTICES"; + std::vector header (sizeof (uint32_t) + magic.size () + 1 + + sizeof (float) + + sizeof (uint32_t) + cloud_name.size () + 1 + + sizeof (uint32_t) + vertices.size () + 1 + + sizeof (uint32_t)); + char* addr = &(header[0]); + const uint32_t magic_size = static_cast (magic.size ()) + 1; + memcpy (addr, &magic_size, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + memcpy (addr, magic.c_str (), magic_size * sizeof (char)); + addr+= magic_size * sizeof (char); + memcpy (addr, &version, sizeof (float)); + addr+= sizeof (float); + const uint32_t cloud_name_size = static_cast (cloud_name.size ()) + 1; + memcpy (addr, &cloud_name_size, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + memcpy (addr, cloud_name.c_str (), cloud_name_size * sizeof (char)); + addr+= cloud_name_size * sizeof (char); + const uint32_t vertices_size = static_cast (vertices.size ()) + 1; + memcpy (addr, &vertices_size, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + memcpy (addr, vertices.c_str (), vertices_size * sizeof (char)); + addr+= vertices_size * sizeof (char); + const uint32_t nb_vertices = cloud.data.size () / cloud.point_step; + memcpy (addr, &nb_vertices, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + + std::size_t data_idx = header.size (); + + boost::iostreams::mapped_file_sink sink; + boost::iostreams::mapped_file_params params; + params.path = file_name; + params.flags = boost::iostreams::mapped_file_base::readwrite; + params.offset = 0; + params.new_file_size = data_idx + cloud.data.size (); + params.length = data_idx + cloud.data.size (); + + try + { + sink.open (params); + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSWriter::write] Error : %s!\n", file_name.c_str (), exception); + sink.close (); + return (-1); + } + + if (!sink.is_open ()) + { + PCL_ERROR ("[pcl::IFSWriter::write] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno)); + sink.close (); + return (-1); + } + + // copy header + memcpy (sink.data (), &header[0], data_idx); + + // Copy the data + memcpy (sink.data () + data_idx, &cloud.data[0], cloud.data.size ()); + + sink.close (); + + return (0); +} diff --git a/io/src/image_depth.cpp b/io/src/image_depth.cpp new file mode 100644 index 00000000..6077c831 --- /dev/null +++ b/io/src/image_depth.cpp @@ -0,0 +1,302 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * Suat Gedikli + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +#include +#include +#include + +#include + +using pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::DepthImage::DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, pcl::uint64_t shadow_value, pcl::uint64_t no_sample_value) +: wrapper_ (depth_metadata) +, baseline_ (baseline) +, focal_length_ (focal_length) +, shadow_value_ (shadow_value) +, no_sample_value_ (no_sample_value) +, timestamp_ (Clock::now ()) +{} + + +pcl::io::DepthImage::DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, pcl::uint64_t shadow_value, pcl::uint64_t no_sample_value, Timestamp timestamp) +: wrapper_(depth_metadata) +, baseline_ (baseline) +, focal_length_ (focal_length) +, shadow_value_ (shadow_value) +, no_sample_value_ (no_sample_value) +, timestamp_(timestamp) +{} + + +pcl::io::DepthImage::~DepthImage () +{} + + +const unsigned short* +pcl::io::DepthImage::getData () +{ + return static_cast (wrapper_->getData ()); +} + + +int +pcl::io::DepthImage::getDataSize () const +{ + return (wrapper_->getDataSize ()); +} + + +const FrameWrapper::Ptr +pcl::io::DepthImage::getMetaData () const +{ + return (wrapper_); +} + + +float +pcl::io::DepthImage::getBaseline () const +{ + return (baseline_); +} + + +float +pcl::io::DepthImage::getFocalLength () const +{ + return (focal_length_); +} + + +pcl::uint64_t +pcl::io::DepthImage::getShadowValue () const +{ + return (shadow_value_); +} + + +pcl::uint64_t +pcl::io::DepthImage::getNoSampleValue () const +{ + return (no_sample_value_); +} + + +unsigned +pcl::io::DepthImage::getWidth () const +{ + return (wrapper_->getWidth ()); +} + + +unsigned +pcl::io::DepthImage::getHeight () const +{ + return (wrapper_->getHeight ()); +} + + +unsigned +pcl::io::DepthImage::getFrameID () const +{ + return (wrapper_->getFrameID ()); +} + + +pcl::uint64_t +pcl::io::DepthImage::getTimestamp () const +{ + return (wrapper_->getTimestamp ()); +} + + +pcl::io::DepthImage::Timestamp +pcl::io::DepthImage::getSystemTimestamp () const +{ + return (timestamp_); +} + +// Fill external buffers //////////////////////////////////////////////////// + +void +pcl::io::DepthImage::fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (unsigned short)); + + // special case no sclaing, no padding => memcopy! + if (width == wrapper_->getWidth () && height == wrapper_->getHeight () && (line_step == width * sizeof (unsigned short))) + { + memcpy (depth_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + return; + } + + // padding skip for destination image + unsigned bufferSkip = line_step - width * static_cast (sizeof (unsigned short)); + + // step and padding skip for source image + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + // Fill in the depth image data, converting mm to m + short bad_point = std::numeric_limits::quiet_NaN (); + unsigned depthIdx = 0; + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer) + { + /// @todo Different values for these cases + unsigned short pixel = inputBuffer[depthIdx]; + if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_) + *depth_buffer = bad_point; + else + { + *depth_buffer = static_cast( pixel ); + } + } + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (depth_buffer); + depth_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} + +void +pcl::io::DepthImage::fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (float)); + + // padding skip for destination image + unsigned bufferSkip = line_step - width * static_cast (sizeof (float)); + + // step and padding skip for source image + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + // Fill in the depth image data, converting mm to m + float bad_point = std::numeric_limits::quiet_NaN (); + unsigned depthIdx = 0; + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer) + { + /// @todo Different values for these cases + unsigned short pixel = inputBuffer[depthIdx]; + if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_) + *depth_buffer = bad_point; + else + { + *depth_buffer = static_cast( pixel ) * 0.001f; // millimeters to meters + } + } + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (depth_buffer); + depth_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} + +void +pcl::io::DepthImage::fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (float)); + + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + unsigned bufferSkip = line_step - width * static_cast (sizeof (float)); + + // Fill in the depth image data + // iterate over all elements and fill disparity matrix: disp[x,y] = f * b / z_distance[x,y]; + // focal length is for the native image resolution -> focal_length = focal_length_ / xStep; + float constant = focal_length_ * baseline_ * 1000.0f / static_cast (xStep); + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0, depthIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++disparity_buffer) + { + unsigned short pixel = inputBuffer[depthIdx]; + if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_) + *disparity_buffer = 0.0; + else + *disparity_buffer = constant / static_cast (pixel); + } + + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (disparity_buffer); + disparity_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} diff --git a/io/src/image_grabber.cpp b/io/src/image_grabber.cpp index b1582033..068ec8fe 100644 --- a/io/src/image_grabber.cpp +++ b/io/src/image_grabber.cpp @@ -119,7 +119,7 @@ struct pcl::ImageGrabberBase::ImageGrabberImpl //! Checks if a timestamp is given in the filename //! And returns if so bool - getTimestampFromFilepath (const std::string &filepath, uint64_t ×tamp) const; + getTimestampFromFilepath (const std::string &filepath, pcl::uint64_t ×tamp) const; size_t numFrames () const; @@ -495,7 +495,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::rewindOnce () bool pcl::ImageGrabberBase::ImageGrabberImpl::getTimestampFromFilepath ( const std::string &filepath, - uint64_t ×tamp) const + pcl::uint64_t ×tamp) const { // For now, we assume the file is of the form frame_[22-char POSIX timestamp]_* char timestamp_str[256]; @@ -504,7 +504,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getTimestampFromFilepath ( timestamp_str); if (result > 0) { - // Convert to uint64_t, microseconds since 1970-01-01 + // Convert to pcl::uint64_t, microseconds since 1970-01-01 boost::posix_time::ptime cur_date = boost::posix_time::from_iso_string (timestamp_str); boost::posix_time::ptime zero_date ( boost::gregorian::date (1970,boost::gregorian::Jan,1)); @@ -625,7 +625,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (size_t idx, } } // Handle timestamps - uint64_t timestamp; + pcl::uint64_t timestamp; if (getTimestampFromFilepath (depth_image_file, timestamp)) { cloud_color.header.stamp = timestamp; @@ -657,7 +657,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (size_t idx, } } // Handle timestamps - uint64_t timestamp; + pcl::uint64_t timestamp; if (getTimestampFromFilepath (depth_image_file, timestamp)) { cloud.header.stamp = timestamp; @@ -748,7 +748,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (size_t idx, depth.readOMP (depth_pclzf_file, cloud_color, num_threads_); } // handle timestamps - uint64_t timestamp; + pcl::uint64_t timestamp; if (getTimestampFromFilepath (depth_pclzf_file, timestamp)) { cloud_color.header.stamp = timestamp; @@ -789,7 +789,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (size_t idx, else depth.readOMP (depth_pclzf_file, cloud, num_threads_); // handle timestamps - uint64_t timestamp; + pcl::uint64_t timestamp; if (getTimestampFromFilepath (depth_pclzf_file, timestamp)) { cloud.header.stamp = timestamp; @@ -1082,7 +1082,7 @@ pcl::ImageGrabberBase::getDepthFileNameAtIndex (size_t idx) const //////////////////////////////////////////////////////////////////////////////////////// bool -pcl::ImageGrabberBase::getTimestampAtIndex (size_t idx, uint64_t ×tamp) const +pcl::ImageGrabberBase::getTimestampAtIndex (size_t idx, pcl::uint64_t ×tamp) const { std::string filename; if (impl_->pclzf_mode_) diff --git a/io/src/image_ir.cpp b/io/src/image_ir.cpp new file mode 100644 index 00000000..26b99110 --- /dev/null +++ b/io/src/image_ir.cpp @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * Suat Gedikli + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +#include +#include +#include + +#include + +using pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::IRImage::IRImage (FrameWrapper::Ptr ir_metadata) + : wrapper_(ir_metadata) + , timestamp_(Clock::now ()) +{} + + +pcl::io::IRImage::IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time) + : wrapper_(ir_metadata) + , timestamp_(time) +{} + + +const unsigned short* +pcl::io::IRImage::getData () +{ + return ( static_cast (wrapper_->getData ()) ); +} + + +int +pcl::io::IRImage::getDataSize () const +{ + return (wrapper_->getDataSize ()); +} + + +const FrameWrapper::Ptr +pcl::io::IRImage::getMetaData () const +{ + return (wrapper_); +} + + +unsigned +pcl::io::IRImage::getWidth () const +{ + return (wrapper_->getWidth ()); +} + + +unsigned +pcl::io::IRImage::getHeight () const +{ + return (wrapper_->getHeight ()); +} + + +unsigned +pcl::io::IRImage::getFrameID () const +{ + return (wrapper_->getFrameID ()); +} + + +pcl::uint64_t +pcl::io::IRImage::getTimestamp () const +{ + return (wrapper_->getTimestamp ()); +} + + +pcl::io::IRImage::Timestamp +pcl::io::IRImage::getSystemTimestamp () const +{ + return (timestamp_); +} + + +void pcl::io::IRImage::fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (unsigned short)); + + // special case no sclaing, no padding => memcopy! + if (width == wrapper_->getWidth () && height == wrapper_->getHeight () && (line_step == width * sizeof (unsigned short))) + { + memcpy (ir_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + return; + } + + // padding skip for destination image + unsigned bufferSkip = line_step - width * static_cast (sizeof (unsigned short)); + + // step and padding skip for source image + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + unsigned irIdx = 0; + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, irIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, irIdx += xStep, ++ir_buffer) + *ir_buffer = inputBuffer[irIdx]; + + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (ir_buffer); + ir_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} + diff --git a/io/src/image_rgb24.cpp b/io/src/image_rgb24.cpp new file mode 100644 index 00000000..44445795 --- /dev/null +++ b/io/src/image_rgb24.cpp @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +#include + +using pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::ImageRGB24::ImageRGB24 (FrameWrapper::Ptr image_metadata) + : Image (image_metadata) +{} + + +pcl::io::ImageRGB24::ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp) + : Image (image_metadata, timestamp) +{} + + +pcl::io::ImageRGB24::~ImageRGB24 () throw () +{} + +bool +pcl::io::ImageRGB24::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const +{ + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); +} + + +void +pcl::io::ImageRGB24::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("Up-sampling not supported. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width == 0 && wrapper_->getHeight () % height == 0) + { + unsigned src_step = wrapper_->getWidth () / width; + unsigned src_skip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + if (gray_line_step == 0) + gray_line_step = width; + + unsigned dst_skip = gray_line_step - width; // skip of padding values in bytes + + unsigned char* dst_line = gray_buffer; + const RGB888Pixel* src_line = (RGB888Pixel*) wrapper_->getData (); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, src_line += src_skip, dst_line += dst_skip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, src_line += src_step, dst_line ++) + { + *dst_line = static_cast((static_cast (src_line->r) * 299 + + static_cast (src_line->g) * 587 + + static_cast (src_line->b) * 114) * 0.001); + } + } + } + else + { + THROW_IO_EXCEPTION ("Down-sampling only possible for integer scale. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + } +} + + +void +pcl::io::ImageRGB24::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("Up-sampling not supported. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (width == wrapper_->getWidth () && height == wrapper_->getHeight ()) + { + unsigned line_size = width * 3; + if (rgb_line_step == 0 || rgb_line_step == line_size) + { + memcpy (rgb_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + } + else // line by line + { + unsigned char* rgb_line = rgb_buffer; + const unsigned char* src_line = static_cast (wrapper_->getData ()); + for (unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_line += rgb_line_step, src_line += line_size) + { + memcpy (rgb_line, src_line, line_size); + } + } + } + else if (wrapper_->getWidth () % width == 0 && wrapper_->getHeight () % height == 0) // downsamplig + { + unsigned src_step = wrapper_->getWidth () / width; + unsigned src_skip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + if (rgb_line_step == 0) + rgb_line_step = width * 3; + + unsigned dst_skip = rgb_line_step - width * 3; // skip of padding values in bytes + + RGB888Pixel* dst_line = reinterpret_cast (rgb_buffer); + const RGB888Pixel* src_line = (RGB888Pixel*) wrapper_->getData (); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, src_line += src_skip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, src_line += src_step, dst_line ++) + { + *dst_line = *src_line; + } + + if (dst_skip != 0) + { + // use bytes to skip rather than XnRGB24Pixel's, since line_step does not need to be multiple of 3 + unsigned char* temp = reinterpret_cast (dst_line); + dst_line = reinterpret_cast (temp + dst_skip); + } + } + } + else + { + THROW_IO_EXCEPTION ("Down-sampling only possible for integer scale. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + } +} + diff --git a/io/src/image_yuv422.cpp b/io/src/image_yuv422.cpp new file mode 100644 index 00000000..3e1b69a1 --- /dev/null +++ b/io/src/image_yuv422.cpp @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 2011 Willow Garage, Inc. + * Suat Gedikli + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include +#include + +#include + +#include +#include + +#define CLIP_CHAR(c) static_cast ((c)>255?255:(c)<0?0:(c)) + +using pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata) + : Image (image_metadata) +{} + + +pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp) + : Image (image_metadata, timestamp) +{} + + +pcl::io::ImageYUV422::~ImageYUV422 () throw () +{} + +bool +pcl::io::ImageYUV422::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const +{ + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); +} + + +void +pcl::io::ImageYUV422::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const +{ + // 0 1 2 3 + // u y1 v y2 + + if (wrapper_->getWidth () != width && wrapper_->getHeight () != height) + { + if (width > wrapper_->getWidth () || height > wrapper_->getHeight () ) + THROW_IO_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if ( wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0 + || (wrapper_->getWidth () / width) & 0x01 || (wrapper_->getHeight () / height & 0x01) ) + THROW_IO_EXCEPTION ("Downsampling only possible for power of two scale in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + } + + register const uint8_t* yuv_buffer = (uint8_t*) wrapper_->getData (); + + unsigned rgb_line_skip = 0; + if (rgb_line_step != 0) + rgb_line_skip = rgb_line_step - width * 3; + + if (wrapper_->getWidth () == width && wrapper_->getHeight () == height) + { + for ( register unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip ) + { + for ( register unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4 ) + { + int v = yuv_buffer[2] - 128; + int u = yuv_buffer[0] - 128; + + rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14)); + rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14)); + rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14)); + + rgb_buffer[3] = CLIP_CHAR (yuv_buffer[3] + ((v * 18678 + 8192 ) >> 14)); + rgb_buffer[4] = CLIP_CHAR (yuv_buffer[3] + ((v * -9519 - u * 6472 + 8192 ) >> 14)); + rgb_buffer[5] = CLIP_CHAR (yuv_buffer[3] + ((u * 33292 + 8192 ) >> 14)); + } + } + } + else + { + register unsigned yuv_step = wrapper_->getWidth () / width; + register unsigned yuv_x_step = yuv_step << 1; + register unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 ); + + for ( register unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip ) + { + for ( register unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step ) + { + int v = yuv_buffer[2] - 128; + int u = yuv_buffer[0] - 128; + + rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14)); + rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14)); + rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14)); + } + } + } +} + + +void +pcl::io::ImageYUV422::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const +{ + // u y1 v y2 + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("Downsampling only possible for integer scales in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + unsigned gray_line_skip = 0; + if (gray_line_step != 0) + gray_line_skip = gray_line_step - width; + + register unsigned yuv_step = wrapper_->getWidth () / width; + register unsigned yuv_x_step = yuv_step << 1; + register unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 ); + register const uint8_t* yuv_buffer = ( (uint8_t*) wrapper_->getData () + 1); + + for ( register unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip ) + { + for ( register unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step ) + { + *gray_buffer = *yuv_buffer; + } + } +} + diff --git a/io/src/io_exception.cpp b/io/src/io_exception.cpp new file mode 100644 index 00000000..614ef1b6 --- /dev/null +++ b/io/src/io_exception.cpp @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include "pcl/io/io_exception.h" +#include + +pcl::io::IOException::IOException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) + : function_name_ (function_name) + , file_name_ (file_name) + , line_number_ (line_number) + , message_ (message) +{ + std::stringstream sstream; + sstream << function_name_ << " @ " << file_name_ << " @ " << line_number_ << " : " << message_; + message_long_ = sstream.str (); +} + +pcl::io::IOException::~IOException () throw () +{ +} + +pcl::io::IOException& +pcl::io::IOException::operator = (const IOException& exception) +{ + message_ = exception.message_; + return (*this); +} + +const char* +pcl::io::IOException::what () const throw () +{ + return (message_long_.c_str ()); +} + +const std::string& +pcl::io::IOException::getFunctionName () const +{ + return (function_name_); +} + +const std::string& +pcl::io::IOException::getFileName () const +{ + return (file_name_); +} + +unsigned +pcl::io::IOException::getLineNumber () const +{ + return (line_number_); +} diff --git a/io/src/libpng_wrapper.cpp b/io/src/libpng_wrapper.cpp index 0a96ccc7..4495ca0f 100644 --- a/io/src/libpng_wrapper.cpp +++ b/io/src/libpng_wrapper.cpp @@ -43,13 +43,13 @@ #include #include #include -#include #include // user defined I/O callback methods for libPNG namespace { + using pcl::uint8_t; ///////////////////////////////////////////////////////////////////////////////////////// void user_read_data (png_structp png_ptr, png_bytep data, png_size_t length) @@ -180,7 +180,7 @@ namespace pcl size_t& height_arg, unsigned int& channels_arg) { - int y; + unsigned long y; png_structp png_ptr; png_infop info_ptr; png_uint_32 png_width; diff --git a/io/src/lzf_image_io.cpp b/io/src/lzf_image_io.cpp index a1ea3fee..7742f169 100644 --- a/io/src/lzf_image_io.cpp +++ b/io/src/lzf_image_io.cpp @@ -59,6 +59,17 @@ #define LZF_HEADER_SIZE 37 + +// The signature of boost::property_tree::xml_parser::write_xml() changed in Boost 1.56 +// See https://github.com/PointCloudLibrary/pcl/issues/864 +#include +#if (BOOST_VERSION >= 105600) + typedef boost::property_tree::xml_writer_settings xml_writer_settings; +#else + typedef boost::property_tree::xml_writer_settings xml_writer_settings; +#endif + + ////////////////////////////////////////////////////////////////////////////// bool pcl::io::LZFImageWriter::saveImageBlob (const char* data, @@ -198,9 +209,8 @@ pcl::io::LZFImageWriter::writeParameter (const double ¶meter, catch (std::exception& e) {} - boost::property_tree::xml_writer_settings settings ('\t', 1); pt.put (tag, parameter); - write_xml (filename, pt, std::locale (), settings); + write_xml (filename, pt, std::locale (), xml_writer_settings ('\t', 1)); return (true); } @@ -218,13 +228,12 @@ pcl::io::LZFDepth16ImageWriter::writeParameters (const pcl::io::CameraParameters catch (std::exception& e) {} - boost::property_tree::xml_writer_settings settings ('\t', 1); pt.put ("depth.focal_length_x", parameters.focal_length_x); pt.put ("depth.focal_length_y", parameters.focal_length_y); pt.put ("depth.principal_point_x", parameters.principal_point_x); pt.put ("depth.principal_point_y", parameters.principal_point_y); pt.put ("depth.z_multiplication_factor", z_multiplication_factor_); - write_xml (filename, pt, std::locale (), settings); + write_xml (filename, pt, std::locale (), xml_writer_settings ('\t', 1)); return (true); } @@ -240,7 +249,7 @@ pcl::io::LZFRGB24ImageWriter::write (const char *data, int ptr1 = 0, ptr2 = width * height, ptr3 = 2 * width * height; - for (int i = 0; i < width * height; ++i, ++ptr1, ++ptr2, ++ptr3) + for (uint32_t i = 0; i < width * height; ++i, ++ptr1, ++ptr2, ++ptr3) { rrggbb[ptr1] = data[i * 3 + 0]; rrggbb[ptr2] = data[i * 3 + 1]; @@ -279,12 +288,11 @@ pcl::io::LZFRGB24ImageWriter::writeParameters (const pcl::io::CameraParameters & catch (std::exception& e) {} - boost::property_tree::xml_writer_settings settings ('\t', 1); pt.put ("rgb.focal_length_x", parameters.focal_length_x); pt.put ("rgb.focal_length_y", parameters.focal_length_y); pt.put ("rgb.principal_point_x", parameters.principal_point_x); pt.put ("rgb.principal_point_y", parameters.principal_point_y); - write_xml (filename, pt, std::locale (), settings); + write_xml (filename, pt, std::locale (), xml_writer_settings ('\t', 1)); return (true); } diff --git a/io/src/obj_io.cpp b/io/src/obj_io.cpp index 90034063..0ddb11ee 100644 --- a/io/src/obj_io.cpp +++ b/io/src/obj_io.cpp @@ -3,6 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2013, Open Perception, Inc. * * All rights reserved. * @@ -33,13 +34,947 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id$ - * */ #include #include #include #include +#include +#include +#include + +pcl::MTLReader::MTLReader () +{ + xyz_to_rgb_matrix_ << 2.3706743, -0.9000405, -0.4706338, + -0.5138850, 1.4253036, 0.0885814, + 0.0052982, -0.0146949, 1.0093968; +} + +inline void +pcl::MTLReader::cie2rgb (const Eigen::Vector3f &xyz, pcl::TexMaterial::RGB& rgb) const +{ + Eigen::Vector3f rgb_vec = xyz_to_rgb_matrix_ * xyz; + rgb.r = rgb_vec[0]; rgb.g = rgb_vec[1]; rgb.b = rgb_vec[2]; +} + +int +pcl::MTLReader::fillRGBfromXYZ (const std::vector& split_line, + pcl::TexMaterial::RGB& rgb) +{ + Eigen::Vector3f xyz; + if (split_line.size () == 5) + { + try + { + xyz[0] = boost::lexical_cast (split_line[2]); + xyz[1] = boost::lexical_cast (split_line[3]); + xyz[2] = boost::lexical_cast (split_line[4]); + } + catch (boost::bad_lexical_cast &) + { + return (-1); + } + } + else + if (split_line.size () == 3) + { + try + { + xyz[0] = xyz[1] = xyz[2] = boost::lexical_cast (split_line[2]); + } + catch (boost::bad_lexical_cast &) + { + return (-1); + } + } + else + return (-1); + + cie2rgb (xyz, rgb); + return (0); +} + +int +pcl::MTLReader::fillRGBfromRGB (const std::vector& split_line, + pcl::TexMaterial::RGB& rgb) +{ + if (split_line.size () == 4) + { + try + { + rgb.r = boost::lexical_cast (split_line[1]); + rgb.g = boost::lexical_cast (split_line[2]); + rgb.b = boost::lexical_cast (split_line[3]); + } + catch (boost::bad_lexical_cast &) + { + rgb.r = rgb.g = rgb.b = 0; + return (-1); + } + } + else + if (split_line.size () == 2) + { + try + { + rgb.r = rgb.g = rgb.b = boost::lexical_cast (split_line[1]); + } + catch (boost::bad_lexical_cast &) + { + return (-1); + } + } + else + return (-1); + + return (0); +} + +std::vector::const_iterator +pcl::MTLReader::getMaterial (const std::string& material_name) const +{ + std::vector::const_iterator mat_it = materials_.begin (); + for (; mat_it != materials_.end (); ++mat_it) + if (mat_it->tex_name == material_name) + break; + return (mat_it); +} + +int +pcl::MTLReader::read (const std::string& obj_file_name, + const std::string& mtl_file_name) +{ + if (obj_file_name == "" || !boost::filesystem::exists (obj_file_name)) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not find file '%s'!\n", + obj_file_name.c_str ()); + return (-1); + } + + if (mtl_file_name == "") + { + PCL_ERROR ("[pcl::MTLReader::read] MTL file name is empty!\n"); + return (-1); + } + + boost::filesystem::path obj_file_path (obj_file_name.c_str ()); + boost::filesystem::path mtl_file_path = obj_file_path.parent_path (); + mtl_file_path /= mtl_file_name; + return (read (mtl_file_path.string ())); +} + +int +pcl::MTLReader::read (const std::string& mtl_file_path) +{ + if (mtl_file_path == "" || !boost::filesystem::exists (mtl_file_path)) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not find file '%s'.\n", mtl_file_path.c_str ()); + return (-1); + } + + std::ifstream mtl_file; + mtl_file.open (mtl_file_path.c_str (), std::ios::binary); + if (!mtl_file.is_open () || mtl_file.fail ()) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not open file '%s'! Error : %s\n", + mtl_file_path.c_str (), strerror(errno)); + mtl_file.close (); + return (-1); + } + + std::string line; + std::vector st; + boost::filesystem::path parent_path = mtl_file_path.c_str (); + parent_path = parent_path.parent_path (); + + try + { + while (!mtl_file.eof ()) + { + getline (mtl_file, line); + // Ignore empty lines + if (line == "") + continue; + + // Tokenize the line + std::stringstream sstream (line); + sstream.imbue (std::locale::classic ()); + line = sstream.str (); + boost::trim (line); + boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on); + // Ignore comments + if (st[0] == "#") + continue; + + if (st[0] == "newmtl") + { + materials_.push_back (pcl::TexMaterial ()); + materials_.back ().tex_name = st[1]; + continue; + } + + if (st[0] == "Ka" || st[0] == "Kd" || st[0] == "Ks") + { + if (st[1] == "spectral") + { + PCL_ERROR ("[pcl::MTLReader::read] Can't handle spectral files!\n"); + mtl_file.close (); + materials_.clear (); + return (-1); + } + else + { + pcl::TexMaterial::RGB &rgb = materials_.back ().tex_Ka; + if (st[0] == "Kd") + rgb = materials_.back ().tex_Kd; + else if (st[0] == "Ks") + rgb = materials_.back ().tex_Ks; + + if (st[1] == "xyz") + { + if (fillRGBfromXYZ (st, rgb)) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to RGB values", + line.c_str ()); + mtl_file.close (); + materials_.clear (); + return (-1); + } + } + else + { + if (fillRGBfromRGB (st, rgb)) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to RGB values", + line.c_str ()); + mtl_file.close (); + materials_.clear (); + return (-1); + } + } + } + continue; + } + + if (st[0] == "illum") + { + try + { + materials_.back ().tex_illum = boost::lexical_cast (st[1]); + } + catch (boost::bad_lexical_cast &) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to illumination model", + line.c_str ()); + mtl_file.close (); + materials_.clear (); + return (-1); + } + continue; + } + + if (st[0] == "d") + { + if (st.size () > 2) + { + try + { + materials_.back ().tex_d = boost::lexical_cast (st[2]); + } + catch (boost::bad_lexical_cast &) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to transparency value", + line.c_str ()); + mtl_file.close (); + materials_.clear (); + return (-1); + } + } + else + { + try + { + materials_.back ().tex_d = boost::lexical_cast (st[1]); + } + catch (boost::bad_lexical_cast &) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to transparency value", + line.c_str ()); + mtl_file.close (); + materials_.clear (); + return (-1); + } + } + continue; + } + + if (st[0] == "Ns") + { + try + { + materials_.back ().tex_d = boost::lexical_cast (st[1]); + } + catch (boost::bad_lexical_cast &) + { + PCL_ERROR ("[pcl::MTLReader::read] Could not convert %s to shininess value", + line.c_str ()); + mtl_file.close (); + materials_.clear (); + return (-1); + } + continue; + } + + if (st[0] == "map_Kd") + { + boost::filesystem::path full_path = parent_path; + full_path/= st.back ().c_str (); + materials_.back ().tex_file = full_path.string (); + continue; + } + // other elements? we don't care for now + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::MTLReader::read] %s\n", exception); + mtl_file.close (); + materials_.clear (); + return (-1); + } + + return (0); +} + +int +pcl::OBJReader::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, int &data_type, unsigned int &data_idx, + const int offset) +{ + origin = Eigen::Vector4f::Zero (); + orientation = Eigen::Quaternionf::Identity (); + file_version = 0; + cloud.width = cloud.height = cloud.point_step = cloud.row_step = 0; + cloud.data.clear (); + data_type = 0; + data_idx = offset; + + std::ifstream fs; + std::string line; + + if (file_name == "" || !boost::filesystem::exists (file_name)) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] Could not find file '%s'.\n", file_name.c_str ()); + return (-1); + } + + // Open file in binary mode to avoid problem of + // std::getline() corrupting the result of ifstream::tellg() + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + + // Seek at the given offset + fs.seekg (offset, std::ios::beg); + + // Read the header and fill it in with wonderful values + bool vertex_normal_found = false; + bool vertex_texture_found = false; + // Material library, skip for now! + // bool material_found = false; + std::vector material_files; + std::size_t nr_point = 0; + std::vector st; + + try + { + while (!fs.eof ()) + { + getline (fs, line); + // Ignore empty lines + if (line == "") + continue; + + // Tokenize the line + std::stringstream sstream (line); + sstream.imbue (std::locale::classic ()); + line = sstream.str (); + boost::trim (line); + boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on); + // Ignore comments + if (st.at (0) == "#") + continue; + + // Vertex + if (st.at (0) == "v") + { + ++nr_point; + continue; + } + + // Vertex texture + if ((st.at (0) == "vt") && !vertex_texture_found) + { + vertex_texture_found = true; + continue; + } + + // Vertex normal + if ((st.at (0) == "vn") && !vertex_normal_found) + { + vertex_normal_found = true; + continue; + } + + // Material library, skip for now! + if (st.at (0) == "mtllib") + { + material_files.push_back (st.at (1)); + continue; + } + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] %s\n", exception); + fs.close (); + return (-1); + } + + if (!nr_point) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] No vertices found!\n"); + fs.close (); + return (-1); + } + + int field_offset = 0; + for (int i = 0; i < 3; ++i, field_offset += 4) + { + cloud.fields.push_back (pcl::PCLPointField ()); + cloud.fields[i].offset = field_offset; + cloud.fields[i].datatype = pcl::PCLPointField::FLOAT32; + cloud.fields[i].count = 1; + } + + cloud.fields[0].name = "x"; + cloud.fields[1].name = "y"; + cloud.fields[2].name = "z"; + + if (vertex_normal_found) + { + std::string normals_names[3] = { "normal_x", "normal_y", "normal_z" }; + for (int i = 0; i < 3; ++i, field_offset += 4) + { + cloud.fields.push_back (pcl::PCLPointField ()); + pcl::PCLPointField& last = cloud.fields.back (); + last.name = normals_names[i]; + last.offset = field_offset; + last.datatype = pcl::PCLPointField::FLOAT32; + last.count = 1; + } + } + + if (material_files.size () > 0) + { + for (std::size_t i = 0; i < material_files.size (); ++i) + { + MTLReader companion; + if (companion.read (file_name, material_files[i])) + PCL_WARN ("[pcl::OBJReader::readHeader] Problem reading material file %s\n", + material_files[i].c_str ()); + companions_.push_back (companion); + } + } + + cloud.point_step = field_offset; + cloud.width = nr_point; + cloud.height = 1; + cloud.row_step = cloud.point_step * cloud.width; + cloud.is_dense = true; + cloud.data.resize (cloud.point_step * nr_point); + fs.close (); + return (0); +} + +int +pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset) +{ + int file_version; + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + return (read (file_name, cloud, origin, orientation, file_version, offset)); +} + +int +pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset) +{ + pcl::console::TicToc tt; + tt.tic (); + + int data_type; + unsigned int data_idx; + if (readHeader (file_name, cloud, origin, orientation, file_version, data_type, data_idx, offset)) + { + PCL_ERROR ("[pcl::OBJReader::read] Problem reading header!\n"); + return (-1); + } + + std::ifstream fs; + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n", + file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + + // Seek at the given offset + fs.seekg (data_idx, std::ios::beg); + + // Get normal_x and rgba fields indices + int normal_x_field = -1; + // std::size_t rgba_field = 0; + for (std::size_t i = 0; i < cloud.fields.size (); ++i) + if (cloud.fields[i].name == "normal_x") + { + normal_x_field = i; + break; + } + + + // else if (cloud.fields[i].name == "rgba") + // rgba_field = i; + + std::size_t point_idx = 0; + std::size_t normal_idx = 0; + std::string line; + std::vector st; + try + { + while (!fs.eof ()) + { + getline (fs, line); + // Ignore empty lines + if (line == "") + continue; + + // Tokenize the line + std::stringstream sstream (line); + sstream.imbue (std::locale::classic ()); + line = sstream.str (); + boost::trim (line); + boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on); + + // Ignore comments + if (st[0] == "#") + continue; + + // Vertex + if (st[0] == "v") + { + try + { + for (int i = 1, f = 0; i < 4; ++i, ++f) + { + float value = boost::lexical_cast (st[i]); + memcpy (&cloud.data[point_idx * cloud.point_step + cloud.fields[f].offset], + &value, + sizeof (float)); + } + ++point_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ()); + return (-1); + } + continue; + } + + // Vertex normal + if (st[0] == "vn") + { + try + { + for (int i = 1, f = normal_x_field; i < 4; ++i, ++f) + { + float value = boost::lexical_cast (st[i]); + memcpy (&cloud.data[normal_idx * cloud.point_step + cloud.fields[f].offset], + &value, + sizeof (float)); + } + ++normal_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ()); + return (-1); + } + continue; + } + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::OBJReader::read] %s\n", exception); + fs.close (); + return (-1); + } + + double total_time = tt.toc (); + PCL_DEBUG ("[pcl::OBJReader::read] Loaded %s as a dense cloud in %g ms with %d points. Available dimensions: %s.\n", + file_name.c_str (), total_time, + cloud.width * cloud.height, pcl::getFieldsList (cloud).c_str ()); + fs.close (); + return (0); +} + +int +pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh, const int offset) +{ + int file_version; + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + return (read (file_name, mesh, origin, orientation, file_version, offset)); +} + +int +pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset) +{ + pcl::console::TicToc tt; + tt.tic (); + + int data_type; + unsigned int data_idx; + if (readHeader (file_name, mesh.cloud, origin, orientation, file_version, data_type, data_idx, offset)) + { + PCL_ERROR ("[pcl::OBJReader::read] Problem reading header!\n"); + return (-1); + } + + std::ifstream fs; + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n", + file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + + // Seek at the given offset + fs.seekg (data_idx, std::ios::beg); + + // Get normal_x and rgba fields indices + int normal_x_field = -1; + // std::size_t rgba_field = 0; + for (std::size_t i = 0; i < mesh.cloud.fields.size (); ++i) + if (mesh.cloud.fields[i].name == "normal_x") + { + normal_x_field = i; + break; + } + + std::size_t v_idx = 0; + std::size_t vn_idx = 0; + std::size_t vt_idx = 0; + std::size_t f_idx = 0; + std::string line; + std::vector st; + std::vector coordinates; + try + { + while (!fs.eof ()) + { + getline (fs, line); + // Ignore empty lines + if (line == "") + continue; + + // Tokenize the line + std::stringstream sstream (line); + sstream.imbue (std::locale::classic ()); + line = sstream.str (); + boost::trim (line); + boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on); + + // Ignore comments + if (st[0] == "#") + continue; + // Vertex + if (st[0] == "v") + { + try + { + for (int i = 1, f = 0; i < 4; ++i, ++f) + { + float value = boost::lexical_cast (st[i]); + memcpy (&mesh.cloud.data[v_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset], + &value, + sizeof (float)); + } + ++v_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ()); + return (-1); + } + continue; + } + // Vertex normal + if (st[0] == "vn") + { + try + { + for (int i = 1, f = normal_x_field; i < 4; ++i, ++f) + { + float value = boost::lexical_cast (st[i]); + memcpy (&mesh.cloud.data[vn_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset], + &value, + sizeof (float)); + } + ++vn_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ()); + return (-1); + } + continue; + } + // Texture coordinates + if (st[0] == "vt") + { + try + { + Eigen::Vector3f c (0, 0, 0); + for (std::size_t i = 1; i < st.size (); ++i) + c[i-1] = boost::lexical_cast (st[i]); + if (c[2] == 0) + coordinates.push_back (Eigen::Vector2f (c[0], c[1])); + else + coordinates.push_back (Eigen::Vector2f (c[0]/c[2], c[1]/c[2])); + ++vt_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert line %s to texture coordinates!", line.c_str ()); + return (-1); + } + continue; + } + // Material + if (st[0] == "usemtl") + { + mesh.tex_polygons.push_back (std::vector ()); + mesh.tex_materials.push_back (pcl::TexMaterial ()); + for (std::size_t i = 0; i < companions_.size (); ++i) + { + std::vector::const_iterator mat_it = companions_[i].getMaterial (st[1]); + if (mat_it != companions_[i].materials_.end ()) + { + mesh.tex_materials.back () = *mat_it; + break; + } + } + // We didn't find the appropriate material so we create it here with name only. + if (mesh.tex_materials.back ().tex_name == "") + mesh.tex_materials.back ().tex_name = st[1]; + mesh.tex_coordinates.push_back (coordinates); + coordinates.clear (); + continue; + } + // Face + if (st[0] == "f") + { + //We only care for vertices indices + pcl::Vertices face_v; face_v.vertices.resize (st.size () - 1); + for (std::size_t i = 1; i < st.size (); ++i) + { + int v; + sscanf (st[i].c_str (), "%d", &v); + v = (v < 0) ? v_idx + v : v - 1; + face_v.vertices[i-1] = v; + } + mesh.tex_polygons.back ().push_back (face_v); + ++f_idx; + continue; + } + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::OBJReader::read] %s\n", exception); + fs.close (); + return (-1); + } + + double total_time = tt.toc (); + PCL_DEBUG ("[pcl::OBJReader::read] Loaded %s as a TextureMesh in %g ms with %g points, %g texture materials, %g polygons.\n", + file_name.c_str (), total_time, + v_idx -1, mesh.tex_materials.size (), f_idx -1); + fs.close (); + return (0); +} + +int +pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset) +{ + int file_version; + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + return (read (file_name, mesh, origin, orientation, file_version, offset)); +} + +int +pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset) +{ + pcl::console::TicToc tt; + tt.tic (); + + int data_type; + unsigned int data_idx; + if (readHeader (file_name, mesh.cloud, origin, orientation, file_version, data_type, data_idx, offset)) + { + PCL_ERROR ("[pcl::OBJReader::read] Problem reading header!\n"); + return (-1); + } + + std::ifstream fs; + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n", + file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + + // Seek at the given offset + fs.seekg (data_idx, std::ios::beg); + + // Get normal_x and rgba fields indices + int normal_x_field = -1; + // std::size_t rgba_field = 0; + for (std::size_t i = 0; i < mesh.cloud.fields.size (); ++i) + if (mesh.cloud.fields[i].name == "normal_x") + { + normal_x_field = i; + break; + } + + std::size_t v_idx = 0; + std::size_t vn_idx = 0; + std::string line; + std::vector st; + try + { + while (!fs.eof ()) + { + getline (fs, line); + // Ignore empty lines + if (line == "") + continue; + + // Tokenize the line + std::stringstream sstream (line); + sstream.imbue (std::locale::classic ()); + line = sstream.str (); + boost::trim (line); + boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on); + + // Ignore comments + if (st[0] == "#") + continue; + + // Vertex + if (st[0] == "v") + { + try + { + for (int i = 1, f = 0; i < 4; ++i, ++f) + { + float value = boost::lexical_cast (st[i]); + memcpy (&mesh.cloud.data[v_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset], + &value, + sizeof (float)); + } + ++v_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ()); + return (-1); + } + continue; + } + + // Vertex normal + if (st[0] == "vn") + { + try + { + for (int i = 1, f = normal_x_field; i < 4; ++i, ++f) + { + float value = boost::lexical_cast (st[i]); + memcpy (&mesh.cloud.data[vn_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset], + &value, + sizeof (float)); + } + ++vn_idx; + } + catch (const boost::bad_lexical_cast &e) + { + PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ()); + return (-1); + } + continue; + } + + // Face + if (st[0] == "f") + { + pcl::Vertices face_vertices; face_vertices.vertices.resize (st.size () - 1); + for (std::size_t i = 1; i < st.size (); ++i) + { + int v; + sscanf (st[i].c_str (), "%d", &v); + v = (v < 0) ? v_idx + v : v - 1; + face_vertices.vertices[i - 1] = v; + } + mesh.polygons.push_back (face_vertices); + continue; + } + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::OBJReader::read] %s\n", exception); + fs.close (); + return (-1); + } + + double total_time = tt.toc (); + PCL_DEBUG ("[pcl::OBJReader::read] Loaded %s as a PolygonMesh in %g ms with %g points and %g polygons.\n", + file_name.c_str (), total_time, + mesh.cloud.width * mesh.cloud.height, mesh.polygons.size ()); + fs.close (); + return (0); +} int pcl::io::saveOBJFile (const std::string &file_name, @@ -193,7 +1128,7 @@ pcl::io::saveOBJFile (const std::string &file_name, size_t j = 0; // There's one UV per vertex per face, i.e., the same vertex can have // different UV depending on the face. - for (j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) + for (j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) { uint32_t idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1; fs << " " << idx @@ -308,7 +1243,7 @@ pcl::io::saveOBJFile (const std::string &file_name, fs << "# "<< nr_points <<" vertices" << std::endl; if(normal_index != -1) - { + { fs << "# Normals in (x,y,z) form; normals might not be unit." << std::endl; // Write vertex normals for (int i = 0; i < nr_points; ++i) @@ -326,7 +1261,7 @@ pcl::io::saveOBJFile (const std::string &file_name, if (mesh.cloud.fields[d].name == "normal_x") // write vertices beginning with vn fs << "vn "; - + float value; memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float)); fs << value; @@ -353,7 +1288,7 @@ pcl::io::saveOBJFile (const std::string &file_name, for(unsigned i = 0; i < nr_faces; i++) { fs << "f "; - size_t j = 0; + size_t j = 0; for (; j < mesh.polygons[i].vertices.size () - 1; ++j) fs << mesh.polygons[i].vertices[j] + 1 << " "; fs << mesh.polygons[i].vertices[j] + 1 << std::endl; @@ -364,15 +1299,15 @@ pcl::io::saveOBJFile (const std::string &file_name, for(unsigned i = 0; i < nr_faces; i++) { fs << "f "; - size_t j = 0; + size_t j = 0; for (; j < mesh.polygons[i].vertices.size () - 1; ++j) - fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1; + fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << " "; fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << std::endl; } } fs << "# End of File" << std::endl; // Close obj file - fs.close (); + fs.close (); return 0; } diff --git a/io/src/openni2/openni2_convert.cpp b/io/src/openni2/openni2_convert.cpp new file mode 100644 index 00000000..39f1e712 --- /dev/null +++ b/io/src/openni2/openni2_convert.cpp @@ -0,0 +1,107 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_convert.h" +#include "pcl/io/io_exception.h" + +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + const OpenNI2DeviceInfo + openni2_convert (const openni::DeviceInfo* pInfo) + { + if (!pInfo) + THROW_IO_EXCEPTION ("openni2_convert called with zero pointer\n"); + + OpenNI2DeviceInfo output; + + output.name_ = pInfo->getName (); + output.uri_ = pInfo->getUri (); + output.vendor_ = pInfo->getVendor (); + output.product_id_ = pInfo->getUsbProductId (); + output.vendor_id_ = pInfo->getUsbVendorId (); + + return (output); + } + + const openni::VideoMode + grabberModeToOpenniMode (const OpenNI2VideoMode& input) + { + + openni::VideoMode output; + + output.setResolution (input.x_resolution_, input.y_resolution_); + output.setFps (input.frame_rate_); + output.setPixelFormat (static_cast(input.pixel_format_)); + + return (output); + } + + + const OpenNI2VideoMode + openniModeToGrabberMode (const openni::VideoMode& input) + { + OpenNI2VideoMode output; + + output.x_resolution_ = input.getResolutionX (); + output.y_resolution_ = input.getResolutionY (); + output.frame_rate_ = input.getFps (); + output.pixel_format_ = static_cast(input.getPixelFormat ()); + + return (output); + } + + const std::vector + openniModeToGrabberMode (const openni::Array& input) + { + std::vector output; + + int size = input.getSize (); + + output.reserve (size); + + for (int i=0; i +#include // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property + +#include +#include +#include +#include +#include + +#include "pcl/io/openni2/openni2_device.h" +#include "pcl/io/openni2/openni2_convert.h" +#include "pcl/io/openni2/openni2_frame_listener.h" + +#include "pcl/io/io_exception.h" + +#include + +using namespace openni; +using namespace pcl::io::openni2; + +using openni::VideoMode; +using std::vector; + +typedef boost::chrono::high_resolution_clock hr_clock; + +pcl::io::openni2::OpenNI2Device::OpenNI2Device (const std::string& device_URI) : + openni_device_(), + ir_video_started_(false), + color_video_started_(false), + depth_video_started_(false) +{ + openni::Status status = openni::OpenNI::initialize (); + if (status != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Initialize failed\n%s\n", OpenNI::getExtendedError ()); + + openni_device_ = boost::make_shared(); + + if (device_URI.length () > 0) + status = openni_device_->open (device_URI.c_str ()); + else + status = openni_device_->open (openni::ANY_DEVICE); + + if (status != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Initialize failed\n%s\n", openni::OpenNI::getExtendedError ()); + + // Get depth calculation parameters + // Some of these are device-spefic and may not exist + baseline_ = 0.0; + if ( getDepthVideoStream ()->isPropertySupported (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE) ) + { + double baseline; + getDepthVideoStream ()->getProperty (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline); // Device specific -- from PS1080.h + baseline_ = static_cast (baseline * 0.01f); // baseline from cm -> meters + } + shadow_value_ = 0; // This does not exist in OpenNI 2, and is maintained for compatibility with the OpenNI 1.x grabber + no_sample_value_ = 0; // This does not exist in OpenNI 2, and is maintained for compatibility with the OpenNI 1.x grabber + + // Set default resolution if not reading a file + if (!openni_device_->isFile ()) + { + setColorVideoMode (getDefaultColorMode ()); + setDepthVideoMode (getDefaultDepthMode ()); + setIRVideoMode (getDefaultIRMode ()); + } + + if (openni_device_->isFile ()) + { + openni_device_->getPlaybackControl ()->setSpeed (1.0f); + } + + device_info_ = boost::make_shared(); + *device_info_ = openni_device_->getDeviceInfo (); + + color_frame_listener = boost::make_shared(); + depth_frame_listener = boost::make_shared(); + ir_frame_listener = boost::make_shared(); +} + +pcl::io::openni2::OpenNI2Device::~OpenNI2Device () +{ + stopAllStreams (); + + shutdown (); + + openni_device_->close (); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getUri () const +{ + return (std::string (device_info_->getUri ())); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getVendor () const +{ + return (std::string (device_info_->getVendor ())); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getName () const +{ + return (std::string (device_info_->getName ())); +} + +uint16_t +pcl::io::openni2::OpenNI2Device::getUsbVendorId () const +{ + return (device_info_->getUsbVendorId ()); +} + +uint16_t +pcl::io::openni2::OpenNI2Device::getUsbProductId () const +{ + return (device_info_->getUsbProductId ()); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getStringID () const +{ + std::string ID_str = getName () + "_" + getVendor (); + + boost::replace_all (ID_str, "/", ""); + boost::replace_all (ID_str, ".", ""); + boost::replace_all (ID_str, "@", ""); + + return (ID_str); +} + +bool +pcl::io::openni2::OpenNI2Device::isValid () const +{ + return (openni_device_.get () != 0) && openni_device_->isValid (); +} + +float +pcl::io::openni2::OpenNI2Device::getIRFocalLength () const +{ + boost::shared_ptr stream = getIRVideoStream (); + + int frameWidth = stream->getVideoMode ().getResolutionX (); + float hFov = stream->getHorizontalFieldOfView (); + float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f)); + return (calculatedFocalLengthX); +} + +float +pcl::io::openni2::OpenNI2Device::getColorFocalLength () const +{ + boost::shared_ptr stream = getColorVideoStream (); + + int frameWidth = stream->getVideoMode ().getResolutionX (); + float hFov = stream->getHorizontalFieldOfView (); + float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f)); + return (calculatedFocalLengthX); +} + +float +pcl::io::openni2::OpenNI2Device::getDepthFocalLength () const +{ + boost::shared_ptr stream = getDepthVideoStream (); + + int frameWidth = stream->getVideoMode ().getResolutionX (); + float hFov = stream->getHorizontalFieldOfView (); + float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f)); + return (calculatedFocalLengthX); +} + +float +pcl::io::openni2::OpenNI2Device::getBaseline() +{ + return (baseline_); +} + +uint64_t +pcl::io::openni2::OpenNI2Device::getShadowValue() +{ + return (shadow_value_); +} + +bool +pcl::io::openni2::OpenNI2Device::isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const +{ + getSupportedIRVideoModes (); + + bool supported = false; + + std::vector::const_iterator it = ir_video_modes_.begin (); + std::vector::const_iterator it_end = ir_video_modes_.end (); + + while (it != it_end && !supported) + { + supported = (*it == video_mode); + ++it; + } + + return (supported); +} + +bool +pcl::io::openni2::OpenNI2Device::isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const +{ + getSupportedColorVideoModes (); + + bool supported = false; + + std::vector::const_iterator it = color_video_modes_.begin (); + std::vector::const_iterator it_end = color_video_modes_.end (); + + while (it != it_end && !supported) + { + supported = (*it == video_mode); + ++it; + } + + return (supported); +} + +bool +pcl::io::openni2::OpenNI2Device::isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const +{ + getSupportedDepthVideoModes (); + + bool supported = false; + + std::vector::const_iterator it = depth_video_modes_.begin (); + std::vector::const_iterator it_end = depth_video_modes_.end (); + + while (it != it_end && !supported) + { + supported = (*it == video_mode); + ++it; + } + + return (supported); +} + +bool +pcl::io::openni2::OpenNI2Device::hasIRSensor () const +{ + return (openni_device_->hasSensor (openni::SENSOR_IR)); +} + +bool +pcl::io::openni2::OpenNI2Device::hasColorSensor () const +{ + return (openni_device_->hasSensor (openni::SENSOR_COLOR)); +} + +bool +pcl::io::openni2::OpenNI2Device::hasDepthSensor () const +{ + return (openni_device_->hasSensor (openni::SENSOR_DEPTH)); +} + +void +pcl::io::openni2::OpenNI2Device::startIRStream () +{ + boost::shared_ptr stream = getIRVideoStream (); + + if (stream) + { + stream->setMirroringEnabled (false); + stream->addNewFrameListener (ir_frame_listener.get ()); + stream->start (); + ir_video_started_ = true; + } +} + +void +pcl::io::openni2::OpenNI2Device::startColorStream () +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + stream->setMirroringEnabled (false); + stream->addNewFrameListener (color_frame_listener.get ()); + stream->start (); + color_video_started_ = true; + } +} +void +pcl::io::openni2::OpenNI2Device::startDepthStream () +{ + boost::shared_ptr stream = getDepthVideoStream (); + + if (stream) + { + stream->setMirroringEnabled (false); + stream->addNewFrameListener (depth_frame_listener.get ()); + stream->start (); + depth_video_started_ = true; + } +} + +void +pcl::io::openni2::OpenNI2Device::stopAllStreams () +{ + stopIRStream (); + stopColorStream (); + stopDepthStream (); +} + +void +pcl::io::openni2::OpenNI2Device::stopIRStream () +{ + if (ir_video_stream_.get () != 0) + { + ir_video_stream_->stop (); + ir_video_started_ = false; + } +} +void +pcl::io::openni2::OpenNI2Device::stopColorStream () +{ + if (color_video_stream_.get () != 0) + { + color_video_stream_->stop (); + color_video_started_ = false; + } +} +void +pcl::io::openni2::OpenNI2Device::stopDepthStream () +{ + if (depth_video_stream_.get () != 0) + { + depth_video_stream_->stop (); + depth_video_started_ = false; + } +} + +void +pcl::io::openni2::OpenNI2Device::shutdown () +{ + if (ir_video_stream_.get () != 0) + ir_video_stream_->destroy (); + + if (color_video_stream_.get () != 0) + color_video_stream_->destroy (); + + if (depth_video_stream_.get () != 0) + depth_video_stream_->destroy (); + +} + +bool +pcl::io::openni2::OpenNI2Device::isIRStreamStarted () +{ + return (ir_video_started_); +} +bool +pcl::io::openni2::OpenNI2Device::isColorStreamStarted () +{ + return (color_video_started_); +} +bool +pcl::io::openni2::OpenNI2Device::isDepthStreamStarted () +{ + return (depth_video_started_); +} + +bool +pcl::io::openni2::OpenNI2Device::isImageRegistrationModeSupported () const +{ + return (openni_device_->isImageRegistrationModeSupported (openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR)); +} + +void +pcl::io::openni2::OpenNI2Device::setImageRegistrationMode (bool setEnable) +{ + bool registrationSupported = isImageRegistrationModeSupported (); + if (registrationSupported) + { + if (setEnable) + { + openni::Status rc = openni_device_->setImageRegistrationMode (openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError ()); + } + else + { + openni::Status rc = openni_device_->setImageRegistrationMode (openni::IMAGE_REGISTRATION_OFF); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Disabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } +} + +bool +pcl::io::openni2::OpenNI2Device::isDepthRegistered () const +{ + return openni_device_->getImageRegistrationMode () == openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR; +} + +void +pcl::io::openni2::OpenNI2Device::setSynchronization (bool enabled) +{ + openni::Status rc = openni_device_->setDepthColorSyncEnabled (enabled); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Enabling depth color synchronization failed: \n%s\n", openni::OpenNI::getExtendedError ()); +} + +const OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getIRVideoMode () +{ + OpenNI2VideoMode ret; + + boost::shared_ptr stream = getIRVideoStream (); + + if (stream) + { + openni::VideoMode video_mode = stream->getVideoMode (); + + ret = openniModeToGrabberMode (video_mode); + } + else + THROW_IO_EXCEPTION ("Could not create video stream."); + + return (ret); +} + +const OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getColorVideoMode () +{ + OpenNI2VideoMode ret; + + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::VideoMode video_mode = stream->getVideoMode (); + + ret = openniModeToGrabberMode (video_mode); + } + else + THROW_IO_EXCEPTION ("Could not create video stream."); + + return (ret); +} + +const OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDepthVideoMode () +{ + OpenNI2VideoMode ret; + + boost::shared_ptr stream = getDepthVideoStream (); + + if (stream) + { + openni::VideoMode video_mode = stream->getVideoMode (); + + ret = openniModeToGrabberMode (video_mode); + } + else + THROW_IO_EXCEPTION ("Could not create video stream."); + + return (ret); +} + +void +pcl::io::openni2::OpenNI2Device::setIRVideoMode (const OpenNI2VideoMode& video_mode) +{ + boost::shared_ptr stream = getIRVideoStream (); + + if (stream) + { + const openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode); + const openni::Status rc = stream->setVideoMode (videoMode); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set IR video mode: \n%s\n", openni::OpenNI::getExtendedError ()); + } +} + +void +pcl::io::openni2::OpenNI2Device::setColorVideoMode (const OpenNI2VideoMode& video_mode) +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode); + const openni::Status rc = stream->setVideoMode (videoMode); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set color video mode: \n%s\n", openni::OpenNI::getExtendedError ()); + } +} + +void +pcl::io::openni2::OpenNI2Device::setDepthVideoMode (const OpenNI2VideoMode& video_mode) +{ + boost::shared_ptr stream = getDepthVideoStream (); + + if (stream) + { + const openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode); + const openni::Status rc = stream->setVideoMode (videoMode); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set depth video mode: \n%s\n", openni::OpenNI::getExtendedError ()); + } +} + +OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDefaultIRMode () const +{ + // Search for and return VGA@30 Hz mode + vector modeList = getSupportedIRVideoModes (); + for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) + { + OpenNI2VideoMode mode = *modeItr; + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) ) + return mode; + } + return (modeList.at (0)); // Return first mode if we can't find VGA +} + +OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDefaultColorMode () const +{ + // Search for and return VGA@30 Hz mode + vector modeList = getSupportedColorVideoModes (); + for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) + { + OpenNI2VideoMode mode = *modeItr; + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) ) + return mode; + } + return (modeList.at (0)); // Return first mode if we can't find VGA +} + +OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDefaultDepthMode () const +{ + // Search for and return VGA@30 Hz mode + vector modeList = getSupportedDepthVideoModes (); + for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) + { + OpenNI2VideoMode mode = *modeItr; + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) ) + return mode; + } + return (modeList.at (0)); // Return first mode if we can't find VGA +} + +const std::vector& +pcl::io::openni2::OpenNI2Device::getSupportedIRVideoModes () const +{ + boost::shared_ptr stream = getIRVideoStream (); + ir_video_modes_.clear (); + + if (stream) + { + const openni::SensorInfo& sensor_info = stream->getSensorInfo (); + + ir_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ()); + } + + return (ir_video_modes_); +} + +const std::vector& +pcl::io::openni2::OpenNI2Device::getSupportedColorVideoModes () const +{ + boost::shared_ptr stream = getColorVideoStream (); + + color_video_modes_.clear (); + + if (stream) + { + const openni::SensorInfo& sensor_info = stream->getSensorInfo (); + + color_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ()); + } + + return (color_video_modes_); +} + +const std::vector& +pcl::io::openni2::OpenNI2Device::getSupportedDepthVideoModes () const +{ + boost::shared_ptr stream = getDepthVideoStream (); + + depth_video_modes_.clear (); + + if (stream) + { + const openni::SensorInfo& sensor_info = stream->getSensorInfo (); + + depth_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ()); + } + + return (depth_video_modes_); +} + +bool +pcl::io::openni2::OpenNI2Device::findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + if ( isIRVideoModeSupported (requested_mode) ) + { + actual_mode = requested_mode; + return (true); + } + else + { + // Find a resize-compatable mode + std::vector supportedModes = getSupportedIRVideoModes (); + bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode); + return (found); + } +} + +bool +pcl::io::openni2::OpenNI2Device::findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + if ( isColorVideoModeSupported (requested_mode) ) + { + actual_mode = requested_mode; + return (true); + } + else + { + // Find a resize-compatable mode + std::vector supportedModes = getSupportedColorVideoModes (); + bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode); + return (found); + } +} + +bool +pcl::io::openni2::OpenNI2Device::findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + if ( isDepthVideoModeSupported (requested_mode) ) + { + actual_mode = requested_mode; + return (true); + } + else + { + // Find a resize-compatable mode + std::vector supportedModes = getSupportedDepthVideoModes (); + bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode); + return (found); + } +} + +// Generic support method for the above findCompatable...Mode calls above +bool +pcl::io::openni2::OpenNI2Device::findCompatibleVideoMode (const std::vector supportedModes, const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + bool found = false; + for (std::vector::const_iterator modeIt = supportedModes.begin (); modeIt != supportedModes.end (); ++modeIt) + { + if (modeIt->frame_rate_ == requested_mode.frame_rate_ + && resizingSupported (modeIt->x_resolution_, modeIt->y_resolution_, requested_mode.x_resolution_, requested_mode.y_resolution_)) + { + if (found) + { // check wheter the new mode is better -> smaller than the current one. + if (actual_mode.x_resolution_ * actual_mode.x_resolution_ > modeIt->x_resolution_ * modeIt->y_resolution_ ) + actual_mode = *modeIt; + } + else + { + actual_mode = *modeIt; + found = true; + } + } + } + return (found); +} + +bool +pcl::io::openni2::OpenNI2Device::resizingSupported (size_t input_width, size_t input_height, size_t output_width, size_t output_height) const +{ + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); +} + +void +pcl::io::openni2::OpenNI2Device::setAutoExposure (bool enable) +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_seeting = stream->getCameraSettings (); + if (camera_seeting) + { + const openni::Status rc = camera_seeting->setAutoExposureEnabled (enable); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set auto exposure: \n%s\n", openni::OpenNI::getExtendedError ()); + } + + } +} + +void +pcl::io::openni2::OpenNI2Device::setAutoWhiteBalance (bool enable) +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_seeting = stream->getCameraSettings (); + if (camera_seeting) + { + const openni::Status rc = camera_seeting->setAutoWhiteBalanceEnabled (enable); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set auto white balance: \n%s\n", openni::OpenNI::getExtendedError ()); + } + + } +} + +bool +pcl::io::openni2::OpenNI2Device::getAutoExposure () const +{ + bool ret = false; + + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_seeting = stream->getCameraSettings (); + if (camera_seeting) + ret = camera_seeting->getAutoExposureEnabled (); + } + + return (ret); +} + +bool +pcl::io::openni2::OpenNI2Device::getAutoWhiteBalance () const +{ + bool ret = false; + + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_setting = stream->getCameraSettings (); + if (camera_setting) + ret = camera_setting->getAutoWhiteBalanceEnabled (); + } + + return (ret); +} + +int pcl::io::openni2::OpenNI2Device::getDepthFrameCount () +{ + if (!openni_device_->isFile () || !getDepthVideoStream ()) + { + return 0; + } + return openni_device_->getPlaybackControl ()->getNumberOfFrames(*getDepthVideoStream ()); +} + +int OpenNI2Device::getColorFrameCount () +{ + if (!openni_device_->isFile () || !getColorVideoStream ()) + { + return 0; + } + return openni_device_->getPlaybackControl ()->getNumberOfFrames (*getColorVideoStream ()); +} + +int OpenNI2Device::getIRFrameCount () +{ + if (!openni_device_->isFile () || !getIRVideoStream ()) + { + return 0; + } + return openni_device_->getPlaybackControl ()->getNumberOfFrames (*getIRVideoStream ()); +} + +bool OpenNI2Device::setPlaybackSpeed (double speed) +{ + return openni_device_->getPlaybackControl ()->setSpeed (speed) == openni::STATUS_OK; +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2Device::getIRVideoStream () const +{ + if (ir_video_stream_.get () == 0) + { + if (hasIRSensor ()) + { + ir_video_stream_ = boost::make_shared(); + + const openni::Status rc = ir_video_stream_->create (*openni_device_, openni::SENSOR_IR); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't create IR video stream: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } + return (ir_video_stream_); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2Device::getColorVideoStream () const +{ + if (color_video_stream_.get () == 0) + { + if (hasColorSensor ()) + { + color_video_stream_ = boost::make_shared(); + + const openni::Status rc = color_video_stream_->create (*openni_device_, openni::SENSOR_COLOR); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't create color video stream: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } + return (color_video_stream_); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2Device::getDepthVideoStream () const +{ + if (depth_video_stream_.get () == 0) + { + if (hasDepthSensor ()) + { + depth_video_stream_ = boost::make_shared(); + + const openni::Status rc = depth_video_stream_->create (*openni_device_, openni::SENSOR_DEPTH); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't create depth video stream: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } + return (depth_video_stream_); +} + +std::ostream& pcl::io::openni2::operator<< (std::ostream& stream, const OpenNI2Device& device) +{ + + stream << "Device info (" << device.getUri () << ")" << std::endl; + stream << " Vendor: " << device.getVendor () << std::endl; + stream << " Name: " << device.getName () << std::endl; + stream << " USB Vendor ID: " << device.getUsbVendorId () << std::endl; + stream << " USB Product ID: " << device.getUsbVendorId () << std::endl << std::endl; + + if (device.hasIRSensor ()) + { + stream << "IR sensor video modes:" << std::endl; + const std::vector& video_modes = device.getSupportedIRVideoModes (); + + std::vector::const_iterator it = video_modes.begin (); + std::vector::const_iterator it_end = video_modes.end (); + for (; it != it_end; ++it) + stream << " - " << *it << std::endl; + } + else + { + stream << "No IR sensor available" << std::endl; + } + + if (device.hasColorSensor ()) + { + stream << "Color sensor video modes:" << std::endl; + const std::vector& video_modes = device.getSupportedColorVideoModes (); + + std::vector::const_iterator it = video_modes.begin (); + std::vector::const_iterator it_end = video_modes.end (); + for (; it != it_end; ++it) + stream << " - " << *it << std::endl; + } + else + { + stream << "No Color sensor available" << std::endl; + } + + if (device.hasDepthSensor ()) + { + stream << "Depth sensor video modes:" << std::endl; + const std::vector& video_modes = device.getSupportedDepthVideoModes (); + + std::vector::const_iterator it = video_modes.begin (); + std::vector::const_iterator it_end = video_modes.end (); + for (; it != it_end; ++it) + stream << " - " << *it << std::endl; + } + else + { + stream << "No Depth sensor available" << std::endl; + } + + return (stream); + +} + +void +pcl::io::openni2::OpenNI2Device::setColorCallback (StreamCallbackFunction color_callback) +{ + color_frame_listener->setCallback (color_callback); +} + +void +pcl::io::openni2::OpenNI2Device::setDepthCallback (StreamCallbackFunction depth_callback) +{ + depth_frame_listener->setCallback (depth_callback); +} + +void +pcl::io::openni2::OpenNI2Device::setIRCallback (StreamCallbackFunction ir_callback) +{ + ir_frame_listener->setCallback (ir_callback); +} diff --git a/io/src/openni2/openni2_device_info.cpp b/io/src/openni2/openni2_device_info.cpp new file mode 100644 index 00000000..d3f9fadc --- /dev/null +++ b/io/src/openni2/openni2_device_info.cpp @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include +#include "pcl/io/openni2/openni2_device_info.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceInfo& device_info) + { + stream << "Uri: " << device_info.uri_ << " (Vendor: " << device_info.vendor_ << + ", Name: " << device_info.name_ << + ", Vendor ID: " << device_info.vendor_id_ << + ", Product ID: " << device_info.product_id_ << + ")" << std::endl; + return stream; + } + + } //namespace + } +} diff --git a/io/src/openni2/openni2_device_manager.cpp b/io/src/openni2/openni2_device_manager.cpp new file mode 100644 index 00000000..4177a1a0 --- /dev/null +++ b/io/src/openni2/openni2_device_manager.cpp @@ -0,0 +1,266 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_device_manager.h" +#include "pcl/io/openni2/openni2_convert.h" +#include "pcl/io/openni2/openni2_device.h" +#include "pcl/io/io_exception.h" + +#include + +#include +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2DeviceInfoComparator + { + public: + bool operator ()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2) + { + return (di1.uri_.compare (di2.uri_) < 0); + } + }; + + typedef std::set DeviceSet; + + + + class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener, + public openni::OpenNI::DeviceDisconnectedListener, + public openni::OpenNI::DeviceStateChangedListener + { + public: + OpenNI2DeviceListener () + : openni::OpenNI::DeviceConnectedListener () + , openni::OpenNI::DeviceDisconnectedListener () + , openni::OpenNI::DeviceStateChangedListener () + { + openni::OpenNI::addDeviceConnectedListener (this); + openni::OpenNI::addDeviceDisconnectedListener (this); + openni::OpenNI::addDeviceStateChangedListener (this); + + // get list of currently connected devices + openni::Array device_info_list; + openni::OpenNI::enumerateDevices (&device_info_list); + + for (int i = 0; i < device_info_list.getSize (); ++i) + { + onDeviceConnected (&device_info_list[i]); + } + } + + ~OpenNI2DeviceListener () + { + openni::OpenNI::removeDeviceConnectedListener (this); + openni::OpenNI::removeDeviceDisconnectedListener (this); + openni::OpenNI::removeDeviceStateChangedListener (this); + } + + virtual void + onDeviceStateChanged (const openni::DeviceInfo* pInfo, openni::DeviceState state) + { + switch (state) + { + case openni::DEVICE_STATE_OK: + onDeviceConnected (pInfo); + break; + case openni::DEVICE_STATE_ERROR: + case openni::DEVICE_STATE_NOT_READY: + case openni::DEVICE_STATE_EOF: + default: + onDeviceDisconnected (pInfo); + break; + } + } + + virtual void + onDeviceConnected (const openni::DeviceInfo* pInfo) + { + boost::mutex::scoped_lock l (device_mutex_); + + const OpenNI2DeviceInfo device_info_wrapped = openni2_convert (pInfo); + + // make sure it does not exist in set before inserting + device_set_.erase (device_info_wrapped); + device_set_.insert (device_info_wrapped); + } + + virtual void + onDeviceDisconnected (const openni::DeviceInfo* pInfo) + { + boost::mutex::scoped_lock l (device_mutex_); + + const OpenNI2DeviceInfo device_info_wrapped = openni2_convert (pInfo); + device_set_.erase (device_info_wrapped); + } + + boost::shared_ptr > + getConnectedDeviceURIs () + { + boost::mutex::scoped_lock l (device_mutex_); + + boost::shared_ptr > result = boost::make_shared >(); + + result->reserve (device_set_.size ()); + + std::set::const_iterator it; + std::set::const_iterator it_end = device_set_.end (); + + for (it = device_set_.begin (); it != it_end; ++it) + result->push_back (it->uri_); + + return result; + } + + boost::shared_ptr > + getConnectedDeviceInfos () + { + boost::mutex::scoped_lock l (device_mutex_); + + boost::shared_ptr > result = boost::make_shared >(); + + result->reserve (device_set_.size ()); + + DeviceSet::const_iterator it; + DeviceSet::const_iterator it_end = device_set_.end (); + + for (it = device_set_.begin (); it != it_end; ++it) + result->push_back (*it); + + return result; + } + + std::size_t + getNumOfConnectedDevices () + { + boost::mutex::scoped_lock l (device_mutex_); + + return device_set_.size (); + } + + boost::mutex device_mutex_; + DeviceSet device_set_; + }; + + } //namespace + } +} + + +////////////////////////////////////////////////////////////////////////// +using pcl::io::openni2::OpenNI2Device; +using pcl::io::openni2::OpenNI2DeviceInfo; +using pcl::io::openni2::OpenNI2DeviceManager; + +pcl::io::openni2::OpenNI2DeviceManager::OpenNI2DeviceManager () +{ + openni::Status rc = openni::OpenNI::initialize (); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Initialize failed\n%s\n", openni::OpenNI::getExtendedError ()); + + device_listener_ = boost::make_shared(); +} + +pcl::io::openni2::OpenNI2DeviceManager::~OpenNI2DeviceManager () +{ +} + +boost::shared_ptr > +pcl::io::openni2::OpenNI2DeviceManager::getConnectedDeviceInfos () const +{ + return device_listener_->getConnectedDeviceInfos (); +} + +boost::shared_ptr > +pcl::io::openni2::OpenNI2DeviceManager::getConnectedDeviceURIs () const +{ + return device_listener_->getConnectedDeviceURIs (); +} + +std::size_t +pcl::io::openni2::OpenNI2DeviceManager::getNumOfConnectedDevices () const +{ + return device_listener_->getNumOfConnectedDevices (); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getAnyDevice () +{ + return boost::make_shared(""); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getDevice (const std::string& device_URI) +{ + return boost::make_shared(device_URI); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getDeviceByIndex (int index) +{ + boost::shared_ptr > URIs = getConnectedDeviceURIs (); + return boost::make_shared( URIs->at (index) ); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getFileDevice (const std::string& path) +{ + return boost::make_shared(path); +} + +std::ostream& +operator<< (std::ostream& stream, const OpenNI2DeviceManager& device_manager) +{ + + boost::shared_ptr > device_info = device_manager.getConnectedDeviceInfos (); + + std::vector::const_iterator it; + std::vector::const_iterator it_end = device_info->end (); + + for (it = device_info->begin (); it != it_end; ++it) + { + stream << "Uri: " << it->uri_ << " (Vendor: " << it->vendor_ << + ", Name: " << it->name_ << + ", Vendor ID: " << it->vendor_id_ << + ", Product ID: " << it->product_id_ << + ")" << std::endl; + } + + return (stream); +} diff --git a/io/src/openni2/openni2_timer_filter.cpp b/io/src/openni2/openni2_timer_filter.cpp new file mode 100644 index 00000000..43fbe6c8 --- /dev/null +++ b/io/src/openni2/openni2_timer_filter.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_timer_filter.h" +#include + + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + OpenNI2TimerFilter::OpenNI2TimerFilter (std::size_t filter_len): + filter_len_(filter_len) + {} + + OpenNI2TimerFilter::~OpenNI2TimerFilter () + {} + + void OpenNI2TimerFilter::addSample (double sample) + { + buffer_.push_back (sample); + if (buffer_.size ()>filter_len_) + buffer_.pop_front (); + } + + double OpenNI2TimerFilter::getMedian () + { + if (buffer_.size ()>0) + { + std::deque sort_buffer = buffer_; + + std::sort (sort_buffer.begin (), sort_buffer.end ()); + + return sort_buffer[sort_buffer.size ()/2]; + } else + return (0.0); + } + + double + OpenNI2TimerFilter::getMovingAvg () + { + if (buffer_.size () > 0) + { + double sum = 0; + + std::deque::const_iterator it = buffer_.begin (); + std::deque::const_iterator it_end = buffer_.end (); + + while (it != it_end) + { + sum += *(it++); + } + + return sum / static_cast(buffer_.size ()); + } else + return (0.0); + } + + + void OpenNI2TimerFilter::clear () + { + buffer_.clear (); + } + + } //namespace + } +} diff --git a/io/src/openni2/openni2_video_mode.cpp b/io/src/openni2/openni2_video_mode.cpp new file mode 100644 index 00000000..145da5d3 --- /dev/null +++ b/io/src/openni2/openni2_video_mode.cpp @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_video_mode.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2VideoMode& video_mode) + { + stream << "Resolution: " << (int)video_mode.x_resolution_ << "x" << (int)video_mode.y_resolution_ << + "@" << video_mode.frame_rate_ << + "Hz Format: "; + + switch (video_mode.pixel_format_) + { + case PIXEL_FORMAT_DEPTH_1_MM: + stream << "Depth 1mm"; + break; + case PIXEL_FORMAT_DEPTH_100_UM: + stream << "Depth 100um"; + break; + case PIXEL_FORMAT_SHIFT_9_2: + stream << "Shift 9/2"; + break; + case PIXEL_FORMAT_SHIFT_9_3: + stream << "Shift 9/3"; + break; + case PIXEL_FORMAT_RGB888: + stream << "RGB888"; + break; + case PIXEL_FORMAT_YUV422: + stream << "YUV422"; + break; + case PIXEL_FORMAT_GRAY8: + stream << "Gray8"; + break; + case PIXEL_FORMAT_GRAY16: + stream << "Gray16"; + break; + case PIXEL_FORMAT_JPEG: + stream << "JPEG"; + break; + + default: + break; + } + + return (stream); + } + + bool + operator==(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b) + { + return (video_mode_a.x_resolution_==video_mode_b.x_resolution_) && + (video_mode_a.y_resolution_==video_mode_b.y_resolution_) && + (video_mode_a.frame_rate_ ==video_mode_b.frame_rate_) && + (video_mode_a.pixel_format_==video_mode_b.pixel_format_); + } + + bool + operator!=(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b) + { + return !(video_mode_a==video_mode_b); + } + + } //namespace + } +} diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp new file mode 100644 index 00000000..9fbdaec0 --- /dev/null +++ b/io/src/openni2_grabber.cpp @@ -0,0 +1,938 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#ifdef HAVE_OPENNI2 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace pcl::io::openni2; + +namespace pcl +{ + // Treat color as chars, float32, or uint32 + typedef union + { + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + float float_value; + uint32_t long_value; + } RGBValue; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::io::OpenNI2Grabber::OpenNI2Grabber (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode) + : color_resize_buffer_(0) + , depth_resize_buffer_(0) + , ir_resize_buffer_(0) + , rgb_sync_ () + , ir_sync_ () + , device_ () + , rgb_frame_id_ () + , depth_frame_id_ () + , image_width_ () + , image_height_ () + , depth_width_ () + , depth_height_ () + , image_required_ (false) + , depth_required_ (false) + , ir_required_ (false) + , sync_required_ (false) + , image_signal_ (), depth_image_signal_ (), ir_image_signal_ (), image_depth_image_signal_ () + , ir_depth_image_signal_ (), point_cloud_signal_ (), point_cloud_i_signal_ () + , point_cloud_rgb_signal_ (), point_cloud_rgba_signal_ () + , config2oni_map_ (), depth_callback_handle_ (), image_callback_handle_ (), ir_callback_handle_ () + , running_ (false) + , rgb_parameters_(std::numeric_limits::quiet_NaN () ) + , depth_parameters_(std::numeric_limits::quiet_NaN () ) +{ + // initialize driver + updateModeMaps (); // registering mapping from PCL enum modes to openni::VideoMode and vice versa + setupDevice (device_id, depth_mode, image_mode); + + rgb_frame_id_ = "/openni2_rgb_optical_frame"; + depth_frame_id_ = "/openni2_depth_optical_frame"; + + + if (!device_->hasDepthSensor () ) + PCL_THROW_EXCEPTION (pcl::IOException, "Device does not provide 3D information."); + + depth_image_signal_ = createSignal (); + ir_image_signal_ = createSignal (); + point_cloud_signal_ = createSignal (); + point_cloud_i_signal_ = createSignal (); + ir_depth_image_signal_ = createSignal (); + ir_sync_.addCallback (boost::bind (&OpenNI2Grabber::irDepthImageCallback, this, _1, _2)); + + if (device_->hasColorSensor ()) + { + // create callback signals + image_signal_ = createSignal (); + image_depth_image_signal_ = createSignal (); + point_cloud_rgb_signal_ = createSignal (); + point_cloud_rgba_signal_ = createSignal (); + rgb_sync_.addCallback (boost::bind (&OpenNI2Grabber::imageDepthImageCallback, this, _1, _2)); + } + + // callbacks from the sensor to the grabber + device_->setColorCallback (boost::bind (&OpenNI2Grabber::processColorFrame, this, _1)); + device_->setDepthCallback (boost::bind (&OpenNI2Grabber::processDepthFrame, this, _1)); + device_->setIRCallback (boost::bind (&OpenNI2Grabber::processIRFrame, this, _1)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::io::OpenNI2Grabber::~OpenNI2Grabber () throw () +{ + try + { + stop (); + + // release the pointer to the device object + device_.reset (); + + // disconnect all listeners + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + } + catch (...) + { + // destructor never throws + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkImageAndDepthSynchronizationRequired () +{ + // do we have anyone listening to images or color point clouds? + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + sync_required_ = true; + else + sync_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkImageStreamRequired () +{ + // do we have anyone listening to images or color point clouds? + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + image_required_ = true; + else + image_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkDepthStreamRequired () +{ + // do we have anyone listening to depth images or (color) point clouds? + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 ) + depth_required_ = true; + else + depth_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkIRStreamRequired () +{ + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + ir_required_ = true; + else + ir_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::start () +{ + try + { + // check if we need to start/stop any stream + if (image_required_ && !device_->isColorStreamStarted () ) + { + block_signals (); + device_->startColorStream (); + startSynchronization (); + } + + if (depth_required_ && !device_->isDepthStreamStarted ()) + { + block_signals (); + if (device_->hasColorSensor () && device_->isImageRegistrationModeSupported () ) + { + device_->setImageRegistrationMode (true); + } + device_->startDepthStream (); + startSynchronization (); + } + + if (ir_required_ && !device_->isIRStreamStarted () ) + { + block_signals (); + device_->startIRStream (); + } + running_ = true; + } + catch (IOException& ex) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start streams. Reason: " << ex.what ()); + } + + // workaround, since the first frame is corrupted + //boost::this_thread::sleep (boost::posix_time::seconds (1)); + unblock_signals (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::stop () +{ + try + { + if (device_->hasDepthSensor () && device_->isDepthStreamStarted () ) + device_->stopDepthStream (); + + if (device_->hasColorSensor () && device_->isColorStreamStarted () ) + device_->stopColorStream (); + + if (device_->hasIRSensor () && device_->isIRStreamStarted ()) + device_->stopIRStream (); + + running_ = false; + } + catch (IOException& ex) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not stop streams. Reason: " << ex.what ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::io::OpenNI2Grabber::isRunning () const +{ + return (running_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::signalsChanged () +{ + // reevaluate which streams are required + checkImageStreamRequired (); + checkDepthStreamRequired (); + checkIRStreamRequired (); + if (ir_required_ && image_required_) + PCL_THROW_EXCEPTION (pcl::IOException, "Can not provide IR stream and RGB stream at the same time."); + + checkImageAndDepthSynchronizationRequired (); + if (running_) + start (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +std::string +pcl::io::OpenNI2Grabber::getName () const +{ + return (std::string ("OpenNI2Grabber")); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode) +{ + // Initialize the openni device + boost::shared_ptr deviceManager = OpenNI2DeviceManager::getInstance (); + + try + { + if (boost::filesystem::exists (device_id)) + { + device_ = deviceManager->getFileDevice (device_id); // Treat as file path + } + else if (deviceManager->getNumOfConnectedDevices () == 0) + { + PCL_THROW_EXCEPTION (pcl::IOException, "No devices connected."); + } + else if (device_id[0] == '#') + { + unsigned index = atoi (device_id.c_str () + 1); + device_ = deviceManager->getDeviceByIndex (index - 1); + } + else + { + device_ = deviceManager->getAnyDevice (); + } + } + catch (const IOException& exception) + { + if (!device_) + PCL_THROW_EXCEPTION (pcl::IOException, "No matching device found. " << exception.what ()) + else + PCL_THROW_EXCEPTION (pcl::IOException, "could not retrieve device. Reason " << exception.what ()) + } + catch (const pcl::IOException&) + { + throw; + } + catch (...) + { + PCL_THROW_EXCEPTION (pcl::IOException, "unknown error occured"); + } + + typedef pcl::io::openni2::OpenNI2VideoMode VideoMode; + + VideoMode depth_md; + // Set the selected output mode + if (depth_mode != OpenNI_Default_Mode) + { + VideoMode actual_depth_md; + if (!mapMode2XnMode (depth_mode, depth_md) || !device_->findCompatibleDepthMode (depth_md, actual_depth_md)) + PCL_THROW_EXCEPTION (pcl::IOException, "could not find compatible depth stream mode " << static_cast (depth_mode) ); + + VideoMode current_depth_md = device_->getDepthVideoMode (); + if (current_depth_md.x_resolution_ != actual_depth_md.x_resolution_ || current_depth_md.y_resolution_ != actual_depth_md.y_resolution_) + device_->setDepthVideoMode (actual_depth_md); + } + else + { + depth_md = device_->getDefaultDepthMode (); + } + + depth_width_ = depth_md.x_resolution_; + depth_height_ = depth_md.y_resolution_; + + if (device_->hasColorSensor ()) + { + VideoMode image_md; + if (image_mode != OpenNI_Default_Mode) + { + VideoMode actual_image_md; + if (!mapMode2XnMode (image_mode, image_md) || !device_->findCompatibleColorMode (image_md, actual_image_md)) + PCL_THROW_EXCEPTION (pcl::IOException, "could not find compatible image stream mode " << static_cast (image_mode) ); + + VideoMode current_image_md = device_->getColorVideoMode (); + if (current_image_md.x_resolution_ != actual_image_md.x_resolution_ || current_image_md.y_resolution_ != actual_image_md.y_resolution_) + device_->setColorVideoMode (actual_image_md); + } + else + { + image_md = device_->getDefaultColorMode (); + } + + image_width_ = image_md.x_resolution_; + image_height_ = image_md.y_resolution_; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::startSynchronization () +{ + try + { + if (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () && + device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_) + device_->setSynchronization (true); + } + catch (const IOException& exception) + { + std::cerr << exception.what() << std::endl; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::stopSynchronization () +{ + try + { + if (device_->isSynchronizationSupported () && device_->isSynchronized ()) + device_->setSynchronization (false); + } + catch (const IOException& exception) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start synchronization " << exception.what ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::imageCallback (Image::Ptr image, void*) +{ + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + rgb_sync_.add0 (image, image->getTimestamp ()); + + int numImageSlots = image_signal_->num_slots (); + if (numImageSlots > 0) + image_signal_->operator ()(image); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::depthCallback (DepthImage::Ptr depth_image, void*) +{ + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + rgb_sync_.add1 (depth_image, depth_image->getTimestamp ()); + + if (num_slots () > 0 || + num_slots () > 0) + ir_sync_.add1 (depth_image, depth_image->getTimestamp ()); + + if (depth_image_signal_->num_slots () > 0) + depth_image_signal_->operator ()(depth_image); + + if (point_cloud_signal_->num_slots () > 0) + point_cloud_signal_->operator ()(convertToXYZPointCloud (depth_image)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::irCallback (IRImage::Ptr ir_image, void*) +{ + if (num_slots () > 0 || + num_slots () > 0) + ir_sync_.add0(ir_image, ir_image->getTimestamp ()); + + if (ir_image_signal_->num_slots () > 0) + ir_image_signal_->operator ()(ir_image); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::imageDepthImageCallback (const Image::Ptr &image, + const DepthImage::Ptr &depth_image) +{ + // check if we have color point cloud slots + if (point_cloud_rgb_signal_->num_slots () > 0) + { + PCL_WARN ("PointXYZRGB callbacks deprecated. Use PointXYZRGBA instead.\n"); + point_cloud_rgb_signal_->operator ()(convertToXYZRGBPointCloud (image, depth_image)); + } + + if (point_cloud_rgba_signal_->num_slots () > 0) + point_cloud_rgba_signal_->operator ()(convertToXYZRGBPointCloud (image, depth_image)); + + if (image_depth_image_signal_->num_slots () > 0) + { + float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (); + image_depth_image_signal_->operator ()(image, depth_image, reciprocalFocalLength); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::irDepthImageCallback (const IRImage::Ptr &ir_image, + const DepthImage::Ptr &depth_image) +{ + // check if we have color point cloud slots + if (point_cloud_i_signal_->num_slots () > 0) + point_cloud_i_signal_->operator ()(convertToXYZIPointCloud (ir_image, depth_image)); + + if (ir_depth_image_signal_->num_slots () > 0) + { + float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (); + ir_depth_image_signal_->operator ()(ir_image, depth_image, reciprocalFocalLength); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::PointCloud::Ptr +pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_image) +{ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud ); + + cloud->header.seq = depth_image->getFrameID (); + cloud->header.stamp = depth_image->getTimestamp (); + cloud->height = depth_height_; + cloud->width = depth_width_; + cloud->is_dense = false; + + cloud->points.resize (cloud->height * cloud->width); + + float constant_x = 1.0f / device_->getDepthFocalLength (); + float constant_y = 1.0f / device_->getDepthFocalLength (); + float centerX = ((float)cloud->width - 1.f) / 2.f; + float centerY = ((float)cloud->height - 1.f) / 2.f; + + if (pcl_isfinite (depth_parameters_.focal_length_x)) + constant_x = 1.0f / static_cast (depth_parameters_.focal_length_x); + + if (pcl_isfinite (depth_parameters_.focal_length_y)) + constant_y = 1.0f / static_cast (depth_parameters_.focal_length_y); + + if (pcl_isfinite (depth_parameters_.principal_point_x)) + centerX = static_cast (depth_parameters_.principal_point_x); + + if (pcl_isfinite (depth_parameters_.principal_point_y)) + centerY = static_cast (depth_parameters_.principal_point_y); + + if ( device_->isDepthRegistered() ) + cloud->header.frame_id = rgb_frame_id_; + else + cloud->header.frame_id = depth_frame_id_; + + + float bad_point = std::numeric_limits::quiet_NaN (); + + const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); + if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) + { + // Resize the image if nessacery + depth_resize_buffer_.resize(depth_width_ * depth_height_); + + depth_image->fillDepthImageRaw (depth_width_, depth_height_, (uint16_t*) depth_resize_buffer_.data() ); + depth_map = depth_resize_buffer_.data(); + } + + int depth_idx = 0; + for (int v = 0; v < depth_height_; ++v) + { + for (int u = 0; u < depth_width_; ++u, ++depth_idx) + { + pcl::PointXYZ& pt = cloud->points[depth_idx]; + // Check for invalid measurements + if (depth_map[depth_idx] == 0 || + depth_map[depth_idx] == depth_image->getNoSampleValue () || + depth_map[depth_idx] == depth_image->getShadowValue ()) + { + // not valid + pt.x = pt.y = pt.z = bad_point; + continue; + } + pt.z = depth_map[depth_idx] * 0.001f; + pt.x = (static_cast (u) - centerX) * pt.z * constant_x; + pt.y = (static_cast (v) - centerY) * pt.z * constant_y; + } + } + cloud->sensor_origin_.setZero (); + cloud->sensor_orientation_.w () = 1.0f; + cloud->sensor_orientation_.x () = 0.0f; + cloud->sensor_orientation_.y () = 0.0f; + cloud->sensor_orientation_.z () = 0.0f; + return (cloud); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::PointCloud::Ptr +pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, const DepthImage::Ptr &depth_image) +{ + boost::shared_ptr > cloud (new pcl::PointCloud); + + cloud->header.seq = depth_image->getFrameID (); + cloud->header.stamp = depth_image->getTimestamp (); + cloud->header.frame_id = rgb_frame_id_; + cloud->height = std::max (image_height_, depth_height_); + cloud->width = std::max (image_width_, depth_width_); + cloud->is_dense = false; + + cloud->points.resize (cloud->height * cloud->width); + + // Generate default camera parameters + float fx = device_->getDepthFocalLength (); // Horizontal focal length + float fy = device_->getDepthFocalLength (); // Vertcal focal length + float cx = ((float)depth_width_ - 1.f) / 2.f; // Center x + float cy = ((float)depth_height_- 1.f) / 2.f; // Center y + + // Load pre-calibrated camera parameters if they exist + if (pcl_isfinite (depth_parameters_.focal_length_x)) + fx = 1.0f / static_cast (depth_parameters_.focal_length_x); + + if (pcl_isfinite (depth_parameters_.focal_length_y)) + fy = 1.0f / static_cast (depth_parameters_.focal_length_y); + + if (pcl_isfinite (depth_parameters_.principal_point_x)) + cx = static_cast (depth_parameters_.principal_point_x); + + if (pcl_isfinite (depth_parameters_.principal_point_y)) + cy = static_cast (depth_parameters_.principal_point_y); + + // Get inverse focal length for calculations below + float fx_inv = 1.0f / fx; + float fy_inv = 1.0f / fy; + + const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); + if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) + { + // Resize the image if nessacery + depth_resize_buffer_.resize(depth_width_ * depth_height_); + depth_map = depth_resize_buffer_.data(); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map ); + } + + const uint8_t* rgb_buffer = (const uint8_t*) image->getData (); + if (image->getWidth () != image_width_ || image->getHeight () != image_height_) + { + // Resize the image if nessacery + color_resize_buffer_.resize(image_width_ * image_height_ * 3); + rgb_buffer = color_resize_buffer_.data(); + image->fillRGB (image_width_, image_height_, (unsigned char*) rgb_buffer, image_width_ * 3); + } + + + float bad_point = std::numeric_limits::quiet_NaN (); + + // set xyz to Nan and rgb to 0 (black) + if (image_width_ != depth_width_) + { + PointT pt; + pt.x = pt.y = pt.z = bad_point; + pt.b = pt.g = pt.r = 0; + pt.a = 255; // point has no color info -> alpha = max => transparent + cloud->points.assign (cloud->points.size (), pt); + } + + // fill in XYZ values + unsigned step = cloud->width / depth_width_; + unsigned skip = cloud->width * step - cloud->width; + + int value_idx = 0; + int point_idx = 0; + for (int v = 0; v < depth_height_; ++v, point_idx += skip) + { + for (int u = 0; u < depth_width_; ++u, ++value_idx, point_idx += step) + { + PointT& pt = cloud->points[point_idx]; + /// @todo Different values for these cases + // Check for invalid measurements + + OniDepthPixel pixel = depth_map[value_idx]; + if (pixel != 0 && + pixel != depth_image->getNoSampleValue () && + pixel != depth_image->getShadowValue () ) + { + pt.z = depth_map[value_idx] * 0.001f; // millimeters to meters + pt.x = (static_cast (u) - cx) * pt.z * fx_inv; + pt.y = (static_cast (v) - cy) * pt.z * fy_inv; + } + else + { + pt.x = pt.y = pt.z = bad_point; + } + } + } + + // fill in the RGB values + step = cloud->width / image_width_; + skip = cloud->width * step - cloud->width; + + value_idx = 0; + point_idx = 0; + RGBValue color; + color.Alpha = 0; + + for (unsigned yIdx = 0; yIdx < image_height_; ++yIdx, point_idx += skip) + { + for (unsigned xIdx = 0; xIdx < image_width_; ++xIdx, point_idx += step, value_idx += 3) + { + PointT& pt = cloud->points[point_idx]; + + color.Red = rgb_buffer[value_idx]; + color.Green = rgb_buffer[value_idx + 1]; + color.Blue = rgb_buffer[value_idx + 2]; + + pt.rgba = color.long_value; + } + } + cloud->sensor_origin_.setZero (); + cloud->sensor_orientation_.setIdentity (); + return (cloud); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::PointCloud::Ptr +pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, const DepthImage::Ptr &depth_image) +{ + boost::shared_ptr > cloud (new pcl::PointCloud ()); + + cloud->header.seq = depth_image->getFrameID (); + cloud->header.stamp = depth_image->getTimestamp (); + cloud->header.frame_id = rgb_frame_id_; + cloud->height = depth_height_; + cloud->width = depth_width_; + cloud->is_dense = false; + + cloud->points.resize (cloud->height * cloud->width); + + + float fx = device_->getDepthFocalLength (); // Horizontal focal length + float fy = device_->getDepthFocalLength (); // Vertcal focal length + float cx = ((float)cloud->width - 1.f) / 2.f; // Center x + float cy = ((float)cloud->height - 1.f) / 2.f; // Center y + + // Load pre-calibrated camera parameters if they exist + if (pcl_isfinite (depth_parameters_.focal_length_x)) + fx = static_cast (depth_parameters_.focal_length_x); + + if (pcl_isfinite (depth_parameters_.focal_length_y)) + fy = static_cast (depth_parameters_.focal_length_y); + + if (pcl_isfinite (depth_parameters_.principal_point_x)) + cx = static_cast (depth_parameters_.principal_point_x); + + if (pcl_isfinite (depth_parameters_.principal_point_y)) + cy = static_cast (depth_parameters_.principal_point_y); + + float fx_inv = 1.0f / fx; + float fy_inv = 1.0f / fy; + + + const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); + if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) + { + // Resize the image if nessacery + depth_resize_buffer_.resize(depth_width_ * depth_height_); + depth_map = depth_resize_buffer_.data(); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map ); + } + + const uint16_t* ir_map = (const uint16_t*) ir_image->getData (); + if (ir_image->getWidth () != depth_width_ || ir_image->getHeight () != depth_height_) + { + // Resize the image if nessacery + ir_resize_buffer_.resize(depth_width_ * depth_height_); + ir_map = ir_resize_buffer_.data(); + ir_image->fillRaw (depth_width_, depth_height_, (unsigned short*) ir_map); + } + + + int depth_idx = 0; + float bad_point = std::numeric_limits::quiet_NaN (); + + for (int v = 0; v < depth_height_; ++v) + { + for (int u = 0; u < depth_width_; ++u, ++depth_idx) + { + pcl::PointXYZI& pt = cloud->points[depth_idx]; + /// @todo Different values for these cases + // Check for invalid measurements + if (depth_map[depth_idx] == 0 || + depth_map[depth_idx] == depth_image->getNoSampleValue () || + depth_map[depth_idx] == depth_image->getShadowValue ()) + { + pt.x = pt.y = pt.z = bad_point; + } + else + { + pt.z = depth_map[depth_idx] * 0.001f; // millimeters to meters + pt.x = (static_cast (u) - cx) * pt.z * fx_inv; + pt.y = (static_cast (v) - cy) * pt.z * fy_inv; + } + + pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0; + pt.intensity = static_cast (ir_map[depth_idx]); + } + } + cloud->sensor_origin_.setZero (); + cloud->sensor_orientation_.setIdentity (); + return (cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::updateModeMaps () +{ + typedef pcl::io::openni2::OpenNI2VideoMode VideoMode; + +pcl::io::openni2::OpenNI2VideoMode output_mode; + + config2oni_map_[OpenNI_SXGA_15Hz] = VideoMode (XN_SXGA_X_RES, XN_SXGA_Y_RES, 15); + + config2oni_map_[OpenNI_VGA_25Hz] = VideoMode (XN_VGA_X_RES, XN_VGA_Y_RES, 25); + config2oni_map_[OpenNI_VGA_30Hz] = VideoMode (XN_VGA_X_RES, XN_VGA_Y_RES, 30); + + config2oni_map_[OpenNI_QVGA_25Hz] = VideoMode (XN_QVGA_X_RES, XN_QVGA_Y_RES, 25); + config2oni_map_[OpenNI_QVGA_30Hz] = VideoMode (XN_QVGA_X_RES, XN_QVGA_Y_RES, 30); + config2oni_map_[OpenNI_QVGA_60Hz] = VideoMode (XN_QVGA_X_RES, XN_QVGA_Y_RES, 60); + + config2oni_map_[OpenNI_QQVGA_25Hz] = VideoMode (XN_QQVGA_X_RES, XN_QQVGA_Y_RES, 25); + config2oni_map_[OpenNI_QQVGA_30Hz] = VideoMode (XN_QQVGA_X_RES, XN_QQVGA_Y_RES, 30); + config2oni_map_[OpenNI_QQVGA_60Hz] = VideoMode (XN_QQVGA_X_RES, XN_QQVGA_Y_RES, 60); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::io::OpenNI2Grabber::mapMode2XnMode (int mode, OpenNI2VideoMode &xnmode) const +{ + std::map::const_iterator it = config2oni_map_.find (mode); + if (it != config2oni_map_.end ()) + { + xnmode = it->second; + return (true); + } + else + { + return (false); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +std::vector > +pcl::io::OpenNI2Grabber::getAvailableDepthModes () const +{ +pcl::io::openni2::OpenNI2VideoMode dummy; + std::vector > result; + for (std::map::const_iterator it = config2oni_map_.begin (); it != config2oni_map_.end (); ++it) + { + if (device_->findCompatibleDepthMode (it->second, dummy)) + result.push_back (*it); + } + + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +std::vector > +pcl::io::OpenNI2Grabber::getAvailableImageModes () const +{ +pcl::io::openni2::OpenNI2VideoMode dummy; + std::vector > result; + for (std::map::const_iterator it = config2oni_map_.begin (); it != config2oni_map_.end (); ++it) + { + if (device_->findCompatibleColorMode (it->second, dummy)) + result.push_back (*it); + } + + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +float +pcl::io::OpenNI2Grabber::getFramesPerSecond () const +{ + return (static_cast (device_->getColorVideoMode ().frame_rate_)); +} + + + +// Convert VideoFrameRef into pcl::Image and forward to registered callbacks +void pcl::io::OpenNI2Grabber::processColorFrame (openni::VideoStream& stream) +{ + Image::Timestamp t_callback = Image::Clock::now (); + + openni::VideoFrameRef frame; + stream.readFrame (&frame); + FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); + + openni::PixelFormat format = frame.getVideoMode ().getPixelFormat (); + boost::shared_ptr image; + + // Convert frame to PCL image type, based on pixel format + if (format == openni::PIXEL_FORMAT_YUV422) + image = boost::make_shared (frameWrapper, t_callback); + else //if (format == PixelFormat::PIXEL_FORMAT_RGB888) + image = boost::make_shared (frameWrapper, t_callback); + + imageCallback (image, NULL); +} + + +void pcl::io::OpenNI2Grabber::processDepthFrame (openni::VideoStream& stream) +{ + openni::VideoFrameRef frame; + stream.readFrame (&frame); + FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); + + float focalLength = device_->getDepthFocalLength (); + + float baseline = device_->getBaseline(); + pcl::uint64_t no_sample_value = device_->getShadowValue(); + pcl::uint64_t shadow_value = no_sample_value; + + boost::shared_ptr image = + boost::make_shared (frameWrapper, baseline, focalLength, shadow_value, no_sample_value); + + depthCallback (image, NULL); +} + + +void pcl::io::OpenNI2Grabber::processIRFrame (openni::VideoStream& stream) +{ + openni::VideoFrameRef frame; + stream.readFrame (&frame); + + FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); + + boost::shared_ptr image = boost::make_shared ( frameWrapper ); + + irCallback (image, NULL); +} + +#endif // HAVE_OPENNI2 diff --git a/io/src/openni_camera/openni_device.cpp b/io/src/openni_camera/openni_device.cpp index 38014b37..cb2bfdf2 100644 --- a/io/src/openni_camera/openni_device.cpp +++ b/io/src/openni_camera/openni_device.cpp @@ -418,7 +418,7 @@ void openni_wrapper::OpenNIDevice::InitShiftToDepthConversion () { // Calculate shift conversion table - pcl::int32_t nIndex = 0; + pcl::uint32_t nIndex = 0; pcl::int32_t nShiftValue = 0; double dFixedRefX = 0; double dMetric = 0; diff --git a/io/src/openni_grabber.cpp b/io/src/openni_grabber.cpp index a5f3af8e..727c9bcf 100644 --- a/io/src/openni_grabber.cpp +++ b/io/src/openni_grabber.cpp @@ -84,6 +84,8 @@ pcl::OpenNIGrabber::OpenNIGrabber (const std::string& device_id, const Mode& dep , point_cloud_rgb_signal_ (), point_cloud_rgba_signal_ () , config2xn_map_ (), depth_callback_handle (), image_callback_handle (), ir_callback_handle () , running_ (false) + , rgb_array_size_ (0) + , depth_buffer_size_ (0) , rgb_focal_length_x_ (std::numeric_limits::quiet_NaN ()) , rgb_focal_length_y_ (std::numeric_limits::quiet_NaN ()) , rgb_principal_point_x_ (std::numeric_limits::quiet_NaN ()) @@ -582,22 +584,19 @@ pcl::OpenNIGrabber::convertToXYZPointCloud (const boost::shared_ptrgetDepthMetaData ().Data (); if (depth_image->getWidth() != depth_width_ || depth_image->getHeight () != depth_height_) { - static unsigned buffer_size = 0; - static boost::shared_array depth_buffer ((unsigned short*)(NULL)); - - if (buffer_size < depth_width_ * depth_height_) + if (depth_buffer_size_ < depth_width_ * depth_height_) { - buffer_size = depth_width_ * depth_height_; - depth_buffer.reset (new unsigned short [buffer_size]); + depth_buffer_size_ = depth_width_ * depth_height_; + depth_buffer_.reset (new unsigned short [depth_buffer_size_]); } - depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); - depth_map = depth_buffer.get (); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer_.get ()); + depth_map = depth_buffer_.get (); } register int depth_idx = 0; - for (int v = 0; v < depth_height_; ++v) + for (unsigned int v = 0; v < depth_height_; ++v) { - for (register int u = 0; u < depth_width_; ++u, ++depth_idx) + for (register unsigned int u = 0; u < depth_width_; ++u, ++depth_idx) { pcl::PointXYZ& pt = cloud->points[depth_idx]; // Check for invalid measurements @@ -627,10 +626,7 @@ template typename pcl::PointCloud::Ptr pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr &image, const boost::shared_ptr &depth_image) const { - static unsigned rgb_array_size = 0; - static boost::shared_array rgb_array ((unsigned char*)(NULL)); - static unsigned char* rgb_buffer = 0; - + unsigned char* rgb_buffer = rgb_array_.get (); boost::shared_ptr > cloud (new pcl::PointCloud); cloud->header.frame_id = rgb_frame_id_; @@ -661,25 +657,22 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptrgetDepthMetaData ().Data (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight() != depth_height_) { - static unsigned buffer_size = 0; - static boost::shared_array depth_buffer ((unsigned short*)(NULL)); - - if (buffer_size < depth_width_ * depth_height_) + if (depth_buffer_size_ < depth_width_ * depth_height_) { - buffer_size = depth_width_ * depth_height_; - depth_buffer.reset (new unsigned short [buffer_size]); + depth_buffer_size_ = depth_width_ * depth_height_; + depth_buffer_.reset (new unsigned short [depth_buffer_size_]); } - depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); - depth_map = depth_buffer.get (); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer_.get ()); + depth_map = depth_buffer_.get (); } // here we need exact the size of the point cloud for a one-one correspondence! - if (rgb_array_size < image_width_ * image_height_ * 3) + if (rgb_array_size_ < image_width_ * image_height_ * 3) { - rgb_array_size = image_width_ * image_height_ * 3; - rgb_array.reset (new unsigned char [rgb_array_size]); - rgb_buffer = rgb_array.get (); + rgb_array_size_ = image_width_ * image_height_ * 3; + rgb_array_.reset (new unsigned char [rgb_array_size_]); + rgb_buffer = rgb_array_.get (); } image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3); float bad_point = std::numeric_limits::quiet_NaN (); @@ -700,9 +693,9 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptrpoints[point_idx]; /// @todo Different values for these cases @@ -790,30 +783,26 @@ pcl::OpenNIGrabber::convertToXYZIPointCloud (const boost::shared_ptrgetWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { - static unsigned buffer_size = 0; - static boost::shared_array depth_buffer ((unsigned short*)(NULL)); - static boost::shared_array ir_buffer ((unsigned short*)(NULL)); - - if (buffer_size < depth_width_ * depth_height_) + if (depth_buffer_size_ < depth_width_ * depth_height_) { - buffer_size = depth_width_ * depth_height_; - depth_buffer.reset (new unsigned short [buffer_size]); - ir_buffer.reset (new unsigned short [buffer_size]); + depth_buffer_size_ = depth_width_ * depth_height_; + depth_buffer_.reset (new unsigned short [depth_buffer_size_]); + ir_buffer_.reset (new unsigned short [depth_buffer_size_]); } - depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); - depth_map = depth_buffer.get (); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer_.get ()); + depth_map = depth_buffer_.get (); - ir_image->fillRaw (depth_width_, depth_height_, ir_buffer.get ()); - ir_map = ir_buffer.get (); + ir_image->fillRaw (depth_width_, depth_height_, ir_buffer_.get ()); + ir_map = ir_buffer_.get (); } register int depth_idx = 0; float bad_point = std::numeric_limits::quiet_NaN (); - for (int v = 0; v < depth_height_; ++v) + for (unsigned int v = 0; v < depth_height_; ++v) { - for (register int u = 0; u < depth_width_; ++u, ++depth_idx) + for (register unsigned int u = 0; u < depth_width_; ++u, ++depth_idx) { pcl::PointXYZI& pt = cloud->points[depth_idx]; /// @todo Different values for these cases diff --git a/io/src/pcd_grabber.cpp b/io/src/pcd_grabber.cpp index 57c76022..4efc47e7 100644 --- a/io/src/pcd_grabber.cpp +++ b/io/src/pcd_grabber.cpp @@ -108,6 +108,10 @@ struct pcl::PCDGrabberBase::PCDGrabberImpl std::vector tar_offsets_; std::vector cloud_idx_to_file_idx_; + // Mutex to ensure that two quick consecutive triggers do not cause + // simultaneous asynchronous read-aheads + boost::mutex read_ahead_mutex_; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -282,6 +286,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::openTARFile (const std::string &file_name) void pcl::PCDGrabberBase::PCDGrabberImpl::trigger () { + boost::mutex::scoped_lock read_ahead_lock(read_ahead_mutex_); if (valid_) grabber_.publish (next_cloud_,origin_,orientation_); diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp index 76d1b683..bbe017ca 100644 --- a/io/src/pcd_io.cpp +++ b/io/src/pcd_io.cpp @@ -870,7 +870,7 @@ pcl::PCDReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, // (we really ought to check this in the compressor and copy the original data in those cases) if (data_size < compressed_size || uncompressed_size < compressed_size) { - PCL_DEBUG ("[pcl::PCDReader::read] Allocated data size (%zu) or uncompressed size (%zu) smaller than compressed size (%u). Need to remap.\n", data_size, uncompressed_size, compressed_size); + PCL_DEBUG ("[pcl::PCDReader::read] Allocated data size (%lu) or uncompressed size (%lu) smaller than compressed size (%u). Need to remap.\n", data_size, uncompressed_size, compressed_size); #ifdef _WIN32 UnmapViewOfFile (map); data_size = compressed_size + data_idx + 8; diff --git a/io/src/ply_io.cpp b/io/src/ply_io.cpp index c98bcdd1..6844f85f 100644 --- a/io/src/ply_io.cpp +++ b/io/src/ply_io.cpp @@ -67,12 +67,13 @@ pcl::PLYReader::elementDefinitionCallback (const std::string& element_name, std: boost::bind (&pcl::PLYReader::vertexBeginCallback, this), boost::bind (&pcl::PLYReader::vertexEndCallback, this))); } - // else if (element_name == "face") - // { - // return (boost::tuple, boost::function > ( - // boost::bind (&pcl::PLYReader::faceBegin, this), - // boost::bind (&pcl::PLYReader::faceEnd, this))); - // } + else if ((element_name == "face") && polygons_) + { + polygons_->reserve (count); + return (boost::tuple, boost::function > ( + boost::bind (&pcl::PLYReader::faceBeginCallback, this), + boost::bind (&pcl::PLYReader::faceEndCallback, this))); + } else if (element_name == "camera") { cloud_->is_dense = true; @@ -80,8 +81,7 @@ pcl::PLYReader::elementDefinitionCallback (const std::string& element_name, std: } else if (element_name == "range_grid") { - (*range_grid_).resize (count); - range_count_ = 0; + range_grid_->reserve (count); return (boost::tuple, boost::function > ( boost::bind (&pcl::PLYReader::rangeGridBeginCallback, this), boost::bind (&pcl::PLYReader::rangeGridEndCallback, this))); @@ -99,16 +99,16 @@ pcl::PLYReader::endHeaderCallback () return (cloud_->data.size () == cloud_->point_step * cloud_->width * cloud_->height); } -void -pcl::PLYReader::appendFloatProperty (const std::string& name, const size_t& size) +template void +pcl::PLYReader::appendScalarProperty (const std::string& name, const size_t& size) { cloud_->fields.push_back (::pcl::PCLPointField ()); ::pcl::PCLPointField ¤t_field = cloud_->fields.back (); current_field.name = name; current_field.offset = cloud_->point_step; - current_field.datatype = ::pcl::PCLPointField::FLOAT32; + current_field.datatype = pcl::traits::asEnum::value; current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (::pcl::PCLPointField::FLOAT32) * size); + cloud_->point_step += static_cast (pcl::getFieldSize (pcl::traits::asEnum::value) * size); } void @@ -124,90 +124,6 @@ pcl::PLYReader::amendProperty (const std::string& old_name, const std::string& n finder->datatype = new_datatype; } -void -pcl::PLYReader::appendUnsignedIntProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (::pcl::PCLPointField ()); - ::pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = ::pcl::PCLPointField::UINT32; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (::pcl::PCLPointField::UINT32) * size); -} - -void -pcl::PLYReader::appendIntProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (pcl::PCLPointField ()); - pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = pcl::PCLPointField::INT32; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (pcl::PCLPointField::INT32) * size); -} - -void -pcl::PLYReader::appendDoubleProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (pcl::PCLPointField ()); - pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = pcl::PCLPointField::FLOAT64; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (pcl::PCLPointField::FLOAT64) * size); -} - -void -pcl::PLYReader::appendUnsignedCharProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (pcl::PCLPointField ()); - pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = pcl::PCLPointField::UINT8; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (pcl::PCLPointField::UINT8) * size); -} - -void -pcl::PLYReader::appendCharProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (pcl::PCLPointField ()); - pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = pcl::PCLPointField::INT8; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (pcl::PCLPointField::INT8) * size); -} - -void -pcl::PLYReader::appendUnsignedShortProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (pcl::PCLPointField ()); - pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = pcl::PCLPointField::UINT16; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (pcl::PCLPointField::UINT16) * size); -} - -void -pcl::PLYReader::appendShortProperty (const std::string& name, const size_t& size) -{ - cloud_->fields.push_back (pcl::PCLPointField ()); - pcl::PCLPointField ¤t_field = cloud_->fields.back (); - current_field.name = name; - current_field.offset = cloud_->point_step; - current_field.datatype = pcl::PCLPointField::INT16; - current_field.count = static_cast (size); - cloud_->point_step += static_cast (pcl::getFieldSize (pcl::PCLPointField::INT16) * size); -} - namespace pcl { template <> @@ -216,8 +132,8 @@ namespace pcl { if (element_name == "vertex") { - appendFloatProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexFloatPropertyCallback, this, _1)); + appendScalarProperty (property_name, 1); + return (boost::bind (&pcl::PLYReader::vertexScalarPropertyCallback, this, _1)); } else if (element_name == "camera") { @@ -289,7 +205,7 @@ namespace pcl (property_name == "diffuse_red") || (property_name == "diffuse_green") || (property_name == "diffuse_blue")) { if ((property_name == "red") || (property_name == "diffuse_red")) - appendFloatProperty ("rgb"); + appendScalarProperty ("rgb"); return boost::bind (&pcl::PLYReader::vertexColorCallback, this, property_name, _1); } else if (property_name == "alpha") @@ -299,13 +215,13 @@ namespace pcl } else if (property_name == "intensity") { - appendFloatProperty (property_name); + appendScalarProperty (property_name); return boost::bind (&pcl::PLYReader::vertexIntensityCallback, this, _1); } else { - appendUnsignedCharProperty (property_name); - return boost::bind (&pcl::PLYReader::vertexUnsignedCharPropertyCallback, this, _1); + appendScalarProperty (property_name); + return boost::bind (&pcl::PLYReader::vertexScalarPropertyCallback, this, _1); } } else @@ -317,8 +233,8 @@ namespace pcl { if (element_name == "vertex") { - appendIntProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexIntPropertyCallback, this, _1)); + appendScalarProperty (property_name, 1); + return (boost::bind (&pcl::PLYReader::vertexScalarPropertyCallback, this, _1)); } if (element_name == "camera") { @@ -331,69 +247,30 @@ namespace pcl return boost::bind (&pcl::PLYReader::cloudHeightCallback, this, _1); } else - { - appendIntProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexIntPropertyCallback, this, _1)); - } + return (0); } else return (0); } - template <> boost::function - PLYReader::scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name) - { - if (element_name == "vertex") - { - appendUnsignedIntProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexUnsignedIntPropertyCallback, this, _1)); - } - return (0); - } - - template <> boost::function - PLYReader::scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name) - { - if (element_name == "vertex") - { - appendDoubleProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexDoublePropertyCallback, this, _1)); - } - return (0); - } - - template <> boost::function - PLYReader::scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name) - { - if (element_name == "vertex") - { - appendUnsignedShortProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexUnsignedShortPropertyCallback, this, _1)); - } - return (0); - } - - template <> boost::function + template boost::function PLYReader::scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name) { if (element_name == "vertex") { - appendShortProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexShortPropertyCallback, this, _1)); + appendScalarProperty (property_name, 1); + return (boost::bind (&pcl::PLYReader::vertexScalarPropertyCallback, this, _1)); } return (0); } - - template <> boost::function - PLYReader::scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name) + template void + PLYReader::vertexScalarPropertyCallback (Scalar value) { - if (element_name == "vertex") - { - appendCharProperty (property_name, 1); - return (boost::bind (&pcl::PLYReader::vertexCharPropertyCallback, this, _1)); - } - return (0); + memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], + &value, + sizeof (Scalar)); + vertex_offset_before_ += static_cast (sizeof (Scalar)); } template void @@ -424,7 +301,7 @@ namespace pcl boost::tuple, boost::function, boost::function > pcl::PLYReader::listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name) { - if ((element_name == "range_grid") && (property_name == "vertex_indices")) + if ((element_name == "range_grid") && (property_name == "vertex_indices") && polygons_) { return boost::tuple, boost::function, boost::function > ( boost::bind (&pcl::PLYReader::rangeGridVertexIndicesBeginCallback, this, _1), @@ -432,6 +309,14 @@ namespace pcl boost::bind (&pcl::PLYReader::rangeGridVertexIndicesEndCallback, this) ); } + else if ((element_name == "face") && (property_name == "vertex_indices")) + { + return boost::tuple, boost::function, boost::function > ( + boost::bind (&pcl::PLYReader::faceVertexIndicesBeginCallback, this, _1), + boost::bind (&pcl::PLYReader::faceVertexIndicesElementCallback, this, _1), + boost::bind (&pcl::PLYReader::faceVertexIndicesEndCallback, this) + ); + } else if (element_name == "vertex") { cloud_->fields.push_back (pcl::PCLPointField ()); @@ -485,79 +370,6 @@ namespace pcl return boost::tuple, boost::function, boost::function > (0, 0, 0); } } - -} - -void -pcl::PLYReader::vertexFloatPropertyCallback (pcl::io::ply::float32 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::float32)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::float32)); -} - -void -pcl::PLYReader::vertexDoublePropertyCallback (pcl::io::ply::float64 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::float64)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::float64)); -} - -void -pcl::PLYReader::vertexUnsignedIntPropertyCallback (pcl::io::ply::uint32 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::uint32)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::uint32)); -} - -void -pcl::PLYReader::vertexIntPropertyCallback (pcl::io::ply::int32 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::int32)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::int32)); -} - -void -pcl::PLYReader::vertexUnsignedShortPropertyCallback (pcl::io::ply::uint16 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::uint16)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::uint16)); -} - -void -pcl::PLYReader::vertexShortPropertyCallback (pcl::io::ply::int16 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::int16)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::int16)); -} - -void -pcl::PLYReader::vertexUnsignedCharPropertyCallback (pcl::io::ply::uint8 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::uint8)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::uint8)); -} - -void -pcl::PLYReader::vertexCharPropertyCallback (pcl::io::ply::int8 value) -{ - memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_], - &value, - sizeof (pcl::io::ply::int8)); - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::int8)); } void @@ -631,28 +443,53 @@ pcl::PLYReader::vertexEndCallback () } void -pcl::PLYReader::rangeGridBeginCallback () { } +pcl::PLYReader::rangeGridBeginCallback () +{ + range_grid_->push_back (std::vector ()); +} void pcl::PLYReader::rangeGridVertexIndicesBeginCallback (pcl::io::ply::uint8 size) { - (*range_grid_)[range_count_].reserve (size); + range_grid_->back ().reserve (size); } -void pcl::PLYReader::rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index) +void +pcl::PLYReader::rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index) +{ + range_grid_->back ().push_back (vertex_index); +} + +void +pcl::PLYReader::rangeGridVertexIndicesEndCallback () {} + +void +pcl::PLYReader::rangeGridEndCallback () {} + +void +pcl::PLYReader::faceBeginCallback () { - (*range_grid_)[range_count_].push_back (vertex_index); + polygons_->push_back (pcl::Vertices ()); } void -pcl::PLYReader::rangeGridVertexIndicesEndCallback () { } +pcl::PLYReader::faceVertexIndicesBeginCallback (pcl::io::ply::uint8 size) +{ + polygons_->back ().vertices.reserve (size); +} void -pcl::PLYReader::rangeGridEndCallback () +pcl::PLYReader::faceVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index) { - ++range_count_; + polygons_->back ().vertices.push_back (vertex_index); } +void +pcl::PLYReader::faceVertexIndicesEndCallback () { } + +void +pcl::PLYReader::faceEndCallback () {} + void pcl::PLYReader::objInfoCallback (const std::string& line) { @@ -694,26 +531,26 @@ pcl::PLYReader::parse (const std::string& istream_filename) ply_parser.end_header_callback (boost::bind (&pcl::PLYReader::endHeaderCallback, this)); pcl::io::ply::ply_parser::scalar_property_definition_callbacks_type scalar_property_definition_callbacks; - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&pcl::PLYReader::scalarPropertyDefinitionCallback, this, _1, _2); ply_parser.scalar_property_definition_callbacks (scalar_property_definition_callbacks); pcl::io::ply::ply_parser::list_property_definition_callbacks_type list_property_definition_callbacks; - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&pcl::PLYReader::listPropertyDefinitionCallback, this, _1, _2); ply_parser.list_property_definition_callbacks (list_property_definition_callbacks); return ply_parser.parse (istream_filename); @@ -722,13 +559,15 @@ pcl::PLYReader::parse (const std::string& istream_filename) //////////////////////////////////////////////////////////////////////////////////////// int pcl::PLYReader::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, - Eigen::Vector4f &, Eigen::Quaternionf &, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &, int &, unsigned int &, const int) { // Silence compiler warnings cloud_ = &cloud; range_grid_ = new std::vector >; cloud_->width = cloud_->height = 0; + origin = Eigen::Vector4f::Zero (); + orientation = Eigen::Quaternionf::Identity (); if (!parse (file_name)) { PCL_ERROR ("[pcl::PLYReader::read] problem parsing header!\n"); @@ -782,8 +621,8 @@ pcl::PLYReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, cloud_->data.swap (data); } - orientation = Eigen::Quaternionf (orientation_); - origin = origin_; + orientation_ = Eigen::Quaternionf (orientation); + origin_ = origin; for (size_t i = 0; i < cloud_->fields.size (); ++i) { @@ -798,7 +637,76 @@ pcl::PLYReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, } //////////////////////////////////////////////////////////////////////////////////////// +int +pcl::PLYReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &ply_version, const int offset) +{ + // kept only for backward compatibility + int data_type; + unsigned int data_idx; + polygons_ = &(mesh.polygons); + if (this->readHeader (file_name, mesh.cloud, origin, orientation, ply_version, data_type, data_idx, offset)) + { + PCL_ERROR ("[pcl::PLYReader::read] problem parsing header!\n"); + return (-1); + } + + // a range_grid element was found ? + size_t r_size; + if ((r_size = (*range_grid_).size ()) > 0 && r_size != vertex_count_) + { + //cloud.header = cloud_->header; + std::vector data ((*range_grid_).size () * mesh.cloud.point_step); + const static float f_nan = std::numeric_limits ::quiet_NaN (); + const static double d_nan = std::numeric_limits ::quiet_NaN (); + for (size_t r = 0; r < r_size; ++r) + { + if ((*range_grid_)[r].size () == 0) + { + for (size_t f = 0; f < cloud_->fields.size (); ++f) + if (cloud_->fields[f].datatype == ::pcl::PCLPointField::FLOAT32) + memcpy (&data[r * cloud_->point_step + cloud_->fields[f].offset], + reinterpret_cast (&f_nan), sizeof (float)); + else if (cloud_->fields[f].datatype == ::pcl::PCLPointField::FLOAT64) + memcpy (&data[r * cloud_->point_step + cloud_->fields[f].offset], + reinterpret_cast (&d_nan), sizeof (double)); + else + memset (&data[r * cloud_->point_step + cloud_->fields[f].offset], 0, + pcl::getFieldSize (cloud_->fields[f].datatype) * cloud_->fields[f].count); + } + else + memcpy (&data[r* cloud_->point_step], &cloud_->data[(*range_grid_)[r][0] * cloud_->point_step], cloud_->point_step); + } + cloud_->data.swap (data); + } + + orientation_ = Eigen::Quaternionf (orientation); + origin_ = origin; + + for (size_t i = 0; i < cloud_->fields.size (); ++i) + { + if (cloud_->fields[i].name == "nx") + cloud_->fields[i].name = "normal_x"; + if (cloud_->fields[i].name == "ny") + cloud_->fields[i].name = "normal_y"; + if (cloud_->fields[i].name == "nz") + cloud_->fields[i].name = "normal_z"; + } + return (0); +} +//////////////////////////////////////////////////////////////////////////////////////// +int +pcl::PLYReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset) +{ + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + int ply_version; + return read (file_name, mesh, origin, orientation, ply_version, offset); +} + +//////////////////////////////////////////////////////////////////////////////////////// std::string pcl::PLYWriter::generateHeader (const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, @@ -1591,7 +1499,7 @@ pcl::io::savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh } // Faces fs << "\nelement face "<< nr_faces; - fs << "\nproperty list uchar int vertex_index"; + fs << "\nproperty list uchar int vertex_indices"; fs << "\nend_header\n"; // Write down vertices @@ -1710,7 +1618,7 @@ pcl::io::savePLYFileBinary (const std::string &file_name, const pcl::PolygonMesh } // Faces fs << "\nelement face "<< nr_faces; - fs << "\nproperty list uchar int vertex_index"; + fs << "\nproperty list uchar int vertex_indices"; fs << "\nend_header\n"; // Close the file diff --git a/io/src/png_io.cpp b/io/src/png_io.cpp index c8da5628..bd3a7cc1 100644 --- a/io/src/png_io.cpp +++ b/io/src/png_io.cpp @@ -93,3 +93,54 @@ pcl::io::saveShortPNGFile (const std::string &file_name, const unsigned short *s flipAndWritePng(file_name, importer); } + +void +pcl::io::saveRgbPNGFile (const std::string& file_name, const unsigned char *rgb_image, int width, int height) +{ + saveCharPNGFile(file_name, rgb_image, width, height, 3); +} + +void +pcl::io::savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) +{ + saveCharPNGFile(file_name, &cloud.points[0], cloud.width, cloud.height, 1); +} + +void +pcl::io::savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) +{ + saveShortPNGFile(file_name, &cloud.points[0], cloud.width, cloud.height, 1); +} + +void +pcl::io::savePNGFile (const std::string& file_name, const pcl::PCLImage& image) +{ + if (image.encoding == "rgb8") + { + saveRgbPNGFile(file_name, &image.data[0], image.width, image.height); + } + else if (image.encoding == "mono8") + { + saveCharPNGFile(file_name, &image.data[0], image.width, image.height, 1); + } + else if (image.encoding == "mono16") + { + saveShortPNGFile(file_name, reinterpret_cast(&image.data[0]), image.width, image.height, 1); + } + else + { + PCL_ERROR ("[pcl::io::savePNGFile] Unsupported image encoding \"%s\".\n", image.encoding.c_str ()); + } +} + +void +pcl::io::savePNGFile (const std::string& file_name, const pcl::PointCloud& cloud) +{ + std::vector data(cloud.width * cloud.height); + for (size_t i = 0; i < cloud.points.size (); ++i) + { + data[i] = static_cast (cloud.points[i].label); + } + saveShortPNGFile(file_name, &data[0], cloud.width, cloud.height,1); +} + diff --git a/io/src/robot_eye_grabber.cpp b/io/src/robot_eye_grabber.cpp index 4b4a793e..3b265766 100644 --- a/io/src/robot_eye_grabber.cpp +++ b/io/src/robot_eye_grabber.cpp @@ -166,7 +166,7 @@ pcl::RobotEyeGrabber::convertPacketData (unsigned char *dataPacket, size_t lengt const size_t bytesPerPoint = 8; const size_t totalPoints = length / bytesPerPoint; - for (int i = 0; i < totalPoints; ++i) + for (size_t i = 0; i < totalPoints; ++i) { PointXYZI xyzi; computeXYZI (xyzi, dataPacket + i*bytesPerPoint); diff --git a/io/src/vtk_lib_io.cpp b/io/src/vtk_lib_io.cpp index 043467cb..257138b6 100644 --- a/io/src/vtk_lib_io.cpp +++ b/io/src/vtk_lib_io.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -80,18 +81,18 @@ pcl::io::savePolygonFile (const std::string &file_name, const pcl::PolygonMesh& // TODO: what about sensor position and orientation?!?!?!? // TODO: how to adequately catch exceptions thrown by the vtk writers?! std::string extension = file_name.substr (file_name.find_last_of (".") + 1); - if (extension == ".pcd") // no Polygon, but only a point cloud + if (extension == "pcd") // no Polygon, but only a point cloud { int error_code = pcl::io::savePCDFile (file_name, mesh.cloud); if (error_code != 0) return (0); return (static_cast (mesh.cloud.width * mesh.cloud.height)); } - else if (extension == ".vtk") + else if (extension == "vtk") return (pcl::io::savePolygonFileVTK (file_name, mesh)); - else if (extension == ".ply") + else if (extension == "ply") return (pcl::io::savePolygonFilePLY (file_name, mesh)); - else if (extension == ".stl" ) + else if (extension == "stl" ) return (pcl::io::savePolygonFileSTL (file_name, mesh)); else { @@ -178,7 +179,11 @@ pcl::io::savePolygonFileVTK (const std::string &file_name, const pcl::PolygonMes pcl::io::mesh2vtk (mesh, poly_data); vtkSmartPointer poly_writer = vtkSmartPointer::New (); +#if VTK_MAJOR_VERSION < 6 poly_writer->SetInput (poly_data); +#else + poly_writer->SetInputData (poly_data); +#endif poly_writer->SetFileName (file_name.c_str ()); poly_writer->Write (); @@ -194,7 +199,11 @@ pcl::io::savePolygonFilePLY (const std::string &file_name, const pcl::PolygonMes pcl::io::mesh2vtk (mesh, poly_data); vtkSmartPointer poly_writer = vtkSmartPointer::New (); +#if VTK_MAJOR_VERSION < 6 poly_writer->SetInput (poly_data); +#else + poly_writer->SetInputData (poly_data); +#endif poly_writer->SetFileName (file_name.c_str ()); poly_writer->SetArrayName ("Colors"); poly_writer->Write (); @@ -209,9 +218,12 @@ pcl::io::savePolygonFileSTL (const std::string &file_name, const pcl::PolygonMes vtkSmartPointer poly_data = vtkSmartPointer::New (); pcl::io::mesh2vtk (mesh, poly_data); - poly_data->Update (); vtkSmartPointer poly_writer = vtkSmartPointer::New (); +#if VTK_MAJOR_VERSION < 6 poly_writer->SetInput (poly_data); +#else + poly_writer->SetInputData (poly_data); +#endif poly_writer->SetFileName (file_name.c_str ()); poly_writer->Write (); @@ -483,9 +495,13 @@ pcl::io::saveRangeImagePlanarFilePNG ( { vtkSmartPointer image = vtkSmartPointer::New(); image->SetDimensions(range_image.width, range_image.height, 1); +#if VTK_MAJOR_VERSION < 6 image->SetNumberOfScalarComponents(1); image->SetScalarTypeToFloat(); image->AllocateScalars(); +#else + image->AllocateScalars (VTK_FLOAT, 1); +#endif int* dims = image->GetDimensions(); @@ -504,7 +520,11 @@ pcl::io::saveRangeImagePlanarFilePNG ( vtkSmartPointer shiftScaleFilter = vtkSmartPointer::New(); shiftScaleFilter->SetOutputScalarTypeToUnsignedChar(); +#if VTK_MAJOR_VERSION < 6 shiftScaleFilter->SetInputConnection(image->GetProducerPort()); +#else + shiftScaleFilter->SetInputData (image); +#endif shiftScaleFilter->SetShift(-1.0f * image->GetScalarRange()[0]); // brings the lower bound to 0 shiftScaleFilter->SetScale(newRange/oldRange); shiftScaleFilter->Update(); diff --git a/io/tools/openni_pcd_recorder.cpp b/io/tools/openni_pcd_recorder.cpp index 6b501f71..1174498b 100644 --- a/io/tools/openni_pcd_recorder.cpp +++ b/io/tools/openni_pcd_recorder.cpp @@ -59,23 +59,39 @@ boost::mutex io_mutex; size_t getTotalSystemMemory () { + uint64_t memory = std::numeric_limits::max (); + +#ifdef _SC_AVPHYS_PAGES uint64_t pages = sysconf (_SC_AVPHYS_PAGES); uint64_t page_size = sysconf (_SC_PAGE_SIZE); - print_info ("Total available memory size: %lluMB.\n", (pages * page_size) / 1048576); - if (pages * page_size > uint64_t (std::numeric_limits::max ())) + + memory = pages * page_size; + +#elif defined(HAVE_SYSCTL) && defined(HW_PHYSMEM) + // This works on *bsd and darwin. + unsigned int physmem; + size_t len = sizeof physmem; + static int mib[2] = { CTL_HW, HW_PHYSMEM }; + + if (sysctl (mib, ARRAY_SIZE (mib), &physmem, &len, NULL, 0) == 0 && len == sizeof (physmem)) { - return std::numeric_limits::max (); + memory = physmem; } - else +#endif + + if (memory > uint64_t (std::numeric_limits::max ())) { - return size_t (pages * page_size); + memory = std::numeric_limits::max (); } + + print_info ("Total available memory size: %lluMB.\n", memory / 1048576ull); + return size_t (memory); } -const int BUFFER_SIZE = int (getTotalSystemMemory () / (640 * 480)); +const size_t BUFFER_SIZE = size_t (getTotalSystemMemory () / (640 * 480 * sizeof (pcl::PointXYZRGBA))); #else -const int BUFFER_SIZE = 200; +const size_t BUFFER_SIZE = 200; #endif ////////////////////////////////////////////////////////////////////////////////////////// @@ -127,7 +143,7 @@ class PCDBuffer private: PCDBuffer (const PCDBuffer&); // Disabled copy constructor - PCDBuffer& operator =(const PCDBuffer&); // Disabled assignment operator + PCDBuffer& operator = (const PCDBuffer&); // Disabled assignment operator boost::mutex bmutex_; boost::condition_variable buff_empty_; @@ -320,23 +336,108 @@ ctrlC (int) } ////////////////////////////////////////////////////////////////////////////////////////// -int +void +printHelp (int default_buff_size, int, char **argv) +{ + using pcl::console::print_error; + using pcl::console::print_info; + + print_error ("Syntax is: %s (( | ) [-xyz] [-shift] [-buf X] | -l [] | -h | --help)]\n", argv [0]); + print_info ("%s -h | --help : shows this help\n", argv [0]); + print_info ("%s -xyz : save only XYZ data, even if the device is RGB capable\n", argv [0]); + print_info ("%s -shift : use OpenNI shift values rather than 12-bit depth\n", argv [0]); + print_info ("%s -buf X ; use a buffer size of X frames (default: ", argv [0]); + print_value ("%d", default_buff_size); print_info (")\n"); + print_info ("%s -l : list all available devices\n", argv [0]); + print_info ("%s -l :list all available modes for specified device\n", argv [0]); + print_info ("\t\t may be \"#1\", \"#2\", ... for the first, second etc device in the list\n"); +#ifndef _WIN32 + print_info ("\t\t bus@address for the device connected to a specific usb-bus / address combination\n"); + print_info ("\t\t \n"); +#endif + print_info ("\n\nexamples:\n"); + print_info ("%s \"#1\"\n", argv [0]); + print_info ("\t\t uses the first device.\n"); + print_info ("%s \"./temp/test.oni\"\n", argv [0]); + print_info ("\t\t uses the oni-player device to play back oni file given by path.\n"); + print_info ("%s -l\n", argv [0]); + print_info ("\t\t list all available devices.\n"); + print_info ("%s -l \"#2\"\n", argv [0]); + print_info ("\t\t list all available modes for the second device.\n"); + #ifndef _WIN32 + print_info ("%s A00361800903049A\n", argv [0]); + print_info ("\t\t uses the device with the serial number \'A00361800903049A\'.\n"); + print_info ("%s 1@16\n", argv [0]); + print_info ("\t\t uses the device on address 16 at USB bus 1.\n"); + #endif +} + +////////////////////////////////////////////////////////////////////////////////////////// +int main (int argc, char** argv) { print_highlight ("PCL OpenNI Recorder for saving buffered PCD (binary compressed to disk). See %s -h for options.\n", argv[0]); + std::string device_id (""); int buff_size = BUFFER_SIZE; - - if (find_switch (argc, argv, "-h") || find_switch (argc, argv, "--help")) + + if (argc >= 2) { - print_info ("Options are: \n" - " -xyz = save only XYZ data, even if the device is RGB capable\n" - " -shift = use OpenNI shift values rather than 12-bit depth\n" - " -buf X = use a buffer size of X frames (default: "); - print_value ("%d", buff_size); print_info (")\n"); - return (0); - } + device_id = argv[1]; + if (device_id == "--help" || device_id == "-h") + { + printHelp (buff_size, argc, argv); + return 0; + } + else if (device_id == "-l") + { + if (argc >= 3) + { + pcl::OpenNIGrabber grabber (argv[2]); + boost::shared_ptr device = grabber.getDevice (); + cout << "Supported depth modes for device: " << device->getVendorName () << " , " << device->getProductName () << endl; + std::vector > modes = grabber.getAvailableDepthModes (); + for (std::vector >::const_iterator it = modes.begin (); it != modes.end (); ++it) + { + cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << endl; + } + + if (device->hasImageStream ()) + { + cout << endl << "Supported image modes for device: " << device->getVendorName () << " , " << device->getProductName () << endl; + modes = grabber.getAvailableImageModes (); + for (std::vector >::const_iterator it = modes.begin (); it != modes.end (); ++it) + { + cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << endl; + } + } + } + else + { + openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance (); + if (driver.getNumberDevices() > 0) + { + for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx) + { + cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx) + << ", connected: " << driver.getBus(deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl; + } + + } + else + cout << "No devices connected." << endl; + cout <<"Virtual Devices available: ONI player" << endl; + } + return 0; + } + } + else + { + openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance (); + if (driver.getNumberDevices () > 0) + cout << "Device Id not set, using first device." << endl; + } bool just_xyz = find_switch (argc, argv, "-xyz"); openni_wrapper::OpenNIDevice::DepthMode depth_mode = openni_wrapper::OpenNIDevice::OpenNI_12_bit_depth; @@ -348,9 +449,9 @@ main (int argc, char** argv) else print_highlight ("Using default buffer size of %d frames.\n", buff_size); - print_highlight ("Starting the producer and consumer threads... Press Cltr+C to end\n"); + print_highlight ("Starting the producer and consumer threads... Press Ctrl+C to end\n"); - OpenNIGrabber grabber (""); + OpenNIGrabber grabber (device_id); if (grabber.providesCallback () && !just_xyz) { diff --git a/io/tools/ply/ply2obj.cpp b/io/tools/ply/ply2obj.cpp index 5a8d9ac4..6ae74ba9 100644 --- a/io/tools/ply/ply2obj.cpp +++ b/io/tools/ply/ply2obj.cpp @@ -304,11 +304,11 @@ ply_to_obj_converter::convert (std::istream&, const std::string& istream_filenam ply_parser.element_definition_callback (boost::bind (&ply_to_obj_converter::element_definition_callback, this, _1, _2)); pcl::io::ply::ply_parser::scalar_property_definition_callbacks_type scalar_property_definition_callbacks; - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&ply_to_obj_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&ply_to_obj_converter::scalar_property_definition_callback, this, _1, _2); ply_parser.scalar_property_definition_callbacks (scalar_property_definition_callbacks); pcl::io::ply::ply_parser::list_property_definition_callbacks_type list_property_definition_callbacks; - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&ply_to_obj_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&ply_to_obj_converter::list_property_definition_callback, this, _1, _2); ply_parser.list_property_definition_callbacks (list_property_definition_callbacks); ostream_ = &ostream; diff --git a/io/tools/ply/ply2ply.cpp b/io/tools/ply/ply2ply.cpp index 0b6eb9ea..62c5d57e 100644 --- a/io/tools/ply/ply2ply.cpp +++ b/io/tools/ply/ply2ply.cpp @@ -353,45 +353,45 @@ ply_to_ply_converter::convert (const std::string &ifilename, std::istream&, std: pcl::io::ply::ply_parser::scalar_property_definition_callbacks_type scalar_property_definition_callbacks; - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); - pcl::io::ply::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(scalar_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::scalar_property_definition_callback, this, _1, _2); ply_parser.scalar_property_definition_callbacks(scalar_property_definition_callbacks); pcl::io::ply::ply_parser::list_property_definition_callbacks_type list_property_definition_callbacks; - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); - pcl::io::ply::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at(list_property_definition_callbacks) = boost::bind(&ply_to_ply_converter::list_property_definition_callback, this, _1, _2); ply_parser.list_property_definition_callbacks(list_property_definition_callbacks); diff --git a/io/tools/ply/ply2raw.cpp b/io/tools/ply/ply2raw.cpp index 9e8d1e6b..496b771a 100644 --- a/io/tools/ply/ply2raw.cpp +++ b/io/tools/ply/ply2raw.cpp @@ -313,11 +313,11 @@ ply_to_raw_converter::convert (std::istream&, const std::string& istream_filenam ply_parser.element_definition_callback (boost::bind (&ply_to_raw_converter::element_definition_callback, this, _1, _2)); pcl::io::ply::ply_parser::scalar_property_definition_callbacks_type scalar_property_definition_callbacks; - pcl::io::ply::at (scalar_property_definition_callbacks) = boost::bind (&ply_to_raw_converter::scalar_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at (scalar_property_definition_callbacks) = boost::bind (&ply_to_raw_converter::scalar_property_definition_callback, this, _1, _2); ply_parser.scalar_property_definition_callbacks (scalar_property_definition_callbacks); pcl::io::ply::ply_parser::list_property_definition_callbacks_type list_property_definition_callbacks; - pcl::io::ply::at (list_property_definition_callbacks) = boost::bind (&ply_to_raw_converter::list_property_definition_callback, this, _1, _2); + pcl::io::ply::ply_parser::at (list_property_definition_callbacks) = boost::bind (&ply_to_raw_converter::list_property_definition_callback, this, _1, _2); ply_parser.list_property_definition_callbacks (list_property_definition_callbacks); ostream_ = &ostream; diff --git a/kdtree/CMakeLists.txt b/kdtree/CMakeLists.txt index 8a13dbb9..311dffc1 100644 --- a/kdtree/CMakeLists.txt +++ b/kdtree/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud kd-tree library") set(SUBSYS_DEPS common) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS flann) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS flann) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -14,28 +14,28 @@ if(build) ) set(incs - include/pcl/${SUBSYS_NAME}/kdtree.h - include/pcl/${SUBSYS_NAME}/io.h - include/pcl/${SUBSYS_NAME}/flann.h - include/pcl/${SUBSYS_NAME}/kdtree_flann.h + "include/pcl/${SUBSYS_NAME}/kdtree.h" + "include/pcl/${SUBSYS_NAME}/io.h" + "include/pcl/${SUBSYS_NAME}/flann.h" + "include/pcl/${SUBSYS_NAME}/kdtree_flann.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/io.hpp - include/pcl/${SUBSYS_NAME}/impl/kdtree_flann.hpp + "include/pcl/${SUBSYS_NAME}/impl/io.hpp" + "include/pcl/${SUBSYS_NAME}/impl/kdtree_flann.hpp" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_common ${FLANN_LIBRARIES}) + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_common ${FLANN_LIBRARIES}) set(EXT_DEPS flann) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/kdtree/include/pcl/kdtree/impl/io.hpp b/kdtree/include/pcl/kdtree/impl/io.hpp index c5fcaec6..abf4db8e 100644 --- a/kdtree/include/pcl/kdtree/impl/io.hpp +++ b/kdtree/include/pcl/kdtree/impl/io.hpp @@ -46,8 +46,8 @@ ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::getApproximateIndices ( - const typename pcl::PointCloud::Ptr &cloud_in, - const typename pcl::PointCloud::Ptr &cloud_ref, + const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, std::vector &indices) { pcl::KdTreeFLANN tree; @@ -58,7 +58,7 @@ pcl::getApproximateIndices ( indices.resize (cloud_in->points.size ()); for (size_t i = 0; i < cloud_in->points.size (); ++i) { - tree.nearestKSearch (*cloud_in, i, 1, nn_idx, nn_dists); + tree.nearestKSearchT ((*cloud_in)[i], 1, nn_idx, nn_dists); indices[i] = nn_idx[0]; } } @@ -66,8 +66,8 @@ pcl::getApproximateIndices ( ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::getApproximateIndices ( - const typename pcl::PointCloud::Ptr &cloud_in, - const typename pcl::PointCloud::Ptr &cloud_ref, + const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, std::vector &indices) { pcl::KdTreeFLANN tree; diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp index b40866b3..4777f8ee 100644 --- a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp +++ b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp @@ -48,7 +48,7 @@ template pcl::KdTreeFLANN::KdTreeFLANN (bool sorted) : pcl::KdTree (sorted) - , flann_index_ (), cloud_ (NULL) + , flann_index_ (), cloud_ () , index_mapping_ (), identity_mapping_ (false) , dim_ (0), total_nr_points_ (0) , param_k_ (::flann::SearchParams (-1 , epsilon_)) @@ -60,7 +60,7 @@ pcl::KdTreeFLANN::KdTreeFLANN (bool sorted) template pcl::KdTreeFLANN::KdTreeFLANN (const KdTreeFLANN &k) : pcl::KdTree (false) - , flann_index_ (), cloud_ (NULL) + , flann_index_ (), cloud_ () , index_mapping_ (), identity_mapping_ (false) , dim_ (0), total_nr_points_ (0) , param_k_ (::flann::SearchParams (-1 , epsilon_)) @@ -120,7 +120,7 @@ pcl::KdTreeFLANN::setInputCloud (const PointCloudConstPtr &cloud, return; } - flann_index_.reset (new FLANNIndex (::flann::Matrix (cloud_, + flann_index_.reset (new FLANNIndex (::flann::Matrix (cloud_.get (), index_mapping_.size (), dim_), ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf @@ -214,11 +214,6 @@ template void pcl::KdTreeFLANN::cleanup () { // Data array cleanup - if (cloud_) - { - free (cloud_); - cloud_ = NULL; - } index_mapping_.clear (); if (indices_) @@ -232,14 +227,14 @@ pcl::KdTreeFLANN::convertCloudToArray (const PointCloud &cloud) // No point in doing anything if the array is empty if (cloud.points.empty ()) { - cloud_ = NULL; + cloud_.reset (); return; } int original_no_of_points = static_cast (cloud.points.size ()); - cloud_ = static_cast (malloc (original_no_of_points * dim_ * sizeof (float))); - float* cloud_ptr = cloud_; + cloud_.reset (new float[original_no_of_points * dim_]); + float* cloud_ptr = cloud_.get (); index_mapping_.reserve (original_no_of_points); identity_mapping_ = true; @@ -266,14 +261,14 @@ pcl::KdTreeFLANN::convertCloudToArray (const PointCloud &cloud, co // No point in doing anything if the array is empty if (cloud.points.empty ()) { - cloud_ = NULL; + cloud_.reset (); return; } int original_no_of_points = static_cast (indices.size ()); - cloud_ = static_cast (malloc (original_no_of_points * dim_ * sizeof (float))); - float* cloud_ptr = cloud_; + cloud_.reset (new float[original_no_of_points * dim_]); + float* cloud_ptr = cloud_.get (); index_mapping_.reserve (original_no_of_points); // its a subcloud -> false // true only identity: diff --git a/kdtree/include/pcl/kdtree/io.h b/kdtree/include/pcl/kdtree/io.h index 7fd244f0..68a2a2f7 100644 --- a/kdtree/include/pcl/kdtree/io.h +++ b/kdtree/include/pcl/kdtree/io.h @@ -53,9 +53,9 @@ namespace pcl * \param[out] indices the resultant set of nearest neighbor indices of \a cloud_in in \a cloud_ref * \ingroup kdtree */ - template void - getApproximateIndices (const typename pcl::PointCloud::Ptr &cloud_in, - const typename pcl::PointCloud::Ptr &cloud_ref, + template void + getApproximateIndices (const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, std::vector &indices); /** \brief Get a set of approximate indices for a given point cloud into a reference point cloud. @@ -67,9 +67,9 @@ namespace pcl * \param[out] indices the resultant set of nearest neighbor indices of \a cloud_in in \a cloud_ref * \ingroup kdtree */ - template void - getApproximateIndices (const typename pcl::PointCloud::Ptr &cloud_in, - const typename pcl::PointCloud::Ptr &cloud_ref, + template void + getApproximateIndices (const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, std::vector &indices); } diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/kdtree/include/pcl/kdtree/kdtree.h index a554991a..586e4945 100644 --- a/kdtree/include/pcl/kdtree/kdtree.h +++ b/kdtree/include/pcl/kdtree/kdtree.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace pcl { @@ -175,11 +176,7 @@ namespace pcl std::vector &k_indices, std::vector &k_sqr_distances) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (nearestKSearch (p, k, k_indices, k_sqr_distances)); } @@ -271,11 +268,7 @@ namespace pcl std::vector &k_sqr_distances, unsigned int max_nn = 0) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); } diff --git a/kdtree/include/pcl/kdtree/kdtree_flann.h b/kdtree/include/pcl/kdtree/kdtree_flann.h index 35ad9a6d..3cf5d9da 100644 --- a/kdtree/include/pcl/kdtree/kdtree_flann.h +++ b/kdtree/include/pcl/kdtree/kdtree_flann.h @@ -44,6 +44,8 @@ #include #include +#include + // Forward declarations namespace flann { @@ -95,12 +97,12 @@ namespace pcl KdTreeFLANN (bool sorted = true); /** \brief Copy constructor - * \param[in] tree the tree to copy into this + * \param[in] k the tree to copy into this */ KdTreeFLANN (const KdTreeFLANN &k); /** \brief Copy operator - * \param[in] tree the tree to copy into this + * \param[in] k the tree to copy into this */ inline KdTreeFLANN& operator = (const KdTreeFLANN& k) @@ -210,7 +212,7 @@ namespace pcl boost::shared_ptr flann_index_; /** \brief Internal pointer to data. */ - float* cloud_; + boost::shared_array cloud_; /** \brief mapping between internal and external indices. */ std::vector index_mapping_; diff --git a/kdtree/kdtree.doxy b/kdtree/kdtree.doxy index 5f5fc8e6..aae221d3 100644 --- a/kdtree/kdtree.doxy +++ b/kdtree/kdtree.doxy @@ -4,7 +4,7 @@ \section secKDtreePresentation Overview The pcl_kdtree library provides the kd-tree data-structure, using - FLANN, + nearest neighbor searches. A Kd-tree (k-dimensional tree) is a space-partitioning data diff --git a/keypoints/CMakeLists.txt b/keypoints/CMakeLists.txt index 69ba7e2a..e043b227 100644 --- a/keypoints/CMakeLists.txt +++ b/keypoints/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud keypoints library") set(SUBSYS_DEPS common search kdtree octree features filters) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -21,37 +21,37 @@ if(build) src/iss_3d.cpp ) set(incs - include/pcl/${SUBSYS_NAME}/keypoint.h - include/pcl/${SUBSYS_NAME}/narf_keypoint.h - include/pcl/${SUBSYS_NAME}/sift_keypoint.h - include/pcl/${SUBSYS_NAME}/uniform_sampling.h - include/pcl/${SUBSYS_NAME}/smoothed_surfaces_keypoint.h - include/pcl/${SUBSYS_NAME}/agast_2d.h - include/pcl/${SUBSYS_NAME}/harris_3d.h - include/pcl/${SUBSYS_NAME}/harris_6d.h - include/pcl/${SUBSYS_NAME}/susan.h - include/pcl/${SUBSYS_NAME}/iss_3d.h + "include/pcl/${SUBSYS_NAME}/keypoint.h" + "include/pcl/${SUBSYS_NAME}/narf_keypoint.h" + "include/pcl/${SUBSYS_NAME}/sift_keypoint.h" + "include/pcl/${SUBSYS_NAME}/uniform_sampling.h" + "include/pcl/${SUBSYS_NAME}/smoothed_surfaces_keypoint.h" + "include/pcl/${SUBSYS_NAME}/agast_2d.h" + "include/pcl/${SUBSYS_NAME}/harris_3d.h" + "include/pcl/${SUBSYS_NAME}/harris_6d.h" + "include/pcl/${SUBSYS_NAME}/susan.h" + "include/pcl/${SUBSYS_NAME}/iss_3d.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/keypoint.hpp - include/pcl/${SUBSYS_NAME}/impl/sift_keypoint.hpp - include/pcl/${SUBSYS_NAME}/impl/uniform_sampling.hpp - include/pcl/${SUBSYS_NAME}/impl/smoothed_surfaces_keypoint.hpp - include/pcl/${SUBSYS_NAME}/impl/agast_2d.hpp - include/pcl/${SUBSYS_NAME}/impl/harris_3d.hpp - include/pcl/${SUBSYS_NAME}/impl/harris_6d.hpp - include/pcl/${SUBSYS_NAME}/impl/susan.hpp - include/pcl/${SUBSYS_NAME}/impl/iss_3d.hpp + "include/pcl/${SUBSYS_NAME}/impl/keypoint.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sift_keypoint.hpp" + "include/pcl/${SUBSYS_NAME}/impl/uniform_sampling.hpp" + "include/pcl/${SUBSYS_NAME}/impl/smoothed_surfaces_keypoint.hpp" + "include/pcl/${SUBSYS_NAME}/impl/agast_2d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/harris_3d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/harris_6d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/susan.hpp" + "include/pcl/${SUBSYS_NAME}/impl/iss_3d.hpp" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_features pcl_filters) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_features pcl_filters) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/keypoints/include/pcl/keypoints/agast_2d.h b/keypoints/include/pcl/keypoints/agast_2d.h index 050f8e8a..051fc566 100644 --- a/keypoints/include/pcl/keypoints/agast_2d.h +++ b/keypoints/include/pcl/keypoints/agast_2d.h @@ -129,7 +129,6 @@ namespace pcl /** \brief Computes corner score. * \param[in] im the pixels to compute the score at - * \param[in] bmax */ virtual int computeCornerScore (const float* im) const = 0; @@ -177,7 +176,6 @@ namespace pcl /** \brief Detects points of interest (i.e., keypoints) in the given image * \param[in] im the image to detect keypoints in - * \param[out] corners_all the resultant set of keypoints detected */ virtual void detect (const float* im, @@ -196,8 +194,8 @@ namespace pcl struct CompareScoreIndex { /** \brief Comparator - * \param[i1] the first score index - * \param[i2] the second score index + * \param[in] i1 the first score index + * \param[in] i2 the second score index */ inline bool operator() (const ScoreIndex &i1, const ScoreIndex &i2) diff --git a/keypoints/include/pcl/keypoints/harris_2d.h b/keypoints/include/pcl/keypoints/harris_2d.h index 6890709c..cfe08a55 100644 --- a/keypoints/include/pcl/keypoints/harris_2d.h +++ b/keypoints/include/pcl/keypoints/harris_2d.h @@ -65,11 +65,15 @@ namespace pcl using Keypoint::name_; using Keypoint::input_; using Keypoint::indices_; + using Keypoint::keypoints_indices_; typedef enum {HARRIS = 1, NOBLE, LOWE, TOMASI} ResponseMethod; /** \brief Constructor * \param[in] method the method to be used to determine the corner responses + * \param window_width + * \param window_height + * \param min_distance * \param[in] threshold the threshold to filter out weak corners */ HarrisKeypoint2D (ResponseMethod method = HARRIS, int window_width = 3, int window_height = 3, int min_distance = 5, float threshold = 0.0) @@ -136,13 +140,13 @@ namespace pcl detectKeypoints (PointCloudOut &output); /** \brief gets the corner response for valid input points*/ void - responseHarris (PointCloudOut &output, float& highest_response) const; + responseHarris (PointCloudOut &output) const; void - responseNoble (PointCloudOut &output, float& highest_response) const; + responseNoble (PointCloudOut &output) const; void - responseLowe (PointCloudOut &output, float& highest_response) const; + responseLowe (PointCloudOut &output) const; void - responseTomasi (PointCloudOut &output, float& highest_response) const; + responseTomasi (PointCloudOut &output) const; // void refineCorners (PointCloudOut &corners) const; /** \brief calculates the upper triangular part of unnormalized * covariance matrix over intensities given by the 2D coordinates diff --git a/keypoints/include/pcl/keypoints/harris_3d.h b/keypoints/include/pcl/keypoints/harris_3d.h index a2bb381d..b0371557 100644 --- a/keypoints/include/pcl/keypoints/harris_3d.h +++ b/keypoints/include/pcl/keypoints/harris_3d.h @@ -72,7 +72,9 @@ namespace pcl using Keypoint::k_; using Keypoint::search_radius_; using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; using Keypoint::initCompute; + using PCLBase::setInputCloud; typedef enum {HARRIS = 1, NOBLE, LOWE, TOMASI, CURVATURE} ResponseMethod; @@ -95,6 +97,12 @@ namespace pcl /** \brief Empty destructor */ virtual ~HarrisKeypoint3D () {} + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + virtual void + setInputCloud (const PointCloudInConstPtr &cloud); + /** \brief Set the method of the response to be calculated. * \param[in] type */ diff --git a/keypoints/include/pcl/keypoints/harris_6d.h b/keypoints/include/pcl/keypoints/harris_6d.h index 70b4d21a..37c0e729 100644 --- a/keypoints/include/pcl/keypoints/harris_6d.h +++ b/keypoints/include/pcl/keypoints/harris_6d.h @@ -66,10 +66,10 @@ namespace pcl using Keypoint::k_; using Keypoint::search_radius_; using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; /** * @brief Constructor - * @param method the method to be used to determine the corner responses * @param radius the radius for normal estimation as well as for non maxima suppression * @param threshold the threshold to filter out weak corners */ diff --git a/keypoints/include/pcl/keypoints/impl/harris_2d.hpp b/keypoints/include/pcl/keypoints/impl/harris_2d.hpp index 5269821b..bffd93d2 100644 --- a/keypoints/include/pcl/keypoints/impl/harris_2d.hpp +++ b/keypoints/include/pcl/keypoints/impl/harris_2d.hpp @@ -105,7 +105,6 @@ pcl::HarrisKeypoint2D::computeSecondMomentMatri int x = static_cast (index % input_->width); int y = static_cast (index / input_->width); - unsigned count = 0; // indices 0 1 2 // coefficients: ixix ixiy iyiy memset (coefficients, 0, sizeof (float) * 3); @@ -184,10 +183,6 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5; derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5; -// #ifdef _OPENMP -// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) -// #pragma omp parallel for num_threads (threads_) -// #endif for(int i = 1; i < w; ++i) { derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5; @@ -196,10 +191,6 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5; derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5; -// #ifdef _OPENMP -// //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_) -// #pragma omp parallel for num_threads (threads_) -// #endif for(int j = 1; j < h; ++j) { // i = 0 --> i-1 out of range ; use 0 @@ -220,10 +211,6 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5; derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5; -// #ifdef _OPENMP -// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) -// #pragma omp parallel for num_threads (threads_) -// #endif for(int i = 1; i < w; ++i) { derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5; @@ -231,33 +218,33 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5; derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5; - float highest_response_; - switch (method_) { case HARRIS: - responseHarris(*response_, highest_response_); + responseHarris(*response_); break; case NOBLE: - responseNoble(*response_, highest_response_); + responseNoble(*response_); break; case LOWE: - responseLowe(*response_, highest_response_); + responseLowe(*response_); break; case TOMASI: - responseTomasi(*response_, highest_response_); + responseTomasi(*response_); break; } if (!nonmax_) + { output = *response_; + for (size_t i = 0; i < response_->size (); ++i) + keypoints_indices_->indices.push_back (i); + } else { - threshold_*= highest_response_; - std::sort (indices_->begin (), indices_->end (), boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2)); - + float threshold = threshold_ * response_->points[indices_->front ()].intensity; output.clear (); output.reserve (response_->size()); std::vector occupency_map (response_->size (), false); @@ -268,20 +255,25 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl #ifdef _OPENMP #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_) #endif - for (int idx = 0; idx < occupency_map_size; ++idx) + for (int i = 0; i < occupency_map_size; ++i) { - if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx])) + int idx = indices_->at (i); + const PointOutT& point_out = response_->points [idx]; + if (occupency_map[idx] || point_out.intensity < threshold || !isFinite (point_out)) continue; #ifdef _OPENMP #pragma omp critical #endif - output.push_back (response_->at (indices_->at (idx))); + { + output.push_back (point_out); + keypoints_indices_->indices.push_back (idx); + } - int u_end = std::min (width, indices_->at (idx) % width + min_distance_); - int v_end = std::min (height, indices_->at (idx) / width + min_distance_); - for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u) - for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v) + int u_end = std::min (width, idx % width + min_distance_); + int v_end = std::min (height, idx / width + min_distance_); + for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u) + for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v) occupency_map[v*input_->width+u] = true; } @@ -298,16 +290,15 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::HarrisKeypoint2D::responseHarris (PointCloudOut &output, float& highest_response) const +pcl::HarrisKeypoint2D::responseHarris (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); - highest_response = - std::numeric_limits::max (); const int output_size (output.size ()); #ifdef _OPENMP -#pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) #endif for (int index = 0; index < output_size; ++index) { @@ -325,11 +316,6 @@ pcl::HarrisKeypoint2D::responseHarris (PointClo { float det = covar[0] * covar[2] - covar[1] * covar[1]; out_point.intensity = 0.04f + det - 0.04f * trace * trace; - -#ifdef _OPENMP -#pragma omp critical -#endif - highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; } } } @@ -340,18 +326,17 @@ pcl::HarrisKeypoint2D::responseHarris (PointClo ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::HarrisKeypoint2D::responseNoble (PointCloudOut &output, float& highest_response) const +pcl::HarrisKeypoint2D::responseNoble (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); - highest_response = - std::numeric_limits::max (); const int output_size (output.size ()); #ifdef _OPENMP -#pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) #endif - for (size_t index = 0; index < output_size; ++index) + for (int index = 0; index < output_size; ++index) { PointOutT &out_point = output.points [index]; const PointInT &in_point = input_->points [index]; @@ -367,11 +352,6 @@ pcl::HarrisKeypoint2D::responseNoble (PointClou { float det = covar[0] * covar[2] - covar[1] * covar[1]; out_point.intensity = det / trace; - -#ifdef _OPENMP -#pragma omp critical -#endif - highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; } } } @@ -382,18 +362,17 @@ pcl::HarrisKeypoint2D::responseNoble (PointClou ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::HarrisKeypoint2D::responseLowe (PointCloudOut &output, float& highest_response) const +pcl::HarrisKeypoint2D::responseLowe (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); - highest_response = -std::numeric_limits::max (); const int output_size (output.size ()); #ifdef _OPENMP -#pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) #endif - for (size_t index = 0; index < output_size; ++index) + for (int index = 0; index < output_size; ++index) { PointOutT &out_point = output.points [index]; const PointInT &in_point = input_->points [index]; @@ -409,11 +388,6 @@ pcl::HarrisKeypoint2D::responseLowe (PointCloud { float det = covar[0] * covar[2] - covar[1] * covar[1]; out_point.intensity = det / (trace * trace); - -#ifdef _OPENMP -#pragma omp critical -#endif - highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; } } } @@ -424,18 +398,17 @@ pcl::HarrisKeypoint2D::responseLowe (PointCloud ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::HarrisKeypoint2D::responseTomasi (PointCloudOut &output, float& highest_response) const +pcl::HarrisKeypoint2D::responseTomasi (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); - highest_response = -std::numeric_limits::max (); const int output_size (output.size ()); #ifdef _OPENMP -#pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) #endif - for (size_t index = 0; index < output_size; ++index) + for (int index = 0; index < output_size; ++index) { PointOutT &out_point = output.points [index]; const PointInT &in_point = input_->points [index]; @@ -448,11 +421,6 @@ pcl::HarrisKeypoint2D::responseTomasi (PointClo computeSecondMomentMatrix (index, covar); // min egenvalue out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f); - -#ifdef _OPENMP -#pragma omp critical -#endif - highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; } } diff --git a/keypoints/include/pcl/keypoints/impl/harris_3d.hpp b/keypoints/include/pcl/keypoints/impl/harris_3d.hpp index 895f392c..82e2c483 100644 --- a/keypoints/include/pcl/keypoints/impl/harris_3d.hpp +++ b/keypoints/include/pcl/keypoints/impl/harris_3d.hpp @@ -50,6 +50,15 @@ #include #endif +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setInputCloud (const PointCloudInConstPtr &cloud) +{ + if (normals_ && input_ && (cloud != input_)) + normals_.reset (); + input_ = cloud; +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::HarrisKeypoint3D::setMethod (ResponseMethod method) @@ -223,6 +232,7 @@ pcl::HarrisKeypoint3D::initCompute () PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_); return (false); } + return (true); } @@ -258,6 +268,8 @@ pcl::HarrisKeypoint3D::detectKeypoints (PointCloud output = *response; // we do not change the denseness in this case output.is_dense = input_->is_dense; + for (size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); } else { @@ -290,7 +302,10 @@ pcl::HarrisKeypoint3D::detectKeypoints (PointCloud #ifdef _OPENMP #pragma omp critical #endif + { output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } } if (refine_) diff --git a/keypoints/include/pcl/keypoints/impl/harris_6d.hpp b/keypoints/include/pcl/keypoints/impl/harris_6d.hpp index e24d241a..fe978516 100644 --- a/keypoints/include/pcl/keypoints/impl/harris_6d.hpp +++ b/keypoints/include/pcl/keypoints/impl/harris_6d.hpp @@ -219,6 +219,8 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud output = *response; // we do not change the denseness in this case output.is_dense = input_->is_dense; + for (size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); } else { @@ -249,7 +251,10 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud #ifdef _OPENMP #pragma omp critical #endif + { output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } } if (refine_) @@ -259,8 +264,6 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud output.width = static_cast (output.points.size()); output.is_dense = true; } - - } template void diff --git a/keypoints/include/pcl/keypoints/impl/iss_3d.hpp b/keypoints/include/pcl/keypoints/impl/iss_3d.hpp index be2e4e9b..d1e86f51 100644 --- a/keypoints/include/pcl/keypoints/impl/iss_3d.hpp +++ b/keypoints/include/pcl/keypoints/impl/iss_3d.hpp @@ -173,7 +173,7 @@ pcl::ISSKeypoint3D::getScatterMatrix (const int& c double cov[9]; memset(cov, 0, sizeof(double) * 9); - for (size_t n_idx = 0; n_idx < n_neighbors; n_idx++) + for (int n_idx = 0; n_idx < n_neighbors; n_idx++) { const PointInT& n_point = (*input_).points[nn_indices[n_idx]]; @@ -316,7 +316,7 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut this->searchForNeighbors (static_cast (index), border_radius_, nn_indices, nn_distances); - for (int j = 0 ; j < nn_indices.size (); j++) + for (size_t j = 0 ; j < nn_indices.size (); j++) { if (edge_points_[nn_indices[j]]) { @@ -329,13 +329,13 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_]; - for (int i = 0; i < threads_; i++) + for (size_t i = 0; i < threads_; i++) omp_mem[i].setZero (3); double *prg_local_mem = new double[input_->size () * 3]; double **prg_mem = new double * [input_->size ()]; - for (int i = 0; i < input_->size (); i++) + for (size_t i = 0; i < input_->size (); i++) prg_mem[i] = prg_local_mem + 3 * i; #ifdef _OPENMP @@ -415,7 +415,7 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut { is_max = true; - for (size_t j = 0 ; j < n_neighbors; j++) + for (int j = 0 ; j < n_neighbors; j++) if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]]) is_max = false; if (is_max) @@ -433,7 +433,12 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut #ifdef _OPENMP #pragma omp critical #endif - output.points.push_back(input_->points[index]); + { + PointOutT p; + p.getVector3fMap () = input_->points[index].getVector3fMap (); + output.points.push_back(p); + keypoints_indices_->indices.push_back (index); + } } output.header = input_->header; diff --git a/keypoints/include/pcl/keypoints/impl/keypoint.hpp b/keypoints/include/pcl/keypoints/impl/keypoint.hpp index 03871b0c..4b84f085 100644 --- a/keypoints/include/pcl/keypoints/impl/keypoint.hpp +++ b/keypoints/include/pcl/keypoints/impl/keypoint.hpp @@ -113,6 +113,9 @@ pcl::Keypoint::initCompute () } } + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + return (true); } diff --git a/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp b/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp index d6589e19..e3eab6c0 100644 --- a/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp +++ b/keypoints/include/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp @@ -147,7 +147,10 @@ pcl::SmoothedSurfacesKeypoint::detectKeypoints (PointCloudT &ou // check if point was minimum/maximum over all the scales if (passed_min || passed_max) + { output.points.push_back (input_->points[point_i]); + keypoints_indices_->indices.push_back (point_i); + } } } @@ -238,7 +241,7 @@ pcl::SmoothedSurfacesKeypoint::initCompute () for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first); PCL_INFO ("\n"); - return true; + return (true); } diff --git a/keypoints/include/pcl/keypoints/impl/susan.hpp b/keypoints/include/pcl/keypoints/impl/susan.hpp index 8a9323f0..7b989238 100644 --- a/keypoints/include/pcl/keypoints/impl/susan.hpp +++ b/keypoints/include/pcl/keypoints/impl/susan.hpp @@ -246,6 +246,7 @@ pcl::SUSANKeypoint::initCompute () PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); return (false); } + return (true); } @@ -413,7 +414,13 @@ pcl::SUSANKeypoint::detectKeypoints (P response->width = static_cast (response->size ()); if (!nonmax_) + { output = *response; + for (size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); + // we don not change the denseness + output.is_dense = input_->is_dense; + } else { output.points.clear (); @@ -447,14 +454,16 @@ pcl::SUSANKeypoint::detectKeypoints (P //#ifdef _OPENMP //#pragma omp critical //#endif + { output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } } output.height = 1; - output.width = static_cast (output.points.size()); + output.width = static_cast (output.points.size()); + output.is_dense = true; } - // we don not change the denseness - output.is_dense = input_->is_dense; } #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint; diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp new file mode 100644 index 00000000..7f58df3f --- /dev/null +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp @@ -0,0 +1,257 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ +#define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ + +template bool +pcl::TrajkovicKeypoint2D::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return (false); + } + + if (indices_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); + return (false); + } + + if ((window_size_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_size_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TrajkovicKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + int w = static_cast (input_->width) - half_window_size_; + int h = static_cast (input_->height) - half_window_size_; + + if (method_ == pcl::TrajkovicKeypoint2D::FOUR_CORNERS) + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + float center = intensity_ ((*input_) (i,j)); + float up = intensity_ ((*input_) (i, j-half_window_size_)); + float down = intensity_ ((*input_) (i, j+half_window_size_)); + float left = intensity_ ((*input_) (i-half_window_size_, j)); + float right = intensity_ ((*input_) (i+half_window_size_, j)); + + float up_center = up - center; + float r1 = up_center * up_center; + float down_center = down - center; + r1+= down_center * down_center; + + float right_center = right - center; + float r2 = right_center * right_center; + float left_center = left - center; + r2+= left_center * left_center; + + float d = std::min (r1, r2); + + if (d < first_threshold_) + continue; + + float b1 = (right - up) * up_center; + b1+= (left - down) * down_center; + float b2 = (right - down) * down_center; + b2+= (left - up) * up_center; + float B = std::min (b1, b2); + float A = r2 - r1 - 2*B; + + (*response_) (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; + } + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + float center = intensity_ ((*input_) (i,j)); + float up = intensity_ ((*input_) (i, j-half_window_size_)); + float down = intensity_ ((*input_) (i, j+half_window_size_)); + float left = intensity_ ((*input_) (i-half_window_size_, j)); + float right = intensity_ ((*input_) (i+half_window_size_, j)); + float upleft = intensity_ ((*input_) (i-half_window_size_, j-half_window_size_)); + float upright = intensity_ ((*input_) (i+half_window_size_, j-half_window_size_)); + float downleft = intensity_ ((*input_) (i-half_window_size_, j+half_window_size_)); + float downright = intensity_ ((*input_) (i+half_window_size_, j+half_window_size_)); + std::vector r (4,0); + + float up_center = up - center; + r[0] = up_center * up_center; + float down_center = down - center; + r[0]+= down_center * down_center; + + float upright_center = upright - center; + r[1] = upright_center * upright_center; + float downleft_center = downleft - center; + r[1]+= downleft_center * downleft_center; + + float right_center = right - center; + r[2] = right_center * right_center; + float left_center = left - center; + r[2]+= left_center * left_center; + + float downright_center = downright - center; + r[3] = downright_center * downright_center; + float upleft_center = upleft - center; + r[3]+= upleft_center * upleft_center; + + float d = *(std::min_element (r.begin (), r.end ())); + + if (d < first_threshold_) + continue; + + std::vector B (4,0); + std::vector A (4,0); + std::vector sumAB (4,0); + B[0] = (upright - up) * up_center; + B[0]+= (downleft - down) * down_center; + B[1] = (right - upright) * upright_center; + B[1]+= (left - downleft) * downleft_center; + B[2] = (downright - right) * downright_center; + B[2]+= (upleft - left) * upleft_center; + B[3] = (down - downright) * downright_center; + B[3]+= (up - upleft) * upleft_center; + A[0] = r[1] - r[0] - B[0] - B[0]; + A[1] = r[2] - r[1] - B[1] - B[1]; + A[2] = r[3] - r[2] - B[2] - B[2]; + A[3] = r[0] - r[3] - B[3] - B[3]; + sumAB[0] = A[0] + B[0]; + sumAB[1] = A[1] + B[1]; + sumAB[2] = A[2] + B[2]; + sumAB[3] = A[3] + B[3]; + if ((*std::max_element (B.begin (), B.end ()) < 0) && + (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) + { + std::vector D (4,0); + D[0] = B[0] * B[0] / A[0]; + D[1] = B[1] * B[1] / A[1]; + D[2] = B[2] * B[2] / A[2]; + D[3] = B[3] * B[3] / A[3]; + (*response_) (i,j) = *(std::min (D.begin (), D.end ())); + } + else + (*response_) (i,j) = d; + } + } + } + + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), + boost::bind (&TrajkovicKeypoint2D::greaterCornernessAtIndices, this, _1, _2)); + + output.clear (); + output.reserve (input_->size ()); + + std::vector occupency_map (indices.size (), false); + const int width (input_->width); + const int height (input_->height); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (int i = 0; i < indices.size (); ++i) + { + int idx = indices[i]; + if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) + continue; + + PointOutT p; + p.getVector3fMap () = input_->points[idx].getVector3fMap (); + p.intensity = response_->points [idx]; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (p); + keypoints_indices_->indices.push_back (idx); + } + + const int x = idx % width; + const int y = idx / width; + const int u_end = std::min (width, x + half_window_size_); + const int v_end = std::min (height, y + half_window_size_); + for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) + for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) + occupency_map[v*width + u] = true; + } + + output.height = 1; + output.width = static_cast (output.size()); + // we don not change the denseness + output.is_dense = input_->is_dense; +} + +#define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D; +#endif diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp new file mode 100644 index 00000000..a191a99a --- /dev/null +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp @@ -0,0 +1,271 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRAJKOVIC_KEYPOINT_3D_IMPL_H_ +#define PCL_TRAJKOVIC_KEYPOINT_3D_IMPL_H_ + +#include + +template bool +pcl::TrajkovicKeypoint3D::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + if (indices_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); + return (false); + } + + if ((window_size_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_size_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + if (!normals_) + { + NormalsPtr normals (new Normals ()); + pcl::IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (input_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals); + normals_ = normals; + } + + if (normals_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); + return (false); + } + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TrajkovicKeypoint3D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + const Normals &normals = *normals_; + const PointCloudIn &input = *input_; + pcl::PointCloud& response = *response_; + const int w = static_cast (input_->width) - half_window_size_; + const int h = static_cast (input_->height) - half_window_size_; + + if (method_ == FOUR_CORNERS) + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + if (!isFinite (input (i,j))) continue; + const NormalT ¢er = normals (i,j); + if (!isFinite (center)) continue; + + int count = 0; + const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); + const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); + const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); + const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); + // Get rid of isolated points + if (!count) continue; + + float sn1 = squaredNormalsDiff (up, center); + float sn2 = squaredNormalsDiff (down, center); + float r1 = sn1 + sn2; + float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center); + + float d = std::min (r1, r2); + if (d < first_threshold_) continue; + + sn1 = sqrt (sn1); + sn2 = sqrt (sn2); + float b1 = normalsDiff (right, up) * sn1; + b1+= normalsDiff (left, down) * sn2; + float b2 = normalsDiff (right, down) * sn2; + b2+= normalsDiff (left, up) * sn1; + float B = std::min (b1, b2); + float A = r2 - r1 - 2*B; + + response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; + } + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + if (!isFinite (input (i,j))) continue; + const NormalT ¢er = normals (i,j); + if (!isFinite (center)) continue; + + int count = 0; + const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); + const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); + const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); + const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); + const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count); + const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count); + const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count); + const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count); + // Get rid of isolated points + if (!count) continue; + + std::vector r (4,0); + + r[0] = squaredNormalsDiff (up, center); + r[0]+= squaredNormalsDiff (down, center); + + r[1] = squaredNormalsDiff (upright, center); + r[1]+= squaredNormalsDiff (downleft, center); + + r[2] = squaredNormalsDiff (right, center); + r[2]+= squaredNormalsDiff (left, center); + + r[3] = squaredNormalsDiff (downright, center); + r[3]+= squaredNormalsDiff (upleft, center); + + float d = *(std::min_element (r.begin (), r.end ())); + + if (d < first_threshold_) continue; + + std::vector B (4,0); + std::vector A (4,0); + std::vector sumAB (4,0); + B[0] = normalsDiff (upright, up) * normalsDiff (up, center); + B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center); + B[1] = normalsDiff (right, upright) * normalsDiff (upright, center); + B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center); + B[2] = normalsDiff (downright, right) * normalsDiff (downright, center); + B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center); + B[3] = normalsDiff (down, downright) * normalsDiff (downright, center); + B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center); + A[0] = r[1] - r[0] - B[0] - B[0]; + A[1] = r[2] - r[1] - B[1] - B[1]; + A[2] = r[3] - r[2] - B[2] - B[2]; + A[3] = r[0] - r[3] - B[3] - B[3]; + sumAB[0] = A[0] + B[0]; + sumAB[1] = A[1] + B[1]; + sumAB[2] = A[2] + B[2]; + sumAB[3] = A[3] + B[3]; + if ((*std::max_element (B.begin (), B.end ()) < 0) && + (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) + { + std::vector D (4,0); + D[0] = B[0] * B[0] / A[0]; + D[1] = B[1] * B[1] / A[1]; + D[2] = B[2] * B[2] / A[2]; + D[3] = B[3] * B[3] / A[3]; + response (i,j) = *(std::min (D.begin (), D.end ())); + } + else + response (i,j) = d; + } + } + } + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), + boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2)); + + output.clear (); + output.reserve (input_->size ()); + + std::vector occupency_map (indices.size (), false); + const int width (input_->width); + const int height (input_->height); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (int i = 0; i < indices.size (); ++i) + { + int idx = indices[i]; + if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) + continue; + + PointOutT p; + p.getVector3fMap () = input_->points[idx].getVector3fMap (); + p.intensity = response_->points [idx]; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (p); + keypoints_indices_->indices.push_back (idx); + } + + const int x = idx % width; + const int y = idx / width; + const int u_end = std::min (width, x + half_window_size_); + const int v_end = std::min (height, y + half_window_size_); + for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) + for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) + occupency_map[v*width + u] = true; + } + + output.height = 1; + output.width = static_cast (output.size()); + // we don not change the denseness + output.is_dense = true; +} + +#define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D; +#endif diff --git a/keypoints/include/pcl/keypoints/iss_3d.h b/keypoints/include/pcl/keypoints/iss_3d.h index f318d82f..de2b8d8c 100644 --- a/keypoints/include/pcl/keypoints/iss_3d.h +++ b/keypoints/include/pcl/keypoints/iss_3d.h @@ -104,6 +104,7 @@ namespace pcl using Keypoint::tree_; using Keypoint::search_radius_; using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; /** \brief Constructor. * \param[in] salient_radius the radius of the spherical neighborhood used to compute the scatter matrix. @@ -140,7 +141,7 @@ namespace pcl /** \brief Set the radius used for the estimation of the surface normals of the input cloud. If the radius is * too large, the temporal performances of the detector may degrade significantly. - * \param[in] normals_radius the radius used to estimate surface normals + * \param[in] normal_radius the radius used to estimate surface normals */ void setNormalRadius (double normal_radius); @@ -197,14 +198,14 @@ namespace pcl /** \brief Compute the boundary points for the given input cloud. * \param[in] input the input cloud * \param[in] border_radius the radius used to compute the boundary points - * \param[in] the decision boundary that marks the points as boundary + * \param[in] angle_threshold the decision boundary that marks the points as boundary * \return the vector of boolean values in which the information about the boundary points is stored */ bool* getBoundaryPoints (PointCloudIn &input, double border_radius, float angle_threshold); /** \brief Compute the scatter matrix for a point index. - * \param[in] index the index of the point + * \param[in] current_index the index of the point * \param[out] cov_m the point scatter matrix */ void diff --git a/keypoints/include/pcl/keypoints/keypoint.h b/keypoints/include/pcl/keypoints/keypoint.h index afbe8639..52fdb281 100644 --- a/keypoints/include/pcl/keypoints/keypoint.h +++ b/keypoints/include/pcl/keypoints/keypoint.h @@ -133,6 +133,12 @@ namespace pcl inline double getRadiusSearch () { return (search_radius_); } + /** \brief \return the keypoints indices in the input cloud. + * \note not all the daughter classes populate the keypoints indices so check emptiness before use. + */ + pcl::PointIndicesConstPtr + getKeypointsIndices () { return (keypoints_indices_); } + /** \brief Base method for key point detection for all points given in using * the surface in setSearchSurface () and the spatial locator in setSearchMethod () * \param output the resultant point cloud model dataset containing the estimated features @@ -187,6 +193,9 @@ namespace pcl /** \brief The number of K nearest neighbors to use for each point. */ int k_; + /** \brief Indices of the keypoints in the input cloud. */ + pcl::PointIndicesPtr keypoints_indices_; + /** \brief Get a string representation of the name of this class. */ inline const std::string& getClassName () const { return (name_); } diff --git a/keypoints/include/pcl/keypoints/smoothed_surfaces_keypoint.h b/keypoints/include/pcl/keypoints/smoothed_surfaces_keypoint.h index dad13f7c..489e9bd7 100644 --- a/keypoints/include/pcl/keypoints/smoothed_surfaces_keypoint.h +++ b/keypoints/include/pcl/keypoints/smoothed_surfaces_keypoint.h @@ -61,6 +61,7 @@ namespace pcl using PCLBase::input_; using Keypoint::name_; using Keypoint::tree_; + using Keypoint::keypoints_indices_; using Keypoint::initCompute; typedef pcl::PointCloud PointCloudT; diff --git a/keypoints/include/pcl/keypoints/susan.h b/keypoints/include/pcl/keypoints/susan.h index a4c1937c..1613f471 100644 --- a/keypoints/include/pcl/keypoints/susan.h +++ b/keypoints/include/pcl/keypoints/susan.h @@ -77,6 +77,7 @@ namespace pcl using Keypoint::k_; using Keypoint::search_radius_; using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; using Keypoint::initCompute; /** \brief Constructor @@ -166,8 +167,8 @@ namespace pcl detectKeypoints (PointCloudOut &output); /** \brief return true if a point lies within the line between the nucleus and the centroid * \param[in] nucleus coordinate of the nucleus - * \param[in] centroid of the USAN - * \parma[in] nucleus to centroid vector (used to speed up since it is constant for a given + * \param[in] centroid of the SUSAN + * \param[in] nc to centroid vector (used to speed up since it is constant for a given * neighborhood) * \param[in] point the query point to test against * \return true if the point lies within [nucleus centroid] diff --git a/keypoints/include/pcl/keypoints/trajkovic_2d.h b/keypoints/include/pcl/keypoints/trajkovic_2d.h new file mode 100644 index 00000000..7458f96b --- /dev/null +++ b/keypoints/include/pcl/keypoints/trajkovic_2d.h @@ -0,0 +1,176 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRAJKOVIC_KEYPOINT_2D_H_ +#define PCL_TRAJKOVIC_KEYPOINT_2D_H_ + +#include +#include + +namespace pcl +{ + /** \brief TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on + * organized pooint cloud using intensity information. + * It uses first order statistics to find variation of intensities in horizontal + * or vertical directions. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template > + class TrajkovicKeypoint2D : public Keypoint + { + public: + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + typedef typename Keypoint::PointCloudIn PointCloudIn; + typedef typename Keypoint::PointCloudOut PointCloudOut; + typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::keypoints_indices_; + + typedef enum { FOUR_CORNERS, EIGHT_CORNERS } ComputationMethod; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] window_size + * \param[in] first_threshold the threshold used in the simple cornerness test. + * \param[in] second_threshold the threshold used to reject weak corners. + */ + TrajkovicKeypoint2D (ComputationMethod method = FOUR_CORNERS, + int window_size = 3, + float first_threshold = 0.1, + float second_threshold = 100.0) + : method_ (method) + , window_size_ (window_size) + , first_threshold_ (first_threshold) + , second_threshold_ (second_threshold) + , threads_ (1) + { + name_ = "TrajkovicKeypoint2D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] method either 4 corners or 8 corners + */ + inline void + setMethod (ComputationMethod method) { method_ = method; } + + /// \brief \return the computation method + inline ComputationMethod + getMethod () const { return (method_); } + + /// \brief Set window size + inline void + setWindowSize (int window_size) { window_size_= window_size; } + + /// \brief \return window size i.e. window width or height + inline int + getWindowSize () const { return (window_size_); } + + /** \brief set the first_threshold to reject corners in the simple cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setFirstThreshold (float threshold) { first_threshold_= threshold; } + + /// \brief \return first threshold + inline float + getFirstThreshold () const { return (first_threshold_); } + + /** \brief set the second threshold to reject corners in the final cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setSecondThreshold (float threshold) { second_threshold_= threshold; } + + /// \brief \return second threshold + inline float + getSecondThreshold () const { return (second_threshold_); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use, 0 for automatic. + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /// \brief \return the number of threads + inline unsigned int + getNumberOfThreads () const { return (threads_); } + + protected: + bool + initCompute (); + + void + detectKeypoints (PointCloudOut &output); + + private: + /// comparator for responses intensity + inline bool + greaterCornernessAtIndices (int a, int b) const + { + return (response_->points [a] > response_->points [b]); + } + + /// computation method + ComputationMethod method_; + /// Window size + int window_size_; + /// half window size + int half_window_size_; + /// intensity field accessor + IntensityT intensity_; + /// first threshold for quick rejection + float first_threshold_; + /// second threshold for corner evaluation + float second_threshold_; + /// number of threads to be used + unsigned int threads_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include + +#endif // #ifndef PCL_TRAJKOVIC_KEYPOINT_2D_H_ diff --git a/keypoints/include/pcl/keypoints/trajkovic_3d.h b/keypoints/include/pcl/keypoints/trajkovic_3d.h new file mode 100644 index 00000000..31ecbfa3 --- /dev/null +++ b/keypoints/include/pcl/keypoints/trajkovic_3d.h @@ -0,0 +1,218 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRAJKOVIC_KEYPOINT_3D_H_ +#define PCL_TRAJKOVIC_KEYPOINT_3D_H_ + +#include +#include + +namespace pcl +{ + /** \brief TrajkovicKeypoint3D implements Trajkovic and Hedley corner detector on + * point cloud using geometric information. + * It uses first order statistics to find variation of normals. + * This work is part of Nizar Sallem PhD thesis. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template + class TrajkovicKeypoint3D : public Keypoint + { + public: + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + typedef typename Keypoint::PointCloudIn PointCloudIn; + typedef typename Keypoint::PointCloudOut PointCloudOut; + typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef typename pcl::PointCloud Normals; + typedef typename Normals::Ptr NormalsPtr; + typedef typename Normals::ConstPtr NormalsConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::keypoints_indices_; + using Keypoint::initCompute; + + typedef enum { FOUR_CORNERS, EIGHT_CORNERS } ComputationMethod; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] window_size + * \param[in] first_threshold the threshold used in the simple cornerness test. + * \param[in] second_threshold the threshold used to reject weak corners. + */ + TrajkovicKeypoint3D (ComputationMethod method = FOUR_CORNERS, + int window_size = 3, + float first_threshold = 0.00046, + float second_threshold = 0.03589) + : method_ (method) + , window_size_ (window_size) + , first_threshold_ (first_threshold) + , second_threshold_ (second_threshold) + , threads_ (1) + { + name_ = "TrajkovicKeypoint3D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] method either 4 corners or 8 corners + */ + inline void + setMethod (ComputationMethod method) { method_ = method; } + + /// \brief \return the computation method + inline ComputationMethod + getMethod () const { return (method_); } + + /// \brief Set window size + inline void + setWindowSize (int window_size) { window_size_= window_size; } + + /// \brief \return window size i.e. window width or height + inline int + getWindowSize () const { return (window_size_); } + + /** \brief set the first_threshold to reject corners in the simple cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setFirstThreshold (float threshold) { first_threshold_= threshold; } + + /// \brief \return first threshold + inline float + getFirstThreshold () const { return (first_threshold_); } + + /** \brief set the second threshold to reject corners in the final cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setSecondThreshold (float threshold) { second_threshold_= threshold; } + + /// \brief \return second threshold + inline float + getSecondThreshold () const { return (second_threshold_); } + + /** \brief Set normals if precalculated normals are available. + * \param normals + */ + inline void + setNormals (const NormalsConstPtr &normals) { normals_ = normals; } + + /// \brief \return points normals as calculated or given + inline void + getNormals (const NormalsConstPtr &normals) const { return (normals_); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use, 0 for automatic. + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /// \brief \return the number of threads + inline unsigned int + getNumberOfThreads () const { return (threads_); } + + protected: + bool + initCompute (); + + void + detectKeypoints (PointCloudOut &output); + + private: + /** Return a const reference to the normal at (i,j) if it is finite else return + * a reference to a null normal. + * If the returned normal is valid \a counter is incremented. + */ + inline const NormalT& + getNormalOrNull (int i, int j, int& counter) const + { + static const NormalT null; + if (!isFinite ((*normals_) (i,j))) return (null); + ++counter; + return ((*normals_) (i,j)); + } + /// \return difference of two normals vectors + inline float + normalsDiff (const NormalT& a, const NormalT& b) const + { + double nx = a.normal_x; double ny = a.normal_y; double nz = a.normal_z; + double mx = b.normal_x; double my = b.normal_y; double mz = b.normal_z; + return (static_cast (1.0 - (nx*mx + ny*my + nz*mz))); + } + /// \return squared difference of two normals vectors + inline float + squaredNormalsDiff (const NormalT& a, const NormalT& b) const + { + float diff = normalsDiff (a,b); + return (diff * diff); + } + /** Comparator for responses intensity + * \return true if \a response_ at index \aa is greater than response at index \ab + */ + inline bool + greaterCornernessAtIndices (int a, int b) const + { + return (response_->points [a] > response_->points [b]); + } + /// computation method + ComputationMethod method_; + /// window size + int window_size_; + /// half window size + int half_window_size_; + /// first threshold for quick rejection + float first_threshold_; + /// second threshold for corner evaluation + float second_threshold_; + /// number of threads to be used + unsigned int threads_; + /// point cloud normals + NormalsConstPtr normals_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include + +#endif // #ifndef PCL_TRAJKOVIC_KEYPOINT_3D_H_ diff --git a/keypoints/keypoints.doxy b/keypoints/keypoints.doxy index 654d3e75..f3f79b71 100644 --- a/keypoints/keypoints.doxy +++ b/keypoints/keypoints.doxy @@ -6,7 +6,7 @@ The pcl_keypoints library contains implementations of two point cloud keypoint detection algorithms. Keypoints (also referred to as interest points) are points in an image or point cloud that are stable, distinctive, and can be identified using a well-defined - detection criteron. Typically, the number of interest points in a point cloud will be much smaller than the total + detection criterion. Typically, the number of interest points in a point cloud will be much smaller than the total number of points in the cloud, and when used in combination with local feature descriptors at each keypoint, the keypoints and descriptors can be used to form a compact—yet descriptive—representation of the original data. @@ -16,7 +16,7 @@ - \ref search "search" - \ref kdtree "kdtree" - \ref octree "octree" - - \ref range_image "range_image" + - \ref pcl::RangeImage "range_image" - \ref features "features" - \ref filters "filters" diff --git a/keypoints/src/agast_2d.cpp b/keypoints/src/agast_2d.cpp index cfa196cf..2c795b9b 100644 --- a/keypoints/src/agast_2d.cpp +++ b/keypoints/src/agast_2d.cpp @@ -260,7 +260,7 @@ pcl::keypoints::agast::AbstractAgastDetector::applyNonMaxSuppression ( // Need to copy the points pcl::PointCloud best_input; best_input.resize (nr_max_keypoints_); - for (int i = 0; i < scores.size (); ++i) + for (size_t i = 0; i < scores.size (); ++i) best_input[i] = input[scores[i].idx]; applyNonMaxSuppression (best_input, scores, output); } @@ -288,7 +288,7 @@ pcl::keypoints::agast::AbstractAgastDetector::applyNonMaxSuppression ( // Need to copy the points pcl::PointCloud best_input; best_input.resize (nr_max_keypoints_); - for (int i = 0; i < scores.size (); ++i) + for (size_t i = 0; i < scores.size (); ++i) best_input[i] = input[scores[i].idx]; applyNonMaxSuppression (best_input, scores, output); } diff --git a/keypoints/src/narf_keypoint.cpp b/keypoints/src/narf_keypoint.cpp index 9a1bf0ec..9d2309a7 100644 --- a/keypoints/src/narf_keypoint.cpp +++ b/keypoints/src/narf_keypoint.cpp @@ -139,52 +139,6 @@ namespace else positive_score = surface_change_score * (1.0f-distance_factor); } - - void - translateDirection180To360 (Eigen::Vector2f& direction_vector) - { - // The following code does the equivalent to this: - // Get the angle of the 2D direction (with positive x) alpha, and return the direction of 2*alpha - // We do this to create a normal angular wrap-around at -180,180 instead of the one at -90,90, - // enabling us to calculate the average angle as the direction of the sum of all unit vectors. - // We use sin (2a)=2*sin (a)*cos (a) and cos (2a)=2cos^2 (a)-1 so that we needn't actually compute the angles, - // which would be expensive - float cos_a = direction_vector[0], - cos_2a = 2*cos_a*cos_a - 1.0f, - sin_a = direction_vector[1], - sin_2a = 2.0f*sin_a*cos_a; - direction_vector[0] = cos_2a; - direction_vector[1] = sin_2a; - } - - void - translateDirection360To180 (Eigen::Vector2f& direction_vector) - { - // Inverse of the above - float cos_2a = direction_vector[0], - cos_a = sqrtf (0.5f* (cos_2a+1.0f)), - sin_2a = direction_vector[1], - sin_a = sin_2a / (2.0f*cos_a); - direction_vector[0] = cos_a; - direction_vector[1] = sin_a; - } - - inline Eigen::Vector2f - nkdGetDirectionVector (const Eigen::Vector3f& direction, const Eigen::Affine3f& rotation) - { - Eigen::Vector3f rotated_direction = rotation*direction; - Eigen::Vector2f direction_vector (rotated_direction[0], rotated_direction[1]); - direction_vector.normalize (); - if (direction_vector[0]<0.0f) - direction_vector *= -1.0f; - - -# if USE_BEAM_AVERAGE - translateDirection180To360 (direction_vector); -# endif - - return direction_vector; - } inline float nkdGetDirectionAngle (const Eigen::Vector3f& direction, const Eigen::Affine3f& rotation) diff --git a/keypoints/src/trajkovic_2d.cpp b/keypoints/src/trajkovic_2d.cpp new file mode 100644 index 00000000..7bf07d15 --- /dev/null +++ b/keypoints/src/trajkovic_2d.cpp @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include + +// Instantiations of specific point types +//PCL_INSTANTIATE_PRODUCT(HarrisKeypoint2D, (PCL_XYZ_POINT_TYPES)); diff --git a/keypoints/src/trajkovic_3d.cpp b/keypoints/src/trajkovic_3d.cpp new file mode 100644 index 00000000..11e51727 --- /dev/null +++ b/keypoints/src/trajkovic_3d.cpp @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include + +// Instantiations of specific point types +//PCL_INSTANTIATE_PRODUCT(HarrisKeypoint2D, (PCL_XYZ_POINT_TYPES)); diff --git a/octree/CMakeLists.txt b/octree/CMakeLists.txt index f6ac317a..1b0a9bd3 100644 --- a/octree/CMakeLists.txt +++ b/octree/CMakeLists.txt @@ -3,56 +3,56 @@ set(SUBSYS_DESC "Point cloud octree library") set(SUBSYS_DEPS common) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs src/octree_inst.cpp ) set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/octree_base.h - include/pcl/${SUBSYS_NAME}/octree_container.h - include/pcl/${SUBSYS_NAME}/octree_impl.h - include/pcl/${SUBSYS_NAME}/octree_nodes.h - include/pcl/${SUBSYS_NAME}/octree_key.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_density.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_occupancy.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_singlepoint.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_pointvector.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_changedetector.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_voxelcentroid.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud.h - include/pcl/${SUBSYS_NAME}/octree_iterator.h - include/pcl/${SUBSYS_NAME}/octree_search.h - include/pcl/${SUBSYS_NAME}/octree.h - include/pcl/${SUBSYS_NAME}/octree2buf_base.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_adjacency.h - include/pcl/${SUBSYS_NAME}/octree_pointcloud_adjacency_container.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/octree_base.h" + "include/pcl/${SUBSYS_NAME}/octree_container.h" + "include/pcl/${SUBSYS_NAME}/octree_impl.h" + "include/pcl/${SUBSYS_NAME}/octree_nodes.h" + "include/pcl/${SUBSYS_NAME}/octree_key.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_density.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_occupancy.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_singlepoint.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_pointvector.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_changedetector.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_voxelcentroid.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud.h" + "include/pcl/${SUBSYS_NAME}/octree_iterator.h" + "include/pcl/${SUBSYS_NAME}/octree_search.h" + "include/pcl/${SUBSYS_NAME}/octree.h" + "include/pcl/${SUBSYS_NAME}/octree2buf_base.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_adjacency.h" + "include/pcl/${SUBSYS_NAME}/octree_pointcloud_adjacency_container.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/octree_base.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_pointcloud.hpp - include/pcl/${SUBSYS_NAME}/impl/octree2buf_base.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_iterator.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_search.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_pointcloud_voxelcentroid.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_pointcloud_adjacency.hpp + "include/pcl/${SUBSYS_NAME}/impl/octree_base.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_pointcloud.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree2buf_base.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_iterator.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_search.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_pointcloud_voxelcentroid.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_pointcloud_adjacency.hpp" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME}) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}") + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp index 37b5adb0..3191a20e 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp @@ -44,7 +44,6 @@ #include -using namespace std; ////////////////////////////////////////////////////////////////////////////////////////////// template @@ -316,13 +315,13 @@ pcl::octree::OctreePointCloud min_z_ = min_z_arg; max_z_ = max_z_arg; - min_x_ = min (min_x_, max_x_); - min_y_ = min (min_y_, max_y_); - min_z_ = min (min_z_, max_z_); + min_x_ = std::min (min_x_, max_x_); + min_y_ = std::min (min_y_, max_y_); + min_z_ = std::min (min_z_, max_z_); - max_x_ = max (min_x_, max_x_); - max_y_ = max (min_y_, max_y_); - max_z_ = max (min_z_, max_z_); + max_x_ = std::max (min_x_, max_x_); + max_y_ = std::max (min_y_, max_y_); + max_z_ = std::max (min_z_, max_z_); // generate bit masks for octree getKeyBitSize (); @@ -351,13 +350,13 @@ pcl::octree::OctreePointCloud min_z_ = 0.0f; max_z_ = max_z_arg; - min_x_ = min (min_x_, max_x_); - min_y_ = min (min_y_, max_y_); - min_z_ = min (min_z_, max_z_); + min_x_ = std::min (min_x_, max_x_); + min_y_ = std::min (min_y_, max_y_); + min_z_ = std::min (min_z_, max_z_); - max_x_ = max (min_x_, max_x_); - max_y_ = max (min_y_, max_y_); - max_z_ = max (min_z_, max_z_); + max_x_ = std::max (min_x_, max_x_); + max_y_ = std::max (min_y_, max_y_); + max_z_ = std::max (min_z_, max_z_); // generate bit masks for octree getKeyBitSize (); @@ -383,13 +382,13 @@ pcl::octree::OctreePointCloud min_z_ = 0.0f; max_z_ = cubeLen_arg; - min_x_ = min (min_x_, max_x_); - min_y_ = min (min_y_, max_y_); - min_z_ = min (min_z_, max_z_); + min_x_ = std::min (min_x_, max_x_); + min_y_ = std::min (min_y_, max_y_); + min_z_ = std::min (min_z_, max_z_); - max_x_ = max (min_x_, max_x_); - max_y_ = max (min_y_, max_y_); - max_z_ = max (min_z_, max_z_); + max_x_ = std::max (min_x_, max_x_); + max_y_ = std::max (min_y_, max_y_); + max_z_ = std::max (min_z_, max_z_); // generate bit masks for octree getKeyBitSize (); @@ -626,11 +625,11 @@ pcl::octree::OctreePointCloud max_key_z = static_cast ((max_z_ - min_z_) / resolution_); // find maximum amount of keys - max_voxels = max (max (max (max_key_x, max_key_y), max_key_z), static_cast (2)); + max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast (2)); // tree depth == amount of bits of max_voxels - this->octree_depth_ = max ((min (static_cast (OctreeKey::maxDepth), static_cast (ceil (this->Log2 (max_voxels)-minValue)))), + this->octree_depth_ = std::max ((std::min (static_cast (OctreeKey::maxDepth), static_cast (ceil (this->Log2 (max_voxels)-minValue)))), static_cast (0)); octree_side_len = static_cast (1 << this->octree_depth_) * resolution_-minValue; @@ -829,7 +828,7 @@ pcl::octree::OctreePointCloud #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; -#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; -#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; #endif /* OCTREE_POINTCLOUD_HPP_ */ diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp index 92963460..96288904 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -54,63 +54,50 @@ template vo pcl::octree::OctreePointCloudAdjacency::addPointsFromInputCloud () { //double t1,t2; + float minX = std::numeric_limits::max (), minY = std::numeric_limits::max (), minZ = std::numeric_limits::max (); + float maxX = -std::numeric_limits::max(), maxY = -std::numeric_limits::max(), maxZ = -std::numeric_limits::max(); - //t1 = timer_.getTime (); - OctreePointCloud::addPointsFromInputCloud (); - + for (size_t i = 0; i < input_->size (); ++i) + { + PointT temp (input_->points[i]); + if (transform_func_) //Search for point with + transform_func_ (temp); + if (!pcl::isFinite (temp)) //Check to make sure transform didn't make point not finite + continue; + if (temp.x < minX) + minX = temp.x; + if (temp.y < minY) + minY = temp.y; + if (temp.z < minZ) + minZ = temp.z; + if (temp.x > maxX) + maxX = temp.x; + if (temp.y > maxY) + maxY = temp.y; + if (temp.z > maxZ) + maxZ = temp.z; + } + this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); - //t2 = timer_.getTime (); - //std::cout << "Add Points:"< > delete_list; - //double t_temp, t_neigh, t_compute, t_getLeaf; - //t_neigh = t_compute = t_getLeaf = 0; + OctreePointCloud::addPointsFromInputCloud (); + LeafContainerT *leaf_container; typename OctreeAdjacencyT::LeafNodeIterator leaf_itr; leaf_vector_.reserve (this->getLeafCount ()); for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr) { - //t_temp = timer_.getTime (); OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey (); leaf_container = &(leaf_itr.getLeafContainer ()); - //t_getLeaf += timer_.getTime () - t_temp; - //t_temp = timer_.getTime (); //Run the leaf's compute function leaf_container->computeData (); - //t_compute += timer_.getTime () - t_temp; - - //t_temp = timer_.getTime (); - // std::cout << "Computing neighbors\n"; + computeNeighbors (leaf_key, leaf_container); - //t_neigh += timer_.getTime () - t_temp; leaf_vector_.push_back (leaf_container); - } - //Go through and delete voxels scheduled - for (typename std::list >::iterator delete_itr = delete_list.begin (); delete_itr != delete_list.end (); ++delete_itr) - { - leaf_container = delete_itr->second; - //Remove pointer to it from all neighbors - typename std::set::iterator neighbor_itr = leaf_container->begin (); - typename std::set::iterator neighbor_end = leaf_container->end (); - for ( ; neighbor_itr != neighbor_end; ++neighbor_itr) - { - //Don't delete self neighbor - if (*neighbor_itr != leaf_container) - (*neighbor_itr)->removeNeighbor (leaf_container); - } - this->removeLeaf (delete_itr->first); - } - //Make sure our leaf vector is correctly sized assert (leaf_vector_.size () == this->getLeafCount ()); - - // std::cout << "Time spent getting leaves ="< PointT temp (point_arg); transform_func_ (temp); // calculate integer key for transformed point coordinates - key_arg.x = static_cast ((temp.x - this->min_x_) / this->resolution_); - key_arg.y = static_cast ((temp.y - this->min_y_) / this->resolution_); - key_arg.z = static_cast ((temp.z - this->min_z_) / this->resolution_); - + if (pcl::isFinite (temp)) //Make sure transformed point is finite - if it is not, it gets default key + { + key_arg.x = static_cast ((temp.x - this->min_x_) / this->resolution_); + key_arg.y = static_cast ((temp.y - this->min_y_) / this->resolution_); + key_arg.z = static_cast ((temp.z - this->min_z_) / this->resolution_); + } + else + { + key_arg = OctreeKey (); + } } else { @@ -147,16 +140,7 @@ pcl::octree::OctreePointCloudAdjacency const PointT& point = this->input_->points[pointIdx_arg]; if (!pcl::isFinite (point)) return; - - if (transform_func_) - { - PointT temp (point); - transform_func_ (temp); - this->adoptBoundingBoxToPoint (temp); - } - else - this->adoptBoundingBoxToPoint (point); - + // generate key this->genOctreeKeyforPoint (point, key); // add point to octree at key @@ -164,23 +148,34 @@ pcl::octree::OctreePointCloudAdjacency container->addPoint (point); } - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloudAdjacency::computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container) { + //Make sure requested key is valid + if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y || key_arg.z > this->max_key_.z) + { + PCL_ERROR ("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for invalid octree key\n"); + return; + } OctreeKey neighbor_key; - - for (int dx = -1; dx <= 1; ++dx) + int dx_min = (key_arg.x > 0) ? -1 : 0; + int dy_min = (key_arg.y > 0) ? -1 : 0; + int dz_min = (key_arg.z > 0) ? -1 : 0; + int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1; + int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1; + int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1; + + for (int dx = dx_min; dx <= dx_max; ++dx) { - for (int dy = -1; dy <= 1; ++dy) + for (int dy = dy_min; dy <= dy_max; ++dy) { - for (int dz = -1; dz <= 1; ++dz) + for (int dz = dz_min; dz <= dz_max; ++dz) { - neighbor_key.x = key_arg.x + dx; - neighbor_key.y = key_arg.y + dy; - neighbor_key.z = key_arg.z + dz; + neighbor_key.x = static_cast (key_arg.x + dx); + neighbor_key.y = static_cast (key_arg.y + dy); + neighbor_key.z = static_cast (key_arg.z + dz); LeafContainerT *neighbor = this->findLeaf (neighbor_key); if (neighbor) { diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp index 0404e460..51d28f8a 100644 --- a/octree/include/pcl/octree/impl/octree_search.hpp +++ b/octree/include/pcl/octree/impl/octree_search.hpp @@ -45,7 +45,6 @@ #include #include -using namespace std; ////////////////////////////////////////////////////////////////////////////////////////////// template bool @@ -104,7 +103,7 @@ pcl::octree::OctreePointCloudSearch::n key.x = key.y = key.z = 0; // initalize smallest point distance in search with high value - double smallest_dist = numeric_limits::max (); + double smallest_dist = std::numeric_limits::max (); getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates); @@ -247,7 +246,7 @@ pcl::octree::OctreePointCloudSearch::g } else { - search_heap[child_idx].point_distance = numeric_limits::infinity (); + search_heap[child_idx].point_distance = std::numeric_limits::infinity (); } } @@ -276,7 +275,7 @@ pcl::octree::OctreePointCloudSearch::g float squared_dist; size_t i; - vector decoded_point_vector; + std::vector decoded_point_vector; const LeafNode* child_leaf = static_cast (child_node); @@ -374,7 +373,7 @@ pcl::octree::OctreePointCloudSearch::g size_t i; const LeafNode* child_leaf = static_cast (child_node); - vector decoded_point_vector; + std::vector decoded_point_vector; // decode leaf node into decoded_point_vector (*child_leaf)->getPointIndices (decoded_point_vector); @@ -422,7 +421,7 @@ pcl::octree::OctreePointCloudSearch::a const OctreeNode* child_node; // set minimum voxel distance to maximum value - min_voxel_center_distance = numeric_limits::max (); + min_voxel_center_distance = std::numeric_limits::max (); min_child_idx = 0xFF; @@ -471,11 +470,11 @@ pcl::octree::OctreePointCloudSearch::a double squared_dist; double smallest_squared_dist; size_t i; - vector decoded_point_vector; + std::vector decoded_point_vector; const LeafNode* child_leaf = static_cast (child_node); - smallest_squared_dist = numeric_limits::max (); + smallest_squared_dist = std::numeric_limits::max (); // decode leaf node into decoded_point_vector (**child_leaf).getPointIndices (decoded_point_vector); @@ -557,7 +556,7 @@ pcl::octree::OctreePointCloudSearch::b { // we reached leaf node level size_t i; - vector decoded_point_vector; + std::vector decoded_point_vector; bool bInBox; const LeafNode* child_leaf = static_cast (child_node); @@ -602,7 +601,7 @@ pcl::octree::OctreePointCloudSearch::g initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); - if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z)) + if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z)) return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key, voxel_center_list, max_voxel_count); @@ -626,7 +625,7 @@ pcl::octree::OctreePointCloudSearch::g initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); - if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z)) + if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z)) return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key, k_indices, max_voxel_count); return (0); diff --git a/octree/include/pcl/octree/octree2buf_base.h b/octree/include/pcl/octree/octree2buf_base.h index 6b8dab48..2d6b21ae 100644 --- a/octree/include/pcl/octree/octree2buf_base.h +++ b/octree/include/pcl/octree/octree2buf_base.h @@ -805,7 +805,7 @@ namespace pcl * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator * \param key_arg: reference to an octree key * \param binary_tree_in_it_arg iterator of binary input data - * \param leaf_container_vector__it_end_arg end iterator of binary input data + * \param binary_tree_in_it_end_arg * \param leaf_container_vector_it_arg: iterator pointing to leaf containter pointers to be added to a leaf node * \param leaf_container_vector_it_end_arg: iterator pointing to leaf containter pointers pointing to last object in input container. * \param branch_reset_arg: Reset pointer array of current branch diff --git a/octree/include/pcl/octree/octree_container.h b/octree/include/pcl/octree/octree_container.h index aa5c20c5..3b06f72b 100644 --- a/octree/include/pcl/octree/octree_container.h +++ b/octree/include/pcl/octree/octree_container.h @@ -93,7 +93,7 @@ namespace pcl * \return number of points/indices stored in leaf node container. */ virtual size_t - getSize () + getSize () const { return 0u; } diff --git a/octree/include/pcl/octree/octree_iterator.h b/octree/include/pcl/octree/octree_iterator.h index 48b71f67..0bcb565f 100644 --- a/octree/include/pcl/octree/octree_iterator.h +++ b/octree/include/pcl/octree/octree_iterator.h @@ -113,7 +113,6 @@ namespace pcl OctreeIteratorBase (const OctreeIteratorBase& src, unsigned int max_depth_arg = 0) : octree_ (src.octree_), current_state_(0), max_octree_depth_(max_depth_arg) { - this->reset (); } /** \brief Copy operator. @@ -135,7 +134,7 @@ namespace pcl } /** \brief Equal comparison operator - * \param[in] OctreeIteratorBase to compare with + * \param[in] other OctreeIteratorBase to compare with */ bool operator==(const OctreeIteratorBase& other) const { @@ -145,7 +144,7 @@ namespace pcl } /** \brief Inequal comparison operator - * \param[in] OctreeIteratorBase to compare with + * \param[in] other OctreeIteratorBase to compare with */ bool operator!=(const OctreeIteratorBase& other) const { diff --git a/octree/include/pcl/octree/octree_nodes.h b/octree/include/pcl/octree/octree_nodes.h index 13a48c82..d9b26897 100644 --- a/octree/include/pcl/octree/octree_nodes.h +++ b/octree/include/pcl/octree/octree_nodes.h @@ -46,6 +46,8 @@ #include +#include + #include #include "octree_container.h" @@ -190,6 +192,10 @@ namespace pcl protected: ContainerT container_; + + public: + //Type ContainerT may have fixed-size Eigen objects inside + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency.h b/octree/include/pcl/octree/octree_pointcloud_adjacency.h index 3f61e70f..b0bb3301 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency.h @@ -40,198 +40,215 @@ #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_H_ #define PCL_OCTREE_POINTCLOUD_ADJACENCY_H_ +#include #include #include -#include -#include -#include +#include #include #include #include -//DEBUG TODO REMOVE -#include - - - namespace pcl -{ - +{ + namespace octree { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud voxel class used for adjacency calculation - * \note This pointcloud octree class generate an octree from a point cloud (zero-copy). - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note This class maintains adjacency information for its voxels - * \note The OctreePointCloudAdjacencyContainer can be used to store data in leaf nodes - * \note An optional transform function can be provided which changes how the voxel grid is computed - this can be used to, for example, make voxel bins larger as they increase in distance from the origin (camera) - * \note See SupervoxelClustering for an example of how to provide a transform function - * \note If used in academic work, please cite: - * - * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter - * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds - * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 - * \ingroup octree - * \author Jeremie Papon (jpapon@gmail.com) - */ + /** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels. + * + * This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is + * initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. + * + * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes. + * + * An optional transform function can be provided which changes how the voxel grid is computed - this can be used to, + * for example, make voxel bins larger as they increase in distance from the origin (camera). + * \note See SupervoxelClustering for an example of how to provide a transform function. + * + * If used in academic work, please cite: + * + * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter + * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 + * + * \ingroup octree + * \author Jeremie Papon (jpapon@gmail.com) */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template< typename PointT, - typename LeafContainerT = OctreePointCloudAdjacencyContainer , - typename BranchContainerT = OctreeContainerEmpty > - class OctreePointCloudAdjacency : public OctreePointCloud< PointT, LeafContainerT, BranchContainerT> - + template , + typename BranchContainerT = OctreeContainerEmpty> + class OctreePointCloudAdjacency : public OctreePointCloud { - - public: - typedef OctreeBase OctreeBaseT; - - typedef OctreePointCloudAdjacency OctreeAdjacencyT; - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; - - typedef OctreePointCloud OctreePointCloudT; - typedef typename OctreePointCloudT::LeafNode LeafNode; - typedef typename OctreePointCloudT::BranchNode BranchNode; - - typedef pcl::PointCloud CloudT; - - //So we can access input - using OctreePointCloudT::input_; - using OctreePointCloudT::resolution_; - - // iterators are friends - friend class OctreeIteratorBase ; - friend class OctreeDepthFirstIterator ; - friend class OctreeBreadthFirstIterator ; - friend class OctreeLeafNodeIterator ; - - // Octree default iterators - typedef OctreeDepthFirstIterator Iterator; - typedef const OctreeDepthFirstIterator ConstIterator; - Iterator depth_begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);}; - const Iterator depth_end() {return Iterator();}; - - // Octree leaf node iterators - typedef OctreeLeafNodeIterator LeafNodeIterator; - typedef const OctreeLeafNodeIterator< OctreeAdjacencyT> ConstLeafNodeIterator; - LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);}; - const LeafNodeIterator leaf_end() {return LeafNodeIterator();}; - - typedef boost::adjacency_list VoxelAdjacencyList; - typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID; - typedef typename VoxelAdjacencyList::edge_descriptor EdgeID; - - //Leaf vector - pointers to all leaves - typedef std::vector LeafVectorT; - //Fast leaf iterators that don't require traversing tree - typedef typename LeafVectorT::iterator iterator; - typedef typename LeafVectorT::const_iterator const_iterator; - inline iterator begin () { return (leaf_vector_.begin ()); } - inline iterator end () { return (leaf_vector_.end ()); } - //size of neighbors - inline size_t size () const { return leaf_vector_.size (); } - - - /** \brief Constructor. - * \param[in] resolution_arg octree resolution at lowest octree level (voxel size) - **/ - OctreePointCloudAdjacency (const double resolution_arg); - - - /** \brief Empty class destructor. */ - virtual ~OctreePointCloudAdjacency () - { - } - - /** \brief Adds points from cloud to the octree - * \note This overrides the addPointsFromInputCloud from the OctreePointCloud class - */ - void - addPointsFromInputCloud (); - - /** \brief Gets the leaf container for a given point - * \param[in] point_arg Point to search for - * \returns Pointer to the leaf container - null if no leaf container found - */ - LeafContainerT * - getLeafContainerAtPoint (const PointT& point_arg) const; - - /** \brief Computes an adjacency graph of voxel relations - * \note WARNING: This slows down rapidly as cloud size increases due number of edges - * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships - vertices are pointT, edges represent touching, and edge lengths are the distance between the points - */ - void - computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph); - - /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud - * This is useful for changing how adjacency is calculated - such as relaxing - * the adjacency criterion for points further from the camera - * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one parameter (a point) which it modifies in place - */ - void - setTransformFunction (boost::function transform_func) - { - transform_func_ = transform_func; - } - - /** \brief Tests whether input point is occluded from specified camera point by other voxels - \param[in] point_arg Point to test for - \param[in] camera_pos Position of camera, defaults to origin - \returns True if path to camera is blocked by a voxel, false otherwise - */ - bool - testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0,0,0)); - - protected: - - /** \brief Add point at index from input pointcloud dataset to octree - * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added - * \note This virtual implementation allows the use of a transform function to compute keys - */ - virtual void - addPointIdx (const int pointIdx_arg); - - /** \brief Fills in the neighbors fields for new voxels - * \param[in] key_arg Key of the voxel to check neighbors for - * \param[in] leaf_container Pointer to container of the leaf to check neighbors for - */ - void - computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container); - - /** \brief Generates octree key for specified point (uses transform if provided) - * \param[in] point_arg Point to generate key for - * \param[out] key_arg Resulting octree key - */ - void - genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const; - - private: - - StopWatch timer_; - - //Local leaf pointer vector used to make iterating through leaves fast - LeafVectorT leaf_vector_; - - boost::function transform_func_; - }; - - } - -} - - - - + public: + + typedef OctreeBase OctreeBaseT; + + typedef OctreePointCloudAdjacency OctreeAdjacencyT; + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef OctreePointCloud OctreePointCloudT; + typedef typename OctreePointCloudT::LeafNode LeafNode; + typedef typename OctreePointCloudT::BranchNode BranchNode; + + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + + // Iterators are friends + friend class OctreeIteratorBase; + friend class OctreeDepthFirstIterator; + friend class OctreeBreadthFirstIterator; + friend class OctreeLeafNodeIterator; + + // Octree default iterators + typedef OctreeDepthFirstIterator Iterator; + typedef const OctreeDepthFirstIterator ConstIterator; + + Iterator depth_begin (unsigned int max_depth_arg = 0) { return Iterator (this, max_depth_arg); } + const Iterator depth_end () { return Iterator (); } + + // Octree leaf node iterators + typedef OctreeLeafNodeIterator LeafNodeIterator; + typedef const OctreeLeafNodeIterator ConstLeafNodeIterator; + + LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0) { return LeafNodeIterator (this, max_depth_arg); } + const LeafNodeIterator leaf_end () { return LeafNodeIterator (); } + + // BGL graph + typedef boost::adjacency_list VoxelAdjacencyList; + typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID; + typedef typename VoxelAdjacencyList::edge_descriptor EdgeID; + + // Leaf vector - pointers to all leaves + typedef std::vector LeafVectorT; + + // Fast leaf iterators that don't require traversing tree + typedef typename LeafVectorT::iterator iterator; + typedef typename LeafVectorT::const_iterator const_iterator; + + inline iterator begin () { return (leaf_vector_.begin ()); } + inline iterator end () { return (leaf_vector_.end ()); } + + // Size of neighbors + inline size_t size () const { return leaf_vector_.size (); } + + /** \brief Constructor. + * + * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */ + OctreePointCloudAdjacency (const double resolution_arg); + + /** \brief Empty class destructor. */ + virtual ~OctreePointCloudAdjacency () + { + } + + /** \brief Adds points from cloud to the octree. + * + * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */ + void + addPointsFromInputCloud (); + + /** \brief Gets the leaf container for a given point. + * + * \param[in] point_arg Point to search for + * + * \returns Pointer to the leaf container - null if no leaf container found. */ + LeafContainerT* + getLeafContainerAtPoint (const PointT& point_arg) const; + + /** \brief Computes an adjacency graph of voxel relations. + * + * \warning This slows down rapidly as cloud size increases due to the number of edges. + * + * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships. + * Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */ + void + computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph); + + /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud. + * + * This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for + * points further from the camera. + * + * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one + * parameter (a point) which it modifies in place. */ + void + setTransformFunction (boost::function transform_func) + { + transform_func_ = transform_func; + } + + /** \brief Tests whether input point is occluded from specified camera point by other voxels. + * + * \param[in] point_arg Point to test for + * \param[in] camera_pos Position of camera, defaults to origin + * + * \returns True if path to camera is blocked by a voxel, false otherwise. */ + bool + testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0)); + + protected: + + /** \brief Add point at index from input pointcloud dataset to octree. + * + * \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added + * + * \note This virtual implementation allows the use of a transform function to compute keys. */ + virtual void + addPointIdx (const int point_idx_arg); + + /** \brief Fills in the neighbors fields for new voxels. + * + * \param[in] key_arg Key of the voxel to check neighbors for + * \param[in] leaf_container Pointer to container of the leaf to check neighbors for */ + void + computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container); + + /** \brief Generates octree key for specified point (uses transform if provided). + * + * \param[in] point_arg Point to generate key for + * \param[out] key_arg Resulting octree key */ + void + genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const; + + private: + + /** \brief Add point at given index from input point cloud to octree. + * + * Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */ + using OctreePointCloudT::addPointFromCloud; + + /** \brief Add point simultaneously to octree and input point cloud. + * + * This functionality is not enabled for adjacency octree. */ + using OctreePointCloudT::addPointToCloud; + + using OctreePointCloudT::input_; + using OctreePointCloudT::resolution_; + using OctreePointCloudT::min_x_; + using OctreePointCloudT::min_y_; + using OctreePointCloudT::min_z_; + using OctreePointCloudT::max_x_; + using OctreePointCloudT::max_y_; + using OctreePointCloudT::max_z_; + + /// Local leaf pointer vector used to make iterating through leaves fast. + LeafVectorT leaf_vector_; + + boost::function transform_func_; + }; + } +} //#ifdef PCL_NO_PRECOMPILE #include //#endif -#endif //PCL_OCTREE_POINTCLOUD_SUPER_VOXEL_H_ +#endif // PCL_OCTREE_POINTCLOUD_ADJACENCY_H_ diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h index d31ac110..326c0f54 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h @@ -92,10 +92,10 @@ namespace pcl /** \brief Add new point to container- this just counts points * \note To actually store data in the leaves, need to specialize this * for your point and data type as in supervoxel_clustering.hpp - * \param[in] new_point the new point to add */ + // param[in] new_point the new point to add void - addPoint (const PointInT& new_point) + addPoint (const PointInT& /*new_point*/) { using namespace pcl::common; ++num_points_; @@ -176,7 +176,7 @@ namespace pcl * \return number of points added to leaf node container. */ virtual size_t - getSize () + getSize () const { return num_points_; } @@ -190,4 +190,4 @@ namespace pcl } } -#endif \ No newline at end of file +#endif diff --git a/octree/include/pcl/octree/octree_pointcloud_changedetector.h b/octree/include/pcl/octree/octree_pointcloud_changedetector.h index d64cd9ef..cdacc074 100644 --- a/octree/include/pcl/octree/octree_pointcloud_changedetector.h +++ b/octree/include/pcl/octree/octree_pointcloud_changedetector.h @@ -98,7 +98,7 @@ namespace pcl for (it=leaf_containers.begin(); it!=it_end; ++it) { - if ((*it)->getSize()>=minPointsPerLeaf_arg) + if (static_cast ((*it)->getSize ()) >= minPointsPerLeaf_arg) (*it)->getPointIndices(indicesVector_arg); } diff --git a/octree/include/pcl/octree/octree_pointcloud_occupancy.h b/octree/include/pcl/octree/octree_pointcloud_occupancy.h index 93e5c67f..53413c45 100644 --- a/octree/include/pcl/octree/octree_pointcloud_occupancy.h +++ b/octree/include/pcl/octree/octree_pointcloud_occupancy.h @@ -102,7 +102,7 @@ namespace pcl genOctreeKeyforPoint (point_arg, key); // add point to octree at key - this->addData (key, 0); + this->createLeaf (key); } /** \brief Set occupied voxels at all points from point cloud. diff --git a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h index 8f3dbae0..10cddadc 100644 --- a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h +++ b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h @@ -77,8 +77,8 @@ namespace pcl } /** \brief Equal comparison operator - set to false - * \param[in] OctreePointCloudVoxelCentroidContainer to compare with */ + // param[in] OctreePointCloudVoxelCentroidContainer to compare with virtual bool operator==(const OctreeContainerBase&) const { return ( false ); @@ -168,8 +168,7 @@ namespace pcl } /** \brief Add DataT object to leaf node at octree key. - * \param[in] key_arg octree key addressing a leaf node. - * \param[in] data_arg DataT object to be added. + * \param pointIdx_arg */ virtual void addPointIdx (const int pointIdx_arg) diff --git a/octree/include/pcl/octree/octree_search.h b/octree/include/pcl/octree/octree_search.h index 5d1ce079..5c039bf3 100644 --- a/octree/include/pcl/octree/octree_search.h +++ b/octree/include/pcl/octree/octree_search.h @@ -600,6 +600,10 @@ namespace pcl } } +#ifdef PCL_NO_PRECOMPILE +#include +#else #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch; +#endif // PCL_NO_PRECOMPILE #endif // PCL_OCTREE_SEARCH_H_ diff --git a/octree/octree.doxy b/octree/octree.doxy index 24b05f48..d05db86a 100644 --- a/octree/octree.doxy +++ b/octree/octree.doxy @@ -6,7 +6,7 @@ The pcl_octree library provides efficient methods for creating a hierarchical tree data structure from point cloud data. This enables spatial partitioning, downsampling and search operations on the point data set. -Each octree node the has either eight children or no children. The root node describes +Each octree node has either eight children or no children. The root node describes a cubic bounding box which encapsulates all points. At every tree level, this space becomes subdivided by a factor of 2 which results in an increased voxel resolution. diff --git a/octree/src/octree_inst.cpp b/octree/src/octree_inst.cpp index 3193292b..1ca86ea7 100644 --- a/octree/src/octree_inst.cpp +++ b/octree/src/octree_inst.cpp @@ -35,10 +35,6 @@ * Author: Julius Kammerl (julius@kammerl.de) */ -#include -#include -#include - #include #include @@ -55,6 +51,15 @@ template class PCL_EXPORTS pcl::octree::OctreeBase< template class PCL_EXPORTS pcl::octree::Octree2BufBase< pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >; + +template class PCL_EXPORTS pcl::octree::OctreeBase< + pcl::octree::OctreeContainerEmpty, + pcl::octree::OctreeContainerEmpty >; + +#ifndef PCL_NO_PRECOMPILE +#include +#include +#include PCL_INSTANTIATE(OctreePointCloudSingleBufferWithLeafDataTVector, PCL_XYZ_POINT_TYPES) PCL_INSTANTIATE(OctreePointCloudDoubleBufferWithLeafDataTVector, PCL_XYZ_POINT_TYPES) @@ -63,7 +68,7 @@ PCL_INSTANTIATE(OctreePointCloudSearch, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudSingleBufferWithLeafDataT, PCL_XYZ_POINT_TYPES) -// PCL_INSTANTIATE(OctreePointCloudSingleBufferWithEmptyLeaf, PCL_XYZ_POINT_TYPES) +PCL_INSTANTIATE(OctreePointCloudSingleBufferWithEmptyLeaf, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudDensity, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudSingleBufferWithDensityLeaf, PCL_XYZ_POINT_TYPES) @@ -75,4 +80,5 @@ PCL_INSTANTIATE(OctreePointCloudSearch, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudChangeDetector, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudVoxelCentroid, PCL_XYZ_POINT_TYPES) +#endif // PCL_NO_PRECOMPILE diff --git a/outofcore/CMakeLists.txt b/outofcore/CMakeLists.txt index d3881904..381d2d47 100644 --- a/outofcore/CMakeLists.txt +++ b/outofcore/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud outofcore library") set(SUBSYS_DEPS common io filters octree visualization) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -16,57 +16,59 @@ if(build) ) set(incs - include/pcl/${SUBSYS_NAME}/metadata.h - include/pcl/${SUBSYS_NAME}/outofcore_base_data.h - include/pcl/${SUBSYS_NAME}/outofcore_node_data.h - include/pcl/${SUBSYS_NAME}/outofcore_iterator_base.h - include/pcl/${SUBSYS_NAME}/outofcore_breadth_first_iterator.h - include/pcl/${SUBSYS_NAME}/outofcore_depth_first_iterator.h - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/cJSON.h - include/pcl/${SUBSYS_NAME}/octree_base.h - include/pcl/${SUBSYS_NAME}/octree_base_node.h - include/pcl/${SUBSYS_NAME}/octree_abstract_node_container.h - include/pcl/${SUBSYS_NAME}/octree_disk_container.h - include/pcl/${SUBSYS_NAME}/octree_ram_container.h - include/pcl/${SUBSYS_NAME}/outofcore.h - include/pcl/${SUBSYS_NAME}/outofcore_impl.h + "include/pcl/${SUBSYS_NAME}/metadata.h" + "include/pcl/${SUBSYS_NAME}/outofcore_base_data.h" + "include/pcl/${SUBSYS_NAME}/outofcore_node_data.h" + "include/pcl/${SUBSYS_NAME}/outofcore_iterator_base.h" + "include/pcl/${SUBSYS_NAME}/outofcore_breadth_first_iterator.h" + "include/pcl/${SUBSYS_NAME}/outofcore_depth_first_iterator.h" + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/cJSON.h" + "include/pcl/${SUBSYS_NAME}/octree_base.h" + "include/pcl/${SUBSYS_NAME}/octree_base_node.h" + "include/pcl/${SUBSYS_NAME}/octree_abstract_node_container.h" + "include/pcl/${SUBSYS_NAME}/octree_disk_container.h" + "include/pcl/${SUBSYS_NAME}/octree_ram_container.h" + "include/pcl/${SUBSYS_NAME}/outofcore.h" + "include/pcl/${SUBSYS_NAME}/outofcore_impl.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/outofcore_breadth_first_iterator.hpp - include/pcl/${SUBSYS_NAME}/impl/outofcore_depth_first_iterator.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_base.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_base_node.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_disk_container.hpp - include/pcl/${SUBSYS_NAME}/impl/octree_ram_container.hpp - include/pcl/${SUBSYS_NAME}/impl/monitor_queue.hpp - include/pcl/${SUBSYS_NAME}/impl/lru_cache.hpp + "include/pcl/${SUBSYS_NAME}/impl/outofcore_breadth_first_iterator.hpp" + "include/pcl/${SUBSYS_NAME}/impl/outofcore_depth_first_iterator.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_base.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_base_node.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_disk_container.hpp" + "include/pcl/${SUBSYS_NAME}/impl/octree_ram_container.hpp" + "include/pcl/${SUBSYS_NAME}/impl/monitor_queue.hpp" + "include/pcl/${SUBSYS_NAME}/impl/lru_cache.hpp" ) set(visualization_incs - include/pcl/${SUBSYS_NAME}/visualization/axes.h - include/pcl/${SUBSYS_NAME}/visualization/camera.h - include/pcl/${SUBSYS_NAME}/visualization/common.h - include/pcl/${SUBSYS_NAME}/visualization/geometry.h - include/pcl/${SUBSYS_NAME}/visualization/grid.h - include/pcl/${SUBSYS_NAME}/visualization/object.h - include/pcl/${SUBSYS_NAME}/visualization/outofcore_cloud.h - include/pcl/${SUBSYS_NAME}/visualization/scene.h - include/pcl/${SUBSYS_NAME}/visualization/viewport.h + "include/pcl/${SUBSYS_NAME}/visualization/axes.h" + "include/pcl/${SUBSYS_NAME}/visualization/camera.h" + "include/pcl/${SUBSYS_NAME}/visualization/common.h" + "include/pcl/${SUBSYS_NAME}/visualization/geometry.h" + "include/pcl/${SUBSYS_NAME}/visualization/grid.h" + "include/pcl/${SUBSYS_NAME}/visualization/object.h" + "include/pcl/${SUBSYS_NAME}/visualization/outofcore_cloud.h" + "include/pcl/${SUBSYS_NAME}/visualization/scene.h" + "include/pcl/${SUBSYS_NAME}/visualization/viewport.h" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) + set(LIB_NAME "pcl_${SUBSYS_NAME}") - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs} ${visualization_incs}) - #PCL_ADD_SSE_FLAGS(${LIB_NAME}) - target_link_libraries(${LIB_NAME} pcl_common pcl_visualization ${Boost_SYSTEM_LIBRARY}) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs} ${visualization_incs}) + #PCL_ADD_SSE_FLAGS("${LIB_NAME}") + target_link_libraries("${LIB_NAME}" pcl_common pcl_visualization ${Boost_SYSTEM_LIBRARY}) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/visualization ${visualization_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/visualization" ${visualization_incs}) - add_subdirectory(tools) + if(BUILD_tools) + add_subdirectory(tools) + endif(BUILD_tools) endif(build) diff --git a/outofcore/include/pcl/outofcore/octree_base.h b/outofcore/include/pcl/outofcore/octree_base.h index 64f2e714..f29fdd71 100644 --- a/outofcore/include/pcl/outofcore/octree_base.h +++ b/outofcore/include/pcl/outofcore/octree_base.h @@ -87,7 +87,7 @@ namespace pcl * http://www.pointclouds.org/blog/urcs/. * * The primary purpose of this class is an interface to the - * recursive traversal (recursion handled by \ref OutofcoreOctreeBaseNode) of the + * recursive traversal (recursion handled by \ref pcl::outofcore::OutofcoreOctreeBaseNode) of the * in-memory/top-level octree structure. The metadata in each node * can be loaded entirely into main memory, from which the tree can be traversed * recursively in this state. This class provides an the interface @@ -103,9 +103,9 @@ namespace pcl * * The format of the octree is stored on disk in a hierarchical * octree structure, where .oct_idx are the JSON-based node - * metadata files managed by \ref OutofcoreOctreeNodeMetadata, + * metadata files managed by \ref pcl::outofcore::OutofcoreOctreeNodeMetadata, * and .octree is the JSON-based octree metadata file managed by - * \ref OutofcoreOctreeBaseMetadata. Children of each node live + * \ref pcl::outofcore::OutofcoreOctreeBaseMetadata. Children of each node live * in up to eight subdirectories named from 0 to 7, where a * metadata and optionally a pcd file will exist. The PCD files * are stored in compressed binary PCD format, containing all of @@ -196,7 +196,7 @@ namespace pcl * otherwise only the root node is actually created, and the rest will be * generated on insertion or query. * - * \param Path to the top-level tree/tree.oct_idx metadata file + * \param root_node_name Path to the top-level tree/tree.oct_idx metadata file * \param load_all Load entire tree metadata (does not load any points from disk) * \throws PCLException for bad extension (root node metadata must be .oct_idx extension) */ @@ -210,10 +210,10 @@ namespace pcl * * \param min Bounding box min * \param max Bounding box max - * \param node_dim_meters Node dimension in meters (assuming your point data is in meters) + * \param resolution_arg Node dimension in meters (assuming your point data is in meters) * \param root_node_name must end in ".oct_idx" * \param coord_sys Coordinate system which is stored in the JSON metadata - * \throws PCLException if root file extension does not match \ref OutofcoreOctreeBaseNode::node_index_extension + * \throws PCLException if root file extension does not match \ref pcl::outofcore::OutofcoreOctreeBaseNode::node_index_extension */ OutofcoreOctreeBase (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const double resolution_arg, const boost::filesystem::path &root_node_name, const std::string &coord_sys); @@ -310,7 +310,8 @@ namespace pcl //templated PointT methods //-------------------------------------------------------------------------------- - /** \brief Get a list of file paths at query_depth that intersect with your bounding box specified by \ref min and \ref max. When querying with this method, you may be stuck with extra data (some outside of your query bounds) that reside in the files. + /** \brief Get a list of file paths at query_depth that intersect with your bounding box specified by \c min and \c max. + * When querying with this method, you may be stuck with extra data (some outside of your query bounds) that reside in the files. * * \param[in] min The minimum corner of the bounding box * \param[in] max The maximum corner of the bounding box @@ -334,7 +335,7 @@ namespace pcl void queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, AlignedPointTVector &dst) const; - /** \brief Query all points falling within the input bounding box at \ref query_depth and return a PCLPointCloud2 object in \ref dst_blob. + /** \brief Query all points falling within the input bounding box at \c query_depth and return a PCLPointCloud2 object in \c dst_blob. * * \param[in] min The minimum corner of the input bounding box. * \param[in] max The maximum corner of the input bounding box. @@ -344,11 +345,11 @@ namespace pcl void queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob) const; - /** \brief Returns a random subsample of points within the given bounding box at \ref query_depth. + /** \brief Returns a random subsample of points within the given bounding box at \c query_depth. * * \param[in] min The minimum corner of the boudning box to query. * \param[out] max The maximum corner of the bounding box to query. - * \param[in] query_depth The depth in the tree at which to look for the points. Only returns points within the given bounding box at the specified \ref query_depth. + * \param[in] query_depth The depth in the tree at which to look for the points. Only returns points within the given bounding box at the specified \c query_depth. * \param[out] dst The destination in which to return the points. * */ @@ -359,7 +360,8 @@ namespace pcl //PCLPointCloud2 methods //-------------------------------------------------------------------------------- - /** \brief Query all points falling within the input bounding box at \ref query_depth and return a PCLPointCloud2 object in \ref dst_blob. If the optional argument for filter is given, points are processed by that filter before returning. + /** \brief Query all points falling within the input bounding box at \c query_depth and return a PCLPointCloud2 object in \c dst_blob. + * If the optional argument for filter is given, points are processed by that filter before returning. * \param[in] min The minimum corner of the input bounding box. * \param[in] max The maximum corner of the input bounding box. * \param[in] query_depth The depth of tree at which to query; only points at this depth are returned @@ -372,6 +374,7 @@ namespace pcl /** \brief Returns list of pcd files from nodes whose bounding boxes intersect with the input bounding box. * \param[in] min The minimum corner of the input bounding box. * \param[in] max The maximum corner of the input bounding box. + * \param query_depth * \param[out] filenames The list of paths to the PCD files which can be loaded and processed. */ inline virtual void @@ -386,12 +389,15 @@ namespace pcl // -------------------------------------------------------------------------------- /** \brief Get the overall bounding box of the outofcore - * octree; this is the same as the bounding box of the \ref root_node_ node */ + * octree; this is the same as the bounding box of the \c root_node_ node + * \param min + * \param max + */ bool getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const; /** \brief Get number of points at specified LOD - * \param[in] depth the level of detail at which we want the number of points (0 is root, 1, 2,...) + * \param[in] depth_index the level of detail at which we want the number of points (0 is root, 1, 2,...) * \return number of points in the tree at \b depth */ inline boost::uint64_t @@ -489,16 +495,16 @@ namespace pcl this->printBoundingBox (metadata_->getDepth ()); } - /** \brief Returns the voxel centers of all existing voxels at \ref query_depth - \param[in] query_depth: the depth of the tree at which to retrieve occupied/existing voxels - \param[out] vector of PointXYZ voxel centers for nodes that exist at that depth + /** \brief Returns the voxel centers of all existing voxels at \c query_depth + \param[out] voxel_centers Vector of PointXYZ voxel centers for nodes that exist at that depth + \param[in] query_depth the depth of the tree at which to retrieve occupied/existing voxels */ void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, size_t query_depth) const; - /** \brief Returns the voxel centers of all existing voxels at \ref query_depth - \param[in] query_depth: the depth of the tree at which to retrieve occupied/existing voxels - \param[out] vector of PointXYZ voxel centers for nodes that exist at that depth + /** \brief Returns the voxel centers of all existing voxels at \c query_depth + \param[out] voxel_centers Vector of PointXYZ voxel centers for nodes that exist at that depth + \param[in] query_depth the depth of the tree at which to retrieve occupied/existing voxels */ void getOccupiedVoxelCenters(std::vector > &voxel_centers, size_t query_depth) const; diff --git a/outofcore/include/pcl/outofcore/octree_base_node.h b/outofcore/include/pcl/outofcore/octree_base_node.h index 22763272..fff47002 100644 --- a/outofcore/include/pcl/outofcore/octree_base_node.h +++ b/outofcore/include/pcl/outofcore/octree_base_node.h @@ -81,7 +81,7 @@ namespace pcl * * \brief OutofcoreOctreeBaseNode Class internally representing nodes of an * outofcore octree, with accessors to its data via the \ref - * octree_disk_container class or \ref octree_ram_container class, + * pcl::outofcore::OutofcoreOctreeDiskContainer class or \ref pcl::outofcore::OutofcoreOctreeRamContainer class, * whichever it is templated against. * * \ingroup outofcore @@ -130,8 +130,8 @@ namespace pcl //query /** \brief gets the minimum and maximum corner of the bounding box represented by this node - * \param[out] minCoord returns the minimum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z - * \param[out] maxCoord returns the maximum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z + * \param[out] min_bb returns the minimum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z + * \param[out] max_bb returns the maximum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z */ virtual inline void getBoundingBox (Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const @@ -187,6 +187,7 @@ namespace pcl * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates * \param[in] query_depth + * \param percent * \param[out] v std::list of points returned by the query */ virtual void @@ -200,7 +201,7 @@ namespace pcl virtual void queryBBIntersects (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const boost::uint32_t query_depth, std::list &file_names); - /** \brief Write the voxel size to stdout at \ref query_depth + /** \brief Write the voxel size to stdout at \c query_depth * \param[in] query_depth The depth at which to print the size of the voxel/bounding boxes */ virtual void @@ -208,7 +209,7 @@ namespace pcl /** \brief add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point * \param[in] p vector of points to add to the leaf - * \param[in] skipBBCheck whether to check if the point's coordinates fall within the bounding box + * \param[in] skip_bb_check whether to check if the point's coordinates fall within the bounding box */ virtual boost::uint64_t addDataToLeaf (const AlignedPointTVector &p, const bool skip_bb_check = true); @@ -312,9 +313,9 @@ namespace pcl * If creating root, path is full name. If creating any other * node, path is dir; throws exception if directory or metadata not found * - * \param[in] Directory pathname + * \param[in] directory_path pathname * \param[in] super - * \param[in] loadAll + * \param[in] load_all * \throws PCLException if directory is missing * \throws PCLException if node index is missing */ @@ -346,14 +347,14 @@ namespace pcl virtual size_t countNumChildren () const; - /** \brief Counts the number of loaded chilren by testing the \ref children_ array; + /** \brief Counts the number of loaded chilren by testing the \c children_ array; * used to update num_loaded_chilren_ internally */ virtual size_t countNumLoadedChildren () const; /** \brief Save node's metadata to file - * \param[in] recursive: if false, save only this node's metadata to file; if true, recursively + * \param[in] recursive if false, save only this node's metadata to file; if true, recursively * save all children's metadata to files as well */ void @@ -386,7 +387,7 @@ namespace pcl addDataAtMaxDepth (const AlignedPointTVector &p, const bool skip_bb_check = true); /** \brief Add data to the leaf when at max depth of tree. If - * \ref skip_bb_check is true, adds to the node regardless of the + * \c skip_bb_check is true, adds to the node regardless of the * bounding box it represents; otherwise only adds points that * fall within the bounding box * @@ -402,7 +403,7 @@ namespace pcl /** \brief Tests whether the input bounding box intersects with the current node's bounding box * \param[in] min_bb The minimum corner of the input bounding box - * \param[in] min_bb The maximum corner of the input bounding box + * \param[in] max_bb The maximum corner of the input bounding box * \return bool True if any portion of the bounding box intersects with this node's bounding box; false otherwise */ inline bool @@ -416,7 +417,7 @@ namespace pcl inline bool inBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const; - /** \brief Tests whether \ref point falls within the input bounding box + /** \brief Tests whether \c point falls within the input bounding box * \param[in] min_bb The minimum corner of the input bounding box * \param[in] max_bb The maximum corner of the input bounding box * \param[in] point The test point @@ -424,7 +425,7 @@ namespace pcl bool pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point); - /** \brief Tests whether \ref p falls within the input bounding box + /** \brief Tests whether \c p falls within the input bounding box * \param[in] min_bb The minimum corner of the input bounding box * \param[in] max_bb The maximum corner of the input bounding box * \param[in] p The point to be tested @@ -432,10 +433,13 @@ namespace pcl static bool pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const PointT &p); - /** \brief Tests whether \ref x, \ref y, and \ref z fall within the input bounding box + /** \brief Tests whether \c x, \c y, and \c z fall within the input bounding box * \param[in] min_bb The minimum corner of the input bounding box * \param[in] max_bb The maximum corner of the input bounding box - **/ + * \param x + * \param y + * \param z + **/ static bool pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const double x, const double y, const double z); @@ -443,7 +447,7 @@ namespace pcl inline bool pointInBoundingBox (const PointT &p) const; - /** \brief Creates child node \ref idx + /** \brief Creates child node \c idx * \param[in] idx Index (0-7) of the child node */ void diff --git a/outofcore/include/pcl/outofcore/octree_disk_container.h b/outofcore/include/pcl/outofcore/octree_disk_container.h index 74e45d95..9978b9ce 100644 --- a/outofcore/include/pcl/outofcore/octree_disk_container.h +++ b/outofcore/include/pcl/outofcore/octree_disk_container.h @@ -130,7 +130,7 @@ namespace pcl * * \param[in] start index of first point to read from disk * \param[in] count offset of last point to read from disk - * \param[out] v std::vector as destination for points read from disk into memory + * \param[out] dst std::vector as destination for points read from disk into memory */ void readRange (const uint64_t start, const uint64_t count, AlignedPointTVector &dst); @@ -138,7 +138,7 @@ namespace pcl void readRange (const uint64_t, const uint64_t, pcl::PCLPointCloud2::Ptr &dst); - /** \brief Reads the entire point contents from disk into \ref output_cloud + /** \brief Reads the entire point contents from disk into \c output_cloud * \param[out] output_cloud */ int @@ -148,10 +148,10 @@ namespace pcl * unique (could have multiple identical points!) * * \param[in] start The starting index of points to select - * \param count[in] The length of the range of points from which to randomly sample + * \param[in] count The length of the range of points from which to randomly sample * (i.e. from start to start+count) - * \param percent[in] The percentage of count that is enough points to make up this random sample - * \param dst[out] std::vector as destination for randomly sampled points; size will + * \param[in] percent The percentage of count that is enough points to make up this random sample + * \param[out] dst std::vector as destination for randomly sampled points; size will * be percentage*count */ void @@ -171,7 +171,7 @@ namespace pcl readRangeSubSample_bernoulli (const uint64_t start, const uint64_t count, const double percent, AlignedPointTVector& dst); - /** \brief Returns the total number of points for which this container is responsible, \ref filelen_ + points in \ref writebuff_ that have not yet been flushed to the disk + /** \brief Returns the total number of points for which this container is responsible, \c filelen_ + points in \c writebuff_ that have not yet been flushed to the disk */ uint64_t size () const @@ -180,7 +180,7 @@ namespace pcl } /** \brief STL-like empty test - * \return true if container has no data on disk or waiting to be written in \ref writebuff_ */ + * \return true if container has no data on disk or waiting to be written in \c writebuff_ */ inline bool empty () const { @@ -270,11 +270,11 @@ namespace pcl private: //no copy construction - OutofcoreOctreeDiskContainer (const OutofcoreOctreeDiskContainer &rval) { } + OutofcoreOctreeDiskContainer (const OutofcoreOctreeDiskContainer& /*rval*/) { } OutofcoreOctreeDiskContainer& - operator= (const OutofcoreOctreeDiskContainer &rval) { } + operator= (const OutofcoreOctreeDiskContainer& /*rval*/) { } void flushWritebuff (const bool force_cache_dealloc); diff --git a/outofcore/include/pcl/outofcore/octree_ram_container.h b/outofcore/include/pcl/outofcore/octree_ram_container.h index f12370b3..d8988188 100644 --- a/outofcore/include/pcl/outofcore/octree_ram_container.h +++ b/outofcore/include/pcl/outofcore/octree_ram_container.h @@ -84,14 +84,14 @@ namespace pcl insertRange (const PointT* const * start, const uint64_t count); void - insertRange (AlignedPointTVector &p) + insertRange (AlignedPointTVector& /*p*/) { PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeRamContainer] Inserting eigen-aligned point vectors is not implemented using the ram containers\n"); //insertRange (&(p.begin ()), p.size ()); } void - insertRange (const AlignedPointTVector &p) + insertRange (const AlignedPointTVector& /*p*/) { PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeRamContainer] Inserting eigen-aligned point vectors is not implemented using the ram containers\n"); } @@ -154,10 +154,10 @@ namespace pcl protected: //no copy construction - OutofcoreOctreeRamContainer (const OutofcoreOctreeRamContainer &rval) { } + OutofcoreOctreeRamContainer (const OutofcoreOctreeRamContainer& /*rval*/) { } OutofcoreOctreeRamContainer& - operator= (const OutofcoreOctreeRamContainer& rval) { } + operator= (const OutofcoreOctreeRamContainer& /*rval*/) { } //the actual container //std::deque container; diff --git a/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h b/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h index 029b27a7..39615190 100644 --- a/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h +++ b/outofcore/include/pcl/outofcore/outofcore_breadth_first_iterator.h @@ -49,7 +49,7 @@ namespace pcl * * \ingroup outofcore * \author Justin Rosen (jmylesrosen@gmail.com) - * \note Code adapted from \ref octree_iterator.h in Module \ref pcl_octree written by Julius Kammerl + * \note Code adapted from \ref octree_iterator.h in Module \ref pcl::octree written by Julius Kammerl */ template > class OutofcoreBreadthFirstIterator : public OutofcoreIteratorBase diff --git a/outofcore/include/pcl/outofcore/outofcore_depth_first_iterator.h b/outofcore/include/pcl/outofcore/outofcore_depth_first_iterator.h index 28e1f622..e110fb55 100644 --- a/outofcore/include/pcl/outofcore/outofcore_depth_first_iterator.h +++ b/outofcore/include/pcl/outofcore/outofcore_depth_first_iterator.h @@ -48,7 +48,7 @@ namespace pcl * * \ingroup outofcore * \author Stephen Fox (foxstephend@gmail.com) - * \note Code adapted from \ref octree_iterator.h in Module \ref pcl_octree written by Julius Kammerl + * \note Code adapted from \ref octree_iterator.h in Module \ref pcl::octree written by Julius Kammerl */ template > class OutofcoreDepthFirstIterator : public OutofcoreIteratorBase diff --git a/outofcore/include/pcl/outofcore/outofcore_iterator_base.h b/outofcore/include/pcl/outofcore/outofcore_iterator_base.h index 0c92fbc3..a409395a 100644 --- a/outofcore/include/pcl/outofcore/outofcore_iterator_base.h +++ b/outofcore/include/pcl/outofcore/outofcore_iterator_base.h @@ -52,7 +52,7 @@ namespace pcl namespace outofcore { /** \brief Abstract octree iterator class - * \note This class is based on the octree_iterator written by Julius Kammerl adapted to the outofcore octree. The interface is very similar, but it does \b not inherit the \ref pcl_octree iterator base. + * \note This class is based on the octree_iterator written by Julius Kammerl adapted to the outofcore octree. The interface is very similar, but it does \b not inherit the \ref pcl::octree iterator base. * \ingroup outofcore * \author Stephen Fox (foxstephend@gmail.com) */ diff --git a/outofcore/include/pcl/outofcore/visualization/axes.h b/outofcore/include/pcl/outofcore/visualization/axes.h index 24c09eab..e4c2bf76 100644 --- a/outofcore/include/pcl/outofcore/visualization/axes.h +++ b/outofcore/include/pcl/outofcore/visualization/axes.h @@ -9,6 +9,7 @@ #include "object.h" // VTK +#include #include #include #include @@ -31,6 +32,7 @@ public: axes_ = vtkSmartPointer::New (); axes_->SetOrigin (0, 0, 0); axes_->SetScaleFactor (size); + axes_->Update (); vtkSmartPointer axes_colors = vtkSmartPointer::New (); axes_colors->Allocate (6); @@ -42,17 +44,24 @@ public: axes_colors->InsertNextValue (1.0); vtkSmartPointer axes_data = axes_->GetOutput (); - axes_data->Update (); axes_data->GetPointData ()->SetScalars (axes_colors); vtkSmartPointer axes_tubes = vtkSmartPointer::New (); +#if VTK_MAJOR_VERSION < 6 axes_tubes->SetInput (axes_data); +#else + axes_tubes->SetInputData (axes_data); +#endif axes_tubes->SetRadius (axes_->GetScaleFactor () / 100.0); axes_tubes->SetNumberOfSides (6); vtkSmartPointer axes_mapper = vtkSmartPointer::New (); axes_mapper->SetScalarModeToUsePointData (); +#if VTK_MAJOR_VERSION < 6 axes_mapper->SetInput (axes_tubes->GetOutput ()); +#else + axes_mapper->SetInputData (axes_tubes->GetOutput ()); +#endif axes_actor_ = vtkSmartPointer::New (); axes_actor_->GetProperty ()->SetLighting (false); diff --git a/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h b/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h index ac882fd7..ee65e140 100644 --- a/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h +++ b/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h @@ -155,12 +155,12 @@ class OutofcoreCloud : public Object { displayDepth = 0; } - else if (displayDepth > octree_->getDepth ()) + else if (static_cast (displayDepth) > octree_->getDepth ()) { displayDepth = octree_->getDepth (); } - if (display_depth_ != displayDepth) + if (display_depth_ != static_cast (displayDepth)) { display_depth_ = displayDepth; updateVoxelData (); diff --git a/outofcore/src/visualization/camera.cpp b/outofcore/src/visualization/camera.cpp index 951a4457..6f89e667 100644 --- a/outofcore/src/visualization/camera.cpp +++ b/outofcore/src/visualization/camera.cpp @@ -98,7 +98,7 @@ Camera::computeFrustum () // // vtkSmartPointer hull_mapper = static_cast (hull_actor_->GetMapper ()); // -//#if VTK_MAJOR_VERSION <= 5 +//#if VTK_MAJOR_VERSION < 6 // hull_mapper->SetInput (hullData); //#else // hull_mapper->SetInputData(hullData); diff --git a/outofcore/src/visualization/grid.cpp b/outofcore/src/visualization/grid.cpp index c48f9aad..b19cbe32 100644 --- a/outofcore/src/visualization/grid.cpp +++ b/outofcore/src/visualization/grid.cpp @@ -3,6 +3,7 @@ #include // VTK +#include #include #include #include @@ -36,7 +37,7 @@ Grid::Grid (std::string name, int size/*=10*/, double spacing/*=1.0*/) : grid_->SetYCoordinates (y_array); grid_->SetZCoordinates (xz_array); -#if VTK_MAJOR_VERSION <= 5 +#if VTK_MAJOR_VERSION < 6 grid_mapper->SetInputConnection (grid_->GetProducerPort ()); #else grid_mapper->SetInputData(grid_); diff --git a/outofcore/src/visualization/outofcore_cloud.cpp b/outofcore/src/visualization/outofcore_cloud.cpp index 356f5934..eaf490c4 100644 --- a/outofcore/src/visualization/outofcore_cloud.cpp +++ b/outofcore/src/visualization/outofcore_cloud.cpp @@ -24,6 +24,7 @@ #include // VTK +#include #include #include #include @@ -159,10 +160,18 @@ OutofcoreCloud::updateVoxelData () double y = voxel_centers[i].y; double z = voxel_centers[i].z; +#if VTK_MAJOR_VERSION < 6 voxel_data->AddInput (getVtkCube (x - s, x + s, y - s, y + s, z - s, z + s)); +#else + voxel_data->AddInputData (getVtkCube (x - s, x + s, y - s, y + s, z - s, z + s)); +#endif } +#if VTK_MAJOR_VERSION < 6 voxel_mapper->SetInput (voxel_data->GetOutput ()); +#else + voxel_mapper->SetInputData (voxel_data->GetOutput ()); +#endif voxel_actor_->SetMapper (voxel_mapper); voxel_actor_->GetProperty ()->SetRepresentationToWireframe (); @@ -304,7 +313,7 @@ OutofcoreCloud::render (vtkRenderer* renderer) } } - for (int i = 0; i < actors_to_remove.size (); i++) + for (size_t i = 0; i < actors_to_remove.size (); i++) { points_loaded_ -= actors_to_remove.back ()->GetMapper ()->GetInput ()->GetNumberOfPoints (); data_loaded_ -= actors_to_remove.back ()->GetMapper ()->GetInput ()->GetActualMemorySize(); diff --git a/outofcore/src/visualization/scene.cpp b/outofcore/src/visualization/scene.cpp index 5c9ffb0b..4d5c9bdf 100644 --- a/outofcore/src/visualization/scene.cpp +++ b/outofcore/src/visualization/scene.cpp @@ -28,7 +28,7 @@ Scene::getCameras () Camera* Scene::getCamera (vtkCamera *camera) { - for (int i = 0; i < cameras_.size (); i++) + for (size_t i = 0; i < cameras_.size (); i++) { if (cameras_[i]->getCamera ().GetPointer () == camera) { @@ -42,7 +42,7 @@ Scene::getCamera (vtkCamera *camera) Camera* Scene::getCamera (std::string name) { - for (int i = 0; i < cameras_.size (); i++) + for (size_t i = 0; i < cameras_.size (); i++) if (cameras_[i]->getName () == name) return cameras_[i]; @@ -60,7 +60,7 @@ Scene::addObject (Object *object) Object* Scene::getObjectByName (std::string name) { - for (int i = 0; i < objects_.size (); i++) + for (size_t i = 0; i < objects_.size (); i++) if (objects_[i]->getName () == name) return objects_[i]; diff --git a/outofcore/src/visualization/viewport.cpp b/outofcore/src/visualization/viewport.cpp index bd05bec9..f2ae2433 100644 --- a/outofcore/src/visualization/viewport.cpp +++ b/outofcore/src/visualization/viewport.cpp @@ -144,7 +144,7 @@ Viewport::viewportActorUpdate () std::vector cameras = scene->getCameras (); - for (int i = 0; i < cameras.size (); i++) + for (size_t i = 0; i < cameras.size (); i++) { cameras[i]->render (renderer_); // if (cameras[i]->getCamera () != renderer_->GetActiveCamera ()) @@ -158,7 +158,7 @@ Viewport::viewportActorUpdate () } std::vector objects = scene->getObjects (); - for (int i = 0; i < objects.size (); i++) + for (size_t i = 0; i < objects.size (); i++) { //std::cout << objects[i]->getName () << std::endl; objects[i]->render (renderer_); @@ -189,7 +189,7 @@ Viewport::viewportHudUpdate () uint64_t points_loaded = 0; uint64_t data_loaded = 0; - for (int i = 0; i < objects.size (); i++) + for (size_t i = 0; i < objects.size (); i++) { //TYPE& dynamic_cast (object); OutofcoreCloud* cloud = dynamic_cast (objects[i]); @@ -201,7 +201,7 @@ Viewport::viewportHudUpdate () } char points_loaded_str[50]; - sprintf (points_loaded_str, "%llu points/%llu mb", points_loaded, data_loaded/1024); + sprintf (points_loaded_str, "%lu points/%lu mb", points_loaded, data_loaded/1024); points_hud_actor_->SetInput (points_loaded_str); } diff --git a/outofcore/tools/CMakeLists.txt b/outofcore/tools/CMakeLists.txt index 25c68e6d..17444d29 100644 --- a/outofcore/tools/CMakeLists.txt +++ b/outofcore/tools/CMakeLists.txt @@ -1,8 +1,8 @@ # pcl_outofcore_process -PCL_ADD_EXECUTABLE(pcl_outofcore_process ${SUBSYS_NAME} outofcore_process.cpp) +PCL_ADD_EXECUTABLE(pcl_outofcore_process "${SUBSYS_NAME}" outofcore_process.cpp) target_link_libraries(pcl_outofcore_process pcl_common pcl_filters pcl_io pcl_octree pcl_outofcore) -PCL_ADD_EXECUTABLE(pcl_outofcore_print ${SUBSYS_NAME} outofcore_print.cpp) +PCL_ADD_EXECUTABLE(pcl_outofcore_print "${SUBSYS_NAME}" outofcore_print.cpp) target_link_libraries(pcl_outofcore_print pcl_common pcl_filters pcl_io pcl_octree pcl_outofcore) if(NOT VTK_FOUND) @@ -11,9 +11,9 @@ if(NOT VTK_FOUND) else(NOT VTK_FOUND) set(DEFAULT TRUE) set(REASON) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") if("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_GREATER "5.8") # pcl_outofcore_viewer uses some functions not present in vtk 5.8 @@ -28,7 +28,7 @@ else(NOT VTK_FOUND) ../src/visualization/viewport.cpp) # pcl_outofcore_viewer - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_outofcore_viewer ${SUBSYS_NAME} ${srcs}) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_outofcore_viewer "${SUBSYS_NAME}" ${srcs}) target_link_libraries(pcl_outofcore_viewer pcl_common pcl_io pcl_outofcore pcl_visualization pcl_octree pcl_filters) endif("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_GREATER "5.8") diff --git a/outofcore/tools/outofcore_print.cpp b/outofcore/tools/outofcore_print.cpp index 349e0c45..18e2910a 100644 --- a/outofcore/tools/outofcore_print.cpp +++ b/outofcore/tools/outofcore_print.cpp @@ -87,7 +87,7 @@ typedef Eigen::aligned_allocator AlignedPointT; void printDepth(size_t depth) { - for (int i=0; i < depth; i++) + for (size_t i = 0; i < depth; i++) PCL_INFO (" "); } diff --git a/outofcore/tools/outofcore_viewer.cpp b/outofcore/tools/outofcore_viewer.cpp index 7402ab34..59332c08 100644 --- a/outofcore/tools/outofcore_viewer.cpp +++ b/outofcore/tools/outofcore_viewer.cpp @@ -133,9 +133,6 @@ typedef Eigen::aligned_allocator AlignedPointT; #include #include -// Definitions -const int MAX_DEPTH (-1); - // Globals vtkSmartPointer window; @@ -322,11 +319,8 @@ outofcoreViewer (boost::filesystem::path tree_root, int depth, bool display_octr } void -print_help (int argc, char **argv) +print_help (int, char **argv) { - //suppress unused parameter warning - assert(argc == argc); - print_info ("This program is used to visualize outofcore data structure"); print_info ("%s \n", argv[0]); print_info ("\n"); diff --git a/pcl_config.h.in b/pcl_config.h.in index 40cc2dd6..f8b4cb37 100644 --- a/pcl_config.h.in +++ b/pcl_config.h.in @@ -16,6 +16,8 @@ #cmakedefine HAVE_OPENNI 1 +#cmakedefine HAVE_OPENNI2 1 + #cmakedefine HAVE_QHULL 1 #cmakedefine HAVE_QHULL_2011 1 @@ -44,6 +46,10 @@ #undef HAVE_OPENNI #endif +#ifdef DISABLE_OPENNI2 +#undef HAVE_OPENNI2 +#endif + #ifdef DISABLE_QHULL #undef HAVE_QHULL #endif @@ -55,3 +61,7 @@ #cmakedefine VERBOSITY_LEVEL_INFO #cmakedefine VERBOSITY_LEVEL_DEBUG #cmakedefine VERBOSITY_LEVEL_VERBOSE + +/* Address the cases where on MacOS and OpenGL and GLUT are not frameworks */ +#cmakedefine OPENGL_IS_A_FRAMEWORK +#cmakedefine GLUT_IS_A_FRAMEWORK diff --git a/people/CMakeLists.txt b/people/CMakeLists.txt index d606adf1..6e99fb8c 100644 --- a/people/CMakeLists.txt +++ b/people/CMakeLists.txt @@ -8,50 +8,50 @@ if(NOT VTK_FOUND) else(NOT VTK_FOUND) set(DEFAULT TRUE) set(REASON) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") endif(NOT VTK_FOUND) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(incs - include/pcl/${SUBSYS_NAME}/ground_based_people_detection_app.h - include/pcl/${SUBSYS_NAME}/head_based_subcluster.h - include/pcl/${SUBSYS_NAME}/height_map_2d.h - include/pcl/${SUBSYS_NAME}/person_classifier.h - include/pcl/${SUBSYS_NAME}/person_cluster.h - include/pcl/${SUBSYS_NAME}/hog.h + "include/pcl/${SUBSYS_NAME}/ground_based_people_detection_app.h" + "include/pcl/${SUBSYS_NAME}/head_based_subcluster.h" + "include/pcl/${SUBSYS_NAME}/height_map_2d.h" + "include/pcl/${SUBSYS_NAME}/person_classifier.h" + "include/pcl/${SUBSYS_NAME}/person_cluster.h" + "include/pcl/${SUBSYS_NAME}/hog.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/ground_based_people_detection_app.hpp - include/pcl/${SUBSYS_NAME}/impl/head_based_subcluster.hpp - include/pcl/${SUBSYS_NAME}/impl/height_map_2d.hpp - include/pcl/${SUBSYS_NAME}/impl/person_classifier.hpp - include/pcl/${SUBSYS_NAME}/impl/person_cluster.hpp + "include/pcl/${SUBSYS_NAME}/impl/ground_based_people_detection_app.hpp" + "include/pcl/${SUBSYS_NAME}/impl/head_based_subcluster.hpp" + "include/pcl/${SUBSYS_NAME}/impl/height_map_2d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/person_classifier.hpp" + "include/pcl/${SUBSYS_NAME}/impl/person_cluster.hpp" ) set(srcs src/hog.cpp) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) - #SET_TARGET_PROPERTIES(pcl_${SUBSYS_NAME} PROPERTIES LINKER_LANGUAGE CXX) + #SET_TARGET_PROPERTIES("${LIB_NAME}" PROPERTIES LINKER_LANGUAGE CXX) if(OPENNI_FOUND AND BUILD_OPENNI) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ground_based_rgbd_people_detector ${SUBSYS_NAME} apps/main_ground_based_people_detection.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ground_based_rgbd_people_detector "${SUBSYS_NAME}" apps/main_ground_based_people_detection.cpp) target_link_libraries(pcl_ground_based_rgbd_people_detector pcl_common pcl_kdtree pcl_search pcl_features pcl_sample_consensus pcl_filters pcl_io pcl_visualization pcl_segmentation pcl_people) endif() endif(build) diff --git a/people/apps/main_ground_based_people_detection.cpp b/people/apps/main_ground_based_people_detection.cpp index 31d22f73..b44d3197 100644 --- a/people/apps/main_ground_based_people_detection.cpp +++ b/people/apps/main_ground_based_people_detection.cpp @@ -118,6 +118,8 @@ int main (int argc, char** argv) // Algorithm parameters: std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml"; float min_confidence = -1.5; + float min_width = 0.1; + float max_width = 8.0; float min_height = 1.3; float max_height = 2.3; float voxel_size = 0.06; @@ -190,7 +192,7 @@ int main (int argc, char** argv) people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier - people_detector.setHeightLimits(min_height, max_height); // set person classifier + people_detector.setPersonClusterLimits(min_height, max_height, min_width, max_width); people_detector.setSamplingFactor(sampling_factor); // set a downsampling factor to the point cloud (for increasing speed) // people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical diff --git a/people/include/pcl/people/ground_based_people_detection_app.h b/people/include/pcl/people/ground_based_people_detection_app.h index 08ff9699..a4672c42 100644 --- a/people/include/pcl/people/ground_based_people_detection_app.h +++ b/people/include/pcl/people/ground_based_people_detection_app.h @@ -51,6 +51,7 @@ #include #include #include +#include namespace pcl { @@ -98,6 +99,14 @@ namespace pcl void setGround (Eigen::VectorXf& ground_coeffs); + /** + * \brief Set the transformation matrix, which is used in order to transform the given point cloud, the ground plane and the intrinsics matrix to the internal coordinate frame. + * + * \param[in] cloud A pointer to the input cloud. + */ + void + setTransformation (const Eigen::Matrix3f& transformation); + /** * \brief Set sampling factor. * @@ -130,6 +139,15 @@ namespace pcl void setClassifier (pcl::people::PersonClassifier person_classifier); + /** + * \brief Set the field of view of the point cloud in z direction. + * + * \param[in] min The beginning of the field of view in z-direction, should be usually set to zero. + * \param[in] max The end of the field of view in z-direction. + */ + void + setFOV (float min, float max); + /** * \brief Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode). * @@ -147,22 +165,15 @@ namespace pcl setHeadCentroid (bool head_centroid); /** - * \brief Set minimum and maximum allowed height for a person cluster. + * \brief Set minimum and maximum allowed height and width for a person cluster. * * \param[in] min_height Minimum allowed height for a person cluster (default = 1.3). * \param[in] max_height Maximum allowed height for a person cluster (default = 2.3). + * \param[in] min_width Minimum width for a person cluster (default = 0.1). + * \param[in] max_width Maximum width for a person cluster (default = 8.0). */ void - setHeightLimits (float min_height, float max_height); - - /** - * \brief Set minimum and maximum allowed number of points for a person cluster. - * - * \param[in] min_points Minimum allowed number of points for a person cluster. - * \param[in] max_points Maximum allowed number of points for a person cluster. - */ - void - setDimensionLimits (int min_points, int max_points); + setPersonClusterLimits (float min_height, float max_height, float min_width, float max_width); /** * \brief Set minimum distance between persons' heads. @@ -173,13 +184,15 @@ namespace pcl setMinimumDistanceBetweenHeads (float heads_minimum_distance); /** - * \brief Get minimum and maximum allowed height for a person cluster. + * \brief Get the minimum and maximum allowed height and width for a person cluster. * * \param[out] min_height Minimum allowed height for a person cluster. * \param[out] max_height Maximum allowed height for a person cluster. + * \param[out] min_width Minimum width for a person cluster. + * \param[out] max_width Maximum width for a person cluster. */ void - getHeightLimits (float& min_height, float& max_height); + getPersonClusterLimits (float& min_height, float& max_height, float& min_width, float& max_width); /** * \brief Get minimum and maximum allowed number of points for a person cluster. @@ -202,6 +215,12 @@ namespace pcl Eigen::VectorXf getGround (); + /** + * \brief Get the filtered point cloud. + */ + PointCloudPtr + getFilteredCloud (); + /** * \brief Get pointcloud after voxel grid filtering and ground removal. */ @@ -225,6 +244,36 @@ namespace pcl void swapDimensions (pcl::PointCloud::Ptr& cloud); + /** + * \brief Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel size. + */ + void + updateMinMaxPoints (); + + /** + * \brief Applies the transformation to the input point cloud. + */ + void + applyTransformationPointCloud (); + + /** + * \brief Applies the transformation to the ground plane. + */ + void + applyTransformationGround (); + + /** + * \brief Applies the transformation to the intrinsics matrix. + */ + void + applyTransformationIntrinsics (); + + /** + * \brief Reduces the input cloud to one point per voxel and limits the field of view. + */ + void + filter (); + /** * \brief Perform people detection on the input data and return people clusters information. * @@ -243,13 +292,28 @@ namespace pcl float voxel_size_; /** \brief ground plane coefficients */ - Eigen::VectorXf ground_coeffs_; - + Eigen::VectorXf ground_coeffs_; + + /** \brief flag stating whether the ground coefficients have been set or not */ + bool ground_coeffs_set_; + + /** \brief the transformed ground coefficients */ + Eigen::VectorXf ground_coeffs_transformed_; + /** \brief ground plane normalization factor */ - float sqrt_ground_coeffs_; - + float sqrt_ground_coeffs_; + + /** \brief rotation matrix which transforms input point cloud to internal people tracker coordinate frame */ + Eigen::Matrix3f transformation_; + + /** \brief flag stating whether the transformation matrix has been set or not */ + bool transformation_set_; + /** \brief pointer to the input cloud */ - PointCloudPtr cloud_; + PointCloudPtr cloud_; + + /** \brief pointer to the filtered cloud */ + PointCloudPtr cloud_filtered_; /** \brief pointer to the cloud after voxel grid filtering and ground removal */ PointCloudPtr no_ground_cloud_; @@ -261,8 +325,20 @@ namespace pcl float max_height_; /** \brief person clusters minimum height from the ground plane */ - float min_height_; - + float min_height_; + + /** \brief person clusters maximum width, used to estimate how many points maximally represent a person cluster */ + float max_width_; + + /** \brief person clusters minimum width, used to estimate how many points minimally represent a person cluster */ + float min_width_; + + /** \brief the beginning of the field of view in z-direction, should be usually set to zero */ + float min_fov_; + + /** \brief the end of the field of view in z-direction */ + float max_fov_; + /** \brief if true, the sensor is considered to be vertically placed (portrait mode) */ bool vertical_; @@ -276,20 +352,23 @@ namespace pcl /** \brief minimum number of points for a person cluster */ int min_points_; - /** \brief true if min_points and max_points have been set by the user, false otherwise */ - bool dimension_limits_set_; - /** \brief minimum distance between persons' heads */ float heads_minimum_distance_; /** \brief intrinsic parameters matrix of the RGB camera */ - Eigen::Matrix3f intrinsics_matrix_; - + Eigen::Matrix3f intrinsics_matrix_; + + /** \brief flag stating whether the intrinsics matrix has been set or not */ + bool intrinsics_matrix_set_; + + /** \brief the transformed intrinsics matrix */ + Eigen::Matrix3f intrinsics_matrix_transformed_; + /** \brief SVM-based person classifier */ pcl::people::PersonClassifier person_classifier_; /** \brief flag stating if the classifier has been set or not */ - bool person_classifier_set_flag_; + bool person_classifier_set_flag_; }; } /* namespace people */ } /* namespace pcl */ diff --git a/people/include/pcl/people/head_based_subcluster.h b/people/include/pcl/people/head_based_subcluster.h index 9a5b5afa..0a60bbcf 100644 --- a/people/include/pcl/people/head_based_subcluster.h +++ b/people/include/pcl/people/head_based_subcluster.h @@ -92,8 +92,8 @@ namespace pcl * \brief Create subclusters centered on the heads position from the current cluster. * * \param[in] cluster A PersonCluster. - * \param[in] maxima_number Number of local maxima to use as centers of the new cluster. - * \param[in] maxima_cloud_indices Cloud indices of local maxima to use as centers of the new cluster. + * \param[in] maxima_number_after_filtering Number of local maxima to use as centers of the new cluster. + * \param[in] maxima_cloud_indices_filtered Cloud indices of local maxima to use as centers of the new cluster. * \param[out] subclusters Output vector of PersonCluster objects derived from the input cluster. */ void diff --git a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp index cce8263d..bade4761 100644 --- a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp +++ b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp @@ -53,16 +53,23 @@ pcl::people::GroundBasedPeopleDetectionApp::GroundBasedPeopleDetectionAp voxel_size_ = 0.06; vertical_ = false; head_centroid_ = true; + min_fov_ = 0; + max_fov_ = 50; min_height_ = 1.3; max_height_ = 2.3; - min_points_ = 30; // this value is adapted to the voxel size in method "compute" - max_points_ = 5000; // this value is adapted to the voxel size in method "compute" - dimension_limits_set_ = false; + min_width_ = 0.1; + max_width_ = 8.0; + updateMinMaxPoints (); heads_minimum_distance_ = 0.3; // set flag values for mandatory parameters: sqrt_ground_coeffs_ = std::numeric_limits::quiet_NaN(); + ground_coeffs_set_ = false; + intrinsics_matrix_set_ = false; person_classifier_set_flag_ = false; + + // set other flags + transformation_set_ = false; } template void @@ -71,11 +78,27 @@ pcl::people::GroundBasedPeopleDetectionApp::setInputCloud (PointCloudPtr cloud_ = cloud; } +template void +pcl::people::GroundBasedPeopleDetectionApp::setTransformation (const Eigen::Matrix3f& transformation) +{ + if (!transformation.isUnitary()) + { + PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::setCloudTransform] The cloud transformation matrix must be an orthogonal matrix!\n"); + } + + transformation_ = transformation; + transformation_set_ = true; + applyTransformationGround(); + applyTransformationIntrinsics(); +} + template void pcl::people::GroundBasedPeopleDetectionApp::setGround (Eigen::VectorXf& ground_coeffs) { ground_coeffs_ = ground_coeffs; + ground_coeffs_set_ = true; sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm(); + applyTransformationGround(); } template void @@ -88,12 +111,15 @@ template void pcl::people::GroundBasedPeopleDetectionApp::setVoxelSize (float voxel_size) { voxel_size_ = voxel_size; + updateMinMaxPoints (); } template void pcl::people::GroundBasedPeopleDetectionApp::setIntrinsics (Eigen::Matrix3f intrinsics_matrix) { intrinsics_matrix_ = intrinsics_matrix; + intrinsics_matrix_set_ = true; + applyTransformationIntrinsics(); } template void @@ -103,25 +129,34 @@ pcl::people::GroundBasedPeopleDetectionApp::setClassifier (pcl::people:: person_classifier_set_flag_ = true; } +template void +pcl::people::GroundBasedPeopleDetectionApp::setFOV (float min_fov, float max_fov) +{ + min_fov_ = min_fov; + max_fov_ = max_fov; +} + template void pcl::people::GroundBasedPeopleDetectionApp::setSensorPortraitOrientation (bool vertical) { vertical_ = vertical; } -template void -pcl::people::GroundBasedPeopleDetectionApp::setHeightLimits (float min_height, float max_height) +template +void pcl::people::GroundBasedPeopleDetectionApp::updateMinMaxPoints () { - min_height_ = min_height; - max_height_ = max_height; + min_points_ = (int) (min_height_ * min_width_ / voxel_size_ / voxel_size_); + max_points_ = (int) (max_height_ * max_width_ / voxel_size_ / voxel_size_); } template void -pcl::people::GroundBasedPeopleDetectionApp::setDimensionLimits (int min_points, int max_points) +pcl::people::GroundBasedPeopleDetectionApp::setPersonClusterLimits (float min_height, float max_height, float min_width, float max_width) { - min_points_ = min_points; - max_points_ = max_points; - dimension_limits_set_ = true; + min_height_ = min_height; + max_height_ = max_height; + min_width_ = min_width; + max_width_ = max_width; + updateMinMaxPoints (); } template void @@ -137,10 +172,12 @@ pcl::people::GroundBasedPeopleDetectionApp::setHeadCentroid (bool head_c } template void -pcl::people::GroundBasedPeopleDetectionApp::getHeightLimits (float& min_height, float& max_height) +pcl::people::GroundBasedPeopleDetectionApp::getPersonClusterLimits (float& min_height, float& max_height, float& min_width, float& max_width) { min_height = min_height_; max_height = max_height_; + min_width = min_width_; + max_width = max_width_; } template void @@ -159,13 +196,19 @@ pcl::people::GroundBasedPeopleDetectionApp::getMinimumDistanceBetweenHea template Eigen::VectorXf pcl::people::GroundBasedPeopleDetectionApp::getGround () { - if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) + if (!ground_coeffs_set_) { PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n"); } return (ground_coeffs_); } +template typename pcl::people::GroundBasedPeopleDetectionApp::PointCloudPtr +pcl::people::GroundBasedPeopleDetectionApp::getFilteredCloud () +{ + return (cloud_filtered_); +} + template typename pcl::people::GroundBasedPeopleDetectionApp::PointCloudPtr pcl::people::GroundBasedPeopleDetectionApp::getNoGroundCloud () { @@ -181,9 +224,9 @@ pcl::people::GroundBasedPeopleDetectionApp::extractRGBFromPointCloud (Po output_cloud->height = input_cloud->height; pcl::RGB rgb_point; - for (int j = 0; j < input_cloud->width; j++) + for (uint32_t j = 0; j < input_cloud->width; j++) { - for (int i = 0; i < input_cloud->height; i++) + for (uint32_t i = 0; i < input_cloud->height; i++) { rgb_point.r = (*input_cloud)(j,i).r; rgb_point.g = (*input_cloud)(j,i).g; @@ -200,9 +243,9 @@ pcl::people::GroundBasedPeopleDetectionApp::swapDimensions (pcl::PointCl output_cloud->points.resize(cloud->height*cloud->width); output_cloud->width = cloud->height; output_cloud->height = cloud->width; - for (int i = 0; i < cloud->width; i++) + for (uint32_t i = 0; i < cloud->width; i++) { - for (int j = 0; j < cloud->height; j++) + for (uint32_t j = 0; j < cloud->height; j++) { (*output_cloud)(j,i) = (*cloud)(cloud->width - i - 1, j); } @@ -210,11 +253,62 @@ pcl::people::GroundBasedPeopleDetectionApp::swapDimensions (pcl::PointCl cloud = output_cloud; } +template void +pcl::people::GroundBasedPeopleDetectionApp::applyTransformationPointCloud () +{ + if (transformation_set_) + { + Eigen::Transform transform; + transform = transformation_; + pcl::transformPointCloud(*cloud_, *cloud_, transform); + } +} + +template void +pcl::people::GroundBasedPeopleDetectionApp::applyTransformationGround () +{ + if (transformation_set_ && ground_coeffs_set_) + { + Eigen::Transform transform; + transform = transformation_; + ground_coeffs_transformed_ = transform.matrix() * ground_coeffs_; + } + else + { + ground_coeffs_transformed_ = ground_coeffs_; + } +} + +template void +pcl::people::GroundBasedPeopleDetectionApp::applyTransformationIntrinsics () +{ + if (transformation_set_ && intrinsics_matrix_set_) + { + intrinsics_matrix_transformed_ = intrinsics_matrix_ * transformation_.transpose(); + } + else + { + intrinsics_matrix_transformed_ = intrinsics_matrix_; + } +} + +template void +pcl::people::GroundBasedPeopleDetectionApp::filter () +{ + cloud_filtered_ = PointCloudPtr (new PointCloud); + pcl::VoxelGrid grid; + grid.setInputCloud(cloud_); + grid.setLeafSize(voxel_size_, voxel_size_, voxel_size_); + grid.setFilterFieldName("z"); + grid.setFilterLimits(min_fov_, max_fov_); + grid.filter(*cloud_filtered_); +} + template bool pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector >& clusters) { // Check if all mandatory variables have been set: - if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) + if (!ground_coeffs_set_) { PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n"); return (false); @@ -224,7 +318,7 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector::compute (std::vector 0.06) - min_points_ = int(float(min_points_) * std::pow(0.06/voxel_size_, 2)); - } - // Fill rgb image: rgb_image_->points.clear(); // clear RGB pointcloud extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud - + // Downsample of sampling_factor in every dimension: if (sampling_factor_ != 1) { @@ -255,9 +341,9 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vectorheight = (cloud_->height)/sampling_factor_; cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width); cloud_downsampled->is_dense = cloud_->is_dense; - for (int j = 0; j < cloud_downsampled->width; j++) + for (uint32_t j = 0; j < cloud_downsampled->width; j++) { - for (int i = 0; i < cloud_downsampled->height; i++) + for (uint32_t i = 0; i < cloud_downsampled->height; i++) { (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i); } @@ -265,25 +351,22 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector voxel_grid_filter_object; - voxel_grid_filter_object.setInputCloud(cloud_); - voxel_grid_filter_object.setLeafSize (voxel_size_, voxel_size_, voxel_size_); - voxel_grid_filter_object.filter (*cloud_filtered); + applyTransformationPointCloud(); + + filter(); // Ground removal and update: pcl::IndicesPtr inliers(new std::vector); - boost::shared_ptr > ground_model(new pcl::SampleConsensusModelPlane(cloud_filtered)); - ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers); + boost::shared_ptr > ground_model(new pcl::SampleConsensusModelPlane(cloud_filtered_)); + ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers); no_ground_cloud_ = PointCloudPtr (new PointCloud); pcl::ExtractIndices extract; - extract.setInputCloud(cloud_filtered); + extract.setInputCloud(cloud_filtered_); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*no_ground_cloud_); if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast (sampling_factor_), 2))) - ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_); + ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_transformed_, ground_coeffs_transformed_); else PCL_INFO ("No groundplane update!\n"); @@ -292,7 +375,7 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector::Ptr tree (new pcl::search::KdTree); tree->setInputCloud(no_ground_cloud_); pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(2 * 0.06); + ec.setClusterTolerance(2 * voxel_size_); ec.setMinClusterSize(min_points_); ec.setMaxClusterSize(max_points_); ec.setSearchMethod(tree); @@ -302,7 +385,7 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector subclustering; subclustering.setInputCloud(no_ground_cloud_); - subclustering.setGround(ground_coeffs_); + subclustering.setGround(ground_coeffs_transformed_); subclustering.setInitialClusters(cluster_indices); subclustering.setHeightLimits(min_height_, max_height_); subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_); @@ -317,11 +400,11 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector >::iterator it = clusters.begin(); it != clusters.end(); ++it) { //Evaluate confidence for the current PersonCluster: - Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter()); + Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (it->getTCenter()); centroid /= centroid(2); - Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop()); + Eigen::Vector3f top = intrinsics_matrix_transformed_ * (it->getTTop()); top /= top(2); - Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom()); + Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (it->getTBottom()); bottom /= bottom(2); it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_)); } diff --git a/people/include/pcl/people/impl/height_map_2d.hpp b/people/include/pcl/people/impl/height_map_2d.hpp index 3821b795..427b4951 100644 --- a/people/include/pcl/people/impl/height_map_2d.hpp +++ b/people/include/pcl/people/impl/height_map_2d.hpp @@ -93,7 +93,7 @@ pcl::people::HeightMap2D::compute (pcl::people::PersonCluster& c index = int((p->x - cluster.getMin()(0)) / bin_size_); else // camera vertical index = int((p->y - cluster.getMin()(1)) / bin_size_); - if(index > (buckets_.size() - 1)) + if (index > (static_cast (buckets_.size ()) - 1)) std::cout << "Error: out of array - " << index << " of " << buckets_.size() << std::endl; else { @@ -136,7 +136,7 @@ pcl::people::HeightMap2D::searchLocalMaxima () // Main loop: int i = 1; - while(i < (buckets_.size()-1)) + while (i < (static_cast (buckets_.size()) - 1)) { right = buckets_[i+1]; if ((buckets_[i] > left) && (buckets_[i] > right)) diff --git a/people/include/pcl/people/impl/person_classifier.hpp b/people/include/pcl/people/impl/person_classifier.hpp index 021ab3dd..970dcbaf 100644 --- a/people/include/pcl/people/impl/person_classifier.hpp +++ b/people/include/pcl/people/impl/person_classifier.hpp @@ -138,9 +138,9 @@ pcl::people::PersonClassifier::resize (PointCloudPtr& input_image, int c1, c2, f1, f2; PointT g1, g2, g3, g4; float w1, w2; - for (unsigned int i = 0; i < height; i++) // for every row + for (int i = 0; i < height; i++) // for every row { - for (unsigned int j = 0; j < width; j++) // for every column + for (int j = 0; j < width; j++) // for every column { A = T_inv * Eigen::Vector3f(i, j, 1); c1 = ceil(A(0)); @@ -150,12 +150,12 @@ pcl::people::PersonClassifier::resize (PointCloudPtr& input_image, if ( (f1 < 0) || (c1 < 0) || - (f1 >= input_image->height) || - (c1 >= input_image->height) || + (f1 >= static_cast (input_image->height)) || + (c1 >= static_cast (input_image->height)) || (f2 < 0) || (c2 < 0) || - (f2 >= input_image->width) || - (c2 >= input_image->width)) + (f2 >= static_cast (input_image->width)) || + (c2 >= static_cast (input_image->width))) { // if out of range, continue continue; } @@ -203,12 +203,12 @@ pcl::people::PersonClassifier::copyMakeBorder (PointCloudPtr& input_imag int y_start_out = std::max(0, -ymin); //int y_end_out = y_start_out + (y_end_in - y_start_in); - for (unsigned int i = 0; i < (y_end_in - y_start_in + 1); i++) + for (int i = 0; i < (y_end_in - y_start_in + 1); i++) { - for (unsigned int j = 0; j < (x_end_in - x_start_in + 1); j++) - { - (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i); - } + for (int j = 0; j < (x_end_in - x_start_in + 1); j++) + { + (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i); + } } } @@ -243,9 +243,9 @@ pcl::people::PersonClassifier::evaluate (float height_person, // Convert the image to array of float: float* sample_float = new float[sample->width * sample->height * 3]; int delta = sample->height * sample->width; - for(int row = 0; row < sample->height; row++) + for (uint32_t row = 0; row < sample->height; row++) { - for(int col = 0; col < sample->width; col++) + for (uint32_t col = 0; col < sample->width; col++) { sample_float[row + sample->height * col] = ((float) ((*sample)(col, row).r))/255; //ptr[col * 3 + 2]; sample_float[row + sample->height * col + delta] = ((float) ((*sample)(col, row).g))/255; //ptr[col * 3 + 1]; diff --git a/people/include/pcl/people/person_cluster.h b/people/include/pcl/people/person_cluster.h index e5e53c3c..13d2cbf9 100644 --- a/people/include/pcl/people/person_cluster.h +++ b/people/include/pcl/people/person_cluster.h @@ -301,7 +301,7 @@ namespace pcl /** * \brief Draws the theoretical 3D bounding box of the cluster in the PCL visualizer. * \param[in] viewer PCL visualizer. - * \param[in] person_numbers progressive number representing the person. + * \param[in] person_number progressive number representing the person. */ void drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number); diff --git a/people/src/hog.cpp b/people/src/hog.cpp index 8daa4390..0eec61d3 100644 --- a/people/src/hog.cpp +++ b/people/src/hog.cpp @@ -339,11 +339,11 @@ pcl::people::HOG::compute (float *I, float *descriptor) const // Select descriptor of internal part of the image (remove borders): int k = 0; - for (unsigned int l = 0; l < (n_orients_ * 4); l++) + for (int l = 0; l < (n_orients_ * 4); l++) { - for (unsigned int j = 1; j < (w_ / bin_size_ - 1); j++) + for (int j = 1; j < (w_ / bin_size_ - 1); j++) { - for (unsigned int i = 1; i < (h_ / bin_size_ - 1); i++) + for (int i = 1; i < (h_ / bin_size_ - 1); i++) { descriptor[k] = G[i + j * h_ / bin_size_ + l * (h_ / bin_size_) * (w_ / bin_size_)]; k++; diff --git a/recognition/CMakeLists.txt b/recognition/CMakeLists.txt index 443fd23c..a091fcc6 100644 --- a/recognition/CMakeLists.txt +++ b/recognition/CMakeLists.txt @@ -3,110 +3,105 @@ set(SUBSYS_DESC "Point cloud recognition library") set(SUBSYS_DEPS common io search kdtree octree features filters registration sample_consensus) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(LINEMOD_INCLUDES - include/pcl/${SUBSYS_NAME}/linemod/line_rgbd.h + "include/pcl/${SUBSYS_NAME}/linemod/line_rgbd.h" ) set(LINEMOD_IMPLS - include/pcl/${SUBSYS_NAME}/impl/linemod/line_rgbd.hpp + "include/pcl/${SUBSYS_NAME}/impl/linemod/line_rgbd.hpp" ) set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/color_gradient_dot_modality.h - include/pcl/${SUBSYS_NAME}/color_gradient_modality.h - include/pcl/${SUBSYS_NAME}/color_modality.h - include/pcl/${SUBSYS_NAME}/crh_alignment.h - include/pcl/${SUBSYS_NAME}/linemod.h - include/pcl/${SUBSYS_NAME}/dotmod.h - include/pcl/${SUBSYS_NAME}/quantizable_modality.h - include/pcl/${SUBSYS_NAME}/quantized_map.h - include/pcl/${SUBSYS_NAME}/dot_modality.h - include/pcl/${SUBSYS_NAME}/region_xy.h - include/pcl/${SUBSYS_NAME}/mask_map.h - include/pcl/${SUBSYS_NAME}/point_types.h - include/pcl/${SUBSYS_NAME}/distance_map.h - include/pcl/${SUBSYS_NAME}/dense_quantized_multi_mod_template.h - include/pcl/${SUBSYS_NAME}/sparse_quantized_multi_mod_template.h - include/pcl/${SUBSYS_NAME}/surface_normal_modality.h - include/pcl/${SUBSYS_NAME}/linemod/line_rgbd.h - include/pcl/${SUBSYS_NAME}/implicit_shape_model.h - include/pcl/${SUBSYS_NAME}/ransac_based/auxiliary.h - include/pcl/${SUBSYS_NAME}/ransac_based/hypothesis.h - include/pcl/${SUBSYS_NAME}/ransac_based/model_library.h - include/pcl/${SUBSYS_NAME}/ransac_based/rigid_transform_space.h - include/pcl/${SUBSYS_NAME}/ransac_based/obj_rec_ransac.h - include/pcl/${SUBSYS_NAME}/ransac_based/orr_graph.h - include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree_zprojection.h - include/pcl/${SUBSYS_NAME}/ransac_based/trimmed_icp.h - include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree.h - include/pcl/${SUBSYS_NAME}/ransac_based/simple_octree.h - include/pcl/${SUBSYS_NAME}/ransac_based/voxel_structure.h - include/pcl/${SUBSYS_NAME}/ransac_based/bvh.h - + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/color_gradient_dot_modality.h" + "include/pcl/${SUBSYS_NAME}/color_gradient_modality.h" + "include/pcl/${SUBSYS_NAME}/color_modality.h" + "include/pcl/${SUBSYS_NAME}/crh_alignment.h" + "include/pcl/${SUBSYS_NAME}/linemod.h" + "include/pcl/${SUBSYS_NAME}/dotmod.h" + "include/pcl/${SUBSYS_NAME}/quantizable_modality.h" + "include/pcl/${SUBSYS_NAME}/quantized_map.h" + "include/pcl/${SUBSYS_NAME}/dot_modality.h" + "include/pcl/${SUBSYS_NAME}/region_xy.h" + "include/pcl/${SUBSYS_NAME}/mask_map.h" + "include/pcl/${SUBSYS_NAME}/point_types.h" + "include/pcl/${SUBSYS_NAME}/distance_map.h" + "include/pcl/${SUBSYS_NAME}/dense_quantized_multi_mod_template.h" + "include/pcl/${SUBSYS_NAME}/sparse_quantized_multi_mod_template.h" + "include/pcl/${SUBSYS_NAME}/surface_normal_modality.h" + "include/pcl/${SUBSYS_NAME}/linemod/line_rgbd.h" + "include/pcl/${SUBSYS_NAME}/implicit_shape_model.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/auxiliary.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/hypothesis.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/model_library.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/rigid_transform_space.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/obj_rec_ransac.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/orr_graph.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree_zprojection.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/trimmed_icp.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/simple_octree.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/voxel_structure.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/bvh.h" ) - set (ransac_based_incs - include/pcl/${SUBSYS_NAME}/ransac_based/auxiliary.h - include/pcl/${SUBSYS_NAME}/ransac_based/hypothesis.h - include/pcl/${SUBSYS_NAME}/ransac_based/model_library.h - include/pcl/${SUBSYS_NAME}/ransac_based/rigid_transform_space.h - include/pcl/${SUBSYS_NAME}/ransac_based/obj_rec_ransac.h - include/pcl/${SUBSYS_NAME}/ransac_based/orr_graph.h - include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree_zprojection.h - include/pcl/${SUBSYS_NAME}/ransac_based/trimmed_icp.h - include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree.h - include/pcl/${SUBSYS_NAME}/ransac_based/simple_octree.h - include/pcl/${SUBSYS_NAME}/ransac_based/voxel_structure.h - include/pcl/${SUBSYS_NAME}/ransac_based/bvh.h + set(ransac_based_incs + "include/pcl/${SUBSYS_NAME}/ransac_based/auxiliary.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/hypothesis.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/model_library.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/rigid_transform_space.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/obj_rec_ransac.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/orr_graph.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree_zprojection.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/trimmed_icp.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/simple_octree.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/voxel_structure.h" + "include/pcl/${SUBSYS_NAME}/ransac_based/bvh.h" ) - set(hv_incs - include/pcl/${SUBSYS_NAME}/hv/occlusion_reasoning.h - include/pcl/${SUBSYS_NAME}/hv/hypotheses_verification.h - include/pcl/${SUBSYS_NAME}/hv/hv_papazov.h - include/pcl/${SUBSYS_NAME}/hv/hv_go.h - include/pcl/${SUBSYS_NAME}/hv/greedy_verification.h - ) - + "include/pcl/${SUBSYS_NAME}/hv/occlusion_reasoning.h" + "include/pcl/${SUBSYS_NAME}/hv/hypotheses_verification.h" + "include/pcl/${SUBSYS_NAME}/hv/hv_papazov.h" + "include/pcl/${SUBSYS_NAME}/hv/hv_go.h" + "include/pcl/${SUBSYS_NAME}/hv/greedy_verification.h" + ) + set(cg_incs - include/pcl/${SUBSYS_NAME}/cg/correspondence_grouping.h - include/pcl/${SUBSYS_NAME}/cg/hough_3d.h - include/pcl/${SUBSYS_NAME}/cg/geometric_consistency.h + "include/pcl/${SUBSYS_NAME}/cg/correspondence_grouping.h" + "include/pcl/${SUBSYS_NAME}/cg/hough_3d.h" + "include/pcl/${SUBSYS_NAME}/cg/geometric_consistency.h" ) - set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/linemod/line_rgbd.hpp - include/pcl/${SUBSYS_NAME}/impl/ransac_based/simple_octree.hpp - include/pcl/${SUBSYS_NAME}/impl/ransac_based/voxel_structure.hpp - include/pcl/${SUBSYS_NAME}/impl/implicit_shape_model.hpp + "include/pcl/${SUBSYS_NAME}/impl/linemod/line_rgbd.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ransac_based/simple_octree.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ransac_based/voxel_structure.hpp" + "include/pcl/${SUBSYS_NAME}/impl/implicit_shape_model.hpp" ) - - set(ransac_based_impl_incs - include/pcl/${SUBSYS_NAME}/impl/ransac_based/simple_octree.hpp - include/pcl/${SUBSYS_NAME}/impl/ransac_based/voxel_structure.hpp + set(ransac_based_impl_incs + "include/pcl/${SUBSYS_NAME}/impl/ransac_based/simple_octree.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ransac_based/voxel_structure.hpp" ) - set(hv_impl_incs - include/pcl/${SUBSYS_NAME}/impl/hv/occlusion_reasoning.hpp - include/pcl/${SUBSYS_NAME}/impl/hv/hv_papazov.hpp - include/pcl/${SUBSYS_NAME}/impl/hv/greedy_verification.hpp - include/pcl/${SUBSYS_NAME}/impl/hv/hv_go.hpp - ) - + "include/pcl/${SUBSYS_NAME}/impl/hv/occlusion_reasoning.hpp" + "include/pcl/${SUBSYS_NAME}/impl/hv/hv_papazov.hpp" + "include/pcl/${SUBSYS_NAME}/impl/hv/greedy_verification.hpp" + "include/pcl/${SUBSYS_NAME}/impl/hv/hv_go.hpp" + ) + set(cg_impl_incs - include/pcl/${SUBSYS_NAME}/impl/cg/correspondence_grouping.hpp - include/pcl/${SUBSYS_NAME}/impl/cg/hough_3d.hpp - include/pcl/${SUBSYS_NAME}/impl/cg/geometric_consistency.hpp + "include/pcl/${SUBSYS_NAME}/impl/cg/correspondence_grouping.hpp" + "include/pcl/${SUBSYS_NAME}/impl/cg/hough_3d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/cg/geometric_consistency.hpp" ) set(srcs @@ -127,34 +122,38 @@ if(build) src/implicit_shape_model.cpp ) - set(metslib_incs - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/abstract-search.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/local-search.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/mets.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/metslib_config.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/model.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/observer.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/simulated-annealing.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/tabu-search.hh - include/pcl/${SUBSYS_NAME}/3rdparty/metslib/termination-criteria.hh - ) + if (HAVE_METSLIB) + set(metslib_incs "") + else(HAVE_METSLIB) + set(metslib_incs + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/abstract-search.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/local-search.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/mets.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/metslib_config.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/model.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/observer.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/simulated-annealing.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/tabu-search.hh" + "include/pcl/${SUBSYS_NAME}/3rdparty/metslib/termination-criteria.hh" + ) + endif(HAVE_METSLIB) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs} ${ransac_based_incs} ${ransac_based_impl_incs} ${hv_incs} ${hv_impl_incs} ${cg_incs} ${cg_impl_incs} ${metslib_incs}) - target_link_libraries(${LIB_NAME} pcl_common pcl_kdtree pcl_octree pcl_search pcl_features pcl_registration pcl_sample_consensus pcl_filters pcl_io) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs} ${ransac_based_incs} ${ransac_based_impl_incs} ${hv_incs} ${hv_impl_incs} ${cg_incs} ${cg_impl_incs} ${metslib_incs}) + target_link_libraries("${LIB_NAME}" pcl_common pcl_kdtree pcl_octree pcl_search pcl_features pcl_registration pcl_sample_consensus pcl_filters pcl_io) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/ransac_based ${ransac_based_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/hv ${hv_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/cg ${cg_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/ransac_based" ${ransac_based_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/hv" ${hv_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/cg" ${cg_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl/ransac_based ${ransac_based_impl_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl/hv ${hv_impl_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl/cg ${cg_impl_incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/linemod ${LINEMOD_INCLUDES}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl/linemod ${LINEMOD_IMPLS}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/ransac_based" ${ransac_based_impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/hv" ${hv_impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/cg" ${cg_impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/linemod" ${LINEMOD_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/linemod" ${LINEMOD_IMPLS}) endif(build) diff --git a/recognition/include/pcl/recognition/cg/hough_3d.h b/recognition/include/pcl/recognition/cg/hough_3d.h index 7b2c0ed6..feb0c154 100644 --- a/recognition/include/pcl/recognition/cg/hough_3d.h +++ b/recognition/include/pcl/recognition/cg/hough_3d.h @@ -486,25 +486,22 @@ namespace pcl void clusterCorrespondences (std::vector &model_instances); - ///** \brief Finds the transformation matrix between the input and the scene cloud for a set of correspondences using a RANSAC algorithm. - // * - // * \param[in] the scene cloud in which the PointSceneT has been converted to PointModelT. - // * \param[in] corrs a set of correspondences. - // * \param[out] transform the transformation matrix between the input cloud and the scene cloud that aligns the found correspondences. - // * \return true if the recognition had been successful or false if errors have occurred. - // */ + /* \brief Finds the transformation matrix between the input and the scene cloud for a set of correspondences using a RANSAC algorithm. + * \param[in] the scene cloud in which the PointSceneT has been converted to PointModelT. + * \param[in] corrs a set of correspondences. + * \param[out] transform the transformation matrix between the input cloud and the scene cloud that aligns the found correspondences. + * \return true if the recognition had been successful or false if errors have occurred. + */ //bool //getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform); /** \brief The Hough space voting procedure. - * * \return true if the voting had been successful or false if errors have occurred. */ bool houghVoting (); /** \brief Computes the reference frame for an input cloud. - * * \param[in] input the input cloud. * \param[out] rf the resulting reference frame. */ diff --git a/recognition/include/pcl/recognition/color_gradient_modality.h b/recognition/include/pcl/recognition/color_gradient_modality.h index 8de65a8c..16187111 100644 --- a/recognition/include/pcl/recognition/color_gradient_modality.h +++ b/recognition/include/pcl/recognition/color_gradient_modality.h @@ -75,7 +75,7 @@ namespace pcl /** \brief Operator for comparing to candidates (by magnitude of the gradient). * \param[in] rhs the candidate to compare with. */ - bool operator< (const Candidate & rhs) + bool operator< (const Candidate & rhs) const { return (gradient.magnitude > rhs.gradient.magnitude); } @@ -167,7 +167,7 @@ namespace pcl * \param[in] mask defines the areas where features are searched in. * \param[in] nr_features defines the number of features to be extracted * (might be less if not sufficient information is present in the modality). - * \param[in] modality_index the index which is stored in the extracted features. + * \param[in] modalityIndex the index which is stored in the extracted features. * \param[out] features the destination for the extracted features. */ void @@ -177,7 +177,7 @@ namespace pcl /** \brief Extracts all possible features from the modality within the specified mask. * \param[in] mask defines the areas where features are searched in. * \param[in] nr_features IGNORED (TODO: remove this parameter). - * \param[in] modality_index the index which is stored in the extracted features. + * \param[in] modalityIndex the index which is stored in the extracted features. * \param[out] features the destination for the extracted features. */ void diff --git a/recognition/include/pcl/recognition/crh_alignment.h b/recognition/include/pcl/recognition/crh_alignment.h index c1347176..8c678f66 100644 --- a/recognition/include/pcl/recognition/crh_alignment.h +++ b/recognition/include/pcl/recognition/crh_alignment.h @@ -111,15 +111,15 @@ namespace pcl } /** \brief returns the computed transformations - * \param[out] transformations + * \param[out] transforms transformations */ void getTransforms(std::vector > & transforms) { transforms = transforms_; } /** \brief sets model and input views - * \param[in] model view * \param[in] input_view + * \param[in] target_view */ void setInputAndTargetView (PointTPtr & input_view, PointTPtr & target_view) @@ -177,9 +177,9 @@ namespace pcl } /** \brief Computes the roll angle that aligns input to modle. - * \param[in] CRH histogram of the input cloud - * \param[in] CRH histogram of the target cloud - * \param[out] Vector containing angles where the histograms correlate + * \param[in] input_ftt CRH histogram of the input cloud + * \param[in] target_ftt CRH histogram of the target cloud + * \param[out] peaks Vector containing angles where the histograms correlate */ void computeRollAngle (pcl::PointCloud > & input_ftt, pcl::PointCloud > & target_ftt, diff --git a/recognition/include/pcl/recognition/dotmod.h b/recognition/include/pcl/recognition/dotmod.h index b31f9e87..96a0be48 100644 --- a/recognition/include/pcl/recognition/dotmod.h +++ b/recognition/include/pcl/recognition/dotmod.h @@ -73,9 +73,11 @@ namespace pcl virtual ~DOTMOD (); /** \brief Creates a template from the specified data and adds it to the matching queue. - * \param - * \param - * \param + * \param modalities + * \param masks + * \param template_anker_x + * \param template_anker_y + * \param region */ size_t createAndAddTemplate (const std::vector & modalities, diff --git a/recognition/include/pcl/recognition/hv/greedy_verification.h b/recognition/include/pcl/recognition/hv/greedy_verification.h index b0685753..03148bd2 100644 --- a/recognition/include/pcl/recognition/hv/greedy_verification.h +++ b/recognition/include/pcl/recognition/hv/greedy_verification.h @@ -163,7 +163,7 @@ namespace pcl public: /** \brief Constructor - * \param[in] Regularizer value + * \param[in] reg Regularizer value **/ GreedyVerification (float reg = 1.5f) : HypothesisVerification () diff --git a/recognition/include/pcl/recognition/hv/hv_go.h b/recognition/include/pcl/recognition/hv/hv_go.h index 49246d3c..fa56fde1 100644 --- a/recognition/include/pcl/recognition/hv/hv_go.h +++ b/recognition/include/pcl/recognition/hv/hv_go.h @@ -11,7 +11,7 @@ #include #include #include -#include "pcl/recognition/3rdparty/metslib/mets.hh" +#include "metslib/mets.hh" #include #include #include diff --git a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp b/recognition/include/pcl/recognition/impl/hv/hv_go.hpp index a247a16c..118e5149 100644 --- a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp +++ b/recognition/include/pcl/recognition/impl/hv/hv_go.hpp @@ -34,6 +34,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#ifndef PCL_RECOGNITION_IMPL_HV_GO_HPP_ +#define PCL_RECOGNITION_IMPL_HV_GO_HPP_ + #include #include #include @@ -738,3 +741,6 @@ void pcl::GlobalHypothesesVerification::computeClutterCue(boost: } #define PCL_INSTANTIATE_GoHV(T1,T2) template class PCL_EXPORTS pcl::GlobalHypothesesVerification; + +#endif /* PCL_RECOGNITION_IMPL_HV_GO_HPP_ */ + diff --git a/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp b/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp index 89e0ce6f..efdbf84f 100644 --- a/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp +++ b/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp @@ -718,7 +718,7 @@ pcl::ism::ImplicitShapeModelEstimation::trainISM ( std::vector vec; trained_model->clusters_.resize (number_of_clusters_, vec); - for (int i_label = 0; i_label < locations.size (); i_label++) + for (size_t i_label = 0; i_label < locations.size (); i_label++) trained_model->clusters_[labels (i_label)].push_back (i_label); calculateSigmas (trained_model->sigmas_); @@ -738,7 +738,7 @@ pcl::ism::ImplicitShapeModelEstimation::trainISM ( trained_model->directions_to_center_.resize (locations.size (), 3); trained_model->classes_.resize (locations.size ()); - for (int i_dir = 0; i_dir < locations.size (); i_dir++) + for (size_t i_dir = 0; i_dir < locations.size (); i_dir++) { trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x; trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y; @@ -817,7 +817,7 @@ pcl::ism::ImplicitShapeModelEstimation::findObject { unsigned int index = model->clusters_[min_dist_idx][i_word]; unsigned int i_class = model->classes_[index]; - if (i_class != in_class_of_interest) + if (static_cast (i_class) != in_class_of_interest) continue;//skip this class //rotate dir to center as needed @@ -1026,7 +1026,7 @@ pcl::ism::ImplicitShapeModelEstimation::calculateW n_vot_2.resize (number_of_clusters_, vect); n_vot.resize (number_of_clusters_, 0); n_ftr.resize (number_of_classes, 0); - for (int i_location = 0; i_location < locations.size (); i_location++) + for (size_t i_location = 0; i_location < locations.size (); i_location++) { int i_class = training_classes_[locations[i_location].model_num_]; int i_cluster = labels (i_location); diff --git a/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp b/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp index 16feff90..39e03996 100644 --- a/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp +++ b/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp @@ -107,7 +107,7 @@ pcl::LineRGBD::loadTemplates (const std::string &file_name // Search for extension std::string chunk_name (ltm_header.file_name); - std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), tolower); + std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower); std::string::size_type it; if ((it = chunk_name.find (pcd_ext)) != std::string::npos && diff --git a/recognition/include/pcl/recognition/implicit_shape_model.h b/recognition/include/pcl/recognition/implicit_shape_model.h index 2148da72..7acd17dc 100644 --- a/recognition/include/pcl/recognition/implicit_shape_model.h +++ b/recognition/include/pcl/recognition/implicit_shape_model.h @@ -104,6 +104,7 @@ namespace pcl * \param[out] out_peaks it will contain the strongest peaks * \param[in] in_class_id class of interest for which peaks are evaluated * \param[in] in_non_maxima_radius non maxima supression radius. The shapes radius is recommended for this value. + * \param in_sigma */ void findStrongestPeaks (std::vector > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma); @@ -425,7 +426,7 @@ namespace pcl /** \brief This method performs training and forms a visual vocabulary. It returns a trained model that * can be saved to file for later usage. - * \param[out] model trained model + * \param[out] trained_model trained model */ bool trainISM (ISMModelPtr& trained_model); @@ -454,7 +455,7 @@ namespace pcl /** \brief This method performs descriptor clustering. * \param[in] histograms descriptors to cluster * \param[out] labels it contains labels for each descriptor - * \param[out] cluster_centers stores the centers of clusters + * \param[out] clusters_centers stores the centers of clusters */ bool clusterDescriptors (std::vector< pcl::Histogram >& histograms, Eigen::MatrixXi& labels, Eigen::MatrixXf& clusters_centers); @@ -467,7 +468,6 @@ namespace pcl /** \brief This function forms a visual vocabulary and evaluates weights * described in [Knopp et al., 2010, (5)]. - * \param[in] classes classes that we want to learn * \param[in] locations array containing description of each keypoint: its position, which cloud belongs * and expected direction to center * \param[in] labels labels that were obtained during k-means clustering @@ -520,7 +520,6 @@ namespace pcl /** \brief This method estimates features for the given point cloud. * \param[in] sampled_point_cloud sampled point cloud for which the features must be computed - * \param[in] point_cloud original point cloud * \param[in] normal_cloud normals for the original point cloud * \param[out] feature_cloud it will store the computed histograms (features) for the given cloud */ diff --git a/recognition/include/pcl/recognition/linemod.h b/recognition/include/pcl/recognition/linemod.h index 4f41647b..54ffc28e 100644 --- a/recognition/include/pcl/recognition/linemod.h +++ b/recognition/include/pcl/recognition/linemod.h @@ -443,6 +443,13 @@ namespace pcl void loadTemplates (const char * file_name); + /** \brief Loads templates from the specified files. + * \param[in] file_names vector of files to load the templates from. + */ + + void + loadTemplates (std::vector & file_names); + /** \brief Serializes the stored templates to the specified stream. * \param[in] stream the stream the templates will be written to. */ diff --git a/recognition/include/pcl/recognition/linemod/line_rgbd.h b/recognition/include/pcl/recognition/linemod/line_rgbd.h index a9d641e9..8b1b3f38 100644 --- a/recognition/include/pcl/recognition/linemod/line_rgbd.h +++ b/recognition/include/pcl/recognition/linemod/line_rgbd.h @@ -121,6 +121,7 @@ namespace pcl * SparseQuantizedMultiModTemplate format. * * \param[in] file_name The name of the file that stores the templates. + * \param object_id * * \return true, if the operation was successful, false otherwise. */ @@ -187,6 +188,8 @@ namespace pcl } /** \brief Creates a template from the specified data and adds it to the matching queue. + * \param cloud + * \param object_id * \param[in] mask_xyz the mask that determine which parts of the xyz-modality are used for creating the template. * \param[in] mask_rgb the mask that determine which parts of the rgb-modality are used for creating the template. * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps). @@ -273,8 +276,8 @@ namespace pcl removeOverlappingDetections (); /** \brief Computes the volume of the intersection between two bounding boxes. - * \param[in] First bounding box. - * \param[in] Second bounding box. + * \param[in] box1 First bounding box. + * \param[in] box2 Second bounding box. */ static float computeBoundingBoxIntersectionVolume (const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2); diff --git a/recognition/include/pcl/recognition/ransac_based/auxiliary.h b/recognition/include/pcl/recognition/ransac_based/auxiliary.h index 39344823..91e775c4 100644 --- a/recognition/include/pcl/recognition/ransac_based/auxiliary.h +++ b/recognition/include/pcl/recognition/ransac_based/auxiliary.h @@ -139,7 +139,14 @@ namespace pcl a[1] = -a[1]; a[2] = -a[2]; } - + + /** \brief a = b */ + template bool + equal3 (const T a[3], const T b[3]) + { + return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]); + } + /** \brief a += b */ template void add3 (T a[3], const T b[3]) diff --git a/recognition/include/pcl/recognition/ransac_based/model_library.h b/recognition/include/pcl/recognition/ransac_based/model_library.h index 4b8f3426..c1dc1369 100644 --- a/recognition/include/pcl/recognition/ransac_based/model_library.h +++ b/recognition/include/pcl/recognition/ransac_based/model_library.h @@ -197,7 +197,7 @@ namespace pcl max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; } - /** \biref Call this method in order NOT to add co-planar point pairs to the hash table. The default behavior + /** \brief Call this method in order NOT to add co-planar point pairs to the hash table. The default behavior * is ignoring co-planar points on. */ inline void ignoreCoplanarPointPairsOn () @@ -205,7 +205,7 @@ namespace pcl ignore_coplanar_opps_ = true; } - /** \biref Call this method in order to add all point pairs (co-planar as well) to the hash table. The default + /** \brief Call this method in order to add all point pairs (co-planar as well) to the hash table. The default * behavior is ignoring co-planar points on. */ inline void ignoreCoplanarPointPairsOff () @@ -218,7 +218,7 @@ namespace pcl * \param[in] points represents the model to be added. * \param[in] normals are the normals at the model points. * \param[in] object_name is the unique name of the object to be added. - * \param[in] num_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing + * \param[in] frac_of_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing * \param[in] user_data is a pointer to some data (can be NULL) * * Returns true if model successfully added and false otherwise (e.g., if object_name is not unique). */ diff --git a/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h b/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h index 8827fd05..fe1ed389 100644 --- a/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h +++ b/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h @@ -433,10 +433,10 @@ namespace pcl } /** \brief Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles between - * n1 and (p2-p1), - * n2 and (p1-p2), - * n1 and n2 - * + * \param p1 + * \param n1 + * \param p2 + * \param n2 * \param[out] signature is an array of three doubles saving the three angles in the order shown above. */ static inline void compute_oriented_point_pair_signature (const float *p1, const float *n1, const float *p2, const float *n2, float signature[3]) diff --git a/recognition/include/pcl/recognition/surface_normal_modality.h b/recognition/include/pcl/recognition/surface_normal_modality.h index b8a8ebae..05c3b400 100644 --- a/recognition/include/pcl/recognition/surface_normal_modality.h +++ b/recognition/include/pcl/recognition/surface_normal_modality.h @@ -165,7 +165,7 @@ namespace pcl /** \brief Initializes the LUT. * \param[in] range_x_arg the range of the LUT in x-direction. * \param[in] range_y_arg the range of the LUT in y-direction. - * \parma[in] range_z_arg the range of the LUT in z-direction. + * \param[in] range_z_arg the range of the LUT in z-direction. */ void initializeLUT (const int range_x_arg, const int range_y_arg, const int range_z_arg) @@ -319,7 +319,7 @@ namespace pcl * \param[in] rhs the candidate to compare with. */ bool - operator< (const Candidate & rhs) + operator< (const Candidate & rhs) const { return (distance > rhs.distance); } @@ -718,13 +718,6 @@ static void accumBilateral(long delta, long i, long j, long * A, long * b, int t * * Implements section 2.6 "Extension to Dense Depth Sensors." * - * \param[in] src The source 16-bit depth image (in mm). - * \param[out] dst The destination 8-bit image. Each bit represents one bin of - * the view cone. - * \param distance_threshold Ignore pixels beyond this distance. - * \param difference_threshold When computing normals, ignore contributions of pixels whose - * depth difference with the central pixel is above this threshold. - * * \todo Should also need camera model, or at least focal lengths? Replace distance_threshold with mask? */ template void @@ -739,9 +732,9 @@ pcl::SurfaceNormalModality::computeAndQuantizeSurfaceNormals2 () surface_normal_orientations_.resize (width, height, 0.0f); - for (size_t row_index = 0; row_index < height; ++row_index) + for (int row_index = 0; row_index < height; ++row_index) { - for (size_t col_index = 0; col_index < width; ++col_index) + for (int col_index = 0; col_index < width; ++col_index) { const float value = input_->points[row_index*width + col_index].z; if (pcl_isfinite (value)) @@ -954,9 +947,9 @@ pcl::SurfaceNormalModality::computeAndQuantizeSurfaceNormals2 () map[0x1<<7] = 7; quantized_surface_normals_.resize (width, height); - for (size_t row_index = 0; row_index < height; ++row_index) + for (int row_index = 0; row_index < height; ++row_index) { - for (size_t col_index = 0; col_index < width; ++col_index) + for (int col_index = 0; col_index < width; ++col_index) { quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]]; } diff --git a/recognition/src/hv/hv_go.cpp b/recognition/src/hv/hv_go.cpp index d1952d0e..22065e13 100644 --- a/recognition/src/hv/hv_go.cpp +++ b/recognition/src/hv/hv_go.cpp @@ -34,14 +34,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include #include -//template class PCL_EXPORTS pcl::GlobalHypothesesVerification; -//template class PCL_EXPORTS pcl::GlobalHypothesesVerification; - +#ifndef PCL_NO_PRECOMPILE +#include +#include PCL_INSTANTIATE_PRODUCT(GoHV, ((pcl::PointXYZ))((pcl::PointXYZ))) PCL_INSTANTIATE_PRODUCT(GoHV, ((pcl::PointXYZRGB))((pcl::PointXYZRGB))) PCL_INSTANTIATE_PRODUCT(GoHV, ((pcl::PointXYZRGBA))((pcl::PointXYZRGBA))) +#endif // PCL_NO_PRECOMPILE + diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index d0efdac5..14693aa9 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -1326,6 +1326,30 @@ pcl::LINEMOD::loadTemplates (const char * file_name) file_stream.close (); } +void +pcl::LINEMOD::loadTemplates (std::vector & file_names) +{ + templates_.clear (); + + for(size_t i=0; i < file_names.size (); i++) + { + std::ifstream file_stream; + file_stream.open (file_names[i].c_str (), std::ofstream::in | std::ofstream::binary); + + int nr_templates; + read (file_stream, nr_templates); + SparseQuantizedMultiModTemplate sqmm_template; + + for (int template_index = 0; template_index < nr_templates; ++template_index) + { + sqmm_template.deserialize (file_stream); + templates_.push_back (sqmm_template); + } + + file_stream.close (); + } +} + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::LINEMOD::serialize (std::ostream & stream) const diff --git a/recognition/src/ransac_based/orr_octree.cpp b/recognition/src/ransac_based/orr_octree.cpp index a9f0db48..d922bb2f 100644 --- a/recognition/src/ransac_based/orr_octree.cpp +++ b/recognition/src/ransac_based/orr_octree.cpp @@ -315,12 +315,16 @@ pcl::recognition::ORROctree::getFullLeavesIntersectedBySphere (const float* p, f for ( i = 0 ; i < 8 ; ++i ) { child = node->getChild (i); - // We do not want to push all children -> only leaves or children with children - if ( child->hasData () || child->hasChildren () ) + // We do not want to push all children -> only children with children or leaves + if (child->hasChildren ()) + nodes.push_back(child); + // only push back the child if it is not the leaf of p + else if (child->hasData () && !aux::equal3 (p, child->getData ()->getPoint ())) nodes.push_back (child); } } - else if ( node->hasData () ) + // only push back the node if it is not the leaf of p + else if (node->hasData () && !aux::equal3 (p, node->getData ()->getPoint ())) out.push_back (node); // We got a full leaf } } diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index bd94b5b2..971cb91f 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -3,112 +3,115 @@ set(SUBSYS_DESC "Point cloud registration library") set(SUBSYS_DEPS common octree kdtree search sample_consensus features filters) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(incs - include/pcl/${SUBSYS_NAME}/eigen.h - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/boost_graph.h - include/pcl/${SUBSYS_NAME}/convergence_criteria.h - include/pcl/${SUBSYS_NAME}/default_convergence_criteria.h - include/pcl/${SUBSYS_NAME}/correspondence_estimation.h - include/pcl/${SUBSYS_NAME}/correspondence_estimation_normal_shooting.h - include/pcl/${SUBSYS_NAME}/correspondence_estimation_backprojection.h - include/pcl/${SUBSYS_NAME}/correspondence_estimation_organized_projection.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_distance.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_median_distance.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_surface_normal.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_features.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_one_to_one.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_poly.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_sample_consensus.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_sample_consensus_2d.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_trimmed.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_var_trimmed.h - include/pcl/${SUBSYS_NAME}/correspondence_rejection_organized_boundary.h - include/pcl/${SUBSYS_NAME}/correspondence_sorting.h - include/pcl/${SUBSYS_NAME}/correspondence_types.h - include/pcl/${SUBSYS_NAME}/ia_ransac.h - include/pcl/${SUBSYS_NAME}/icp.h - include/pcl/${SUBSYS_NAME}/icp_nl.h - include/pcl/${SUBSYS_NAME}/lum.h - include/pcl/${SUBSYS_NAME}/elch.h - include/pcl/${SUBSYS_NAME}/ndt.h - include/pcl/${SUBSYS_NAME}/ndt_2d.h - include/pcl/${SUBSYS_NAME}/ppf_registration.h + "include/pcl/${SUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/boost_graph.h" + "include/pcl/${SUBSYS_NAME}/convergence_criteria.h" + "include/pcl/${SUBSYS_NAME}/default_convergence_criteria.h" + "include/pcl/${SUBSYS_NAME}/correspondence_estimation.h" + "include/pcl/${SUBSYS_NAME}/correspondence_estimation_normal_shooting.h" + "include/pcl/${SUBSYS_NAME}/correspondence_estimation_backprojection.h" + "include/pcl/${SUBSYS_NAME}/correspondence_estimation_organized_projection.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_distance.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_median_distance.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_surface_normal.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_features.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_one_to_one.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_poly.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_sample_consensus.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_sample_consensus_2d.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_trimmed.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_var_trimmed.h" + "include/pcl/${SUBSYS_NAME}/correspondence_rejection_organized_boundary.h" + "include/pcl/${SUBSYS_NAME}/correspondence_sorting.h" + "include/pcl/${SUBSYS_NAME}/correspondence_types.h" + "include/pcl/${SUBSYS_NAME}/ia_ransac.h" + "include/pcl/${SUBSYS_NAME}/icp.h" + "include/pcl/${SUBSYS_NAME}/joint_icp.h" + "include/pcl/${SUBSYS_NAME}/icp_nl.h" + "include/pcl/${SUBSYS_NAME}/lum.h" + "include/pcl/${SUBSYS_NAME}/elch.h" + "include/pcl/${SUBSYS_NAME}/ndt.h" + "include/pcl/${SUBSYS_NAME}/ndt_2d.h" + "include/pcl/${SUBSYS_NAME}/ppf_registration.h" - include/pcl/${SUBSYS_NAME}/impl/pairwise_graph_registration.hpp + "include/pcl/${SUBSYS_NAME}/impl/pairwise_graph_registration.hpp" - include/pcl/${SUBSYS_NAME}/pyramid_feature_matching.h - include/pcl/${SUBSYS_NAME}/registration.h - include/pcl/${SUBSYS_NAME}/transforms.h - include/pcl/${SUBSYS_NAME}/transformation_estimation.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_2D.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_svd.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_svd_scale.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_dual_quaternion.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_lm.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_weighted.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls.h - include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls_weighted.h - include/pcl/${SUBSYS_NAME}/transformation_validation.h - include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h - include/pcl/${SUBSYS_NAME}/gicp.h - include/pcl/${SUBSYS_NAME}/bfgs.h - include/pcl/${SUBSYS_NAME}/warp_point_rigid.h - include/pcl/${SUBSYS_NAME}/warp_point_rigid_6d.h - include/pcl/${SUBSYS_NAME}/warp_point_rigid_3d.h - include/pcl/${SUBSYS_NAME}/distances.h - include/pcl/${SUBSYS_NAME}/exceptions.h - include/pcl/${SUBSYS_NAME}/sample_consensus_prerejective.h + "include/pcl/${SUBSYS_NAME}/pyramid_feature_matching.h" + "include/pcl/${SUBSYS_NAME}/registration.h" + "include/pcl/${SUBSYS_NAME}/transforms.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_2D.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_svd.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_svd_scale.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_dual_quaternion.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_lm.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_weighted.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls_weighted.h" + "include/pcl/${SUBSYS_NAME}/transformation_validation.h" + "include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h" + "include/pcl/${SUBSYS_NAME}/gicp.h" + "include/pcl/${SUBSYS_NAME}/gicp6d.h" + "include/pcl/${SUBSYS_NAME}/bfgs.h" + "include/pcl/${SUBSYS_NAME}/warp_point_rigid.h" + "include/pcl/${SUBSYS_NAME}/warp_point_rigid_6d.h" + "include/pcl/${SUBSYS_NAME}/warp_point_rigid_3d.h" + "include/pcl/${SUBSYS_NAME}/distances.h" + "include/pcl/${SUBSYS_NAME}/exceptions.h" + "include/pcl/${SUBSYS_NAME}/sample_consensus_prerejective.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/default_convergence_criteria.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation_normal_shooting.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation_backprojection.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation_organized_projection.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_distance.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_median_distance.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_surface_normal.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_features.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_one_to_one.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_poly.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_sample_consensus.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_sample_consensus_2d.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_trimmed.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_var_trimmed.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_organized_boundary.hpp - include/pcl/${SUBSYS_NAME}/impl/correspondence_types.hpp - include/pcl/${SUBSYS_NAME}/impl/ia_ransac.hpp - include/pcl/${SUBSYS_NAME}/impl/icp.hpp - include/pcl/${SUBSYS_NAME}/impl/icp_nl.hpp - include/pcl/${SUBSYS_NAME}/impl/elch.hpp - include/pcl/${SUBSYS_NAME}/impl/lum.hpp - include/pcl/${SUBSYS_NAME}/impl/ndt.hpp - include/pcl/${SUBSYS_NAME}/impl/ndt_2d.hpp - include/pcl/${SUBSYS_NAME}/impl/ppf_registration.hpp - include/pcl/${SUBSYS_NAME}/impl/pyramid_feature_matching.hpp - include/pcl/${SUBSYS_NAME}/impl/registration.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_2D.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd_scale.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_dual_quaternion.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_lm.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls_weighted.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_weighted.hpp - include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp - include/pcl/${SUBSYS_NAME}/impl/gicp.hpp - include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp + "include/pcl/${SUBSYS_NAME}/impl/default_convergence_criteria.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation_normal_shooting.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation_backprojection.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_estimation_organized_projection.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_distance.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_median_distance.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_surface_normal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_features.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_one_to_one.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_poly.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_sample_consensus.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_sample_consensus_2d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_trimmed.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_var_trimmed.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_rejection_organized_boundary.hpp" + "include/pcl/${SUBSYS_NAME}/impl/correspondence_types.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ia_ransac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/icp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/joint_icp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/icp_nl.hpp" + "include/pcl/${SUBSYS_NAME}/impl/elch.hpp" + "include/pcl/${SUBSYS_NAME}/impl/lum.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ndt.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ndt_2d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ppf_registration.hpp" + "include/pcl/${SUBSYS_NAME}/impl/pyramid_feature_matching.hpp" + "include/pcl/${SUBSYS_NAME}/impl/registration.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_2D.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd_scale.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_dual_quaternion.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_lm.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls_weighted.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_weighted.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp" + "include/pcl/${SUBSYS_NAME}/impl/gicp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp" ) set(srcs @@ -134,7 +137,9 @@ if(build) #src/pairwise_graph_registration.cpp src/ia_ransac.cpp src/icp.cpp + src/joint_icp.cpp src/gicp.cpp + src/gicp6d.cpp src/icp_nl.cpp src/elch.cpp src/lum.cpp @@ -152,14 +157,14 @@ if(build) src/sample_consensus_prerejective.cpp ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_kdtree pcl_search pcl_sample_consensus pcl_features pcl_filters) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_kdtree pcl_search pcl_sample_consensus pcl_features pcl_filters) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/registration/include/pcl/registration/correspondence_estimation.h b/registration/include/pcl/registration/correspondence_estimation.h index ec5422c7..bbcf9c46 100644 --- a/registration/include/pcl/registration/correspondence_estimation.h +++ b/registration/include/pcl/registration/correspondence_estimation.h @@ -113,11 +113,14 @@ namespace pcl * * \param[in] cloud the input point cloud source */ - PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::CorrespondenceEstimationBase::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceEstimationBase::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.") + void + setInputCloud (const PointCloudSourceConstPtr &cloud); /** \brief Get a pointer to the input point cloud dataset target. */ - PCL_DEPRECATED (PointCloudSourceConstPtr const getInputCloud (), - "[pcl::registration::CorrespondenceEstimationBase::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceEstimationBase::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.") + PointCloudSourceConstPtr const + getInputCloud (); /** \brief Provide a pointer to the input source * (e.g., the point cloud that we want to align to the target) @@ -150,6 +153,31 @@ namespace pcl inline PointCloudTargetConstPtr const getInputTarget () { return (target_ ); } + + /** \brief See if this rejector requires source normals */ + virtual bool + requiresSourceNormals () const + { return (false); } + + /** \brief Abstract method for setting the source normals */ + virtual void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ()); + } + + /** \brief See if this rejector requires target normals */ + virtual bool + requiresTargetNormals () const + { return (false); } + + /** \brief Abstract method for setting the target normals */ + virtual void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ()); + } + /** \brief Provide a pointer to the vector of indices that represent the * input source point cloud. * \param[in] indices a pointer to the vector of indices @@ -265,6 +293,9 @@ namespace pcl point_representation_ = point_representation; } + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual boost::shared_ptr< CorrespondenceEstimationBase > clone () const = 0; + protected: /** \brief The correspondence estimation method name. */ std::string corr_name_; @@ -405,6 +436,15 @@ namespace pcl virtual void determineReciprocalCorrespondences (pcl::Correspondences &correspondences, double max_distance = std::numeric_limits::max ()); + + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual boost::shared_ptr< CorrespondenceEstimationBase > + clone () const + { + Ptr copy (new CorrespondenceEstimation (*this)); + return (copy); + } }; } } diff --git a/registration/include/pcl/registration/correspondence_estimation_backprojection.h b/registration/include/pcl/registration/correspondence_estimation_backprojection.h index 75cfc6b5..9be00439 100644 --- a/registration/include/pcl/registration/correspondence_estimation_backprojection.h +++ b/registration/include/pcl/registration/correspondence_estimation_backprojection.h @@ -123,6 +123,35 @@ namespace pcl inline NormalsConstPtr getTargetNormals () const { return (target_normals_); } + + /** \brief See if this rejector requires source normals */ + bool + requiresSourceNormals () const + { return (true); } + + /** \brief Blob method for setting the source normals */ + void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + NormalsPtr cloud (new PointCloudNormals); + fromPCLPointCloud2 (*cloud2, *cloud); + setSourceNormals (cloud); + } + + /** \brief See if this rejector requires target normals*/ + bool + requiresTargetNormals () const + { return (true); } + + /** \brief Method for setting the target normals */ + void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + NormalsPtr cloud (new PointCloudNormals); + fromPCLPointCloud2 (*cloud2, *cloud); + setTargetNormals (cloud); + } + /** \brief Determine the correspondences between input and target cloud. * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target @@ -157,6 +186,14 @@ namespace pcl */ inline void getKSearch () const { return (k_); } + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual boost::shared_ptr< CorrespondenceEstimationBase > + clone () const + { + Ptr copy (new CorrespondenceEstimationBackProjection (*this)); + return (copy); + } protected: diff --git a/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h b/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h index 129f2955..84e7b32a 100644 --- a/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h +++ b/registration/include/pcl/registration/correspondence_estimation_normal_shooting.h @@ -133,6 +133,21 @@ namespace pcl inline NormalsConstPtr getSourceNormals () const { return (source_normals_); } + + /** \brief See if this rejector requires source normals */ + bool + requiresSourceNormals () const + { return (true); } + + /** \brief Blob method for setting the source normals */ + void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + NormalsPtr cloud (new PointCloudNormals); + fromPCLPointCloud2 (*cloud2, *cloud); + setSourceNormals (cloud); + } + /** \brief Determine the correspondences between input and target cloud. * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target @@ -168,6 +183,14 @@ namespace pcl inline void getKSearch () const { return (k_); } + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual boost::shared_ptr< CorrespondenceEstimationBase > + clone () const + { + Ptr copy (new CorrespondenceEstimationNormalShooting (*this)); + return (copy); + } + protected: using CorrespondenceEstimationBase::corr_name_; diff --git a/registration/include/pcl/registration/correspondence_estimation_organized_projection.h b/registration/include/pcl/registration/correspondence_estimation_organized_projection.h index cb4c2cb4..a6ba9fdd 100644 --- a/registration/include/pcl/registration/correspondence_estimation_organized_projection.h +++ b/registration/include/pcl/registration/correspondence_estimation_organized_projection.h @@ -140,7 +140,7 @@ namespace pcl /** \brief Reads back the transformation from the source point cloud to the target point cloud. * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct * for that. - * \param[out] src_to_tgt_transformation the transformation + * \return the transformation */ inline Eigen::Matrix4f getSourceTransformation () const @@ -158,23 +158,33 @@ namespace pcl /** \brief Reads back the depth threshold; after projecting the source points in the image space of the target * camera, this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too * far from each other. - * \param[out] depth_threshold the depth threshold + * \return the depth threshold */ inline float getDepthThreshold () const { return (depth_threshold_); } /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold. + * \param correspondences * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected */ void determineCorrespondences (Correspondences &correspondences, double max_distance); /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold. + * \param correspondences * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected */ void determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance); + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual boost::shared_ptr< CorrespondenceEstimationBase > + clone () const + { + Ptr copy (new CorrespondenceEstimationOrganizedProjection (*this)); + return (copy); + } protected: using CorrespondenceEstimationBase::target_; diff --git a/registration/include/pcl/registration/correspondence_rejection.h b/registration/include/pcl/registration/correspondence_rejection.h index 7a61bcf6..36254e2b 100644 --- a/registration/include/pcl/registration/correspondence_rejection.h +++ b/registration/include/pcl/registration/correspondence_rejection.h @@ -134,6 +134,54 @@ namespace pcl inline const std::string& getClassName () const { return (rejection_name_); } + + /** \brief See if this rejector requires source points */ + virtual bool + requiresSourcePoints () const + { return (false); } + + /** \brief Abstract method for setting the source cloud */ + virtual void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setSourcePoints] This class does not require an input source cloud", getClassName ().c_str ()); + } + + /** \brief See if this rejector requires source normals */ + virtual bool + requiresSourceNormals () const + { return (false); } + + /** \brief Abstract method for setting the source normals */ + virtual void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ()); + } + /** \brief See if this rejector requires a target cloud */ + virtual bool + requiresTargetPoints () const + { return (false); } + + /** \brief Abstract method for setting the target cloud */ + virtual void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setTargetPoints] This class does not require an input target cloud", getClassName ().c_str ()); + } + + /** \brief See if this rejector requires target normals */ + virtual bool + requiresTargetNormals () const + { return (false); } + + /** \brief Abstract method for setting the target normals */ + virtual void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ()); + } + protected: /** \brief The name of the rejection method. */ @@ -201,10 +249,14 @@ namespace pcl * data!), used to compute the correspondence distance. * \param[in] cloud a cloud containing XYZ data */ - PCL_DEPRECATED (void setInputCloud (const PointCloudConstPtr &cloud), "[pcl::registration::DataContainer::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::DataContainer::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.") + void + setInputCloud (const PointCloudConstPtr &cloud); /** \brief Get a pointer to the input point cloud dataset target. */ - PCL_DEPRECATED (PointCloudConstPtr const getInputCloud (), "[pcl::registration::DataContainer::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::DataContainer::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.") + PointCloudConstPtr const + getInputCloud (); /** \brief Provide a source point cloud dataset (must contain XYZ * data!), used to compute the correspondence distance. diff --git a/registration/include/pcl/registration/correspondence_rejection_distance.h b/registration/include/pcl/registration/correspondence_rejection_distance.h index ef681ba2..cbbe5f48 100644 --- a/registration/include/pcl/registration/correspondence_rejection_distance.h +++ b/registration/include/pcl/registration/correspondence_rejection_distance.h @@ -135,6 +135,35 @@ namespace pcl boost::static_pointer_cast > (data_container_)->setInputTarget (target); } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + /** \brief Provide a pointer to the search object used to find correspondences in * the target cloud. * \param[in] tree a pointer to the spatial search object. diff --git a/registration/include/pcl/registration/correspondence_rejection_features.h b/registration/include/pcl/registration/correspondence_rejection_features.h index 373f3f77..e69a6dbd 100644 --- a/registration/include/pcl/registration/correspondence_rejection_features.h +++ b/registration/include/pcl/registration/correspondence_rejection_features.h @@ -239,7 +239,7 @@ namespace pcl } /** \brief Obtain a score between a pair of correspondences. - * \param[in] the index to check in the list of correspondences + * \param[in] index the index to check in the list of correspondences * \return score the resultant computed score */ virtual inline double @@ -272,7 +272,7 @@ namespace pcl /** \brief Check whether the correspondence pair at the given index is valid * by computing the score and testing it against the user given threshold - * \param[in] the index to check in the list of correspondences + * \param[in] index the index to check in the list of correspondences * \return true if the correspondence is good, false otherwise */ virtual inline bool diff --git a/registration/include/pcl/registration/correspondence_rejection_median_distance.h b/registration/include/pcl/registration/correspondence_rejection_median_distance.h index e8f079d9..71655f91 100644 --- a/registration/include/pcl/registration/correspondence_rejection_median_distance.h +++ b/registration/include/pcl/registration/correspondence_rejection_median_distance.h @@ -125,6 +125,34 @@ namespace pcl data_container_.reset (new DataContainer); boost::static_pointer_cast > (data_container_)->setInputTarget (target); } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } /** \brief Provide a pointer to the search object used to find correspondences in * the target cloud. diff --git a/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h b/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h index 990e759d..4ceec185 100644 --- a/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h +++ b/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h @@ -92,6 +92,34 @@ namespace pcl boost::static_pointer_cast > (data_container_)->setInputTarget (cloud); } + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + virtual bool updateSource (const Eigen::Matrix4d &) { return (true); } diff --git a/registration/include/pcl/registration/correspondence_rejection_poly.h b/registration/include/pcl/registration/correspondence_rejection_poly.h index 957d7f77..4be18370 100644 --- a/registration/include/pcl/registration/correspondence_rejection_poly.h +++ b/registration/include/pcl/registration/correspondence_rejection_poly.h @@ -126,6 +126,34 @@ namespace pcl target_ = target; } + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloudSourcePtr cloud (new PointCloudSource); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloudTargetPtr cloud (new PointCloudTarget); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + /** \brief Set the polygon cardinality * \param cardinality polygon cardinality */ @@ -146,7 +174,7 @@ namespace pcl /** \brief Set the similarity threshold in [0,1[ between edge lengths, * where 1 is a perfect match - * \param similarity similarity threshold + * \param similarity_threshold similarity threshold */ inline void setSimilarityThreshold (float similarity_threshold) diff --git a/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h b/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h index e671d6f5..6267ade0 100644 --- a/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h +++ b/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h @@ -100,11 +100,14 @@ namespace pcl /** \brief Provide a source point cloud dataset (must contain XYZ data!) * \param[in] cloud a cloud containing XYZ data */ - PCL_DEPRECATED (virtual void setInputCloud (const PointCloudConstPtr &cloud), - "[pcl::registration::CorrespondenceRejectorSampleConsensus::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceRejectorSampleConsensus::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.") + virtual void + setInputCloud (const PointCloudConstPtr &cloud); /** \brief Get a pointer to the input point cloud dataset target. */ - PCL_DEPRECATED (PointCloudConstPtr const getInputCloud (), "[pcl::registration::CorrespondenceRejectorSampleConsensus::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.") + PointCloudConstPtr const + getInputCloud (); /** \brief Provide a source point cloud dataset (must contain XYZ data!) * \param[in] cloud a cloud containing XYZ data @@ -122,7 +125,9 @@ namespace pcl /** \brief Provide a target point cloud dataset (must contain XYZ data!) * \param[in] cloud a cloud containing XYZ data */ - PCL_DEPRECATED (virtual void setTargetCloud (const PointCloudConstPtr &cloud), "[pcl::registration::CorrespondenceRejectorSampleConsensus::setTargetCloud] setTargetCloud is deprecated. Please use setInputTarget instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceRejectorSampleConsensus::setTargetCloud] setTargetCloud is deprecated. Please use setInputTarget instead.") + virtual void + setTargetCloud (const PointCloudConstPtr &cloud); /** \brief Provide a target point cloud dataset (must contain XYZ data!) * \param[in] cloud a cloud containing XYZ data @@ -134,6 +139,35 @@ namespace pcl inline PointCloudConstPtr const getInputTarget () { return (target_ ); } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloudPtr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloudPtr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + /** \brief Set the maximum distance between corresponding points. * Correspondences with distances below the threshold are considered as inliers. * \param[in] threshold Distance threshold in the same dimension as source and target data sets. @@ -145,12 +179,14 @@ namespace pcl * \return Distance threshold in the same dimension as source and target data sets. */ inline double - getInlierThreshold() { return inlier_threshold_; }; + getInlierThreshold () { return inlier_threshold_; }; /** \brief Set the maximum number of iterations. * \param[in] max_iterations Maximum number if iterations to run */ - PCL_DEPRECATED (void setMaxIterations (int max_iterations), "[pcl::registration::CorrespondenceRejectorSampleConsensus::setMaxIterations] setMaxIterations is deprecated. Please use setMaximumIterations instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceRejectorSampleConsensus::setMaxIterations] setMaxIterations is deprecated. Please use setMaximumIterations instead.") + void + setMaxIterations (int max_iterations); /** \brief Set the maximum number of iterations. * \param[in] max_iterations Maximum number if iterations to run @@ -161,7 +197,9 @@ namespace pcl /** \brief Get the maximum number of iterations. * \return max_iterations Maximum number if iterations to run */ - PCL_DEPRECATED (int getMaxIterations (), "[pcl::registration::CorrespondenceRejectorSampleConsensus::getMaxIterations] getMaxIterations is deprecated. Please use getMaximumIterations instead."); + PCL_DEPRECATED ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getMaxIterations] getMaxIterations is deprecated. Please use getMaximumIterations instead.") + int + getMaxIterations (); /** \brief Get the maximum number of iterations. * \return max_iterations Maximum number if iterations to run diff --git a/registration/include/pcl/registration/correspondence_rejection_surface_normal.h b/registration/include/pcl/registration/correspondence_rejection_surface_normal.h index 50eae04f..9eda90b7 100644 --- a/registration/include/pcl/registration/correspondence_rejection_surface_normal.h +++ b/registration/include/pcl/registration/correspondence_rejection_surface_normal.h @@ -101,7 +101,7 @@ namespace pcl } /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. - * \param[in] cloud a cloud containing XYZ data + * \param[in] input a cloud containing XYZ data */ template inline void setInputCloud (const typename pcl::PointCloud::ConstPtr &input) @@ -116,7 +116,7 @@ namespace pcl } /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. - * \param[in] cloud a cloud containing XYZ data + * \param[in] input a cloud containing XYZ data */ template inline void setInputSource (const typename pcl::PointCloud::ConstPtr &input) @@ -234,6 +234,71 @@ namespace pcl return (boost::static_pointer_cast > (data_container_)->getTargetNormals ()); } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief See if this rejector requires source normals */ + bool + requiresSourceNormals () const + { return (true); } + + /** \brief Blob method for setting the source normals */ + void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputNormals (cloud); + } + + /** \brief See if this rejector requires target normals*/ + bool + requiresTargetNormals () const + { return (true); } + + /** \brief Method for setting the target normals */ + void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setTargetNormals (cloud); + } + protected: /** \brief Apply the rejection algorithm. diff --git a/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h b/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h index 1b276b0e..5a92a667 100644 --- a/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h +++ b/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h @@ -131,7 +131,37 @@ namespace pcl data_container_.reset (new DataContainer); boost::static_pointer_cast > (data_container_)->setInputTarget (target); } + + + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + /** \brief Provide a pointer to the search object used to find correspondences in * the target cloud. * \param[in] tree a pointer to the spatial search object. diff --git a/registration/include/pcl/registration/elch.h b/registration/include/pcl/registration/elch.h index c37cbb77..aa45b3ea 100644 --- a/registration/include/pcl/registration/elch.h +++ b/registration/include/pcl/registration/elch.h @@ -73,6 +73,7 @@ namespace pcl { Vertex () : cloud () {} PointCloudPtr cloud; + Eigen::Affine3f transform; }; /** \brief graph structure to hold the SLAM graph */ @@ -196,7 +197,7 @@ namespace pcl compute_loop_ = false; } - /** \brief Computes now poses for all point clouds by closing the loop + /** \brief Computes new poses for all point clouds by closing the loop * between start and end point cloud. This will transform all given point * clouds for now! */ diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 7323674e..a7afb898 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -122,7 +122,9 @@ namespace pcl /** \brief Provide a pointer to the input dataset * \param cloud the const boost shared pointer to a PointCloud message */ - PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.") + void + setInputCloud (const PointCloudSourceConstPtr &cloud); /** \brief Provide a pointer to the input dataset * \param cloud the const boost shared pointer to a PointCloud message @@ -279,7 +281,7 @@ namespace pcl * neighbors. K is set via setCorrespondenceRandomness() methode. * \param cloud pointer to point cloud * \param tree KD tree performer for nearest neighbors search - * \return cloud_covariance covariances matrices for each point in the cloud + * \param[out] cloud_covariances covariances matrices for each point in the cloud */ template void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, diff --git a/registration/include/pcl/registration/gicp6d.h b/registration/include/pcl/registration/gicp6d.h new file mode 100644 index 00000000..a7222044 --- /dev/null +++ b/registration/include/pcl/registration/gicp6d.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_GICP6D_H_ +#define PCL_GICP6D_H_ + +#include +#include +#include +#include +#include + +namespace pcl +{ + struct EIGEN_ALIGN16 _PointXYZLAB + { + PCL_ADD_POINT4D; // this adds the members x,y,z + union + { + struct + { + float L; + float a; + float b; + }; + float data_lab[4]; + }; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief A custom point type for position and CIELAB color value */ + struct PointXYZLAB : public _PointXYZLAB + { + inline PointXYZLAB () + { + x = y = z = 0.0f; data[3] = 1.0f; // important for homogeneous coordinates + L = a = b = 0.0f; data_lab[3] = 0.0f; + } + }; +} + +// register the custom point type in PCL +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::_PointXYZLAB, + (float, x, x) + (float, y, y) + (float, z, z) + (float, L, L) + (float, a, a) + (float, b, b) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB) + +namespace pcl +{ + /** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information into the + * Generalized Iterative Closest Point (GICP) algorithm. + * + * The suggested input is PointXYZRGBA. + * + * \note If you use this code in any academic work, please cite: + * + * - M. Korn, M. Holzkothen, J. Pauli + * Color Supported Generalized-ICP. + * In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory and Applications, + * Lisbon, Portugal, January 2014. + * + * \author Martin Holzkothen, Michael Korn + * \ingroup registration + */ + class GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint + { + typedef PointXYZRGBA PointSource; + typedef PointXYZRGBA PointTarget; + + public: + + /** \brief constructor. + * + * \param[in] lab_weight the color weight + */ + GeneralizedIterativeClosestPoint6D (float lab_weight = 0.032f); + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud source + */ + void + setInputSource (const PointCloudSourceConstPtr& cloud); + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align the input source to) + * + * \param[in] cloud the input point cloud target + */ + void + setInputTarget (const PointCloudTargetConstPtr& target); + + protected: + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + void + computeTransformation (PointCloudSource& output, + const Eigen::Matrix4f& guess); + + /** \brief Search for the closest nearest neighbor of a given point. + * \param query the point to search a nearest neighbour for + * \param index vector of size 1 to store the index of the nearest neighbour found + * \param distance vector of size 1 to store the distance to nearest neighbour found + */ + inline bool + searchForNeighbors (const PointXYZLAB& query, std::vector& index, std::vector& distance); + + protected: + /** \brief Holds the converted (LAB) data cloud. */ + pcl::PointCloud::Ptr cloud_lab_; + + /** \brief Holds the converted (LAB) model cloud. */ + pcl::PointCloud::Ptr target_lab_; + + /** \brief 6d-tree to search in model cloud. */ + KdTreeFLANN target_tree_lab_; + + /** \brief The color weight. */ + float lab_weight_; + + /** \brief Custom point representation to perform kdtree searches in more than 3 (i.e. in all 6) dimensions. */ + class MyPointRepresentation : public PointRepresentation + { + using PointRepresentation::nr_dimensions_; + using PointRepresentation::trivial_; + + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + MyPointRepresentation () + { + nr_dimensions_ = 6; + trivial_ = false; + } + + virtual + ~MyPointRepresentation () + { + } + + inline Ptr + makeShared () const + { + return Ptr (new MyPointRepresentation (*this)); + } + + virtual void + copyToFloatArray (const PointXYZLAB &p, float * out) const + { + // copy all of the six values + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + out[3] = p.L; + out[4] = p.a; + out[5] = p.b; + } + }; + + /** \brief Enables 6d searches with kd-tree class using the color weight. */ + MyPointRepresentation point_rep_; + }; +} + +#endif //#ifndef PCL_GICP6D_H_ diff --git a/registration/include/pcl/registration/ia_ransac.h b/registration/include/pcl/registration/ia_ransac.h index 6ac45bb0..01888cfa 100644 --- a/registration/include/pcl/registration/ia_ransac.h +++ b/registration/include/pcl/registration/ia_ransac.h @@ -65,6 +65,7 @@ namespace pcl using Registration::max_iterations_; using Registration::tree_; using Registration::transformation_estimation_; + using Registration::converged_; using Registration::getClassName; typedef typename Registration::PointCloudSource PointCloudSource; @@ -245,6 +246,7 @@ namespace pcl /** \brief Rigid transformation computation method. * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess The computed transforamtion */ virtual void computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess); diff --git a/registration/include/pcl/registration/icp.h b/registration/include/pcl/registration/icp.h index baf06be0..5bf49ff0 100644 --- a/registration/include/pcl/registration/icp.h +++ b/registration/include/pcl/registration/icp.h @@ -161,7 +161,7 @@ namespace pcl * method is called. Please note that the align method sets max_iterations_, * euclidean_fitness_epsilon_ and transformation_epsilon_ and therefore overrides the default / set * values of the DefaultConvergenceCriteria instance. - * \param[out] Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria. + * \return Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria. */ inline typename pcl::registration::DefaultConvergenceCriteria::Ptr getConvergeCriteria () @@ -264,6 +264,10 @@ namespace pcl virtual void computeTransformation (PointCloudSource &output, const Matrix4 &guess); + /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */ + virtual void + determineRequiredBlobData (); + /** \brief XYZ fields offset. */ size_t x_idx_offset_, y_idx_offset_, z_idx_offset_; @@ -277,6 +281,9 @@ namespace pcl bool source_has_normals_; /** \brief Internal check whether target dataset has normals or not. */ bool target_has_normals_; + + /** \brief Checks for whether estimators and rejectors need various data */ + bool need_source_blob_, need_target_blob_; }; /** \brief @b IterativeClosestPointWithNormals is a special case of diff --git a/registration/include/pcl/registration/impl/correspondence_estimation.hpp b/registration/include/pcl/registration/impl/correspondence_estimation.hpp index 4b02d680..bdc2e87b 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation.hpp @@ -40,8 +40,8 @@ #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ -#include #include +#include /////////////////////////////////////////////////////////////////////////////////////////// template void @@ -132,7 +132,6 @@ pcl::registration::CorrespondenceEstimation::d double max_dist_sqr = max_distance * max_distance; - typedef typename pcl::traits::fieldList::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector index (1); @@ -165,9 +164,7 @@ pcl::registration::CorrespondenceEstimation::d for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { // Copy the source data to a target PointTarget format so we can search in the tree - pcl::for_each_type (pcl::NdConcatenateFunctor ( - input_->points[*idx], - pt)); + copyPoint (input_->points[*idx], pt); tree_->nearestKSearch (pt, 1, index, distance); if (distance[0] > max_dist_sqr) @@ -190,11 +187,7 @@ pcl::registration::CorrespondenceEstimation::d { if (!initCompute ()) return; - - typedef typename pcl::traits::fieldList::type FieldListSource; - typedef typename pcl::traits::fieldList::type FieldListTarget; - typedef typename pcl::intersect::type FieldList; - + // setup tree for reciprocal search // Set the internal point representation of choice if (!initComputeReciprocal()) @@ -242,9 +235,7 @@ pcl::registration::CorrespondenceEstimation::d for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { // Copy the source data to a target PointTarget format so we can search in the tree - pcl::for_each_type (pcl::NdConcatenateFunctor ( - input_->points[*idx], - pt_src)); + copyPoint (input_->points[*idx], pt_src); tree_->nearestKSearch (pt_src, 1, index, distance); if (distance[0] > max_dist_sqr) @@ -253,9 +244,7 @@ pcl::registration::CorrespondenceEstimation::d target_idx = index[0]; // Copy the target data to a target PointSource format so we can search in the tree_reciprocal - pcl::for_each_type (pcl::NdConcatenateFunctor ( - target_->points[target_idx], - pt_tgt)); + copyPoint (target_->points[target_idx], pt_tgt); tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal); if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0]) diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp index dad8e1a6..7284f3e7 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp @@ -39,6 +39,8 @@ #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ +#include + /////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::registration::CorrespondenceEstimationBackProjection::initCompute () @@ -60,7 +62,6 @@ pcl::registration::CorrespondenceEstimationBackProjection::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector nn_indices (k_); @@ -125,9 +126,7 @@ pcl::registration::CorrespondenceEstimationBackProjection (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + @@ -161,8 +160,6 @@ pcl::registration::CorrespondenceEstimationBackProjection::type FieldListTarget; - // Set the internal point representation of choice if(!initComputeReciprocal()) return; @@ -241,9 +238,7 @@ pcl::registration::CorrespondenceEstimationBackProjection (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp index b6fdb33d..4402c546 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp @@ -40,6 +40,8 @@ #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ +#include + /////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::registration::CorrespondenceEstimationNormalShooting::initCompute () @@ -61,7 +63,6 @@ pcl::registration::CorrespondenceEstimationNormalShooting::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector nn_indices (k_); @@ -134,9 +135,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html @@ -178,8 +177,6 @@ pcl::registration::CorrespondenceEstimationNormalShooting::type FieldListTarget; - // setup tree for reciprocal search // Set the internal point representation of choice if (!initComputeReciprocal ()) @@ -268,9 +265,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp index 1f7806c6..7ab5a2d9 100644 --- a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp +++ b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp @@ -98,6 +98,9 @@ pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCo return; } + if (save_inliers_) + inlier_indices_.clear (); + int nr_correspondences = static_cast (original_correspondences.size ()); std::vector source_indices (nr_correspondences); std::vector target_indices (nr_correspondences); diff --git a/registration/include/pcl/registration/impl/elch.hpp b/registration/include/pcl/registration/impl/elch.hpp index 3ff0a9e6..21e1daa9 100644 --- a/registration/include/pcl/registration/impl/elch.hpp +++ b/registration/include/pcl/registration/impl/elch.hpp @@ -266,6 +266,7 @@ pcl::registration::ELCH::compute () //a = aend * a * aendI; pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a); + (*loop_graph_)[i].transform = a; } add_edge (loop_start_, loop_end_, *loop_graph_); diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index e3e6aeef..ab9ba8de 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -60,7 +60,7 @@ pcl::GeneralizedIterativeClosestPoint::computeCovarian { if (k_correspondences_ > int (cloud->size ())) { - PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%zu) is less than k_correspondences_ (%zu)!\n", cloud->size (), k_correspondences_); + PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_); return; } @@ -227,6 +227,7 @@ pcl::GeneralizedIterativeClosestPoint::estimateRigidTr int inner_iterations_ = 0; int result = bfgs.minimizeInit (x); + result = BFGSSpace::Running; do { inner_iterations_++; diff --git a/registration/include/pcl/registration/impl/ia_ransac.hpp b/registration/include/pcl/registration/impl/ia_ransac.hpp index 6a0c7763..180572c6 100644 --- a/registration/include/pcl/registration/impl/ia_ransac.hpp +++ b/registration/include/pcl/registration/impl/ia_ransac.hpp @@ -77,7 +77,7 @@ pcl::SampleConsensusInitialAlignment::select if (nr_samples > static_cast (cloud.points.size ())) { PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); - PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%zu)!\n", + PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n", nr_samples, cloud.points.size ()); return; } @@ -215,6 +215,7 @@ pcl::SampleConsensusInitialAlignment::comput final_transformation_ = guess; int i_iter = 0; + converged_ = false; if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) { // If guess is not the Identity matrix we check it. @@ -243,6 +244,7 @@ pcl::SampleConsensusInitialAlignment::comput { lowest_error = error; final_transformation_ = transformation_; + converged_=true; } } diff --git a/registration/include/pcl/registration/impl/icp.hpp b/registration/include/pcl/registration/impl/icp.hpp index 81ffeff1..84626b99 100644 --- a/registration/include/pcl/registration/impl/icp.hpp +++ b/registration/include/pcl/registration/impl/icp.hpp @@ -140,15 +140,25 @@ pcl::IterativeClosestPoint::computeTransformat transformation_ = Matrix4::Identity (); + // Make blobs if necessary + determineRequiredBlobData (); + PCLPointCloud2::Ptr target_blob (new PCLPointCloud2); + if (need_target_blob_) + pcl::toPCLPointCloud2 (*target_, *target_blob); + // Pass in the default target for the Correspondence Estimation/Rejection code correspondence_estimation_->setInputTarget (target_); - // We should be doing something like this - // for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) - // { - // correspondence_rejectors_[i]->setTargetCloud (target_); - // if (target_has_normals_) - // correspondence_rejectors_[i]->setTargetNormals (target_); - // } + if (correspondence_estimation_->requiresTargetNormals ()) + correspondence_estimation_->setTargetNormals (target_blob); + // Correspondence Rejectors need a binary blob + for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + if (rej->requiresTargetPoints ()) + rej->setTargetPoints (target_blob); + if (rej->requiresTargetNormals () && target_has_normals_) + rej->setTargetNormals (target_blob); + } convergence_criteria_->setMaximumIterations (max_iterations_); convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_); @@ -158,11 +168,20 @@ pcl::IterativeClosestPoint::computeTransformat // Repeat until convergence do { + // Get blob data if needed + PCLPointCloud2::Ptr input_transformed_blob; + if (need_source_blob_) + { + input_transformed_blob.reset (new PCLPointCloud2); + toPCLPointCloud2 (*input_transformed, *input_transformed_blob); + } // Save the previously estimated transformation previous_transformation_ = transformation_; // Set the source each iteration, to ensure the dirty flag is updated correspondence_estimation_->setInputSource (input_transformed); + if (correspondence_estimation_->requiresSourceNormals ()) + correspondence_estimation_->setSourceNormals (input_transformed_blob); // Estimate correspondences if (use_reciprocal_correspondence_) correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_); @@ -173,13 +192,14 @@ pcl::IterativeClosestPoint::computeTransformat CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_)); for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) { - PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ()); - // We should be doing something like this - // correspondence_rejectors_[i]->setInputSource (input_transformed); - // if (source_has_normals_) - // correspondence_rejectors_[i]->setInputNormals (input_transformed); - correspondence_rejectors_[i]->setInputCorrespondences (temp_correspondences); - correspondence_rejectors_[i]->getCorrespondences (*correspondences_); + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ()); + if (rej->requiresSourcePoints ()) + rej->setSourcePoints (input_transformed_blob); + if (rej->requiresSourceNormals () && source_has_normals_) + rej->setSourceNormals (input_transformed_blob); + rej->setInputCorrespondences (temp_correspondences); + rej->getCorrespondences (*correspondences_); // Modify input for the next iteration if (i < correspondence_rejectors_.size () - 1) *temp_correspondences = *correspondences_; @@ -187,7 +207,7 @@ pcl::IterativeClosestPoint::computeTransformat size_t cnt = correspondences_->size (); // Check whether we have enough correspondences - if (cnt < min_number_correspondences_) + if (static_cast (cnt) < min_number_correspondences_) { PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); @@ -227,6 +247,42 @@ pcl::IterativeClosestPoint::computeTransformat transformCloud (*input_, output, final_transformation_); } +template void +pcl::IterativeClosestPoint::determineRequiredBlobData () +{ + need_source_blob_ = false; + need_target_blob_ = false; + // Check estimator + need_source_blob_ |= correspondence_estimation_->requiresSourceNormals (); + need_target_blob_ |= correspondence_estimation_->requiresTargetNormals (); + // Add warnings if necessary + if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ()); + } + if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ()); + } + // Check rejectors + for (size_t i = 0; i < correspondence_rejectors_.size (); i++) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + need_source_blob_ |= rej->requiresSourcePoints (); + need_source_blob_ |= rej->requiresSourceNormals (); + need_target_blob_ |= rej->requiresTargetPoints (); + need_target_blob_ |= rej->requiresTargetNormals (); + if (rej->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + if (rej->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + } +} + /////////////////////////////////////////////////////////////////////////////////////////// template void pcl::IterativeClosestPointWithNormals::transformCloud ( @@ -236,5 +292,6 @@ pcl::IterativeClosestPointWithNormals::transfo { pcl::transformPointCloudWithNormals (input, output, transform); } + #endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */ diff --git a/registration/include/pcl/registration/impl/joint_icp.hpp b/registration/include/pcl/registration/impl/joint_icp.hpp new file mode 100644 index 00000000..a3623c12 --- /dev/null +++ b/registration/include/pcl/registration/impl/joint_icp.hpp @@ -0,0 +1,326 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ +#define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ + +#include +#include +#include + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::JointIterativeClosestPoint::computeTransformation ( + PointCloudSource &output, const Matrix4 &guess) +{ + // Point clouds containing the correspondences of each point in + if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n", + getClassName ().c_str ()); + return; + } + bool manual_correspondence_estimations_set = true; + if (correspondence_estimations_.empty ()) + { + manual_correspondence_estimations_set = false; + correspondence_estimations_.resize (sources_.size ()); + for (size_t i = 0; i < sources_.size (); i++) + { + correspondence_estimations_[i] = correspondence_estimation_->clone (); + KdTreeReciprocalPtr src_tree (new KdTreeReciprocal); + KdTreePtr tgt_tree (new KdTree); + correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree); + correspondence_estimations_[i]->setSearchMethodSource (src_tree); + } + } + if (correspondence_estimations_.size () != sources_.size ()) + { + PCL_ERROR ("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n", + getClassName ().c_str ()); + return; + } + std::vector inputs_transformed (sources_.size ()); + for (size_t i = 0; i < sources_.size (); i++) + { + inputs_transformed[i].reset (new PointCloudSource); + } + + nr_iterations_ = 0; + converged_ = false; + + // Initialise final transformation to the guessed one + final_transformation_ = guess; + + // Make a combined transformed input and output + std::vector input_offsets (sources_.size ()); + std::vector target_offsets (targets_.size ()); + PointCloudSourcePtr sources_combined (new PointCloudSource); + PointCloudSourcePtr inputs_transformed_combined (new PointCloudSource); + PointCloudTargetPtr targets_combined (new PointCloudTarget); + size_t input_offset = 0; + size_t target_offset = 0; + for (size_t i = 0; i < sources_.size (); i++) + { + // If the guessed transformation is non identity + if (guess != Matrix4::Identity ()) + { + // Apply guessed transformation prior to search for neighbours + this->transformCloud (*sources_[i], *inputs_transformed[i], guess); + } + else + { + *inputs_transformed[i] = *sources_[i]; + } + *sources_combined += *sources_[i]; + *inputs_transformed_combined += *inputs_transformed[i]; + *targets_combined += *targets_[i]; + input_offsets[i] = input_offset; + target_offsets[i] = target_offset; + input_offset += inputs_transformed[i]->size (); + target_offset += targets_[i]->size (); + } + + + + transformation_ = Matrix4::Identity (); + // Make blobs if necessary + determineRequiredBlobData (); + // Pass in the default target for the Correspondence Estimation/Rejection code + for (size_t i = 0; i < sources_.size (); i++) + { + correspondence_estimations_[i]->setInputTarget (targets_[i]); + if (correspondence_estimations_[i]->requiresTargetNormals ()) + { + PCLPointCloud2::Ptr target_blob (new PCLPointCloud2); + pcl::toPCLPointCloud2 (*targets_[i], *target_blob); + correspondence_estimations_[i]->setTargetNormals (target_blob); + } + } + + PCLPointCloud2::Ptr targets_combined_blob (new PCLPointCloud2); + if (!correspondence_rejectors_.empty () && need_target_blob_) + pcl::toPCLPointCloud2 (*targets_combined, *targets_combined_blob); + + for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + if (rej->requiresTargetPoints ()) + rej->setTargetPoints (targets_combined_blob); + if (rej->requiresTargetNormals () && target_has_normals_) + rej->setTargetNormals (targets_combined_blob); + } + + convergence_criteria_->setMaximumIterations (max_iterations_); + convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_); + convergence_criteria_->setTranslationThreshold (transformation_epsilon_); + convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_); + + // Repeat until convergence + std::vector partial_correspondences_ (sources_.size ()); + for (size_t i = 0; i < sources_.size (); i++) + { + partial_correspondences_[i].reset (new pcl::Correspondences); + } + + do + { + // Save the previously estimated transformation + previous_transformation_ = transformation_; + + // Set the source each iteration, to ensure the dirty flag is updated + correspondences_->clear (); + for (size_t i = 0; i < correspondence_estimations_.size (); i++) + { + correspondence_estimations_[i]->setInputSource (inputs_transformed[i]); + // Get blob data if needed + if (correspondence_estimations_[i]->requiresSourceNormals ()) + { + PCLPointCloud2::Ptr input_transformed_blob (new PCLPointCloud2); + toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob); + correspondence_estimations_[i]->setSourceNormals (input_transformed_blob); + } + + // Estimate correspondences on each cloud pair separately + if (use_reciprocal_correspondence_) + { + correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); + } + else + { + correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); + } + PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n", + getClassName ().c_str (), + partial_correspondences_[i]->size (), i); + for (size_t j = 0; j < partial_correspondences_[i]->size (); j++) + { + pcl::Correspondence corr = partial_correspondences_[i]->at (j); + // Update the offsets to be for the combined clouds + corr.index_query += input_offsets[i]; + corr.index_match += target_offsets[i]; + correspondences_->push_back (corr); + } + } + PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ()); + + PCLPointCloud2::Ptr inputs_transformed_combined_blob; + if (need_source_blob_) + { + inputs_transformed_combined_blob.reset (new PCLPointCloud2); + toPCLPointCloud2 (*inputs_transformed_combined, *inputs_transformed_combined_blob); + } + CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_)); + for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ()); + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ()); + if (rej->requiresSourcePoints ()) + rej->setSourcePoints (inputs_transformed_combined_blob); + if (rej->requiresSourceNormals () && source_has_normals_) + rej->setSourceNormals (inputs_transformed_combined_blob); + rej->setInputCorrespondences (temp_correspondences); + rej->getCorrespondences (*correspondences_); + // Modify input for the next iteration + if (i < correspondence_rejectors_.size () - 1) + *temp_correspondences = *correspondences_; + } + + int cnt = correspondences_->size (); + // Check whether we have enough correspondences + if (cnt < min_number_correspondences_) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); + convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); + converged_ = false; + break; + } + + // Estimate the transform jointly, on a combined correspondence set + transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_); + + // Tranform the combined data + this->transformCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_); + // And all its components + for (size_t i = 0; i < sources_.size (); i++) + { + this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_); + } + + // Obtain the final transformation + final_transformation_ = transformation_ * final_transformation_; + + ++nr_iterations_; + + // Update the vizualization of icp convergence + //if (update_visualizer_ != 0) + // update_visualizer_(output, source_indices_good, *target_, target_indices_good ); + + converged_ = static_cast ((*convergence_criteria_)); + } + while (!converged_); + + PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", + final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3), + final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3), + final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3), + final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3)); + + // For fitness checks, etc, we'll use an aggregated cloud for now (should be evaluating independently for correctness, but this requires propagating a few virtual methods from Registration) + IterativeClosestPoint::setInputSource (sources_combined); + IterativeClosestPoint::setInputTarget (targets_combined); + + // If we automatically set the correspondence estimators, we should clear them now + if (!manual_correspondence_estimations_set) + { + correspondence_estimations_.clear (); + } + + + // By definition, this method will return an empty cloud (for compliance with the ICP API). + // We can figure out a better solution, if necessary. + output = PointCloudSource (); +} + + template void +pcl::JointIterativeClosestPoint::determineRequiredBlobData () +{ + need_source_blob_ = false; + need_target_blob_ = false; + // Check estimators + for (size_t i = 0; i < correspondence_estimations_.size (); i++) + { + CorrespondenceEstimationPtr& ce = correspondence_estimations_[i]; + + need_source_blob_ |= ce->requiresSourceNormals (); + need_target_blob_ |= ce->requiresTargetNormals (); + // Add warnings if necessary + if (ce->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ()); + } + if (ce->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ()); + } + } + // Check rejectors + for (size_t i = 0; i < correspondence_rejectors_.size (); i++) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + need_source_blob_ |= rej->requiresSourcePoints (); + need_source_blob_ |= rej->requiresSourceNormals (); + need_target_blob_ |= rej->requiresTargetPoints (); + need_target_blob_ |= rej->requiresTargetNormals (); + if (rej->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + if (rej->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + } +} + + +#endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */ + + diff --git a/registration/include/pcl/registration/impl/ppf_registration.hpp b/registration/include/pcl/registration/impl/ppf_registration.hpp index 05fd86af..fc752a48 100644 --- a/registration/include/pcl/registration/impl/ppf_registration.hpp +++ b/registration/include/pcl/registration/impl/ppf_registration.hpp @@ -43,6 +43,7 @@ #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ +#include #include #include @@ -114,7 +115,6 @@ pcl::PPFRegistration::setInputTarget (const PointCloud scene_search_tree_->setInputCloud (target_); } - ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PPFRegistration::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) @@ -149,9 +149,11 @@ pcl::PPFRegistration::computeTransformation (PointClou Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (), scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap (); - Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())), - scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); - Eigen::Affine3f transform_sg (Eigen::Translation3f (rotation_sg * ((-1) * scene_reference_point)) * rotation_sg); + float rotation_angle_sg = acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg); + Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg); // For every other point in the scene => now have pair (s_r, s_i) fixed std::vector indices; @@ -178,18 +180,9 @@ pcl::PPFRegistration::computeTransformation (PointClou // Compute alpha_s angle Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap (); - Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())), - scene_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); - Eigen::Affine3f transform_sg = Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg; -// float alpha_s = acos (Eigen::Vector3f::UnitY ().dot ((transform_sg * scene_point).normalized ())); Eigen::Vector3f scene_point_transformed = transform_sg * scene_point; float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1)); - if ( alpha_s != alpha_s) - { - PCL_ERROR ("alpha_s is nan\n"); - continue; - } if (sin (alpha_s) * scene_point_transformed(2) < 0.0f) alpha_s *= (-1); alpha_s *= (-1); @@ -205,7 +198,7 @@ pcl::PPFRegistration::computeTransformation (PointClou accumulator_array[model_reference_index][alpha_discretized] ++; } } - else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %zu and %zu went wrong.\n", scene_reference_index, scene_point_index); + else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index); } } @@ -227,8 +220,11 @@ pcl::PPFRegistration::computeTransformation (PointClou Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (), model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap (); - Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); - Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + float rotation_angle_mg = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg); + Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); Eigen::Affine3f max_transform = transform_sg.inverse () * Eigen::AngleAxisf ((static_cast (max_votes_j) - floorf (static_cast (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) * diff --git a/registration/include/pcl/registration/impl/registration.hpp b/registration/include/pcl/registration/impl/registration.hpp index 56a37f1f..bbee1733 100644 --- a/registration/include/pcl/registration/impl/registration.hpp +++ b/registration/include/pcl/registration/impl/registration.hpp @@ -136,16 +136,7 @@ pcl::Registration::getFitnessScore (double max // Transform the input dataset using the final transformation PointCloudSource input_transformed; - //transformPointCloud (*input_, input_transformed, final_transformation_); - input_transformed.resize (input_->size ()); - for (size_t i = 0; i < input_->size (); ++i) - { - const PointSource &src = input_->points[i]; - PointTarget &tgt = input_transformed.points[i]; - tgt.x = static_cast (final_transformation_ (0, 0) * src.x + final_transformation_ (0, 1) * src.y + final_transformation_ (0, 2) * src.z + final_transformation_ (0, 3)); - tgt.y = static_cast (final_transformation_ (1, 0) * src.x + final_transformation_ (1, 1) * src.y + final_transformation_ (1, 2) * src.z + final_transformation_ (1, 3)); - tgt.z = static_cast (final_transformation_ (2, 0) * src.x + final_transformation_ (2, 1) * src.y + final_transformation_ (2, 2) * src.z + final_transformation_ (2, 3)); - } + transformPointCloud (*input_, input_transformed, final_transformation_); std::vector nn_indices (1); std::vector nn_dists (1); @@ -154,22 +145,16 @@ pcl::Registration::getFitnessScore (double max int nr = 0; for (size_t i = 0; i < input_transformed.points.size (); ++i) { - Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.points[i].x, - input_transformed.points[i].y, - input_transformed.points[i].z, 0); // Find its nearest neighbor in the target tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); // Deal with occlusions (incomplete targets) - if (nn_dists[0] > max_range) - continue; - - Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x, - target_->points[nn_indices[0]].y, - target_->points[nn_indices[0]].z, 0); - // Calculate the fitness score - fitness_score += fabs ((p1-p2).squaredNorm ()); - nr++; + if (nn_dists[0] <= max_range) + { + // Add to the fitness score + fitness_score += nn_dists[0]; + nr++; + } } if (nr > 0) diff --git a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp index fb560b94..a755e518 100644 --- a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp +++ b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp @@ -69,63 +69,73 @@ pcl::SampleConsensusPrerejective::setTargetF //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusPrerejective::selectSamples ( - const PointCloudSource &cloud, int nr_samples, - std::vector &sample_indices) + const PointCloudSource &cloud, int nr_samples, std::vector &sample_indices) { if (nr_samples > static_cast (cloud.points.size ())) { PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); - PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%zu)!\n", + PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n", nr_samples, cloud.points.size ()); return; } + + sample_indices.resize (nr_samples); + int temp_sample; - // Iteratively draw random samples until nr_samples is reached - sample_indices.clear (); - std::vector sampled_indices (cloud.points.size (), false); - while (static_cast (sample_indices.size ()) < nr_samples) + // Draw random samples until n samples is reached + for (int i = 0; i < nr_samples; i++) { - // Choose a unique sample at random - int sample_index; - do + // Select a random number + sample_indices[i] = getRandomIndex (static_cast (cloud.points.size ()) - i); + + // Run trough list of numbers, starting at the lowest, to avoid duplicates + for (int j = 0; j < i; j++) { - sample_index = getRandomIndex (static_cast (cloud.points.size ())); + // Move value up if it is higher than previous selections to ensure true randomness + if (sample_indices[i] >= sample_indices[j]) + { + sample_indices[i]++; + } + else + { + // The new number is lower, place it at the correct point and break for a sorted list + temp_sample = sample_indices[i]; + for (int k = i; k > j; k--) + sample_indices[k] = sample_indices[k - 1]; + + sample_indices[j] = temp_sample; + break; + } } - while (sampled_indices[sample_index]); - - // Mark index as sampled - sampled_indices[sample_index] = true; - - // Store - sample_indices.push_back (sample_index); } } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusPrerejective::findSimilarFeatures ( - const FeatureCloud &input_features, const std::vector &sample_indices, - std::vector &corresponding_indices) + const std::vector &sample_indices, + std::vector >& similar_features, + std::vector &corresponding_indices) { - std::vector nn_indices (k_correspondences_); - std::vector nn_distances (k_correspondences_); - + // Allocate results corresponding_indices.resize (sample_indices.size ()); + std::vector nn_distances (k_correspondences_); + + // Loop over the sampled features for (size_t i = 0; i < sample_indices.size (); ++i) { - // Find the k features nearest to input_features.points[sample_indices[i]] - feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances); + // Current feature index + const int idx = sample_indices[i]; + + // Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already + if (similar_features[idx].empty ()) + feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances); // Select one at random and add it to corresponding_indices if (k_correspondences_ == 1) - { - corresponding_indices[i] = nn_indices[0]; - } + corresponding_indices[i] = similar_features[idx][0]; else - { - int random_correspondence = getRandomIndex (k_correspondences_); - corresponding_indices[i] = nn_indices[random_correspondence]; - } + corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)]; } } @@ -180,11 +190,18 @@ pcl::SampleConsensusPrerejective::computeTra return; } + if (k_correspondences_ <= 0) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("Illegal correspondence randomness %d, must be > 0!\n", + k_correspondences_); + return; + } + // Initialize prerejector (similarity threshold already set to default value in constructor) correspondence_rejector_poly_->setInputSource (input_); correspondence_rejector_poly_->setInputTarget (target_); correspondence_rejector_poly_->setCardinality (nr_samples_); - std::vector accepted (input_->size (), false); // Indices of sampled points that passed prerejection int num_rejections = 0; // For debugging // Initialize results @@ -213,34 +230,25 @@ pcl::SampleConsensusPrerejective::computeTra } } + // Feature correspondence cache + std::vector > similar_features (input_->size ()); + // Start for (int i = 0; i < max_iterations_; ++i) { // Temporary containers - std::vector sample_indices (nr_samples_); - std::vector corresponding_indices (nr_samples_); + std::vector sample_indices; + std::vector corresponding_indices; // Draw nr_samples_ random samples selectSamples (*input_, nr_samples_, sample_indices); - // Check if all sampled points already been accepted - bool samples_accepted = true; - for (unsigned int j = 0; j < sample_indices.size(); ++j) { - if (!accepted[j]) { - samples_accepted = false; - break; - } - } - - // All points have already been accepted, avoid - if (samples_accepted) - continue; - // Find corresponding features in the target cloud - findSimilarFeatures (*input_features_, sample_indices, corresponding_indices); + findSimilarFeatures (sample_indices, similar_features, corresponding_indices); // Apply prerejection - if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)) { + if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)) + { ++num_rejections; continue; } @@ -262,19 +270,14 @@ pcl::SampleConsensusPrerejective::computeTra // If the new fit is better, update results inlier_fraction = static_cast (inliers.size ()) / static_cast (input_->size ()); - - if (inlier_fraction >= inlier_fraction_) { - // Mark the sampled points accepted - for (int j = 0; j < nr_samples_; ++j) - accepted[j] = true; - - // Update result if pose hypothesis is better - if (error < lowest_error) { - inliers_ = inliers; - lowest_error = error; - converged_ = true; - final_transformation_ = transformation_; - } + + // Update result if pose hypothesis is better + if (inlier_fraction >= inlier_fraction_ && error < lowest_error) + { + inliers_ = inliers; + lowest_error = error; + converged_ = true; + final_transformation_ = transformation_; } } @@ -315,16 +318,11 @@ pcl::SampleConsensusPrerejective::getFitness // Check if point is an inlier if (nn_dists[0] < max_range) { - // Errors - const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x; - const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y; - const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z; - // Update inliers inliers.push_back (static_cast (i)); // Update fitness score - fitness_score += dx*dx + dy*dy + dz*dz; + fitness_score += nn_dists[0]; } } diff --git a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp index 4229a1eb..759daa57 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp @@ -47,7 +47,7 @@ pcl::registration::TransformationEstimation2D: size_t nr_points = cloud_src.points.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); return; } @@ -66,7 +66,7 @@ pcl::registration::TransformationEstimation2D: { if (indices_src.size () != cloud_tgt.points.size ()) { - PCL_ERROR ("[pcl::Transformation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::Transformation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); return; } @@ -87,7 +87,7 @@ pcl::registration::TransformationEstimation2D: { if (indices_src.size () != indices_tgt.size ()) { - PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); + PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); return; } diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp index 68669860..cf7b0b39 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp @@ -51,7 +51,7 @@ pcl::registration::TransformationEstimationDQ: size_t nr_points = cloud_src.points.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); return; } @@ -70,7 +70,7 @@ pcl::registration::TransformationEstimationDQ: { if (indices_src.size () != cloud_tgt.points.size ()) { - PCL_ERROR ("[pcl::TransformationDQ::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); return; } @@ -90,7 +90,7 @@ pcl::registration::TransformationEstimationDQ: { if (indices_src.size () != indices_tgt.size ()) { - PCL_ERROR ("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); + PCL_ERROR ("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); return; } diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp index 01078804..f5f20cdf 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp @@ -51,7 +51,7 @@ pcl::registration::TransformationEstimationDualQuaternion &cloud_src, size_t nr_points = cloud_src.points.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); return; } @@ -71,7 +71,7 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, size_t nr_points = indices_src.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); return; } @@ -93,7 +93,7 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, size_t nr_points = indices_src.size (); if (indices_tgt.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); return; } @@ -159,9 +159,6 @@ estimateRigidTransformation (ConstCloudIterator& source_it, ConstCl if (!pcl_isfinite (source_it->x) || !pcl_isfinite (source_it->y) || !pcl_isfinite (source_it->z) || - !pcl_isfinite (source_it->normal_x) || - !pcl_isfinite (source_it->normal_y) || - !pcl_isfinite (source_it->normal_z) || !pcl_isfinite (target_it->x) || !pcl_isfinite (target_it->y) || !pcl_isfinite (target_it->z) || diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp index 79893522..7f2ce74b 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp @@ -50,7 +50,7 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, size_t nr_points = cloud_src.points.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); return; } @@ -77,7 +77,7 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, size_t nr_points = indices_src.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); return; } @@ -107,7 +107,7 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, size_t nr_points = indices_src.size (); if (indices_tgt.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); return; } @@ -186,9 +186,6 @@ estimateRigidTransformation (ConstCloudIterator& source_it, if (!pcl_isfinite (source_it->x) || !pcl_isfinite (source_it->y) || !pcl_isfinite (source_it->z) || - !pcl_isfinite (source_it->normal_x) || - !pcl_isfinite (source_it->normal_y) || - !pcl_isfinite (source_it->normal_z) || !pcl_isfinite (target_it->x) || !pcl_isfinite (target_it->y) || !pcl_isfinite (target_it->z) || diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp index 1204b150..d6994379 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp @@ -70,14 +70,14 @@ pcl::registration::TransformationEstimationPointToPlaneWeighted size_t nr_points = cloud_src.points.size (); if (cloud_tgt.points.size () != nr_points) { - PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); return; } @@ -71,7 +71,7 @@ pcl::registration::TransformationEstimationSVD { if (indices_src.size () != cloud_tgt.points.size ()) { - PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); + PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); return; } @@ -91,7 +91,7 @@ pcl::registration::TransformationEstimationSVD { if (indices_src.size () != indices_tgt.size ()) { - PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); + PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); return; } @@ -123,24 +123,26 @@ pcl::registration::TransformationEstimationSVD // Convert to Eigen format const int npts = static_cast (source_it.size ()); - Eigen::Matrix cloud_src (3, npts); - Eigen::Matrix cloud_tgt (3, npts); - for (int i = 0; i < npts; ++i) - { - cloud_src (0, i) = source_it->x; - cloud_src (1, i) = source_it->y; - cloud_src (2, i) = source_it->z; - ++source_it; - - cloud_tgt (0, i) = target_it->x; - cloud_tgt (1, i) = target_it->y; - cloud_tgt (2, i) = target_it->z; - ++target_it; - } if (use_umeyama_) { + Eigen::Matrix cloud_src (3, npts); + Eigen::Matrix cloud_tgt (3, npts); + + for (int i = 0; i < npts; ++i) + { + cloud_src (0, i) = source_it->x; + cloud_src (1, i) = source_it->y; + cloud_src (2, i) = source_it->z; + ++source_it; + + cloud_tgt (0, i) = target_it->x; + cloud_tgt (1, i) = target_it->y; + cloud_tgt (2, i) = target_it->z; + ++target_it; + } + // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); } diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp index 9cb35469..553342ef 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp @@ -98,7 +98,7 @@ pcl::registration::TransformationEstimationSVDScale Rc (R * centroid_src.head (3)); + const Eigen::Matrix Rc (scale * R * centroid_src.head (3)); transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc; } diff --git a/registration/include/pcl/registration/joint_icp.h b/registration/include/pcl/registration/joint_icp.h new file mode 100644 index 00000000..d51d76b5 --- /dev/null +++ b/registration/include/pcl/registration/joint_icp.h @@ -0,0 +1,233 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_JOINT_ICP_H_ +#define PCL_JOINT_ICP_H_ + +// PCL includes +#include +namespace pcl +{ + /** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which + * share the same transform. This is particularly useful when solving for + * camera extrinsics using multiple observations. When given a single pair of + * clouds, this reduces to vanilla ICP. + * + * \author Stephen Miller + * \ingroup registration + */ + template + class JointIterativeClosestPoint : public IterativeClosestPoint + { + public: + typedef typename IterativeClosestPoint::PointCloudSource PointCloudSource; + typedef typename PointCloudSource::Ptr PointCloudSourcePtr; + typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; + + typedef typename IterativeClosestPoint::PointCloudTarget PointCloudTarget; + typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; + typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; + + typedef pcl::search::KdTree KdTree; + typedef typename pcl::search::KdTree::Ptr KdTreePtr; + + typedef pcl::search::KdTree KdTreeReciprocal; + typedef typename KdTree::Ptr KdTreeReciprocalPtr; + + + typedef PointIndices::Ptr PointIndicesPtr; + typedef PointIndices::ConstPtr PointIndicesConstPtr; + + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + typedef typename pcl::registration::CorrespondenceEstimationBase CorrespondenceEstimation; + typedef typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr; + typedef typename CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr; + + + using IterativeClosestPoint::reg_name_; + using IterativeClosestPoint::getClassName; + using IterativeClosestPoint::setInputSource; + using IterativeClosestPoint::input_; + using IterativeClosestPoint::indices_; + using IterativeClosestPoint::target_; + using IterativeClosestPoint::nr_iterations_; + using IterativeClosestPoint::max_iterations_; + using IterativeClosestPoint::previous_transformation_; + using IterativeClosestPoint::final_transformation_; + using IterativeClosestPoint::transformation_; + using IterativeClosestPoint::transformation_epsilon_; + using IterativeClosestPoint::converged_; + using IterativeClosestPoint::corr_dist_threshold_; + using IterativeClosestPoint::inlier_threshold_; + using IterativeClosestPoint::min_number_correspondences_; + using IterativeClosestPoint::update_visualizer_; + using IterativeClosestPoint::euclidean_fitness_epsilon_; + using IterativeClosestPoint::correspondences_; + using IterativeClosestPoint::transformation_estimation_; + using IterativeClosestPoint::correspondence_estimation_; + using IterativeClosestPoint::correspondence_rejectors_; + + using IterativeClosestPoint::use_reciprocal_correspondence_; + + using IterativeClosestPoint::convergence_criteria_; + using IterativeClosestPoint::source_has_normals_; + using IterativeClosestPoint::target_has_normals_; + using IterativeClosestPoint::need_source_blob_; + using IterativeClosestPoint::need_target_blob_; + + + typedef typename IterativeClosestPoint::Matrix4 Matrix4; + + /** \brief Empty constructor. */ + JointIterativeClosestPoint () + { + IterativeClosestPoint (); + reg_name_ = "JointIterativeClosestPoint"; + }; + + /** \brief Empty destructor */ + virtual ~JointIterativeClosestPoint () {} + + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + */ + virtual void + setInputSource (const PointCloudSourceConstPtr& /*cloud*/) + { + PCL_WARN ("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputSource.", + getClassName ().c_str ()); + return; + } + + /** \brief Add a source cloud to the joint solver + * + * \param[in] cloud source cloud + */ + inline void + addInputSource (const PointCloudSourceConstPtr &cloud) + { + // Set the parent InputSource, just to get all cached values (e.g. the existence of normals). + if (sources_.empty ()) + IterativeClosestPoint::setInputSource (cloud); + sources_.push_back (cloud); + } + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align to the target) + */ + virtual void + setInputTarget (const PointCloudTargetConstPtr& /*cloud*/) + { + PCL_WARN ("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputTarget.", + getClassName ().c_str ()); + return; + } + + /** \brief Add a target cloud to the joint solver + * + * \param[in] cloud target cloud + */ + inline void + addInputTarget (const PointCloudTargetConstPtr &cloud) + { + // Set the parent InputTarget, just to get all cached values (e.g. the existence of normals). + if (targets_.empty ()) + IterativeClosestPoint::setInputTarget (cloud); + targets_.push_back (cloud); + } + + /** \brief Add a manual correspondence estimator + * If you choose to do this, you must add one for each + * input source / target pair. They do not need to have trees + * or input clouds set ahead of time. + * + * \param[in] ce Correspondence estimation + */ + inline void + addCorrespondenceEstimation (CorrespondenceEstimationPtr ce) + { + correspondence_estimations_.push_back (ce); + } + + /** \brief Reset my list of input sources + */ + inline void + clearInputSources () + { sources_.clear (); } + + /** \brief Reset my list of input targets + */ + inline void + clearInputTargets () + { targets_.clear (); } + + /** \brief Reset my list of correspondence estimation methods. + */ + inline void + clearCorrespondenceEstimations () + { correspondence_estimations_.clear (); } + + + protected: + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + virtual void + computeTransformation (PointCloudSource &output, const Matrix4 &guess); + + /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */ + void + determineRequiredBlobData (); + + std::vector sources_; + std::vector targets_; + std::vector correspondence_estimations_; + }; + +} + +#include + +#endif //#ifndef PCL_JOINT_ICP_H_ + + diff --git a/registration/include/pcl/registration/ndt.h b/registration/include/pcl/registration/ndt.h index efc84bf7..fe7d20c6 100644 --- a/registration/include/pcl/registration/ndt.h +++ b/registration/include/pcl/registration/ndt.h @@ -369,7 +369,7 @@ namespace pcl * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$ * until some value satifies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ * then \f$ \phi(\alpha_k) \f$ is used from then on. - * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).<\b> + * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100). * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994) * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994) diff --git a/registration/include/pcl/registration/registration.h b/registration/include/pcl/registration/registration.h index 25371513..b30ae685 100644 --- a/registration/include/pcl/registration/registration.h +++ b/registration/include/pcl/registration/registration.h @@ -178,10 +178,14 @@ namespace pcl * * \param[in] cloud the input point cloud source */ - PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::Registration::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::Registration::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.") + void + setInputCloud (const PointCloudSourceConstPtr &cloud); /** \brief Get a pointer to the input point cloud dataset target. */ - PCL_DEPRECATED (PointCloudSourceConstPtr const getInputCloud (), "[pcl::registration::Registration::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead."); + PCL_DEPRECATED ("[pcl::registration::Registration::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.") + PointCloudSourceConstPtr const + getInputCloud (); /** \brief Provide a pointer to the input source * (e.g., the point cloud that we want to align to the target) diff --git a/registration/include/pcl/registration/sample_consensus_prerejective.h b/registration/include/pcl/registration/sample_consensus_prerejective.h index d6d9add3..8ef2ada3 100644 --- a/registration/include/pcl/registration/sample_consensus_prerejective.h +++ b/registration/include/pcl/registration/sample_consensus_prerejective.h @@ -257,22 +257,23 @@ namespace pcl * \param sample_indices the resulting sample indices */ void - selectSamples (const PointCloudSource &cloud, int nr_samples, - std::vector &sample_indices); + selectSamples (const PointCloudSource &cloud, int nr_samples, std::vector &sample_indices); /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to * the sample points' features. From these, select one randomly which will be considered that sample point's - * correspondence. - * \param input_features a cloud of feature descriptors + * correspondence. * \param sample_indices the indices of each sample point + * \param similar_features correspondence cache, which is used to read/write already computed correspondences * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud */ void - findSimilarFeatures (const FeatureCloud &input_features, const std::vector &sample_indices, - std::vector &corresponding_indices); + findSimilarFeatures (const std::vector &sample_indices, + std::vector >& similar_features, + std::vector &corresponding_indices); /** \brief Rigid transformation computation method. * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess The computed transformation */ void computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess); diff --git a/registration/include/pcl/registration/transformation_estimation_lm.h b/registration/include/pcl/registration/transformation_estimation_lm.h index 04e5a1d9..7f3e43bd 100644 --- a/registration/include/pcl/registration/transformation_estimation_lm.h +++ b/registration/include/pcl/registration/transformation_estimation_lm.h @@ -267,7 +267,7 @@ namespace pcl {} /** Copy constructor - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctor (const OptimizationFunctor &src) : Functor (src.m_data_points_), estimator_ () @@ -276,7 +276,7 @@ namespace pcl } /** Copy operator - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctor& operator = (const OptimizationFunctor &src) @@ -313,7 +313,7 @@ namespace pcl {} /** Copy constructor - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src) : Functor (src.m_data_points_), estimator_ () @@ -322,7 +322,7 @@ namespace pcl } /** Copy operator - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctorWithIndices& operator = (const OptimizationFunctorWithIndices &src) diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h index 916ccc12..04d3583f 100644 --- a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls.h @@ -142,7 +142,7 @@ namespace pcl * \param[in] tx the x translation * \param[in] ty the y translation * \param[in] tz the z translation - * \param[out] transformation the resultant transformation matrix + * \param[out] transformation_matrix the resultant transformation matrix */ inline void constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h index e4a36105..33d8866d 100644 --- a/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h @@ -136,6 +136,7 @@ namespace pcl /** \brief Estimate a rigid rotation transformation between a source and a target * \param[in] source_it an iterator over the source point cloud dataset * \param[in] target_it an iterator over the target point cloud dataset + * \param weights_it * \param[out] transformation_matrix the resultant transformation matrix */ void @@ -151,7 +152,7 @@ namespace pcl * \param[in] tx the x translation * \param[in] ty the y translation * \param[in] tz the z translation - * \param[out] transformation the resultant transformation matrix + * \param[out] transformation_matrix the resultant transformation matrix */ inline void constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h index ca2f820a..3013d670 100644 --- a/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h @@ -252,7 +252,7 @@ namespace pcl {} /** Copy constructor - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctor (const OptimizationFunctor &src) : Functor (src.m_data_points_), estimator_ () @@ -261,7 +261,7 @@ namespace pcl } /** Copy operator - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctor& operator = (const OptimizationFunctor &src) @@ -298,7 +298,7 @@ namespace pcl {} /** Copy constructor - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src) : Functor (src.m_data_points_), estimator_ () @@ -307,7 +307,7 @@ namespace pcl } /** Copy operator - * \param[in] the optimization functor to copy into this + * \param[in] src the optimization functor to copy into this */ inline OptimizationFunctorWithIndices& operator = (const OptimizationFunctorWithIndices &src) diff --git a/registration/src/correspondence_rejection_organized_boundary.cpp b/registration/src/correspondence_rejection_organized_boundary.cpp index 459f05ac..31752461 100644 --- a/registration/src/correspondence_rejection_organized_boundary.cpp +++ b/registration/src/correspondence_rejection_organized_boundary.cpp @@ -62,8 +62,8 @@ pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorresp int nan_count_tgt = 0; for (int x_d = -window_size_/2; x_d <= window_size_/2; ++x_d) for (int y_d = -window_size_/2; y_d <= window_size_/2; ++y_d) - if (x + x_d >= 0 && x + x_d < cloud->width && - y + y_d >= 0 && y + y_d < cloud->height) + if (x + x_d >= 0 && x + x_d < static_cast (cloud->width) && + y + y_d >= 0 && y + y_d < static_cast (cloud->height)) { if (!pcl_isfinite ((*cloud)(x + x_d, y + y_d).z) || fabs ((*cloud)(x, y).z - (*cloud)(x + x_d, y + y_d).z) > depth_step_threshold_) diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp new file mode 100644 index 00000000..08ebeeb8 --- /dev/null +++ b/registration/src/gicp6d.cpp @@ -0,0 +1,317 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +namespace pcl +{ + // convert sRGB to CIELAB + Eigen::Vector3f + RGB2Lab (const Eigen::Vector3i& colorRGB) + { + // for sRGB -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2 + // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7 + + double R, G, B, X, Y, Z; + + // map sRGB values to [0, 1] + R = colorRGB[0] / 255.0; + G = colorRGB[1] / 255.0; + B = colorRGB[2] / 255.0; + + // linearize sRGB values + if (R > 0.04045) + R = pow ( (R + 0.055) / 1.055, 2.4); + else + R = R / 12.92; + + if (G > 0.04045) + G = pow ( (G + 0.055) / 1.055, 2.4); + else + G = G / 12.92; + + if (B > 0.04045) + B = pow ( (B + 0.055) / 1.055, 2.4); + else + B = B / 12.92; + + // postponed: + // R *= 100.0; + // G *= 100.0; + // B *= 100.0; + + // linear sRGB -> CIEXYZ + X = R * 0.4124 + G * 0.3576 + B * 0.1805; + Y = R * 0.2126 + G * 0.7152 + B * 0.0722; + Z = R * 0.0193 + G * 0.1192 + B * 0.9505; + + // *= 100.0 including: + X /= 0.95047; //95.047; + // Y /= 1;//100.000; + Z /= 1.08883; //108.883; + + // CIEXYZ -> CIELAB + if (X > 0.008856) + X = pow (X, 1.0 / 3.0); + else + X = 7.787 * X + 16.0 / 116.0; + + if (Y > 0.008856) + Y = pow (Y, 1.0 / 3.0); + else + Y = 7.787 * Y + 16.0 / 116.0; + + if (Z > 0.008856) + Z = pow (Z, 1.0 / 3.0); + else + Z = 7.787 * Z + 16.0 / 116.0; + + Eigen::Vector3f colorLab; + colorLab[0] = static_cast (116.0 * Y - 16.0); + colorLab[1] = static_cast (500.0 * (X - Y)); + colorLab[2] = static_cast (200.0 * (Y - Z)); + + return colorLab; + } + + // convert a PointXYZRGBA cloud to a PointXYZLAB cloud + void + convertRGBAToLAB (const PointCloud& in, PointCloud& out) + { + out.resize (in.size ()); + + for (size_t i = 0; i < in.size (); ++i) + { + out[i].x = in[i].x; + out[i].y = in[i].y; + out[i].z = in[i].z; + out[i].data[3] = 1.0; // important for homogeneous coordinates + + Eigen::Vector3f lab = RGB2Lab (in[i].getRGBVector3i ()); + out[i].L = lab[0]; + out[i].a = lab[1]; + out[i].b = lab[2]; + } + } + + GeneralizedIterativeClosestPoint6D::GeneralizedIterativeClosestPoint6D (float lab_weight) : + cloud_lab_ (new pcl::PointCloud), target_lab_ (new pcl::PointCloud), lab_weight_ (lab_weight) + { + // set rescale mask (leave x,y,z unchanged, scale L,a,b by lab_weight) + float alpha[6] = { 1.0, 1.0, 1.0, lab_weight_, lab_weight_, lab_weight_ }; + point_rep_.setRescaleValues (alpha); + } + + void + GeneralizedIterativeClosestPoint6D::setInputSource (const PointCloudSourceConstPtr& cloud) + { + // call corresponding base class method + GeneralizedIterativeClosestPoint::setInputSource (cloud); + + // in addition, convert colors of the cloud to CIELAB + convertRGBAToLAB (*cloud, *cloud_lab_); + } + + void + GeneralizedIterativeClosestPoint6D::setInputTarget (const PointCloudTargetConstPtr& target) + { + // call corresponding base class method + GeneralizedIterativeClosestPoint::setInputTarget ( + target); + + // in addition, convert colors of the cloud to CIELAB... + convertRGBAToLAB (*target, *target_lab_); + + // ...and build 6d-tree + target_tree_lab_.setInputCloud (target_lab_); + target_tree_lab_.setPointRepresentation ( + boost::make_shared (point_rep_)); + } + + bool + GeneralizedIterativeClosestPoint6D::searchForNeighbors (const PointXYZLAB& query, std::vector& index, + std::vector& distance) + { + int k = target_tree_lab_.nearestKSearch (query, 1, index, distance); + + // check if neighbor was found + return (k != 0); + } + +// taken from the original GICP class and modified slightly to make use of color values + void + GeneralizedIterativeClosestPoint6D::computeTransformation (PointCloudSource& output, const Eigen::Matrix4f& guess) + { + using namespace pcl; + using namespace std; + + IterativeClosestPoint::initComputeReciprocal (); + + // Difference between consecutive transforms + double delta = 0; + // Get the size of the target + const size_t N = indices_->size (); + + // Set the mahalanobis matrices to identity + mahalanobis_.resize (N, Eigen::Matrix3d::Identity ()); + + // Compute target cloud covariance matrices + computeCovariances (target_, tree_, target_covariances_); + // Compute input cloud covariance matrices + computeCovariances (input_, tree_reciprocal_, + input_covariances_); + + base_transformation_ = guess; + nr_iterations_ = 0; + converged_ = false; + double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; + std::vector nn_indices (1); + std::vector nn_dists (1); + + while (!converged_) + { + size_t cnt = 0; + std::vector source_indices (indices_->size ()); + std::vector target_indices (indices_->size ()); + + // guess corresponds to base_t and transformation_ to t + Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero (); + for (size_t i = 0; i < 4; i++) + for (size_t j = 0; j < 4; j++) + for (size_t k = 0; k < 4; k++) + transform_R (i, j) += double (transformation_ (i, k)) + * double (guess (k, j)); + + Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3> (); + + for (size_t i = 0; i < N; i++) + { + // MODIFICATION: take point from the CIELAB cloud instead + PointXYZLAB query = (*cloud_lab_)[i]; + query.getVector4fMap () = guess * query.getVector4fMap (); + query.getVector4fMap () = transformation_ * query.getVector4fMap (); + + if (!searchForNeighbors (query, nn_indices, nn_dists)) + { + PCL_ERROR( + "[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", + getClassName ().c_str (), (*indices_)[i]); + return; + } + + // Check if the distance to the nearest neighbor is smaller than the user imposed threshold + if (nn_dists[0] < dist_threshold) + { + Eigen::Matrix3d &C1 = input_covariances_[i]; + Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]]; + Eigen::Matrix3d &M = mahalanobis_[i]; + // M = R*C1 + M = R * C1; + // temp = M*R' + C2 = R*C1*R' + C2 + Eigen::Matrix3d temp = M * R.transpose (); + temp += C2; + // M = temp^-1 + M = temp.inverse (); + source_indices[cnt] = static_cast (i); + target_indices[cnt] = nn_indices[0]; + cnt++; + } + } + // Resize to the actual number of valid correspondences + source_indices.resize (cnt); + target_indices.resize (cnt); + /* optimize transformation using the current assignment and Mahalanobis metrics*/ + previous_transformation_ = transformation_; + //optimization right here + try + { + rigid_transformation_estimation_ (output, source_indices, *target_, + target_indices, transformation_); + /* compute the delta from this iteration */ + delta = 0.; + for (int k = 0; k < 4; k++) + { + for (int l = 0; l < 4; l++) + { + double ratio = 1; + if (k < 3 && l < 3) // rotation part of the transform + ratio = 1. / rotation_epsilon_; + else + ratio = 1. / transformation_epsilon_; + double c_delta = ratio + * fabs ( + previous_transformation_ (k, l) - transformation_ (k, l)); + if (c_delta > delta) + delta = c_delta; + } + } + } + catch (PCLException &e) + { + PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n", + getClassName ().c_str (), e.what ()); + break; + } + + nr_iterations_++; + // Check for convergence + if (nr_iterations_ >= max_iterations_ || delta < 1) + { + converged_ = true; + previous_transformation_ = transformation_; + PCL_DEBUG( + "[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", + getClassName ().c_str (), nr_iterations_, max_iterations_, + (transformation_ - previous_transformation_).array ().abs ().sum ()); + } + else + PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n", + getClassName ().c_str ()); + } + //for some reason the static equivalent methode raises an error + // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0)); + // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3); + final_transformation_.topLeftCorner (3, 3) = + previous_transformation_.topLeftCorner (3, 3) + * guess.topLeftCorner (3, 3); + final_transformation_ (0, 3) = previous_transformation_ (0, 3) + guess (0, 3); + final_transformation_ (1, 3) = previous_transformation_ (1, 3) + guess (1, 3); + final_transformation_ (2, 3) = previous_transformation_ (2, 3) + guess (2, 3); + } + +} diff --git a/registration/src/joint_icp.cpp b/registration/src/joint_icp.cpp new file mode 100644 index 00000000..b83032f6 --- /dev/null +++ b/registration/src/joint_icp.cpp @@ -0,0 +1,39 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include diff --git a/sample_consensus/CMakeLists.txt b/sample_consensus/CMakeLists.txt index 403db3f3..15f28126 100644 --- a/sample_consensus/CMakeLists.txt +++ b/sample_consensus/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud sample consensus library") set(SUBSYS_DEPS common search) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -26,72 +26,72 @@ if(build) ) set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/eigen.h - include/pcl/${SUBSYS_NAME}/lmeds.h - include/pcl/${SUBSYS_NAME}/method_types.h - include/pcl/${SUBSYS_NAME}/mlesac.h - include/pcl/${SUBSYS_NAME}/model_types.h - include/pcl/${SUBSYS_NAME}/msac.h - include/pcl/${SUBSYS_NAME}/ransac.h - include/pcl/${SUBSYS_NAME}/rmsac.h - include/pcl/${SUBSYS_NAME}/rransac.h - include/pcl/${SUBSYS_NAME}/prosac.h - include/pcl/${SUBSYS_NAME}/sac.h - include/pcl/${SUBSYS_NAME}/sac_model.h - include/pcl/${SUBSYS_NAME}/sac_model_circle.h - include/pcl/${SUBSYS_NAME}/sac_model_circle3d.h - include/pcl/${SUBSYS_NAME}/sac_model_cylinder.h - include/pcl/${SUBSYS_NAME}/sac_model_cone.h - include/pcl/${SUBSYS_NAME}/sac_model_line.h - include/pcl/${SUBSYS_NAME}/sac_model_stick.h - include/pcl/${SUBSYS_NAME}/sac_model_normal_parallel_plane.h - include/pcl/${SUBSYS_NAME}/sac_model_normal_plane.h - include/pcl/${SUBSYS_NAME}/sac_model_normal_sphere.h - include/pcl/${SUBSYS_NAME}/sac_model_parallel_line.h - include/pcl/${SUBSYS_NAME}/sac_model_parallel_plane.h - include/pcl/${SUBSYS_NAME}/sac_model_perpendicular_plane.h - include/pcl/${SUBSYS_NAME}/sac_model_plane.h - include/pcl/${SUBSYS_NAME}/sac_model_registration.h - include/pcl/${SUBSYS_NAME}/sac_model_registration_2d.h - include/pcl/${SUBSYS_NAME}/sac_model_sphere.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/lmeds.h" + "include/pcl/${SUBSYS_NAME}/method_types.h" + "include/pcl/${SUBSYS_NAME}/mlesac.h" + "include/pcl/${SUBSYS_NAME}/model_types.h" + "include/pcl/${SUBSYS_NAME}/msac.h" + "include/pcl/${SUBSYS_NAME}/ransac.h" + "include/pcl/${SUBSYS_NAME}/rmsac.h" + "include/pcl/${SUBSYS_NAME}/rransac.h" + "include/pcl/${SUBSYS_NAME}/prosac.h" + "include/pcl/${SUBSYS_NAME}/sac.h" + "include/pcl/${SUBSYS_NAME}/sac_model.h" + "include/pcl/${SUBSYS_NAME}/sac_model_circle.h" + "include/pcl/${SUBSYS_NAME}/sac_model_circle3d.h" + "include/pcl/${SUBSYS_NAME}/sac_model_cylinder.h" + "include/pcl/${SUBSYS_NAME}/sac_model_cone.h" + "include/pcl/${SUBSYS_NAME}/sac_model_line.h" + "include/pcl/${SUBSYS_NAME}/sac_model_stick.h" + "include/pcl/${SUBSYS_NAME}/sac_model_normal_parallel_plane.h" + "include/pcl/${SUBSYS_NAME}/sac_model_normal_plane.h" + "include/pcl/${SUBSYS_NAME}/sac_model_normal_sphere.h" + "include/pcl/${SUBSYS_NAME}/sac_model_parallel_line.h" + "include/pcl/${SUBSYS_NAME}/sac_model_parallel_plane.h" + "include/pcl/${SUBSYS_NAME}/sac_model_perpendicular_plane.h" + "include/pcl/${SUBSYS_NAME}/sac_model_plane.h" + "include/pcl/${SUBSYS_NAME}/sac_model_registration.h" + "include/pcl/${SUBSYS_NAME}/sac_model_registration_2d.h" + "include/pcl/${SUBSYS_NAME}/sac_model_sphere.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/lmeds.hpp - include/pcl/${SUBSYS_NAME}/impl/mlesac.hpp - include/pcl/${SUBSYS_NAME}/impl/msac.hpp - include/pcl/${SUBSYS_NAME}/impl/ransac.hpp - include/pcl/${SUBSYS_NAME}/impl/rmsac.hpp - include/pcl/${SUBSYS_NAME}/impl/rransac.hpp - include/pcl/${SUBSYS_NAME}/impl/prosac.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_circle.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_circle3d.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_cylinder.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_cone.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_line.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_stick.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_normal_parallel_plane.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_normal_plane.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_normal_sphere.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_parallel_line.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_parallel_plane.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_perpendicular_plane.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_plane.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_registration.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_registration_2d.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_model_sphere.hpp + "include/pcl/${SUBSYS_NAME}/impl/lmeds.hpp" + "include/pcl/${SUBSYS_NAME}/impl/mlesac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/msac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/ransac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/rmsac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/rransac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/prosac.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_circle.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_circle3d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_cylinder.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_cone.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_line.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_stick.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_normal_parallel_plane.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_normal_plane.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_normal_sphere.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_parallel_line.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_parallel_plane.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_perpendicular_plane.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_plane.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_registration.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_registration_2d.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_model_sphere.hpp" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_common) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_common) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp index abddebc4..c24e988a 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp @@ -155,7 +155,7 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) if (distances.size () != indices.size ()) { - PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ()); + PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); return (false); } @@ -170,7 +170,7 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count); + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp index eeaa1241..7c6141e2 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp @@ -182,7 +182,7 @@ pcl::MaximumLikelihoodSampleConsensus::computeModel (int debug_verbosity std::vector &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { - PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ()); + PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); return (false); } @@ -197,7 +197,7 @@ pcl::MaximumLikelihoodSampleConsensus::computeModel (int debug_verbosity inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count); + PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp index 7b561403..659d338e 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp @@ -141,7 +141,7 @@ pcl::MEstimatorSampleConsensus::computeModel (int debug_verbosity_level) if (distances.size () != indices.size ()) { - PCL_ERROR ("[pcl::MEstimatorSampleConsensus::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ()); + PCL_ERROR ("[pcl::MEstimatorSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); return (false); } @@ -156,7 +156,7 @@ pcl::MEstimatorSampleConsensus::computeModel (int debug_verbosity_level) inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count); + PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp index 91c72522..674eaa3f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp @@ -223,7 +223,7 @@ pcl::ProgressiveSampleConsensus::computeModel (int debug_verbosity_level } if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), I_N_best); + PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), I_N_best); if (model_.empty ()) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp index dab6ddce..3c174fea 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp @@ -122,7 +122,7 @@ pcl::RandomSampleConsensus::computeModel (int) } } - PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_best_inliers_count); + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_best_inliers_count); if (model_.empty ()) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp index feaf95c2..186f3033 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp @@ -158,7 +158,7 @@ pcl::RandomizedMEstimatorSampleConsensus::computeModel (int debug_verbos std::vector &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { - PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ()); + PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); return (false); } @@ -173,7 +173,7 @@ pcl::RandomizedMEstimatorSampleConsensus::computeModel (int debug_verbos inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count); + PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp index 9ac900fc..338780dc 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp @@ -132,7 +132,7 @@ pcl::RandomizedRandomSampleConsensus::computeModel (int debug_verbosity_ } if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_best_inliers_count); + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_best_inliers_count); if (model_.empty ()) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp index c7b5f3da..99a1009f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp @@ -71,7 +71,7 @@ pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const std:: // Need 3 samples if (samples.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -203,14 +203,14 @@ pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( // Needs a set of valid model coefficients if (model_coefficients.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } // Need at least 3 samples if (inliers.size () <= 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -235,7 +235,7 @@ pcl::SampleConsensusModelCircle2D::projectPoints ( // Needs a valid set of model coefficients if (model_coefficients.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -301,7 +301,7 @@ pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel ( // Needs a valid model coefficients if (model_coefficients.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } @@ -326,7 +326,7 @@ pcl::SampleConsensusModelCircle2D::isModelValid (const Eigen::VectorXf & // Needs a valid model coefficients if (model_coefficients.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp index ebc809be..d9ee61af 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp @@ -70,7 +70,7 @@ pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const std:: // Need 3 samples if (samples.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -262,14 +262,14 @@ pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( // Needs a set of valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } // Need at least 3 samples if (inliers.size () <= 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -280,7 +280,7 @@ pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( Eigen::LevenbergMarquardt, double> lm (num_diff); Eigen::VectorXd coeff; int info = lm.minimize (coeff); - for (size_t i = 0; i < coeff.size (); ++i) + for (int i = 0; i < coeff.size (); ++i) optimized_coefficients[i] = static_cast (coeff[i]); // Compute the L2 norm of the residuals @@ -297,7 +297,7 @@ pcl::SampleConsensusModelCircle3D::projectPoints ( // Needs a valid set of model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -400,7 +400,7 @@ pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel ( // Needs a valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } @@ -442,7 +442,7 @@ pcl::SampleConsensusModelCircle3D::isModelValid (const Eigen::VectorXf & // Needs a valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp index 723b6100..aca3df54 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp @@ -58,7 +58,7 @@ pcl::SampleConsensusModelCone::computeModelCoefficients ( // Need 3 samples if (samples.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -314,7 +314,7 @@ pcl::SampleConsensusModelCone::optimizeModelCoefficients ( // Needs a set of valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -345,7 +345,7 @@ pcl::SampleConsensusModelCone::projectPoints ( // Needs a valid set of model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -441,7 +441,7 @@ pcl::SampleConsensusModelCone::doSamplesVerifyModel ( // Needs a valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } @@ -493,7 +493,7 @@ pcl::SampleConsensusModelCone::isModelValid (const Eigen::Vecto // Needs a valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp index 38b3b46d..fb3a3243 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp @@ -60,7 +60,7 @@ pcl::SampleConsensusModelCylinder::computeModelCoefficients ( // Need 2 samples if (samples.size () != 2) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -271,7 +271,7 @@ pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( // Needs a set of valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -308,7 +308,7 @@ pcl::SampleConsensusModelCylinder::projectPoints ( // Needs a valid set of model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -394,7 +394,7 @@ pcl::SampleConsensusModelCylinder::doSamplesVerifyModel ( // Needs a valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } @@ -446,7 +446,7 @@ pcl::SampleConsensusModelCylinder::isModelValid (const Eigen::V // Needs a valid model coefficients if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp index 457fd801..af019a1a 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp @@ -68,7 +68,7 @@ pcl::SampleConsensusModelLine::computeModelCoefficients ( // Need 2 samples if (samples.size () != 2) { - PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -203,7 +203,7 @@ pcl::SampleConsensusModelLine::optimizeModelCoefficients ( // Need at least 2 points to estimate a line if (inliers.size () <= 2) { - PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp index c956444f..5177bf8f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp @@ -43,137 +43,6 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelNormalParallelPlane::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) -{ - if (!normals_) - { - PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::selectWithinDistance] No input dataset containing normals was given!\n"); - return; - } - - // Check if the model is valid given the user constraints - if (!isModelValid (model_coefficients)) - { - inliers.clear (); - return; - } - - // Obtain the plane normal - Eigen::Vector4f coeff = model_coefficients; - - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); - - // Iterate through the 3d points and calculate the distances from them to the plane - for (size_t i = 0; i < indices_->size (); ++i) - { - // Calculate the distance from the point to the plane normal as the dot product - // D = (P-A).N/|N| - Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); - double d_euclid = fabs (coeff.dot (p)); - - // Calculate the angular distance between the point normal and the plane normal - double d_normal = getAngle3D (n, coeff); - d_normal = (std::min) (d_normal, fabs(M_PI - d_normal)); - - double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); - if (distance < threshold) - { - // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = distance; - ++nr_p; - } - } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::SampleConsensusModelNormalParallelPlane::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) -{ - if (!normals_) - { - PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::countWithinDistance] No input dataset containing normals was given!\n"); - return (0); - } - - // Check if the model is valid given the user constraints - if (!isModelValid (model_coefficients)) - return (0); - - // Obtain the plane normal - Eigen::Vector4f coeff = model_coefficients; - - int nr_p = 0; - - // Iterate through the 3d points and calculate the distances from them to the plane - for (size_t i = 0; i < indices_->size (); ++i) - { - // Calculate the distance from the point to the plane normal as the dot product - // D = (P-A).N/|N| - Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); - double d_euclid = fabs (coeff.dot (p)); - - // Calculate the angular distance between the point normal and the plane normal - double d_normal = fabs (getAngle3D (n, coeff)); - d_normal = (std::min) (d_normal, fabs(M_PI - d_normal)); - - if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) - nr_p++; - } - return (nr_p); -} - - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelNormalParallelPlane::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) -{ - if (!normals_) - { - PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::getDistancesToModel] No input dataset containing normals was given!\n"); - return; - } - - // Check if the model is valid given the user constraints - if (!isModelValid (model_coefficients)) - { - distances.clear (); - return; - } - - // Obtain the plane normal - Eigen::Vector4f coeff = model_coefficients; - - distances.resize (indices_->size ()); - - // Iterate through the 3d points and calculate the distances from them to the plane - for (size_t i = 0; i < indices_->size (); ++i) - { - // Calculate the distance from the point to the plane normal as the dot product - // D = (P-A).N/|N| - Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); - double d_euclid = fabs (coeff.dot (p)); - - // Calculate the angular distance between the point normal and the plane normal - double d_normal = getAngle3D (n, coeff); - d_normal = (std::min) (d_normal, fabs (M_PI - d_normal)); - - distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); - } -} - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelNormalParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) @@ -181,7 +50,7 @@ pcl::SampleConsensusModelNormalParallelPlane::isModelValid (con // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp index 389d76d3..4db84c15 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp @@ -212,7 +212,7 @@ pcl::SampleConsensusModelNormalSphere::isModelValid (const Eige // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp index 27007569..56627f30 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp @@ -92,7 +92,7 @@ pcl::SampleConsensusModelParallelLine::isModelValid (const Eigen::Vector // Needs a valid model coefficients if (model_coefficients.size () != 6) { - PCL_ERROR ("[pcl::SampleConsensusParallelLine::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusParallelLine::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp index 001fcbde..a87ec99d 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp @@ -92,7 +92,7 @@ pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::Vecto // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp index 38cb023e..53317ae3 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp @@ -92,7 +92,7 @@ pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen:: // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPerpendicularPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPerpendicularPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp index 7c986acc..67749d4d 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp @@ -72,7 +72,7 @@ pcl::SampleConsensusModelPlane::computeModelCoefficients ( // Need 3 samples if (samples.size () != 3) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -115,7 +115,7 @@ pcl::SampleConsensusModelPlane::getDistancesToModel ( // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -146,7 +146,7 @@ pcl::SampleConsensusModelPlane::selectWithinDistance ( // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -186,7 +186,7 @@ pcl::SampleConsensusModelPlane::countWithinDistance ( // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (0); } @@ -215,7 +215,7 @@ pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); optimized_coefficients = model_coefficients; return; } @@ -223,7 +223,7 @@ pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( // Need at least 3 points to estimate a plane if (inliers.size () < 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } @@ -264,7 +264,7 @@ pcl::SampleConsensusModelPlane::projectPoints ( // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -348,7 +348,7 @@ pcl::SampleConsensusModelPlane::doSamplesVerifyModel ( // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp index 87988ab3..6f2de207 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -89,7 +89,7 @@ pcl::SampleConsensusModelRegistration::getDistancesToModel (const Eigen: { if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); distances.clear (); return; } @@ -135,7 +135,7 @@ pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen { if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); inliers.clear (); return; } @@ -195,7 +195,7 @@ pcl::SampleConsensusModelRegistration::countWithinDistance ( { if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); return (0); } if (!target_) @@ -240,7 +240,7 @@ pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const { if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); optimized_coefficients = model_coefficients; return; } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp index c4687f13..54b8688e 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp @@ -44,7 +44,7 @@ ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelRegistration2D::isSampleGood (const std::vector &samples) const +pcl::SampleConsensusModelRegistration2D::isSampleGood (const std::vector&) const { return (true); //using namespace pcl::common; @@ -66,7 +66,7 @@ pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eige PCL_INFO ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel]\n"); if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); distances.clear (); return; } @@ -90,9 +90,6 @@ pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eige Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); - Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, - target_->points[(*indices_tgt_)[i]].y, - target_->points[(*indices_tgt_)[i]].z, 1); Eigen::Vector4f p_tr (transform * pt_src); @@ -120,7 +117,7 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig { if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); inliers.clear (); return; } @@ -147,9 +144,6 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); - Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, - target_->points[(*indices_tgt_)[i]].y, - target_->points[(*indices_tgt_)[i]].z, 1); Eigen::Vector4f p_tr (transform * pt_src); @@ -186,7 +180,7 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( { if (indices_->size () != indices_tgt_->size ()) { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); return (0); } if (!target_) @@ -210,9 +204,6 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); - Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, - target_->points[(*indices_tgt_)[i]].y, - target_->points[(*indices_tgt_)[i]].z, 1); Eigen::Vector4f p_tr (transform * pt_src); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp index 1f07ccd6..a86dac61 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp @@ -59,7 +59,7 @@ pcl::SampleConsensusModelSphere::computeModelCoefficients ( // Need 4 samples if (samples.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -231,14 +231,14 @@ pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( // Needs a set of valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } // Need at least 4 samples if (inliers.size () <= 4) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -262,7 +262,7 @@ pcl::SampleConsensusModelSphere::projectPoints ( // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return; } @@ -285,7 +285,7 @@ pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp index 2079cb03..3e43cded 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp @@ -68,7 +68,7 @@ pcl::SampleConsensusModelStick::computeModelCoefficients ( // Need 2 samples if (samples.size () != 2) { - PCL_ERROR ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } @@ -233,7 +233,7 @@ pcl::SampleConsensusModelStick::optimizeModelCoefficients ( // Need at least 2 points to estimate a line if (inliers.size () <= 2) { - PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac.h b/sample_consensus/include/pcl/sample_consensus/sac.h index d6742e7a..28cbbfd4 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac.h +++ b/sample_consensus/include/pcl/sample_consensus/sac.h @@ -200,7 +200,7 @@ namespace pcl // Select the new inliers based on the optimized coefficients and new threshold sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers); - PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %zu/%zu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold); + PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %lu/%lu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold); if (new_inliers.empty ()) { @@ -300,7 +300,7 @@ namespace pcl getInliers (std::vector &inliers) { inliers = inliers_; } /** \brief Return the model coefficients of the best model found so far. - * \param[out] model_coefficients the resultant model coefficients + * \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus */ inline void getModelCoefficients (Eigen::VectorXf &model_coefficients) { model_coefficients = model_coefficients_; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h index 932a3ced..56597562 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model.h @@ -157,7 +157,7 @@ namespace pcl if (indices_->size () > input_->points.size ()) { - PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %zu while the input PointCloud has size %zu!\n", indices_->size (), input_->points.size ()); + PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", indices_->size (), input_->points.size ()); indices_->clear (); } shuffled_indices_ = *indices_; @@ -180,7 +180,7 @@ namespace pcl // We're assuming that indices_ have already been set in the constructor if (indices_->size () < getSampleSize ()) { - PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n", + PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n", samples.size (), indices_->size ()); // one of these will make it stop :) samples.clear (); @@ -201,7 +201,7 @@ namespace pcl // If it's a good sample, stop here if (isSampleGood (samples)) { - PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %zu samples.\n", samples.size ()); + PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ()); return; } } @@ -386,6 +386,7 @@ namespace pcl /** \brief Set the maximum distance allowed when drawing random samples * \param[in] radius the maximum distance (L2 norm) + * \param search */ inline void setSamplesMaxDist (const double &radius, SearchPtr search) diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h index 4ba9c5f3..5eed499b 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h @@ -257,4 +257,8 @@ namespace pcl }; } +#ifdef PCL_NO_PRECOMPILE +#include +#endif + #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_ diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h index 23ca2826..6d495183 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h @@ -177,7 +177,7 @@ namespace pcl { if (model_coefficients.size () != 6) { - PCL_ERROR ("[pcl::SampleConsensusModelLine::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelLine::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h index f57208f5..73a6e959 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h @@ -41,9 +41,7 @@ #ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_ #define PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_ -#include -#include -#include +#include #include namespace pcl @@ -83,7 +81,7 @@ namespace pcl * \ingroup sample_consensus */ template - class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelPlane, public SampleConsensusModelFromNormals + class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelNormalPlane { public: using SampleConsensusModel::input_; @@ -107,8 +105,7 @@ namespace pcl */ SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, bool random = false) - : SampleConsensusModelPlane (cloud, random) - , SampleConsensusModelFromNormals () + : SampleConsensusModelNormalPlane (cloud, random) , axis_ (Eigen::Vector4f::Zero ()) , distance_from_origin_ (0) , eps_angle_ (-1.0) @@ -125,8 +122,7 @@ namespace pcl SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, const std::vector &indices, bool random = false) - : SampleConsensusModelPlane (cloud, indices, random) - , SampleConsensusModelFromNormals () + : SampleConsensusModelNormalPlane (cloud, indices, random) , axis_ (Eigen::Vector4f::Zero ()) , distance_from_origin_ (0) , eps_angle_ (-1.0) @@ -179,34 +175,6 @@ namespace pcl inline double getEpsDist () { return (eps_dist_); } - /** \brief Select all the points which respect the given model coefficients as inliers. - * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to - * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers - * \param[out] inliers the resultant model inliers - */ - void - selectWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold, - std::vector &inliers); - - /** \brief Count all the points which respect the given model coefficients as inliers. - * - * \param[in] model_coefficients the coefficients of a model that we need to compute distances to - * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers - * \return the resultant number of inliers - */ - virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); - - /** \brief Compute all distances from the cloud data to a given plane model. - * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to - * \param[out] distances the resultant estimated distances - */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); - /** \brief Return an unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */ inline pcl::SacModel getModelType () const { return (SACMODEL_NORMAL_PARALLEL_PLANE); } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h index 5753e2ca..831f32e8 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h @@ -251,7 +251,7 @@ namespace pcl // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h index aad8746f..09dd7280 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h @@ -262,6 +262,7 @@ namespace pcl /** \brief Computes an "optimal" sample distance threshold based on the * principal directions of the input cloud. * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param indices */ inline void computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h index 2917a8c2..99785b15 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h @@ -148,10 +148,9 @@ namespace pcl /** \brief Computes an "optimal" sample distance threshold based on the * principal directions of the input cloud. - * \param[in] cloud the const boost shared pointer to a PointCloud message */ inline void - computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) + computeSampleDistanceThreshold (const PointCloudConstPtr&) { //// Compute the principal directions via PCA //Eigen::Vector4f xyz_centroid; @@ -176,11 +175,10 @@ namespace pcl /** \brief Computes an "optimal" sample distance threshold based on the * principal directions of the input cloud. - * \param[in] cloud the const boost shared pointer to a PointCloud message */ inline void - computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, - const std::vector &indices) + computeSampleDistanceThreshold (const PointCloudConstPtr&, + const std::vector&) { //// Compute the principal directions via PCA //Eigen::Vector4f xyz_centroid; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h index 8d96751c..05a40386 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h @@ -200,7 +200,7 @@ namespace pcl // Needs a valid model coefficients if (model_coefficients.size () != 4) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h index 8727028f..36ea13c3 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h @@ -181,7 +181,7 @@ namespace pcl { if (model_coefficients.size () != 7) { - PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); return (false); } diff --git a/sample_consensus/sample_consensus.doxy b/sample_consensus/sample_consensus.doxy index 06ff3dc6..b6396c84 100644 --- a/sample_consensus/sample_consensus.doxy +++ b/sample_consensus/sample_consensus.doxy @@ -5,7 +5,7 @@ The pcl_sample_consensus library holds SAmple Consensus (SAC) methods like RANSAC and models like planes and cylinders. These can - combined freely in order to detect specific models and their paramters in point clouds. + combined freely in order to detect specific models and their parameters in point clouds. Some of the models implemented in this library include: lines, planes, cylinders, and spheres. Plane fitting is often applied to the task of detecting common indoor surfaces, such as walls, floors, and table tops. @@ -14,22 +14,22 @@ \image html http://www.pointclouds.org/assets/images/contents/documentation/sample_consensus_planes_cylinders.png - As of PCL 1.0, the following models are supported: + The following models are supported:
    -
  • SACMODEL_PLANE - used to determine plane models. The four coefficients of the plane are its Hessian Normal form: [normal_x normal_y normal_z d]
    • -
  • SACMODEL_LINE - used to determine line models. The six coefficients of the line are given by a point on the line and the direction of the line as: [point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z]
    • -
  • SACMODEL_CIRCLE2D - used to determine 2D circles in a plane. The circle's three coefficients are given by its center and radius as: [center.x center.y radius]
    • -
  • SACMODEL_CIRCLE3D - not implemented yet
    • -
  • SACMODEL_SPHERE - used to determine sphere models. The four coefficients of the sphere are given by its 3D center and radius as: [center.x center.y center.z radius]
    • -
  • SACMODEL_CYLINDER - used to determine cylinder models. The seven coefficients of the cylinder are given by a point on its axis, the axis direction, and a radius, as: [point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]
    • -
  • SACMODEL_CONE - not implemented yet
    • +
  • \link pcl::SampleConsensusModelPlane SACMODEL_PLANE \endlink - used to determine plane models. The four coefficients of the plane are its Hessian Normal form: [normal_x normal_y normal_z d]
    • +
  • \link pcl::SampleConsensusModelLine SACMODEL_LINE \endlink - used to determine line models. The six coefficients of the line are given by a point on the line and the direction of the line as: [point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z]
    • +
  • \link pcl::SampleConsensusModelCircle2D SACMODEL_CIRCLE2D \endlink - used to determine 2D circles in a plane. The circle's three coefficients are given by its center and radius as: [center.x center.y radius]
    • +
  • \link pcl::SampleConsensusModelCircle3D SACMODEL_CIRCLE3D \endlink - used to determine 3D circles in a plane. The circle's seven coefficients are given by its center, radius and normal as: [center.x, center.y, center.z, radius, normal.x, normal.y, normal.z]
    • +
  • \link pcl::SampleConsensusModelSphere SACMODEL_SPHERE \endlink - used to determine sphere models. The four coefficients of the sphere are given by its 3D center and radius as: [center.x center.y center.z radius]
    • +
  • \link pcl::SampleConsensusModelCylinder SACMODEL_CYLINDER \endlink - used to determine cylinder models. The seven coefficients of the cylinder are given by a point on its axis, the axis direction, and a radius, as: [point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]
    • +
  • \link pcl::SampleConsensusModelCone SACMODEL_CONE \endlink - used to determine cone models. The seven coefficients of the cone are given by a point of its apex, the axis direction and the opening angle, as: [apex.x, apex.y, apex.z, axis_direction.x, axis_direction.y, axis_direction.z, opening_angle]
  • SACMODEL_TORUS - not implemented yet
    • -
  • SACMODEL_PARALLEL_LINE - a model for determining a line parallel with a given axis, within a maximum specified angular deviation. The line coefficients are similar to SACMODEL_LINE.
    • -
  • SACMODEL_PERPENDICULAR_PLANE - a model for determining a plane perpendicular to an user-specified axis, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE.
    • +
  • \link pcl::SampleConsensusModelParallelLine SACMODEL_PARALLEL_LINE \endlink - a model for determining a line parallel with a given axis, within a maximum specified angular deviation. The line coefficients are similar to \link pcl::SampleConsensusModelLine SACMODEL_LINE \endlink.
    • +
  • \link pcl::SampleConsensusModelPerpendicularPlane SACMODEL_PERPENDICULAR_PLANE \endlink - a model for determining a plane perpendicular to an user-specified axis, within a maximum specified angular deviation. The plane coefficients are similar to \link pcl::SampleConsensusModelPlane SACMODEL_PLANE \endlink.
  • SACMODEL_PARALLEL_LINES - not implemented yet
    • -
  • SACMODEL_NORMAL_PLANE - a model for determining plane models using an additional constraint: the surface normals at each inlier point has to be parallel to the surface normal of the output plane, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE.
    • -
  • SACMODEL_PARALLEL_PLANE - a model for determining a plane parallel to an user-specified axis, within a maximim specified angular deviation. SACMODEL_PLANE.
    • -
  • SACMODEL_NORMAL_PARALLEL_PLANE defines a model for 3D plane segmentation using additional surface normal constraints. The plane must lie parallel to a user-specified axis. SACMODEL_NORMAL_PARALLEL_PLANE therefore is equivallent to SACMODEL_NORMAL_PLANE + SACMODEL_PARALLEL_PLANE. The plane coefficients are similar to SACMODEL_PLANE.
    • +
  • \link pcl::SampleConsensusModelNormalPlane SACMODEL_NORMAL_PLANE \endlink - a model for determining plane models using an additional constraint: the surface normals at each inlier point has to be parallel to the surface normal of the output plane, within a maximum specified angular deviation. The plane coefficients are similar to \link pcl::SampleConsensusModelPlane SACMODEL_PLANE \endlink.
    • +
  • \link pcl::SampleConsensusModelParallelPlane SACMODEL_PARALLEL_PLANE \endlink - a model for determining a plane parallel to an user-specified axis, within a maximum specified angular deviation. \link pcl::SampleConsensusModelPlane SACMODEL_PLANE \endlink.
    • +
  • \link pcl::SampleConsensusModelNormalParallelPlane SACMODEL_NORMAL_PARALLEL_PLANE \endlink defines a model for 3D plane segmentation using additional surface normal constraints. The plane must lie parallel to a user-specified axis. SACMODEL_NORMAL_PARALLEL_PLANE therefore is equivalent to SACMODEL_NORMAL_PLANE + SACMODEL_PARALLEL_PLANE. The plane coefficients are similar to \link pcl::SampleConsensusModelPlane SACMODEL_PLANE \endlink.
The following list describes the robust sample consensus estimators implemented: diff --git a/search/CMakeLists.txt b/search/CMakeLists.txt index 59e52a7a..cd82a0ef 100644 --- a/search/CMakeLists.txt +++ b/search/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud generic search library") set(SUBSYS_DEPS common kdtree octree) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS flann) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS flann) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -18,32 +18,32 @@ if(build) ) set(incs - include/pcl/${SUBSYS_NAME}/search.h - include/pcl/${SUBSYS_NAME}/kdtree.h - include/pcl/${SUBSYS_NAME}/brute_force.h - include/pcl/${SUBSYS_NAME}/organized.h - include/pcl/${SUBSYS_NAME}/octree.h - include/pcl/${SUBSYS_NAME}/flann_search.h - include/pcl/${SUBSYS_NAME}/pcl_search.h + "include/pcl/${SUBSYS_NAME}/search.h" + "include/pcl/${SUBSYS_NAME}/kdtree.h" + "include/pcl/${SUBSYS_NAME}/brute_force.h" + "include/pcl/${SUBSYS_NAME}/organized.h" + "include/pcl/${SUBSYS_NAME}/octree.h" + "include/pcl/${SUBSYS_NAME}/flann_search.h" + "include/pcl/${SUBSYS_NAME}/pcl_search.h" ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/search.hpp - include/pcl/${SUBSYS_NAME}/impl/kdtree.hpp - include/pcl/${SUBSYS_NAME}/impl/flann_search.hpp - include/pcl/${SUBSYS_NAME}/impl/brute_force.hpp - include/pcl/${SUBSYS_NAME}/impl/organized.hpp + "include/pcl/${SUBSYS_NAME}/impl/search.hpp" + "include/pcl/${SUBSYS_NAME}/impl/kdtree.hpp" + "include/pcl/${SUBSYS_NAME}/impl/flann_search.hpp" + "include/pcl/${SUBSYS_NAME}/impl/brute_force.hpp" + "include/pcl/${SUBSYS_NAME}/impl/organized.hpp" ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_common ${FLANN_LIBRARIES} pcl_octree pcl_kdtree) + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_common ${FLANN_LIBRARIES} pcl_octree pcl_kdtree) list(APPEND EXT_DEPS flann) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/search/include/pcl/search/flann_search.h b/search/include/pcl/search/flann_search.h index a0f41b62..33be27f8 100644 --- a/search/include/pcl/search/flann_search.h +++ b/search/include/pcl/search/flann_search.h @@ -59,27 +59,47 @@ namespace pcl /** \brief @b search::FlannSearch is a generic FLANN wrapper class for the new search interface. * It is able to wrap any FLANN index type, e.g. the kd tree as well as indices for high-dimensional * searches and intended as a more powerful and cleaner successor to KdTreeFlann. + * + * By default, this class creates a single kd tree for indexing the input data. However, for high dimensions + * (> 10), it is often better to use the multiple randomized kd tree index provided by FLANN in combination with + * the \ref flann::L2 distance functor. During search in this type of index, the number of checks to perform before + * terminating the search can be controlled. Here is a code example if a high-dimensional 2-NN search: + * + * \code + * // Feature and distance type + * typedef SHOT352 FeatureT; + * typedef flann::L2 DistanceT; + * + * // Search and index types + * typedef search::FlannSearch SearchT; + * typedef typename SearchT::FlannIndexCreatorPtr CreatorPtrT; + * typedef typename SearchT::KdTreeMultiIndexCreator IndexT; + * typedef typename SearchT::PointRepresentationPtr RepresentationPtrT; + * + * // Features + * PointCloud::Ptr query, target; + * + * // Fill query and target with calculated features... + * + * // Instantiate search object with 4 randomized trees and 256 checks + * SearchT search (true, CreatorPtrT (new IndexT (4))); + * search.setPointRepresentation (RepresentationPtrT (new DefaultFeatureRepresentation)); + * search.setChecks (256); + * search.setInputCloud (target); + * + * // Do search + * std::vector > k_indices; + * std::vector > k_sqr_distances; + * search.nearestKSearch (*query, std::vector (), 2, k_indices, k_sqr_distances); + * \endcode * * \author Andreas Muetzel + * \author Anders Glent Buch (multiple randomized kd tree interface) * \ingroup search */ template > class FlannSearch: public Search { - typedef typename Search::PointCloud PointCloud; - typedef typename Search::PointCloudConstPtr PointCloudConstPtr; - - typedef boost::shared_ptr > IndicesPtr; - typedef boost::shared_ptr > IndicesConstPtr; - typedef flann::NNIndex< FlannDistance > Index; - typedef boost::shared_ptr > IndexPtr; - typedef boost::shared_ptr > MatrixPtr; - typedef boost::shared_ptr > MatrixConstPtr; - - typedef pcl::PointRepresentation PointRepresentation; - //typedef boost::shared_ptr PointRepresentationPtr; - typedef boost::shared_ptr PointRepresentationConstPtr; - using Search::input_; using Search::indices_; using Search::sorted_results_; @@ -87,6 +107,22 @@ namespace pcl public: typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr > ConstPtr; + + typedef typename Search::PointCloud PointCloud; + typedef typename Search::PointCloudConstPtr PointCloudConstPtr; + + typedef boost::shared_ptr > IndicesPtr; + typedef boost::shared_ptr > IndicesConstPtr; + + typedef boost::shared_ptr > MatrixPtr; + typedef boost::shared_ptr > MatrixConstPtr; + + typedef flann::NNIndex< FlannDistance > Index; + typedef boost::shared_ptr > IndexPtr; + + typedef pcl::PointRepresentation PointRepresentation; + typedef boost::shared_ptr PointRepresentationPtr; + typedef boost::shared_ptr PointRepresentationConstPtr; /** \brief Helper class that creates a FLANN index from a given FLANN matrix. To * use a FLANN index type with FlannSearch, implement this interface and @@ -136,7 +172,7 @@ namespace pcl class KMeansIndexCreator: public FlannIndexCreator { public: - /** \param[in] max_leaf_size All FLANN kd trees created by this class will have + /** \brief All FLANN kd trees created by this class will have * a maximum of max_leaf_size points per leaf node. Higher values make index creation * cheaper, but search more costly (and the other way around). */ @@ -153,6 +189,29 @@ namespace pcl private: }; + /** \brief Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data, + * suitable for feature matching. Note that in this case, it is often more efficient to use the + * \ref flann::L2 distance functor. + */ + class KdTreeMultiIndexCreator: public FlannIndexCreator + { + public: + /** \param[in] trees Number of randomized trees to create. + */ + KdTreeMultiIndexCreator (int trees = 4) : trees_ (trees) {} + + /** \brief Empty destructor */ + virtual ~KdTreeMultiIndexCreator () {} + + /** \brief Create a FLANN Index from the input data. + * \param[in] data The FLANN matrix containing the input. + * \return The FLANN index. + */ + virtual IndexPtr createIndex (MatrixConstPtr data); + private: + int trees_; + }; + FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ())); /** \brief Destructor for FlannSearch. */ @@ -179,6 +238,22 @@ namespace pcl return (eps_); } + /** \brief Set the number of checks to perform during approximate searches in multiple randomized trees. + * \param[in] checks number of checks to perform during approximate searches in multiple randomized trees. + */ + inline void + setChecks (int checks) + { + checks_ = checks; + } + + /** \brief Get the number of checks to perform during approximate searches in multiple randomized trees. */ + inline int + getChecks () + { + return (checks_); + } + /** \brief Provide a pointer to the input dataset. * \param[in] cloud the const boost shared pointer to a PointCloud message * \param[in] indices the point indices subset that is to be used from \a cloud @@ -276,6 +351,11 @@ namespace pcl /** Epsilon for approximate NN search. */ float eps_; + + /** Number of checks to perform for approximate NN search using the multiple randomized tree index + */ + int checks_; + bool input_copied_for_flann_; PointRepresentationConstPtr point_representation_; diff --git a/search/include/pcl/search/impl/flann_search.hpp b/search/include/pcl/search/impl/flann_search.hpp index 3bf93c57..1d310c31 100644 --- a/search/include/pcl/search/impl/flann_search.hpp +++ b/search/include/pcl/search/impl/flann_search.hpp @@ -59,10 +59,18 @@ pcl::search::FlannSearch::KMeansIndexCreator::createIndex return (IndexPtr (new flann::KMeansIndex (*data,flann::KMeansIndexParams ()))); } +////////////////////////////////////////////////////////////////////////////////////////////// +template +typename pcl::search::FlannSearch::IndexPtr +pcl::search::FlannSearch::KdTreeMultiIndexCreator::createIndex (MatrixConstPtr data) +{ + return (IndexPtr (new flann::KDTreeIndex (*data, flann::KDTreeIndexParams (trees_)))); +} + ////////////////////////////////////////////////////////////////////////////////////////////// template pcl::search::FlannSearch::FlannSearch(bool sorted, FlannIndexCreatorPtr creator) : pcl::search::Search ("FlannSearch",sorted), - index_(), creator_ (creator), input_flann_(), eps_ (0), input_copied_for_flann_ (false), point_representation_ (new DefaultPointRepresentation), + index_(), creator_ (creator), input_flann_(), eps_ (0), checks_ (32), input_copied_for_flann_ (false), point_representation_ (new DefaultPointRepresentation), dim_ (0), index_mapping_(), identity_mapping_() { dim_ = point_representation_->getNumberOfDimensions (); @@ -104,9 +112,10 @@ pcl::search::FlannSearch::nearestKSearch (const PointT &p float* cdata = can_cast ? const_cast (reinterpret_cast (&point)): data; const flann::Matrix m (cdata ,1, point_representation_->getNumberOfDimensions ()); - flann::SearchParams p(-1); + flann::SearchParams p; p.eps = eps_; p.sorted = sorted_results_; + p.checks = checks_; if (indices.size() != static_cast (k)) indices.resize (k,-1); if (dists.size() != static_cast (k)) @@ -169,6 +178,7 @@ pcl::search::FlannSearch::nearestKSearch ( flann::SearchParams p; p.sorted = sorted_results_; p.eps = eps_; + p.checks = checks_; index_->knnSearch (m,k_indices,k_sqr_distances,k, p); delete [] data; @@ -197,6 +207,7 @@ pcl::search::FlannSearch::nearestKSearch ( flann::SearchParams p; p.sorted = sorted_results_; p.eps = eps_; + p.checks = checks_; index_->knnSearch (m,k_indices,k_sqr_distances,k, p); delete[] data; @@ -237,6 +248,7 @@ pcl::search::FlannSearch::radiusSearch (const PointT& poi p.sorted = sorted_results_; p.eps = eps_; p.max_neighbors = max_nn > 0 ? max_nn : -1; + p.checks = checks_; std::vector > i (1); std::vector > d (1); int result = index_->radiusSearch (m,i,d,static_cast (radius * radius), p); @@ -294,6 +306,7 @@ pcl::search::FlannSearch::radiusSearch ( flann::SearchParams p; p.sorted = sorted_results_; p.eps = eps_; + p.checks = checks_; // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors p.max_neighbors = max_nn != 0 ? max_nn : -1; index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast (radius * radius), p); @@ -324,6 +337,7 @@ pcl::search::FlannSearch::radiusSearch ( flann::SearchParams p; p.sorted = sorted_results_; p.eps = eps_; + p.checks = checks_; // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors p.max_neighbors = max_nn != 0 ? max_nn : -1; index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast (radius * radius), p); diff --git a/search/include/pcl/search/impl/kdtree.hpp b/search/include/pcl/search/impl/kdtree.hpp index 2a8b9400..a6d7fb22 100644 --- a/search/include/pcl/search/impl/kdtree.hpp +++ b/search/include/pcl/search/impl/kdtree.hpp @@ -42,39 +42,39 @@ #include /////////////////////////////////////////////////////////////////////////////////////////// -template -pcl::search::KdTree::KdTree (bool sorted) +template +pcl::search::KdTree::KdTree (bool sorted) : pcl::search::Search ("KdTree", sorted) - , tree_ (new pcl::KdTreeFLANN (sorted)) + , tree_ (new Tree (sorted)) { } /////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::search::KdTree::setPointRepresentation ( +template void +pcl::search::KdTree::setPointRepresentation ( const PointRepresentationConstPtr &point_representation) { tree_->setPointRepresentation (point_representation); } /////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::search::KdTree::setSortedResults (bool sorted_results) +template void +pcl::search::KdTree::setSortedResults (bool sorted_results) { sorted_results_ = sorted_results; tree_->setSortedResults (sorted_results); } /////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::search::KdTree::setEpsilon (float eps) +template void +pcl::search::KdTree::setEpsilon (float eps) { tree_->setEpsilon (eps); } /////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::search::KdTree::setInputCloud ( +template void +pcl::search::KdTree::setInputCloud ( const PointCloudConstPtr& cloud, const IndicesConstPtr& indices) { @@ -84,8 +84,8 @@ pcl::search::KdTree::setInputCloud ( } /////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::search::KdTree::nearestKSearch ( +template int +pcl::search::KdTree::nearestKSearch ( const PointT &point, int k, std::vector &k_indices, std::vector &k_sqr_distances) const { @@ -93,8 +93,8 @@ pcl::search::KdTree::nearestKSearch ( } /////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::search::KdTree::radiusSearch ( +template int +pcl::search::KdTree::radiusSearch ( const PointT& point, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const diff --git a/search/include/pcl/search/kdtree.h b/search/include/pcl/search/kdtree.h index 1fde8c94..dac72543 100644 --- a/search/include/pcl/search/kdtree.h +++ b/search/include/pcl/search/kdtree.h @@ -58,7 +58,7 @@ namespace pcl * \author Radu B. Rusu * \ingroup search */ - template + template > class KdTree: public Search { public: @@ -76,11 +76,11 @@ namespace pcl using pcl::search::Search::radiusSearch; using pcl::search::Search::sorted_results_; - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; - typedef boost::shared_ptr > KdTreeFLANNPtr; - typedef boost::shared_ptr > KdTreeFLANNConstPtr; + typedef boost::shared_ptr KdTreePtr; + typedef boost::shared_ptr KdTreeConstPtr; typedef boost::shared_ptr > PointRepresentationConstPtr; /** \brief Constructor for KdTree. @@ -167,13 +167,17 @@ namespace pcl std::vector &k_sqr_distances, unsigned int max_nn = 0) const; protected: - /** \brief A pointer to the internal KdTreeFLANN object. */ - KdTreeFLANNPtr tree_; + /** \brief A pointer to the internal KdTree object. */ + KdTreePtr tree_; }; } } +#ifdef PCL_NO_PRECOMPILE +#include +#else #define PCL_INSTANTIATE_KdTree(T) template class PCL_EXPORTS pcl::search::KdTree; +#endif #endif // PCL_SEARCH_KDTREE_H_ diff --git a/search/include/pcl/search/search.h b/search/include/pcl/search/search.h index 6110c18c..1d362c1a 100644 --- a/search/include/pcl/search/search.h +++ b/search/include/pcl/search/search.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace pcl { @@ -159,11 +160,7 @@ namespace pcl std::vector &k_indices, std::vector &k_sqr_distances) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (nearestKSearch (p, k, k_indices, k_sqr_distances)); } @@ -291,11 +288,7 @@ namespace pcl std::vector &k_sqr_distances, unsigned int max_nn = 0) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); } diff --git a/segmentation/CMakeLists.txt b/segmentation/CMakeLists.txt index 2d8c140c..56cc254d 100644 --- a/segmentation/CMakeLists.txt +++ b/segmentation/CMakeLists.txt @@ -3,10 +3,10 @@ set(SUBSYS_DESC "Point cloud segmentation library") set(SUBSYS_DEPS common geometry search sample_consensus kdtree octree features filters) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) set(srcs @@ -23,83 +23,89 @@ if(build) src/crf_normal_segmentation.cpp src/conditional_euclidean_clustering.cpp src/supervoxel_clustering.cpp + src/grabcut_segmentation.cpp + src/progressive_morphological_filter.cpp + src/approximate_progressive_morphological_filter.cpp ) # NOTE: boost/graph/boykov_kolmogorov_max_flow.hpp only exists for versions > 1.43 if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 43) - set(srcs - ${srcs} + list(APPEND srcs src/min_cut_segmentation.cpp ) endif() set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/extract_clusters.h - include/pcl/${SUBSYS_NAME}/extract_labeled_clusters.h - include/pcl/${SUBSYS_NAME}/extract_polygonal_prism_data.h - include/pcl/${SUBSYS_NAME}/sac_segmentation.h - include/pcl/${SUBSYS_NAME}/seeded_hue_segmentation.h - include/pcl/${SUBSYS_NAME}/segment_differences.h - include/pcl/${SUBSYS_NAME}/region_growing.h - include/pcl/${SUBSYS_NAME}/region_growing_rgb.h - include/pcl/${SUBSYS_NAME}/comparator.h - include/pcl/${SUBSYS_NAME}/plane_coefficient_comparator.h - include/pcl/${SUBSYS_NAME}/euclidean_plane_coefficient_comparator.h - include/pcl/${SUBSYS_NAME}/edge_aware_plane_comparator.h - include/pcl/${SUBSYS_NAME}/rgb_plane_coefficient_comparator.h - include/pcl/${SUBSYS_NAME}/plane_refinement_comparator.h - include/pcl/${SUBSYS_NAME}/euclidean_cluster_comparator.h - include/pcl/${SUBSYS_NAME}/ground_plane_comparator.h - include/pcl/${SUBSYS_NAME}/organized_connected_component_segmentation.h - include/pcl/${SUBSYS_NAME}/organized_multi_plane_segmentation.h - include/pcl/${SUBSYS_NAME}/region_3d.h - include/pcl/${SUBSYS_NAME}/planar_region.h - include/pcl/${SUBSYS_NAME}/planar_polygon_fusion.h - include/pcl/${SUBSYS_NAME}/crf_normal_segmentation.h - include/pcl/${SUBSYS_NAME}/conditional_euclidean_clustering.h - include/pcl/${SUBSYS_NAME}/supervoxel_clustering.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/extract_clusters.h" + "include/pcl/${SUBSYS_NAME}/extract_labeled_clusters.h" + "include/pcl/${SUBSYS_NAME}/extract_polygonal_prism_data.h" + "include/pcl/${SUBSYS_NAME}/sac_segmentation.h" + "include/pcl/${SUBSYS_NAME}/seeded_hue_segmentation.h" + "include/pcl/${SUBSYS_NAME}/segment_differences.h" + "include/pcl/${SUBSYS_NAME}/region_growing.h" + "include/pcl/${SUBSYS_NAME}/region_growing_rgb.h" + "include/pcl/${SUBSYS_NAME}/comparator.h" + "include/pcl/${SUBSYS_NAME}/plane_coefficient_comparator.h" + "include/pcl/${SUBSYS_NAME}/euclidean_plane_coefficient_comparator.h" + "include/pcl/${SUBSYS_NAME}/edge_aware_plane_comparator.h" + "include/pcl/${SUBSYS_NAME}/rgb_plane_coefficient_comparator.h" + "include/pcl/${SUBSYS_NAME}/plane_refinement_comparator.h" + "include/pcl/${SUBSYS_NAME}/euclidean_cluster_comparator.h" + "include/pcl/${SUBSYS_NAME}/ground_plane_comparator.h" + "include/pcl/${SUBSYS_NAME}/organized_connected_component_segmentation.h" + "include/pcl/${SUBSYS_NAME}/organized_multi_plane_segmentation.h" + "include/pcl/${SUBSYS_NAME}/region_3d.h" + "include/pcl/${SUBSYS_NAME}/planar_region.h" + "include/pcl/${SUBSYS_NAME}/planar_polygon_fusion.h" + "include/pcl/${SUBSYS_NAME}/crf_normal_segmentation.h" + "include/pcl/${SUBSYS_NAME}/conditional_euclidean_clustering.h" + "include/pcl/${SUBSYS_NAME}/supervoxel_clustering.h" + "include/pcl/${SUBSYS_NAME}/grabcut_segmentation.h" + "include/pcl/${SUBSYS_NAME}/progressive_morphological_filter.h" + "include/pcl/${SUBSYS_NAME}/approximate_progressive_morphological_filter.h" ) # NOTE: boost/graph/boykov_kolmogorov_max_flow.hpp only exists for versions > 1.43 if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 43) - set(incs - ${incs} - include/pcl/${SUBSYS_NAME}/min_cut_segmentation.h + list(APPEND incs + "include/pcl/${SUBSYS_NAME}/min_cut_segmentation.h" ) endif() set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/extract_clusters.hpp - include/pcl/${SUBSYS_NAME}/impl/extract_labeled_clusters.hpp - include/pcl/${SUBSYS_NAME}/impl/extract_polygonal_prism_data.hpp - include/pcl/${SUBSYS_NAME}/impl/sac_segmentation.hpp - include/pcl/${SUBSYS_NAME}/impl/seeded_hue_segmentation.hpp - include/pcl/${SUBSYS_NAME}/impl/segment_differences.hpp - include/pcl/${SUBSYS_NAME}/impl/region_growing.hpp - include/pcl/${SUBSYS_NAME}/impl/region_growing_rgb.hpp - include/pcl/${SUBSYS_NAME}/impl/organized_connected_component_segmentation.hpp - include/pcl/${SUBSYS_NAME}/impl/organized_multi_plane_segmentation.hpp - include/pcl/${SUBSYS_NAME}/impl/planar_polygon_fusion.hpp - include/pcl/${SUBSYS_NAME}/impl/crf_normal_segmentation.hpp - include/pcl/${SUBSYS_NAME}/impl/conditional_euclidean_clustering.hpp - include/pcl/${SUBSYS_NAME}/impl/supervoxel_clustering.hpp + "include/pcl/${SUBSYS_NAME}/impl/extract_clusters.hpp" + "include/pcl/${SUBSYS_NAME}/impl/extract_labeled_clusters.hpp" + "include/pcl/${SUBSYS_NAME}/impl/extract_polygonal_prism_data.hpp" + "include/pcl/${SUBSYS_NAME}/impl/sac_segmentation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/seeded_hue_segmentation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/segment_differences.hpp" + "include/pcl/${SUBSYS_NAME}/impl/region_growing.hpp" + "include/pcl/${SUBSYS_NAME}/impl/region_growing_rgb.hpp" + "include/pcl/${SUBSYS_NAME}/impl/organized_connected_component_segmentation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/organized_multi_plane_segmentation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/planar_polygon_fusion.hpp" + "include/pcl/${SUBSYS_NAME}/impl/crf_normal_segmentation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/conditional_euclidean_clustering.hpp" + "include/pcl/${SUBSYS_NAME}/impl/supervoxel_clustering.hpp" + "include/pcl/${SUBSYS_NAME}/impl/grabcut_segmentation.hpp" + "include/pcl/${SUBSYS_NAME}/impl/progressive_morphological_filter.hpp" + "include/pcl/${SUBSYS_NAME}/impl/approximate_progressive_morphological_filter.hpp" ) # NOTE: boost/graph/boykov_kolmogorov_max_flow.hpp only exists for versions > 1.43 if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 43) - set(impl_incs - ${impl_incs} - include/pcl/${SUBSYS_NAME}/impl/min_cut_segmentation.hpp + list(APPEND impl_incs + "include/pcl/${SUBSYS_NAME}/impl/min_cut_segmentation.hpp" ) endif() - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs}) - target_link_libraries(${LIB_NAME} pcl_search pcl_sample_consensus pcl_filters pcl_features) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + target_link_libraries("${LIB_NAME}" pcl_search pcl_sample_consensus pcl_filters pcl_features) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) endif(build) diff --git a/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h b/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h new file mode 100644 index 00000000..88101e41 --- /dev/null +++ b/segmentation/include/pcl/segmentation/approximate_progressive_morphological_filter.h @@ -0,0 +1,178 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_H_ +#define PCL_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_H_ + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the Progressive Morphological Filter for segmentation of ground points. + * Description can be found in the article + * "A Progressive Morphological Filter for Removing Nonground Measurements from + * Airborne LIDAR Data" + * by K. Zhang, S. Chen, D. Whitman, M. Shyu, J. Yan, and C. Zhang. + */ + template + class PCL_EXPORTS ApproximateProgressiveMorphologicalFilter : public pcl::PCLBase + { + public: + + typedef pcl::PointCloud PointCloud; + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + public: + + /** \brief Constructor that sets default values for member variables. */ + ApproximateProgressiveMorphologicalFilter (); + + virtual + ~ApproximateProgressiveMorphologicalFilter (); + + /** \brief Get the maximum window size to be used in filtering ground returns. */ + inline int + getMaxWindowSize () const { return (max_window_size_); } + + /** \brief Set the maximum window size to be used in filtering ground returns. */ + inline void + setMaxWindowSize (int max_window_size) { max_window_size_ = max_window_size; } + + /** \brief Get the slope value to be used in computing the height threshold. */ + inline float + getSlope () const { return (slope_); } + + /** \brief Set the slope value to be used in computing the height threshold. */ + inline void + setSlope (float slope) { slope_ = slope; } + + /** \brief Get the maximum height above the parameterized ground surface to be considered a ground return. */ + inline float + getMaxDistance () const { return (max_distance_); } + + /** \brief Set the maximum height above the parameterized ground surface to be considered a ground return. */ + inline void + setMaxDistance (float max_distance) { max_distance_ = max_distance; } + + /** \brief Get the initial height above the parameterized ground surface to be considered a ground return. */ + inline float + getInitialDistance () const { return (initial_distance_); } + + /** \brief Set the initial height above the parameterized ground surface to be considered a ground return. */ + inline void + setInitialDistance (float initial_distance) { initial_distance_ = initial_distance; } + + /** \brief Get the cell size. */ + inline float + getCellSize () const { return (cell_size_); } + + /** \brief Set the cell size. */ + inline void + setCellSize (float cell_size) { cell_size_ = cell_size; } + + /** \brief Get the base to be used in computing progressive window sizes. */ + inline float + getBase () const { return (base_); } + + /** \brief Set the base to be used in computing progressive window sizes. */ + inline void + setBase (float base) { base_ = base; } + + /** \brief Get flag indicating whether or not to exponentially grow window sizes? */ + inline bool + getExponential () const { return (exponential_); } + + /** \brief Set flag indicating whether or not to exponentially grow window sizes? */ + inline void + setExponential (bool exponential) { exponential_ = exponential; } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /** \brief This method launches the segmentation algorithm and returns indices of + * points determined to be ground returns. + * \param[out] ground indices of points determined to be ground returns. + */ + virtual void + extract (std::vector& ground); + + protected: + + /** \brief Maximum window size to be used in filtering ground returns. */ + int max_window_size_; + + /** \brief Slope value to be used in computing the height threshold. */ + float slope_; + + /** \brief Maximum height above the parameterized ground surface to be considered a ground return. */ + float max_distance_; + + /** \brief Initial height above the parameterized ground surface to be considered a ground return. */ + float initial_distance_; + + /** \brief Cell size. */ + float cell_size_; + + /** \brief Base to be used in computing progressive window sizes. */ + float base_; + + /** \brief Exponentially grow window sizes? */ + bool exponential_; + + /** \brief Number of threads to be used. */ + unsigned int threads_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif + diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h index 46140b68..1f11ec61 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -127,7 +127,8 @@ namespace pcl } /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. - * \param[in] distance_threshold the tolerance in meters + * \param[in] distance_threshold the tolerance in meters + * \param depth_dependent */ inline void setDistanceThreshold (float distance_threshold, bool depth_dependent) diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h index 2a3d5b7b..841c451d 100644 --- a/segmentation/include/pcl/segmentation/extract_clusters.h +++ b/segmentation/include/pcl/segmentation/extract_clusters.h @@ -105,12 +105,12 @@ namespace pcl { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (cloud.points.size () != normals.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); return; } @@ -206,17 +206,17 @@ namespace pcl //and indices[i] if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (tree->getIndices ()->size () != indices.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%zu) than the input set (%zu)!\n", tree->getIndices ()->size (), indices.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); return; } if (cloud.points.size () != normals.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false diff --git a/segmentation/include/pcl/segmentation/grabcut.h b/segmentation/include/pcl/segmentation/grabcut.h deleted file mode 100644 index 0f5d836d..00000000 --- a/segmentation/include/pcl/segmentation/grabcut.h +++ /dev/null @@ -1,491 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * - */ - -#ifndef PCL_SEGMENTATION_GRABCUT -#define PCL_SEGMENTATION_GRABCUT - -#include -#include -#include -#include -#include - -namespace pcl -{ - namespace segmentation - { - namespace grabcut - { - /** boost implementation of Boykov and Kolmogorov's maxflow algorithm doesn't support - * negative flows which makes it inappropriate for this conext. - * This implementation of Boykov and Kolmogorov's maxflow algorithm by Stephen Gould - * in DARWIN under BSD does the trick however solwer than original - * implementation. - */ - class BoykovKolmogorov - { - public: - typedef int vertex_descriptor; - typedef double edge_capacity_type; - - /// construct a maxflow/mincut problem with estimated max_nodes - BoykovKolmogorov (std::size_t max_nodes = 0); - /// destructor - virtual ~BoykovKolmogorov () {} - /// get number of nodes in the graph - size_t - numNodes () const { return nodes_.size (); } - /// reset all edge capacities to zero (but don't free the graph) - void - reset (); - /// clear the graph and internal datastructures - void - clear (); - /// add nodes to the graph (returns the id of the first node added) - int - addNodes (std::size_t n = 1); - /// add constant flow to graph - void - addConstant (double c) { flow_value_ += c; } - /// add edge from s to nodeId - void - addSourceEdge (int u, double cap); - /// add edge from nodeId to t - void - addTargetEdge (int u, double cap); - /// add edge from u to v and edge from v to u - /// (requires cap_uv + cap_vu >= 0) - void - addEdge (int u, int v, double cap_uv, double cap_vu = 0.0); - /// solve the max-flow problem and return the flow - double - solve (); - /// return true if \p u is in the s-set after calling \ref solve. - bool - inSourceTree (int u) const { return (cut_[u] == SOURCE); } - /// return true if \p u is in the t-set after calling \ref solve - bool - inSinkTree (int u) const { return (cut_[u] == TARGET); } - /// returns the residual capacity for an edge (use -1 for terminal (-1,-1) is the current flow - double - operator() (int u, int v) const; - - protected: - /// tree states - typedef enum { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 } nodestate; - /// capacitated edge - typedef std::map capacitated_edge; - /// edge pair - typedef std::pair edge_pair; - /// pre-augment s-u-t and s-u-v-t paths - void - preAugmentPaths (); - /// initialize trees from source and target - void - initializeTrees (); - /// expand trees until a path is found (or no path (-1, -1)) - std::pair - expandTrees (); - /// augment the path found by expandTrees; return orphaned subtrees - void - augmentPath (const std::pair& path, std::deque& orphans); - /// adopt orphaned subtrees - void - adoptOrphans (std::deque& orphans); - /// clear active set - void clearActive (); - /// \return true if active set is empty - inline bool - isActiveSetEmpty () const { return (active_head_ == TERMINAL); } - /// active if head or previous node is not the terminal - inline bool - isActive (int u) const { return ((u == active_head_) || (active_list_[u].first != TERMINAL)); } - /// mark vertex as active - void - markActive (int u); - /// mark vertex as inactive - void - markInactive (int u); - /// edges leaving the source - std::vector source_edges_; - /// edges entering the target - std::vector target_edges_; - /// nodes and their outgoing internal edges - std::vector nodes_; - /// current flow value (includes constant) - double flow_value_; - /// identifies which side of the cut a node falls - std::vector cut_; - - private: - /// parents_ flag for terminal state - static const int TERMINAL = -1; - /// search tree (also uses cut_) - std::vector > parents_; - /// doubly-linked list (prev, next) - std::vector > active_list_; - int active_head_, active_tail_; - }; - - /**\brief Structure to save RGB colors into floats */ - struct Color - { - Color () : r (0), g (0), b (0) {} - Color (float _r, float _g, float _b) : r(_r), g(_g), b(_b) {} - Color (const pcl::RGB& color) : r (color.r), g (color.g), b (color.b) {} - - template - Color (const PointT& p) - { - r = static_cast (p.r); - g = static_cast (p.g); - b = static_cast (p.b); - } - - template - operator PointT () const - { - PointT p; - p.r = static_cast (r); - p.g = static_cast (g); - p.b = static_cast (b); - return (p); - } - - float r, g, b; - }; - /// An Image is a point cloud of Color - typedef pcl::PointCloud Image; - /** \brief Compute squared distance between two colors - * \param[in] c1 first color - * \param[in] c2 second color - * \return the squared distance measure in RGB space - */ - float - colorDistance (const Color& c1, const Color& c2); - /// User supplied Trimap values - enum TrimapValue { TrimapUnknown = -1, TrimapForeground, TrimapBackground }; - /// Grabcut derived hard segementation values - enum SegmentationValue { SegmentationForeground = 0, SegmentationBackground }; - /// Gaussian structure - struct Gaussian - { - Gaussian () {} - /// mean of the gaussian - Color mu; - /// covariance matrix of the gaussian - Eigen::Matrix3f covariance; - /// determinant of the covariance matrix - float determinant; - /// inverse of the covariance matrix - Eigen::Matrix3f inverse; - /// weighting of this gaussian in the GMM. - float pi; - /// heighest eigenvalue of covariance matrix - float eigenvalue; - /// eigenvector corresponding to the heighest eigenvector - Eigen::Vector3f eigenvector; - }; - - class GMM - { - public: - /// Initialize GMM with ddesired number of gaussians. - GMM () : gaussians_ (0) {} - /// Initialize GMM with ddesired number of gaussians. - GMM (std::size_t K) : gaussians_ (K) {} - /// Destructor - ~GMM () {} - /// \return K - std::size_t - getK () const { return gaussians_.size (); } - /// resize gaussians - void - resize (std::size_t K) { gaussians_.resize (K); } - /// \return a reference to the gaussian at a given position - Gaussian& - operator[] (std::size_t pos) { return (gaussians_[pos]); } - /// \return a const reference to the gaussian at a given position - const Gaussian& - operator[] (std::size_t pos) const { return (gaussians_[pos]); } - /// \brief \return the computed probability density of a color in this GMM - float - probabilityDensity (const Color &c); - /// \brief \return the computed probability density of a color in just one Gaussian - float - probabilityDensity(std::size_t i, const Color &c); - - private: - /// array of gaussians - std::vector gaussians_; - }; - - /** Helper class that fits a single Gaussian to color samples */ - class GaussianFitter - { - public: - GaussianFitter (float epsilon = 0.0001) - : sum_ (Eigen::Vector3f::Zero ()) - , accumulator_ (Eigen::Matrix3f::Zero ()) - , count_ (0) - , epsilon_ (epsilon) - { } - - /// Add a color sample - void - add (const Color &c); - /// Build the gaussian out of all the added color samples - void - fit (Gaussian& g, std::size_t total_count, bool compute_eigens = false) const; - /// \return epsilon - float - getEpsilon () { return (epsilon_); } - /** set epsilon which will be added to the covariance matrix diagonal which avoids singular - * covariance matrix - * \param[in] epsilon user defined epsilon - */ - void - setEpsilon (float epsilon) { epsilon_ = epsilon; } - - private: - /// sum of r,g, and b - Eigen::Vector3f sum_; - /// matrix of products (i.e. r*r, r*g, r*b), some values are duplicated. - Eigen::Matrix3f accumulator_; - /// count of color samples added to the gaussian - uint32_t count_; - /// small value to add to covariance matrix diagonal to avoid singular values - float epsilon_; - }; - - /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */ - void - buildGMMs (const Image &image, - const std::vector& indices, - const std::vector &hardSegmentation, - std::vector &components, - GMM &background_GMM, GMM &foreground_GMM); - /** Iteratively learn GMMs using GrabCut updating algorithm */ - void - learnGMMs (const Image& image, - const std::vector& indices, - const std::vector& hard_segmentation, - std::vector& components, - GMM& background_GMM, GMM& foreground_GMM); - } - }; - - /** \brief Implementation of the GrabCut segmentation in - * "GrabCut — Interactive Foreground Extraction using Iterated Graph Cuts" by - * Carsten Rother, Vladimir Kolmogorov and Andrew Blake. - * - * \author Justin Talbot, jtalbot@stanford.edu placed in Public Domain, 2010 - * \author Nizar Sallem port to PCL and adaptation of original code. - * \ingroup segmentation - */ - template - class GrabCut : public pcl::PCLBase - { - public: - typedef typename pcl::search::Search KdTree; - typedef typename pcl::search::Search::Ptr KdTreePtr; - typedef typename PCLBase::PointCloudConstPtr PointCloudConstPtr; - typedef typename PCLBase::PointCloudPtr PointCloudPtr; - using PCLBase::input_; - using PCLBase::indices_; - using PCLBase::fake_indices_; - - /// Constructor - GrabCut (uint32_t K = 5, float lambda = 50.f) - : K_ (K) - , lambda_ (lambda) - , initialized_ (false) - , nb_neighbours_ (9) - {} - /// Desctructor - virtual ~GrabCut () {}; - // /// Set input cloud - void - setInputCloud (const PointCloudConstPtr& cloud); - /// Set background points, foreground points = points \ background points - void - setBackgroundPoints (const PointCloudConstPtr& background_points); - /// Set background indices, foreground indices = indices \ background indices - void - setBackgroundPointsIndices (int x1, int y1, int x2, int y2); - /// Set background indices, foreground indices = indices \ background indices - void - setBackgroundPointsIndices (const PointIndicesConstPtr& indices); - /// Run Grabcut refinement on the hard segmentation - virtual void - refine (); - /// \return the number of pixels that have changed from foreground to background or vice versa - virtual int - refineOnce (); - /// \return lambda - float - getLambda () { return (lambda_); } - /** Set lambda parameter to user given value. Suggested value by the authors is 50 - * \param[in] lambda - */ - void - setLambda (float lambda) { lambda_ = lambda; } - /// \return the number of components in the GMM - uint32_t - getK () { return (K_); } - /** Set K parameter to user given value. Suggested value by the authors is 5 - * \param[in] K the number of components used in GMM - */ - void - setK (uint32_t K) { K_ = K; } - /** \brief Provide a pointer to the search object. - * \param tree a pointer to the spatial search object. - */ - inline void - setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } - /** \brief Get a pointer to the search method used. */ - inline KdTreePtr - getSearchMethod () { return (tree_); } - /** \brief Allows to set the number of neighbours to find. - * \param[in] number_of_neighbours new number of neighbours - */ - void - setNumberOfNeighbours (int nb_neighbours) { nb_neighbours_ = nb_neighbours; } - /** \brief Returns the number of neighbours to find. */ - int - getNumberOfNeighbours () const { return (nb_neighbours_); } - /** \brief This method launches the segmentation algorithm and returns the clusters that were - * obtained during the segmentation. The indices of points belonging to the object will be stored - * in the cluster with index 1, other indices will be stored in the cluster with index 0. - * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. - */ - void - extract (std::vector& clusters); - - protected: - // Storage for N-link weights, each pixel stores links to nb_neighbours - struct NLinks - { - NLinks () : nb_links (0), indices (0), dists (0), weights (0) {} - - int nb_links; - std::vector indices; - std::vector dists; - std::vector weights; - }; - bool - initCompute (); - typedef pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor; - // /** Update hard segmentation after running GraphCut, \return the number of pixels that have - // * changed from foreground to background or vice versa. - // */ - // int - // updateHardSegmentation (); - /// Compute beta from image - void - computeBeta (); - /// Compute L parameter from given lambda - void - computeL (); - /// Compute NLinks - void - computeNLinks (); - /// Compute NLinks at a specific rectangular location - float - computeNLink (uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2); - /// Edit Trimap - void - setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t); - int - updateHardSegmentation (); - /// Fit Gaussian Multi Models - virtual void - fitGMMs (); - /// Build the graph for GraphCut - void - initGraph (); - /// Add an edge to the graph, graph must be oriented so we add the edge and its reverse - void - addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity); - /// Set the weights of SOURCE --> v and v --> SINK - void - setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity); - /// \return true if v is in source tree - inline bool - isSource (vertex_descriptor v) { return (graph_.inSourceTree (v)); } - /// image width - uint32_t width_; - /// image height - uint32_t height_; - // Variables used in formulas from the paper. - /// Number of GMM components - uint32_t K_; - /// lambda = 50. This value was suggested the GrabCut paper. - float lambda_; - /// beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels. - float beta_; - /// L = a large value to force a pixel to be foreground or background - float L_; - /// Pointer to the spatial search object. - KdTreePtr tree_; - /// Number of neighbours - int nb_neighbours_; - /// is segmentation initialized - bool initialized_; - /// Precomputed N-link weights - std::vector n_links_; - /// Converted input - segmentation::grabcut::Image::Ptr image_; - std::vector trimap_; - std::vector GMM_component_; - std::vector hard_segmentation_; - // Not yet implemented (this would be interpreted as alpha) - std::vector soft_segmentation_; - segmentation::grabcut::GMM background_GMM_, foreground_GMM_; - // Graph part - /// Graph for Graphcut - pcl::segmentation::grabcut::BoykovKolmogorov graph_; - /// Graph nodes - std::vector graph_nodes_; - }; -} - -#include - -#endif diff --git a/segmentation/include/pcl/segmentation/grabcut_segmentation.h b/segmentation/include/pcl/segmentation/grabcut_segmentation.h new file mode 100644 index 00000000..9319f872 --- /dev/null +++ b/segmentation/include/pcl/segmentation/grabcut_segmentation.h @@ -0,0 +1,483 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEGMENTATION_GRABCUT +#define PCL_SEGMENTATION_GRABCUT + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace segmentation + { + namespace grabcut + { + /** boost implementation of Boykov and Kolmogorov's maxflow algorithm doesn't support + * negative flows which makes it inappropriate for this conext. + * This implementation of Boykov and Kolmogorov's maxflow algorithm by Stephen Gould + * in DARWIN under BSD does the trick however solwer than original + * implementation. + */ + class PCL_EXPORTS BoykovKolmogorov + { + public: + typedef int vertex_descriptor; + typedef double edge_capacity_type; + + /// construct a maxflow/mincut problem with estimated max_nodes + BoykovKolmogorov (std::size_t max_nodes = 0); + /// destructor + virtual ~BoykovKolmogorov () {} + /// get number of nodes in the graph + size_t + numNodes () const { return nodes_.size (); } + /// reset all edge capacities to zero (but don't free the graph) + void + reset (); + /// clear the graph and internal datastructures + void + clear (); + /// add nodes to the graph (returns the id of the first node added) + int + addNodes (std::size_t n = 1); + /// add constant flow to graph + void + addConstant (double c) { flow_value_ += c; } + /// add edge from s to nodeId + void + addSourceEdge (int u, double cap); + /// add edge from nodeId to t + void + addTargetEdge (int u, double cap); + /// add edge from u to v and edge from v to u + /// (requires cap_uv + cap_vu >= 0) + void + addEdge (int u, int v, double cap_uv, double cap_vu = 0.0); + /// solve the max-flow problem and return the flow + double + solve (); + /// return true if \p u is in the s-set after calling \ref solve. + bool + inSourceTree (int u) const { return (cut_[u] == SOURCE); } + /// return true if \p u is in the t-set after calling \ref solve + bool + inSinkTree (int u) const { return (cut_[u] == TARGET); } + /// returns the residual capacity for an edge (use -1 for terminal (-1,-1) is the current flow + double + operator() (int u, int v) const; + + double + getSourceEdgeCapacity (int u) const; + + double + getTargetEdgeCapacity (int u) const; + + protected: + /// tree states + typedef enum { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 } nodestate; + /// capacitated edge + typedef std::map capacitated_edge; + /// edge pair + typedef std::pair edge_pair; + /// pre-augment s-u-t and s-u-v-t paths + void + preAugmentPaths (); + /// initialize trees from source and target + void + initializeTrees (); + /// expand trees until a path is found (or no path (-1, -1)) + std::pair + expandTrees (); + /// augment the path found by expandTrees; return orphaned subtrees + void + augmentPath (const std::pair& path, std::deque& orphans); + /// adopt orphaned subtrees + void + adoptOrphans (std::deque& orphans); + /// clear active set + void clearActive (); + /// \return true if active set is empty + inline bool + isActiveSetEmpty () const { return (active_head_ == TERMINAL); } + /// active if head or previous node is not the terminal + inline bool + isActive (int u) const { return ((u == active_head_) || (active_list_[u].first != TERMINAL)); } + /// mark vertex as active + void + markActive (int u); + /// mark vertex as inactive + void + markInactive (int u); + /// edges leaving the source + std::vector source_edges_; + /// edges entering the target + std::vector target_edges_; + /// nodes and their outgoing internal edges + std::vector nodes_; + /// current flow value (includes constant) + double flow_value_; + /// identifies which side of the cut a node falls + std::vector cut_; + + private: + /// parents_ flag for terminal state + static const int TERMINAL = -1; + /// search tree (also uses cut_) + std::vector > parents_; + /// doubly-linked list (prev, next) + std::vector > active_list_; + int active_head_, active_tail_; + }; + + /**\brief Structure to save RGB colors into floats */ + struct Color + { + Color () : r (0), g (0), b (0) {} + Color (float _r, float _g, float _b) : r(_r), g(_g), b(_b) {} + Color (const pcl::RGB& color) : r (color.r), g (color.g), b (color.b) {} + + template + Color (const PointT& p); + + template + operator PointT () const; + + float r, g, b; + }; + /// An Image is a point cloud of Color + typedef pcl::PointCloud Image; + /** \brief Compute squared distance between two colors + * \param[in] c1 first color + * \param[in] c2 second color + * \return the squared distance measure in RGB space + */ + float + colorDistance (const Color& c1, const Color& c2); + /// User supplied Trimap values + enum TrimapValue { TrimapUnknown = -1, TrimapForeground, TrimapBackground }; + /// Grabcut derived hard segementation values + enum SegmentationValue { SegmentationForeground = 0, SegmentationBackground }; + /// Gaussian structure + struct Gaussian + { + Gaussian () {} + /// mean of the gaussian + Color mu; + /// covariance matrix of the gaussian + Eigen::Matrix3f covariance; + /// determinant of the covariance matrix + float determinant; + /// inverse of the covariance matrix + Eigen::Matrix3f inverse; + /// weighting of this gaussian in the GMM. + float pi; + /// heighest eigenvalue of covariance matrix + float eigenvalue; + /// eigenvector corresponding to the heighest eigenvector + Eigen::Vector3f eigenvector; + }; + + class PCL_EXPORTS GMM + { + public: + /// Initialize GMM with ddesired number of gaussians. + GMM () : gaussians_ (0) {} + /// Initialize GMM with ddesired number of gaussians. + GMM (std::size_t K) : gaussians_ (K) {} + /// Destructor + ~GMM () {} + /// \return K + std::size_t + getK () const { return gaussians_.size (); } + /// resize gaussians + void + resize (std::size_t K) { gaussians_.resize (K); } + /// \return a reference to the gaussian at a given position + Gaussian& + operator[] (std::size_t pos) { return (gaussians_[pos]); } + /// \return a const reference to the gaussian at a given position + const Gaussian& + operator[] (std::size_t pos) const { return (gaussians_[pos]); } + /// \brief \return the computed probability density of a color in this GMM + float + probabilityDensity (const Color &c); + /// \brief \return the computed probability density of a color in just one Gaussian + float + probabilityDensity(std::size_t i, const Color &c); + + private: + /// array of gaussians + std::vector gaussians_; + }; + + /** Helper class that fits a single Gaussian to color samples */ + class GaussianFitter + { + public: + GaussianFitter (float epsilon = 0.0001) + : sum_ (Eigen::Vector3f::Zero ()) + , accumulator_ (Eigen::Matrix3f::Zero ()) + , count_ (0) + , epsilon_ (epsilon) + { } + + /// Add a color sample + void + add (const Color &c); + /// Build the gaussian out of all the added color samples + void + fit (Gaussian& g, std::size_t total_count, bool compute_eigens = false) const; + /// \return epsilon + float + getEpsilon () { return (epsilon_); } + /** set epsilon which will be added to the covariance matrix diagonal which avoids singular + * covariance matrix + * \param[in] epsilon user defined epsilon + */ + void + setEpsilon (float epsilon) { epsilon_ = epsilon; } + + private: + /// sum of r,g, and b + Eigen::Vector3f sum_; + /// matrix of products (i.e. r*r, r*g, r*b), some values are duplicated. + Eigen::Matrix3f accumulator_; + /// count of color samples added to the gaussian + uint32_t count_; + /// small value to add to covariance matrix diagonal to avoid singular values + float epsilon_; + }; + + /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */ + PCL_EXPORTS void + buildGMMs (const Image &image, + const std::vector& indices, + const std::vector &hardSegmentation, + std::vector &components, + GMM &background_GMM, GMM &foreground_GMM); + /** Iteratively learn GMMs using GrabCut updating algorithm */ + PCL_EXPORTS void + learnGMMs (const Image& image, + const std::vector& indices, + const std::vector& hard_segmentation, + std::vector& components, + GMM& background_GMM, GMM& foreground_GMM); + } + }; + + /** \brief Implementation of the GrabCut segmentation in + * "GrabCut — Interactive Foreground Extraction using Iterated Graph Cuts" by + * Carsten Rother, Vladimir Kolmogorov and Andrew Blake. + * + * \author Justin Talbot, jtalbot@stanford.edu placed in Public Domain, 2010 + * \author Nizar Sallem port to PCL and adaptation of original code. + * \ingroup segmentation + */ + template + class GrabCut : public pcl::PCLBase + { + public: + typedef typename pcl::search::Search KdTree; + typedef typename pcl::search::Search::Ptr KdTreePtr; + typedef typename PCLBase::PointCloudConstPtr PointCloudConstPtr; + typedef typename PCLBase::PointCloudPtr PointCloudPtr; + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::fake_indices_; + + /// Constructor + GrabCut (uint32_t K = 5, float lambda = 50.f) + : K_ (K) + , lambda_ (lambda) + , nb_neighbours_ (9) + , initialized_ (false) + {} + /// Desctructor + virtual ~GrabCut () {}; + // /// Set input cloud + void + setInputCloud (const PointCloudConstPtr& cloud); + /// Set background points, foreground points = points \ background points + void + setBackgroundPoints (const PointCloudConstPtr& background_points); + /// Set background indices, foreground indices = indices \ background indices + void + setBackgroundPointsIndices (int x1, int y1, int x2, int y2); + /// Set background indices, foreground indices = indices \ background indices + void + setBackgroundPointsIndices (const PointIndicesConstPtr& indices); + /// Run Grabcut refinement on the hard segmentation + virtual void + refine (); + /// \return the number of pixels that have changed from foreground to background or vice versa + virtual int + refineOnce (); + /// \return lambda + float + getLambda () { return (lambda_); } + /** Set lambda parameter to user given value. Suggested value by the authors is 50 + * \param[in] lambda + */ + void + setLambda (float lambda) { lambda_ = lambda; } + /// \return the number of components in the GMM + uint32_t + getK () { return (K_); } + /** Set K parameter to user given value. Suggested value by the authors is 5 + * \param[in] K the number of components used in GMM + */ + void + setK (uint32_t K) { K_ = K; } + /** \brief Provide a pointer to the search object. + * \param tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () { return (tree_); } + /** \brief Allows to set the number of neighbours to find. + * \param[in] nb_neighbours new number of neighbours + */ + void + setNumberOfNeighbours (int nb_neighbours) { nb_neighbours_ = nb_neighbours; } + /** \brief Returns the number of neighbours to find. */ + int + getNumberOfNeighbours () const { return (nb_neighbours_); } + /** \brief This method launches the segmentation algorithm and returns the clusters that were + * obtained during the segmentation. The indices of points belonging to the object will be stored + * in the cluster with index 1, other indices will be stored in the cluster with index 0. + * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. + */ + void + extract (std::vector& clusters); + + protected: + // Storage for N-link weights, each pixel stores links to nb_neighbours + struct NLinks + { + NLinks () : nb_links (0), indices (0), dists (0), weights (0) {} + + int nb_links; + std::vector indices; + std::vector dists; + std::vector weights; + }; + bool + initCompute (); + typedef pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor; + /// Compute beta from image + void + computeBetaOrganized (); + /// Compute beta from cloud + void + computeBetaNonOrganized (); + /// Compute L parameter from given lambda + void + computeL (); + /// Compute NLinks from image + void + computeNLinksOrganized (); + /// Compute NLinks from cloud + void + computeNLinksNonOrganized (); + /// Edit Trimap + void + setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t); + int + updateHardSegmentation (); + /// Fit Gaussian Multi Models + virtual void + fitGMMs (); + /// Build the graph for GraphCut + void + initGraph (); + /// Add an edge to the graph, graph must be oriented so we add the edge and its reverse + void + addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity); + /// Set the weights of SOURCE --> v and v --> SINK + void + setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity); + /// \return true if v is in source tree + inline bool + isSource (vertex_descriptor v) { return (graph_.inSourceTree (v)); } + /// image width + uint32_t width_; + /// image height + uint32_t height_; + // Variables used in formulas from the paper. + /// Number of GMM components + uint32_t K_; + /// lambda = 50. This value was suggested the GrabCut paper. + float lambda_; + /// beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels. + float beta_; + /// L = a large value to force a pixel to be foreground or background + float L_; + /// Pointer to the spatial search object. + KdTreePtr tree_; + /// Number of neighbours + int nb_neighbours_; + /// is segmentation initialized + bool initialized_; + /// Precomputed N-link weights + std::vector n_links_; + /// Converted input + segmentation::grabcut::Image::Ptr image_; + std::vector trimap_; + std::vector GMM_component_; + std::vector hard_segmentation_; + // Not yet implemented (this would be interpreted as alpha) + std::vector soft_segmentation_; + segmentation::grabcut::GMM background_GMM_, foreground_GMM_; + // Graph part + /// Graph for Graphcut + pcl::segmentation::grabcut::BoykovKolmogorov graph_; + /// Graph nodes + std::vector graph_nodes_; + }; +} + +#include + +#endif diff --git a/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp b/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp new file mode 100644 index 00000000..0660e40b --- /dev/null +++ b/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp @@ -0,0 +1,264 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ +#define PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ApproximateProgressiveMorphologicalFilter::ApproximateProgressiveMorphologicalFilter () : + max_window_size_ (33), + slope_ (0.7f), + max_distance_ (10.0f), + initial_distance_ (0.15f), + cell_size_ (1.0f), + base_ (2.0f), + exponential_ (true), + threads_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ApproximateProgressiveMorphologicalFilter::~ApproximateProgressiveMorphologicalFilter () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ApproximateProgressiveMorphologicalFilter::extract (std::vector& ground) +{ + bool segmentation_is_possible = initCompute (); + if (!segmentation_is_possible) + { + deinitCompute (); + return; + } + + // Compute the series of window sizes and height thresholds + std::vector height_thresholds; + std::vector window_sizes; + std::vector half_sizes; + int iteration = 0; + int half_size = 0.0f; + float window_size = 0.0f; + float height_threshold = 0.0f; + + while (window_size < max_window_size_) + { + // Determine the initial window size. + if (exponential_) + half_size = static_cast (std::pow (static_cast (base_), iteration)); + else + half_size = (iteration+1) * base_; + + window_size = 2 * half_size + 1; + + // Calculate the height threshold to be used in the next iteration. + if (iteration == 0) + height_threshold = initial_distance_; + else + height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_; + + // Enforce max distance on height threshold + if (height_threshold > max_distance_) + height_threshold = max_distance_; + + half_sizes.push_back (half_size); + window_sizes.push_back (window_size); + height_thresholds.push_back (height_threshold); + + iteration++; + } + + // setup grid based on scale and extents + Eigen::Vector4f global_max, global_min; + pcl::getMinMax3D (*input_, global_min, global_max); + + float xextent = global_max.x () - global_min.x (); + float yextent = global_max.y () - global_min.y (); + + int rows = static_cast (std::floor (yextent / cell_size_) + 1); + int cols = static_cast (std::floor (xextent / cell_size_) + 1); + + Eigen::MatrixXf A (rows, cols); + A.setConstant (std::numeric_limits::quiet_NaN ()); + + Eigen::MatrixXf Z (rows, cols); + Z.setConstant (std::numeric_limits::quiet_NaN ()); + + Eigen::MatrixXf Zf (rows, cols); + Zf.setConstant (std::numeric_limits::quiet_NaN ()); + +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < (int)input_->points.size (); ++i) + { + // ...then test for lower points within the cell + PointT p = input_->points[i]; + int row = std::floor(p.y - global_min.y ()); + int col = std::floor(p.x - global_min.x ()); + + if (p.z < A (row, col) || pcl_isnan (A (row, col))) + { + A (row, col) = p.z; + } + } + + // Ground indices are initially limited to those points in the input cloud we + // wish to process + ground = *indices_; + + // Progressively filter ground returns using morphological open + for (size_t i = 0; i < window_sizes.size (); ++i) + { + PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...", + i, height_thresholds[i], window_sizes[i], half_sizes[i]); + + // Limit filtering to those points currently considered ground returns + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::copyPointCloud (*input_, ground, *cloud); + + // Apply the morphological opening operation at the current window size. +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int row = 0; row < rows; ++row) + { + int rs, re; + rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i]; + re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i]; + + for (int col = 0; col < cols; ++col) + { + int cs, ce; + cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i]; + ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i]; + + float min_coeff = std::numeric_limits::max (); + + for (int j = rs; j < (re + 1); ++j) + { + for (int k = cs; k < (ce + 1); ++k) + { + if (A (j, k) != std::numeric_limits::quiet_NaN ()) + { + if (A (j, k) < min_coeff) + min_coeff = A (j, k); + } + } + } + + if (min_coeff != std::numeric_limits::max ()) + Z(row, col) = min_coeff; + } + } + +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int row = 0; row < rows; ++row) + { + int rs, re; + rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i]; + re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i]; + + for (int col = 0; col < cols; ++col) + { + int cs, ce; + cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i]; + ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i]; + + float max_coeff = -std::numeric_limits::max (); + + for (int j = rs; j < (re + 1); ++j) + { + for (int k = cs; k < (ce + 1); ++k) + { + if (Z (j, k) != std::numeric_limits::quiet_NaN ()) + { + if (Z (j, k) > max_coeff) + max_coeff = Z (j, k); + } + } + } + + if (max_coeff != -std::numeric_limits::max ()) + Zf (row, col) = max_coeff; + } + } + + // Find indices of the points whose difference between the source and + // filtered point clouds is less than the current height threshold. + std::vector pt_indices; + for (size_t p_idx = 0; p_idx < ground.size (); ++p_idx) + { + PointT p = cloud->points[p_idx]; + int erow = static_cast (std::floor ((p.y - global_min.y ()) / cell_size_)); + int ecol = static_cast (std::floor ((p.x - global_min.x ()) / cell_size_)); + + float diff = p.z - Zf (erow, ecol); + if (diff < height_thresholds[i]) + pt_indices.push_back (ground[p_idx]); + } + + A.swap (Zf); + + // Ground is now limited to pt_indices + ground.swap (pt_indices); + + PCL_DEBUG ("ground now has %d points\n", ground.size ()); + } + + deinitCompute (); +} + + +#define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter; + +#endif // PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + diff --git a/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp b/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp index 0b3ae9c9..d1634b5c 100644 --- a/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp +++ b/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp @@ -116,7 +116,9 @@ pcl::ConditionalEuclideanClustering::segment (pcl::IndicesClusters &clus } // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range - if (extract_removed_clusters_ || (current_cluster.size () >= min_cluster_size_ && current_cluster.size () <= max_cluster_size_)) + if (extract_removed_clusters_ || + (static_cast (current_cluster.size ()) >= min_cluster_size_ && + static_cast (current_cluster.size ()) <= max_cluster_size_)) { pcl::PointIndices pi; pi.header = input_->header; @@ -124,9 +126,9 @@ pcl::ConditionalEuclideanClustering::segment (pcl::IndicesClusters &clus for (int ii = 0; ii < static_cast (current_cluster.size ()); ++ii) // ii = indices iterator pi.indices[ii] = current_cluster[ii]; - if (extract_removed_clusters_ && current_cluster.size () < min_cluster_size_) + if (extract_removed_clusters_ && static_cast (current_cluster.size ()) < min_cluster_size_) small_clusters_->push_back (pi); - else if (extract_removed_clusters_ && current_cluster.size () > max_cluster_size_) + else if (extract_removed_clusters_ && static_cast (current_cluster.size ()) > max_cluster_size_) large_clusters_->push_back (pi); else clusters.push_back (pi); diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp index d9288ed3..002d3ed9 100644 --- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp +++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp @@ -50,7 +50,7 @@ pcl::extractEuclideanClusters (const PointCloud &cloud, { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } // Check if the tree is sorted -- if it is we don't need to check the first element @@ -126,12 +126,12 @@ pcl::extractEuclideanClusters (const PointCloud &cloud, //and indices[i] if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (tree->getIndices ()->size () != indices.size ()) { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%zu) than the input set (%zu)!\n", tree->getIndices ()->size (), indices.size ()); + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); return; } // Check if the tree is sorted -- if it is we don't need to check the first element diff --git a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp index ae693e25..9fde0931 100644 --- a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp +++ b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp @@ -51,7 +51,7 @@ pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false diff --git a/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp b/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp index 3ab8fc70..f4d95ee0 100644 --- a/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp +++ b/segmentation/include/pcl/segmentation/impl/extract_polygonal_prism_data.hpp @@ -179,7 +179,7 @@ pcl::ExtractPolygonalPrismData::segment (pcl::PointIndices &output) if (static_cast (planar_hull_->points.size ()) < min_pts_hull_) { - PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ()); + PCL_ERROR ("[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ()); output.indices.clear (); return; } diff --git a/segmentation/include/pcl/segmentation/impl/grabcut.hpp b/segmentation/include/pcl/segmentation/impl/grabcut.hpp deleted file mode 100644 index 26de5653..00000000 --- a/segmentation/include/pcl/segmentation/impl/grabcut.hpp +++ /dev/null @@ -1,385 +0,0 @@ -/////////////////////////////////////////////////////// -// grabCut->initialize (xstart, ystart, xend, yend); // -// grabCut->fitGMMs (); // -/////////////////////////////////////////////////////// -#ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP -#define PCL_SEGMENTATION_IMPL_GRABCUT_HPP - -#include -#include -#include - -namespace pcl -{ - template <> - float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1, - const pcl::segmentation::grabcut::Color &c2) - { - return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b)); - } -} - -template void -pcl::GrabCut::setInputCloud (const PointCloudConstPtr &cloud) -{ - input_ = cloud; -} - -template bool -pcl::GrabCut::initCompute () -{ - using namespace pcl::segmentation::grabcut; - if (!pcl::PCLBase::initCompute ()) - { - PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!"); - return (false); - } - - if (!input_->isOrganized ()) - { - PCL_ERROR ("[pcl::GrabCut::initCompute ()] Need an organized point cloud to proceed!"); - return (false); - } - - std::vector in_fields_; - if ((pcl::getFieldIndex (*input_, "rgb", in_fields_) == -1) && - (pcl::getFieldIndex (*input_, "rgba", in_fields_) == -1)) - { - PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!"); - return (false); - } - - // Initialize the working image - image_.reset (new Image (input_->width, input_->height)); - for (std::size_t i = 0; i < input_->size (); ++i) - { - (*image_) [i] = Color (input_->points[i]); - } - width_ = image_->width; - height_ = image_->height; - - // Initialize the spatial locator - if (!tree_) - { - if (input_->isOrganized ()) - tree_.reset (new pcl::search::OrganizedNeighbor ()); - else - tree_.reset (new pcl::search::KdTree (false)); - tree_->setInputCloud (input_); - } - const std::size_t indices_size = indices_->size (); - trimap_ = std::vector (indices_size, TrimapUnknown); - hard_segmentation_ = std::vector (indices_size, - SegmentationBackground); - GMM_component_.resize (indices_size); - n_links_.resize (indices_size); - // soft_segmentation_ = 0; // Not yet implemented - foreground_GMM_.resize (K_); - background_GMM_.resize (K_); - //set some constants - computeL (); - computeBeta (); - computeNLinks (); - - initialized_ = false; - return (true); -} - -template void -pcl::GrabCut::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity) -{ - graph_.addEdge (v1, v2, capacity, rev_capacity); -} - -template void -pcl::GrabCut::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity) -{ - graph_.addSourceEdge (v, source_capacity); - graph_.addTargetEdge (v, sink_capacity); -} - -// template void -// pcl::GrabCut::setBackgroundPointsIndices (int x1, int y1, int x2, int y2) -// { -// using namespace pcl::segmentation::grabcut; - -// // Step 1: User creates inital Trimap with rectangle, Background outside, Unknown inside -// fill (trimap_.begin (), trimap_.end (), TrimapBackground); -// fillRectangle (trimap_, width_, height_, x1, y1, x2, y2, TrimapUnknown); - -// // Step 2: Initial segmentation, Background where Trimap is Background, Foreground where Trimap is Unknown. -// fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground); -// fillRectangle (hard_segmentation_, width_, height_, x1, y1, x2, y2, SegmentationForeground); -// if (!initialized_) -// { -// fitGMMs (); -// initialized_ = true; -// } -// } - -template void -pcl::GrabCut::setBackgroundPointsIndices (const PointIndicesConstPtr &indices) -{ - using namespace pcl::segmentation::grabcut; - if (!initCompute ()) - return; - - std::fill (trimap_.begin (), trimap_.end (), TrimapBackground); - std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground); - for (std::vector::const_iterator idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) - { - trimap_[*idx] = TrimapUnknown; - hard_segmentation_[*idx] = SegmentationForeground; - } - - if (!initialized_) - { - fitGMMs (); - initialized_ = true; - } -} - -template void -pcl::GrabCut::fitGMMs () -{ - // Step 3: Build GMMs using Orchard-Bouman clustering algorithm - buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); - - // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized) - initGraph (); -} - -template int -pcl::GrabCut::refineOnce () -{ - // Steps 4 and 5: Learn new GMMs from current segmentation - learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); - - // Step 6: Run GraphCut and update segmentation - initGraph (); - - float flow = graph_.solve (); - - int changed = updateHardSegmentation (); - PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow); - - return (changed); -} - -template void -pcl::GrabCut::refine () -{ - std::size_t changed = indices_->size (); - - while (changed) - changed = refineOnce (); -} - -template int -pcl::GrabCut::updateHardSegmentation () -{ - using namespace pcl::segmentation::grabcut; - - int changed = 0; - - const int number_of_indices = static_cast (indices_->size ()); - for (int i_point = 0; i_point < number_of_indices; ++i_point) - { - SegmentationValue old_value = hard_segmentation_ [i_point]; - - if (trimap_ [i_point] == TrimapBackground) - hard_segmentation_ [i_point] = SegmentationBackground; - else - if (trimap_ [i_point] == TrimapForeground) - hard_segmentation_ [i_point] = SegmentationForeground; - else // TrimapUnknown - { - if (isSource (graph_nodes_[i_point])) - hard_segmentation_ [i_point] = SegmentationForeground; - else - hard_segmentation_ [i_point] = SegmentationBackground; - } - - if (old_value != hard_segmentation_ [i_point]) - ++changed; - } - return (changed); -} - -template void -pcl::GrabCut::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t) -{ - using namespace pcl::segmentation::grabcut; - std::vector::const_iterator idx = indices->indices.begin (); - for (; idx != indices->indices.end (); ++idx) - trimap_[*idx] = t; - - // Immediately set the hard segmentation as well so that the display will update. - if (t == TrimapForeground) - for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) - hard_segmentation_[*idx] = SegmentationForeground; - else - if (t == TrimapBackground) - for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) - hard_segmentation_[*idx] = SegmentationBackground; -} - -template void -pcl::GrabCut::initGraph () -{ - using namespace pcl::segmentation::grabcut; - const int number_of_indices = static_cast (indices_->size ()); - // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated) - graph_.clear (); - graph_nodes_.clear (); - graph_nodes_.resize (indices_->size ()); - int start = graph_.addNodes (indices_->size ()); - for (int idx = 0; idx < indices_->size (); ++idx) - { - graph_nodes_[idx] = start; - ++start; - } - - // Set T-Link weights - for (int i_point = 0; i_point < number_of_indices; ++i_point) - { - int point_index = (*indices_) [i_point]; - float back, fore; - - switch (trimap_[point_index]) - { - case TrimapUnknown : - { - fore = static_cast (-log (background_GMM_.probabilityDensity (image_->points[point_index]))); - back = static_cast (-log (foreground_GMM_.probabilityDensity (image_->points[point_index]))); - break; - } - case TrimapBackground : - { - fore = 0; - back = L_; - break; - } - default : - { - fore = L_; - back = 0; - } - } - - setTerminalWeights (graph_nodes_[i_point], fore, back); - } - - // Set N-Link weights from precomputed values - for (int i_point = 0; i_point < number_of_indices; ++i_point) - { - const NLinks &n_link = n_links_[i_point]; - if (n_link.nb_links > 0) - { - int point_index = (*indices_) [i_point]; - std::vector::const_iterator indices_it = n_link.indices.begin (); - std::vector::const_iterator weights_it = n_link.weights.begin (); - for (; indices_it != n_link.indices.end (); ++indices_it, ++weights_it) - { - if (*indices_it != point_index) - { - addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it); - } - } - } - } -} - -template void -pcl::GrabCut::computeNLinks () -{ - const int number_of_indices = static_cast (indices_->size ()); - for (int i_point = 0; i_point < number_of_indices; ++i_point) - { - NLinks &n_link = n_links_[i_point]; - if (n_link.nb_links > 0) - { - int point_index = (*indices_) [i_point]; - std::vector::const_iterator indices_it = n_link.indices.begin (); - std::vector::const_iterator dists_it = n_link.dists.begin (); - std::vector::iterator weights_it = n_link.weights.begin (); - for (; indices_it != n_link.indices.end (); ++indices_it, ++dists_it, ++weights_it) - { - if (*indices_it != point_index) - { - // We saved the color distance previously at the computeBeta stage for optimization purpose - float color_distance = *weights_it; - // Set the real weight - *weights_it = static_cast (lambda_ * exp (-beta_ * color_distance) / sqrt (*dists_it)); - } - } - } - } -} - -template void -pcl::GrabCut::computeBeta () -{ - float result = 0; - std::size_t edges = 0; - - const int number_of_indices = static_cast (indices_->size ()); - - for (int i_point = 0; i_point < number_of_indices; i_point++) - { - int point_index = (*indices_)[i_point]; - const PointT& point = input_->points [point_index]; - if (pcl::isFinite (point)) - { - NLinks &links = n_links_[i_point]; - int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists); - if (found > 1) - { - links.nb_links = found - 1; - links.weights.reserve (links.nb_links); - for (std::vector::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it) - { - if (*nn_it != point_index) - { - float color_distance = squaredEuclideanDistance (image_->points[point_index], image_->points[*nn_it]); - links.weights.push_back (color_distance); - result+= color_distance; - ++edges; - } - else - links.weights.push_back (0.f); - } - } - } - } - std::cout << "result " << result << std::endl; - std::cout << "edges " << edges << std::endl; - beta_ = 1e5 / (2*result / edges); - std::cout << "beta " << beta_ << std::endl; -} - -template void -pcl::GrabCut::computeL () -{ - L_ = 8*lambda_ + 1; -} - -template void -pcl::GrabCut::extract (std::vector& clusters) -{ - using namespace pcl::segmentation::grabcut; - clusters.clear (); - clusters.resize (2); - clusters[0].indices.reserve (indices_->size ()); - clusters[1].indices.reserve (indices_->size ()); - refine (); - assert (hard_segmentation_.size () == indices_->size ()); - const int indices_size = static_cast (indices_->size ()); - for (int i = 0; i < indices_size; ++i) - if (hard_segmentation_[i] == SegmentationForeground) - clusters[1].indices.push_back (i); - else - clusters[0].indices.push_back (i); -} - -#endif diff --git a/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp new file mode 100644 index 00000000..3d8ebe96 --- /dev/null +++ b/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp @@ -0,0 +1,515 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP +#define PCL_SEGMENTATION_IMPL_GRABCUT_HPP + +#include +#include +#include + +namespace pcl +{ + template <> + float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1, + const pcl::segmentation::grabcut::Color &c2) + { + return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b)); + } +} + +template +pcl::segmentation::grabcut::Color::Color (const PointT& p) +{ + r = static_cast (p.r) / 255.0; + g = static_cast (p.g) / 255.0; + b = static_cast (p.b) / 255.0; +} + +template +pcl::segmentation::grabcut::Color::operator PointT () const +{ + PointT p; + p.r = static_cast (r * 255); + p.g = static_cast (g * 255); + p.b = static_cast (b * 255); + return (p); +} + +template void +pcl::GrabCut::setInputCloud (const PointCloudConstPtr &cloud) +{ + input_ = cloud; +} + +template bool +pcl::GrabCut::initCompute () +{ + using namespace pcl::segmentation::grabcut; + if (!pcl::PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!"); + return (false); + } + + std::vector in_fields_; + if ((pcl::getFieldIndex (*input_, "rgb", in_fields_) == -1) && + (pcl::getFieldIndex (*input_, "rgba", in_fields_) == -1)) + { + PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!"); + return (false); + } + + // Initialize the working image + image_.reset (new Image (input_->width, input_->height)); + for (std::size_t i = 0; i < input_->size (); ++i) + { + (*image_) [i] = Color (input_->points[i]); + } + width_ = image_->width; + height_ = image_->height; + + // Initialize the spatial locator + if (!tree_ && !input_->isOrganized ()) + { + tree_.reset (new pcl::search::KdTree (true)); + tree_->setInputCloud (input_); + } + + const std::size_t indices_size = indices_->size (); + trimap_ = std::vector (indices_size, TrimapUnknown); + hard_segmentation_ = std::vector (indices_size, + SegmentationBackground); + GMM_component_.resize (indices_size); + n_links_.resize (indices_size); + + // soft_segmentation_ = 0; // Not yet implemented + foreground_GMM_.resize (K_); + background_GMM_.resize (K_); + + //set some constants + computeL (); + + if (image_->isOrganized ()) + { + computeBetaOrganized (); + computeNLinksOrganized (); + } + else + { + computeBetaNonOrganized (); + computeNLinksNonOrganized (); + } + + initialized_ = false; + return (true); +} + +template void +pcl::GrabCut::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity) +{ + graph_.addEdge (v1, v2, capacity, rev_capacity); +} + +template void +pcl::GrabCut::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity) +{ + graph_.addSourceEdge (v, source_capacity); + graph_.addTargetEdge (v, sink_capacity); +} + +template void +pcl::GrabCut::setBackgroundPointsIndices (const PointIndicesConstPtr &indices) +{ + using namespace pcl::segmentation::grabcut; + if (!initCompute ()) + return; + + std::fill (trimap_.begin (), trimap_.end (), TrimapBackground); + std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground); + for (std::vector::const_iterator idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) + { + trimap_[*idx] = TrimapUnknown; + hard_segmentation_[*idx] = SegmentationForeground; + } + + if (!initialized_) + { + fitGMMs (); + initialized_ = true; + } +} + +template void +pcl::GrabCut::fitGMMs () +{ + // Step 3: Build GMMs using Orchard-Bouman clustering algorithm + buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); + + // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized) + initGraph (); +} + +template int +pcl::GrabCut::refineOnce () +{ + // Steps 4 and 5: Learn new GMMs from current segmentation + learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); + + // Step 6: Run GraphCut and update segmentation + initGraph (); + + float flow = graph_.solve (); + + int changed = updateHardSegmentation (); + PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow); + + return (changed); +} + +template void +pcl::GrabCut::refine () +{ + std::size_t changed = indices_->size (); + + while (changed) + changed = refineOnce (); +} + +template int +pcl::GrabCut::updateHardSegmentation () +{ + using namespace pcl::segmentation::grabcut; + + int changed = 0; + + const int number_of_indices = static_cast (indices_->size ()); + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + SegmentationValue old_value = hard_segmentation_ [i_point]; + + if (trimap_ [i_point] == TrimapBackground) + hard_segmentation_ [i_point] = SegmentationBackground; + else + if (trimap_ [i_point] == TrimapForeground) + hard_segmentation_ [i_point] = SegmentationForeground; + else // TrimapUnknown + { + if (isSource (graph_nodes_[i_point])) + hard_segmentation_ [i_point] = SegmentationForeground; + else + hard_segmentation_ [i_point] = SegmentationBackground; + } + + if (old_value != hard_segmentation_ [i_point]) + ++changed; + } + return (changed); +} + +template void +pcl::GrabCut::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t) +{ + using namespace pcl::segmentation::grabcut; + std::vector::const_iterator idx = indices->indices.begin (); + for (; idx != indices->indices.end (); ++idx) + trimap_[*idx] = t; + + // Immediately set the hard segmentation as well so that the display will update. + if (t == TrimapForeground) + for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) + hard_segmentation_[*idx] = SegmentationForeground; + else + if (t == TrimapBackground) + for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) + hard_segmentation_[*idx] = SegmentationBackground; +} + +template void +pcl::GrabCut::initGraph () +{ + using namespace pcl::segmentation::grabcut; + const int number_of_indices = static_cast (indices_->size ()); + // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated) + graph_.clear (); + graph_nodes_.clear (); + graph_nodes_.resize (indices_->size ()); + int start = graph_.addNodes (indices_->size ()); + for (int idx = 0; idx < indices_->size (); ++idx) + { + graph_nodes_[idx] = start; + ++start; + } + + // Set T-Link weights + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + int point_index = (*indices_) [i_point]; + float back, fore; + + switch (trimap_[point_index]) + { + case TrimapUnknown : + { + fore = static_cast (-log (background_GMM_.probabilityDensity (image_->points[point_index]))); + back = static_cast (-log (foreground_GMM_.probabilityDensity (image_->points[point_index]))); + break; + } + case TrimapBackground : + { + fore = 0; + back = L_; + break; + } + default : + { + fore = L_; + back = 0; + } + } + + setTerminalWeights (graph_nodes_[i_point], fore, back); + } + + // Set N-Link weights from precomputed values + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + const NLinks &n_link = n_links_[i_point]; + if (n_link.nb_links > 0) + { + int point_index = (*indices_) [i_point]; + std::vector::const_iterator indices_it = n_link.indices.begin (); + std::vector::const_iterator weights_it = n_link.weights.begin (); + for (; indices_it != n_link.indices.end (); ++indices_it, ++weights_it) + { + if ((*indices_it != point_index) && (*indices_it > -1)) + { + addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it); + } + } + } + } +} + +template void +pcl::GrabCut::computeNLinksNonOrganized () +{ + const int number_of_indices = static_cast (indices_->size ()); + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + NLinks &n_link = n_links_[i_point]; + if (n_link.nb_links > 0) + { + int point_index = (*indices_) [i_point]; + std::vector::const_iterator indices_it = n_link.indices.begin (); + std::vector::const_iterator dists_it = n_link.dists.begin (); + std::vector::iterator weights_it = n_link.weights.begin (); + for (; indices_it != n_link.indices.end (); ++indices_it, ++dists_it, ++weights_it) + { + if (*indices_it != point_index) + { + // We saved the color distance previously at the computeBeta stage for optimization purpose + float color_distance = *weights_it; + // Set the real weight + *weights_it = static_cast (lambda_ * exp (-beta_ * color_distance) / sqrt (*dists_it)); + } + } + } + } +} + +template void +pcl::GrabCut::computeNLinksOrganized () +{ + for( unsigned int y = 0; y < image_->height; ++y ) + { + for( unsigned int x = 0; x < image_->width; ++x ) + { + // We saved the color and euclidean distance previously at the computeBeta stage for + // optimization purpose but here we compute the real weight + std::size_t point_index = y * input_->width + x; + NLinks &links = n_links_[point_index]; + + if( x > 0 && y < image_->height-1 ) + links.weights[0] = lambda_ * exp (-beta_ * links.weights[0]) / links.dists[0]; + + if( y < image_->height-1 ) + links.weights[1] = lambda_ * exp (-beta_ * links.weights[1]) / links.dists[1]; + + if( x < image_->width-1 && y < image_->height-1 ) + links.weights[2] = lambda_ * exp (-beta_ * links.weights[2]) / links.dists[2]; + + if( x < image_->width-1 ) + links.weights[3] = lambda_ * exp (-beta_ * links.weights[3]) / links.dists[3]; + } + } +} + +template void +pcl::GrabCut::computeBetaNonOrganized () +{ + float result = 0; + std::size_t edges = 0; + + const int number_of_indices = static_cast (indices_->size ()); + + for (int i_point = 0; i_point < number_of_indices; i_point++) + { + int point_index = (*indices_)[i_point]; + const PointT& point = input_->points [point_index]; + if (pcl::isFinite (point)) + { + NLinks &links = n_links_[i_point]; + int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists); + if (found > 1) + { + links.nb_links = found - 1; + links.weights.reserve (links.nb_links); + for (std::vector::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it) + { + if (*nn_it != point_index) + { + float color_distance = squaredEuclideanDistance (image_->points[point_index], image_->points[*nn_it]); + links.weights.push_back (color_distance); + result+= color_distance; + ++edges; + } + else + links.weights.push_back (0.f); + } + } + } + } + + beta_ = 1e5 / (2*result / edges); +} + +template void +pcl::GrabCut::computeBetaOrganized () +{ + float result = 0; + std::size_t edges = 0; + + for (unsigned int y = 0; y < input_->height; ++y) + { + for (unsigned int x = 0; x < input_->width; ++x) + { + std::size_t point_index = y * input_->width + x; + NLinks &links = n_links_[point_index]; + links.nb_links = 4; + links.weights.resize (links.nb_links, 0); + links.dists.resize (links.nb_links, 0); + links.indices.resize (links.nb_links, -1); + + if (x > 0 && y < input_->height-1) + { + std::size_t upleft = (y+1) * input_->width + x - 1; + links.indices[0] = upleft; + links.dists[0] = sqrt (2.f); + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points[upleft]); + links.weights[0] = color_dist; + result+= color_dist; + edges++; + } + + if (y < input_->height-1) + { + std::size_t up = (y+1) * input_->width + x; + links.indices[1] = up; + links.dists[1] = 1; + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points[up]); + links.weights[1] = color_dist; + result+= color_dist; + edges++; + } + + if (x < input_->width-1 && y < input_->height-1) + { + std::size_t upright = (y+1) * input_->width + x + 1; + links.indices[2] = upright; + links.dists[2] = sqrt (2.f); + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points [upright]); + links.weights[2] = color_dist; + result+= color_dist; + edges++; + } + + if (x < input_->width-1) + { + std::size_t right = y * input_->width + x + 1; + links.indices[3] = right; + links.dists[3] = 1; + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points[right]); + links.weights[3] = color_dist; + result+= color_dist; + edges++; + } + } + } + + beta_ = 1e5 / (2*result / edges); +} + +template void +pcl::GrabCut::computeL () +{ + L_ = 8*lambda_ + 1; +} + +template void +pcl::GrabCut::extract (std::vector& clusters) +{ + using namespace pcl::segmentation::grabcut; + clusters.clear (); + clusters.resize (2); + clusters[0].indices.reserve (indices_->size ()); + clusters[1].indices.reserve (indices_->size ()); + refine (); + assert (hard_segmentation_.size () == indices_->size ()); + const int indices_size = static_cast (indices_->size ()); + for (int i = 0; i < indices_size; ++i) + if (hard_segmentation_[i] == SegmentationForeground) + clusters[1].indices.push_back (i); + else + clusters[0].indices.push_back (i); +} + +#endif diff --git a/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp index f02b769a..76b6245f 100644 --- a/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/organized_connected_component_segmentation.hpp @@ -182,7 +182,7 @@ pcl::OrganizedConnectedComponentSegmentation::segment (pcl::Poi { if (labels[current_row + colIdx].label == invalid_label) labels[current_row + colIdx].label = labels[previous_row + colIdx].label; - else + else if (labels[previous_row + colIdx].label != invalid_label) { unsigned root1 = findRoot (run_ids, labels[current_row + colIdx].label); unsigned root2 = findRoot (run_ids, labels[previous_row + colIdx].label); diff --git a/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp index f564b328..211d44ce 100644 --- a/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp @@ -94,7 +94,7 @@ pcl::OrganizedMultiPlaneSegmentation::segment (std::ve // Check that we got the same number of points and normals if (static_cast (normals_->points.size ()) != static_cast (input_->points.size ())) { - PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n", + PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%lu) and normal cloud (%lu) do not match!\n", getClassName ().c_str (), input_->points.size (), normals_->points.size ()); return; diff --git a/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp b/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp new file mode 100644 index 00000000..eaadf65a --- /dev/null +++ b/segmentation/include/pcl/segmentation/impl/progressive_morphological_filter.hpp @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ +#define PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ProgressiveMorphologicalFilter::ProgressiveMorphologicalFilter () : + max_window_size_ (33), + slope_ (0.7f), + max_distance_ (10.0f), + initial_distance_ (0.15f), + cell_size_ (1.0f), + base_ (2.0f), + exponential_ (true) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ProgressiveMorphologicalFilter::~ProgressiveMorphologicalFilter () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ProgressiveMorphologicalFilter::extract (std::vector& ground) +{ + bool segmentation_is_possible = initCompute (); + if (!segmentation_is_possible) + { + deinitCompute (); + return; + } + + // Compute the series of window sizes and height thresholds + std::vector height_thresholds; + std::vector window_sizes; + int iteration = 0; + float window_size = 0.0f; + float height_threshold = 0.0f; + + while (window_size < max_window_size_) + { + // Determine the initial window size. + if (exponential_) + window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f); + else + window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f); + + // Calculate the height threshold to be used in the next iteration. + if (iteration == 0) + height_threshold = initial_distance_; + else + height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_; + + // Enforce max distance on height threshold + if (height_threshold > max_distance_) + height_threshold = max_distance_; + + window_sizes.push_back (window_size); + height_thresholds.push_back (height_threshold); + + iteration++; + } + + // Ground indices are initially limited to those points in the input cloud we + // wish to process + ground = *indices_; + + // Progressively filter ground returns using morphological open + for (size_t i = 0; i < window_sizes.size (); ++i) + { + PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f)...", + i, height_thresholds[i], window_sizes[i]); + + // Limit filtering to those points currently considered ground returns + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::copyPointCloud (*input_, ground, *cloud); + + // Create new cloud to hold the filtered results. Apply the morphological + // opening operation at the current window size. + typename pcl::PointCloud::Ptr cloud_f (new pcl::PointCloud); + pcl::applyMorphologicalOperator (cloud, window_sizes[i], MORPH_OPEN, *cloud_f); + + // Find indices of the points whose difference between the source and + // filtered point clouds is less than the current height threshold. + std::vector pt_indices; + for (size_t p_idx = 0; p_idx < ground.size (); ++p_idx) + { + float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z; + if (diff < height_thresholds[i]) + pt_indices.push_back (ground[p_idx]); + } + + // Ground is now limited to pt_indices + ground.swap (pt_indices); + + PCL_DEBUG ("ground now has %d points\n", ground.size ()); + } + + deinitCompute (); +} + +#define PCL_INSTANTIATE_ProgressiveMorphologicalFilter(T) template class pcl::ProgressiveMorphologicalFilter; + +#endif // PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + diff --git a/segmentation/include/pcl/segmentation/impl/region_growing.hpp b/segmentation/include/pcl/segmentation/impl/region_growing.hpp index 5d378729..a682fe85 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing.hpp @@ -289,8 +289,8 @@ pcl::RegionGrowing::extract (std::vector & c std::vector::iterator cluster_iter_input = clusters.begin (); for (std::vector::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++) { - if ((cluster_iter->indices.size () >= min_pts_per_cluster_) && - (cluster_iter->indices.size () <= max_pts_per_cluster_)) + if ((static_cast (cluster_iter->indices.size ()) >= min_pts_per_cluster_) && + (static_cast (cluster_iter->indices.size ()) <= max_pts_per_cluster_)) { *cluster_iter_input = *cluster_iter; cluster_iter_input++; @@ -358,13 +358,27 @@ pcl::RegionGrowing::findPointNeighbours () std::vector distances; point_neighbours_.resize (input_->points.size (), neighbours); - - for (int i_point = 0; i_point < point_number; i_point++) + if (input_->is_dense) { - int point_index = (*indices_)[i_point]; - neighbours.clear (); - search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); - point_neighbours_[point_index].swap (neighbours); + for (int i_point = 0; i_point < point_number; i_point++) + { + int point_index = (*indices_)[i_point]; + neighbours.clear (); + search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + } + } + else + { + for (int i_point = 0; i_point < point_number; i_point++) + { + neighbours.clear (); + int point_index = (*indices_)[i_point]; + if (!pcl::isFinite (input_->points[point_index])) + continue; + search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + } } } @@ -576,7 +590,7 @@ pcl::RegionGrowing::getSegmentFromPoint (int index, pcl::PointI // first of all we need to find out if this point belongs to cloud bool point_was_found = false; int number_of_points = static_cast (indices_->size ()); - for (size_t point = 0; point < number_of_points; point++) + for (int point = 0; point < number_of_points; point++) if ( (*indices_)[point] == index) { point_was_found = true; diff --git a/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp b/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp index cd6ef9d8..a49fa681 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp @@ -197,11 +197,12 @@ pcl::RegionGrowingRGB::extract (std::vector std::vector::iterator cluster_iter = clusters_.begin (); while (cluster_iter != clusters_.end ()) { - if (cluster_iter->indices.size () < min_pts_per_cluster_ || cluster_iter->indices.size () > max_pts_per_cluster_) + if (static_cast (cluster_iter->indices.size ()) < min_pts_per_cluster_ || + static_cast (cluster_iter->indices.size ()) > max_pts_per_cluster_) { cluster_iter = clusters_.erase (cluster_iter); } - else + else cluster_iter++; } @@ -462,7 +463,7 @@ pcl::RegionGrowingRGB::applyRegionMergingAlgorithm () int final_segment_number = homogeneous_region_number; for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) { - if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_) + if (static_cast (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_) { if ( region_neighbours[i_reg].empty () ) continue; @@ -584,15 +585,26 @@ pcl::RegionGrowingRGB::assembleRegions (std::vector::iterator i_region; - i_region = clusters_.begin (); - while(i_region != clusters_.end ()) + if (clusters_.empty ()) + return; + + std::vector::iterator itr1, itr2; + itr1 = clusters_.begin (); + itr2 = clusters_.end () - 1; + + while (itr1 < itr2) { - if ( i_region->indices.empty () ) - i_region = clusters_.erase (i_region); - else - i_region++; + while (!(itr1->indices.empty ()) && itr1 < itr2) + itr1++; + while ( itr2->indices.empty () && itr1 < itr2) + itr2--; + + if (itr1 != itr2) + itr1->indices.swap (itr2->indices); } + + if (itr2->indices.empty ()) + clusters_.erase (itr2, clusters_.end ()); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -690,7 +702,7 @@ pcl::RegionGrowingRGB::getSegmentFromPoint (int index, pcl::Poi // first of all we need to find out if this point belongs to cloud bool point_was_found = false; int number_of_points = static_cast (indices_->size ()); - for (size_t point = 0; point < number_of_points; point++) + for (int point = 0; point < number_of_points; point++) if ( (*indices_)[point] == index) { point_was_found = true; diff --git a/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp index be2e5be8..2980ee13 100644 --- a/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp @@ -52,7 +52,7 @@ pcl::seededHueSegmentation (const PointCloud { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false @@ -128,7 +128,7 @@ pcl::seededHueSegmentation (const PointCloud { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { - PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false diff --git a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp index 8b457b46..2c206f0e 100644 --- a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp +++ b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp @@ -39,7 +39,7 @@ #define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ #include -#include +#include ////////////////////////////////////////////////////////////////////////// template void @@ -64,7 +64,7 @@ pcl::getPointCloudDifference ( // Search for the closest point in the target data set (number of neighbors to find = 1) if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances)) { - PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z); + PCL_WARN ("No neighbor found for point %lu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z); continue; } @@ -85,11 +85,7 @@ pcl::getPointCloudDifference ( //output.is_dense = false; // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldList; - // Iterate over each point - for (size_t i = 0; i < src_indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (NdConcatenateFunctor (src.points[src_indices[i]], output.points[i])); + copyPointCloud (src, src_indices, output); } ////////////////////////////////////////////////////////////////////////// diff --git a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp index 6dfc91ab..1c7386f8 100644 --- a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp +++ b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp @@ -54,7 +54,7 @@ pcl::SupervoxelClustering::SupervoxelClustering (float voxel_resolution, normal_importance_(1.0f), label_colors_ (0) { - adjacency_octree_ = boost::make_shared (resolution_); + adjacency_octree_.reset (new OctreeAdjacencyT (resolution_)); if (use_single_camera_transform) adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction, this, _1)); } @@ -68,7 +68,7 @@ pcl::SupervoxelClustering::~SupervoxelClustering () ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::SupervoxelClustering::setInputCloud (typename pcl::PointCloud::ConstPtr cloud) +pcl::SupervoxelClustering::setInputCloud (const typename pcl::PointCloud::ConstPtr& cloud) { if ( cloud->size () == 0 ) { @@ -205,7 +205,7 @@ pcl::SupervoxelClustering::prepareForSegmentation () template void pcl::SupervoxelClustering::computeVoxelData () { - voxel_centroid_cloud_ = boost::make_shared (); + voxel_centroid_cloud_.reset (new PointCloudT); voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ()); typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin (); typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin (); @@ -326,7 +326,7 @@ pcl::SupervoxelClustering::makeSupervoxels (std::mapgetLabel (); - supervoxel_clusters[label] = boost::make_shared > (); + supervoxel_clusters[label].reset (new Supervoxel); sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z); sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba); sv_itr->getNormal (supervoxel_clusters[label]->normal_); @@ -344,7 +344,7 @@ pcl::SupervoxelClustering::createSupervoxelHelpers (std::vector::selectInitialSupervoxelSeeds (std::vector >(); + voxel_kdtree_.reset (new pcl::search::KdTree); voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_); } @@ -403,7 +403,7 @@ pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vectorradiusSearch (seed_indices_orig[i], search_radius , neighbors, sqr_distances); int min_index = seed_indices_orig[i]; @@ -548,7 +548,7 @@ pcl::SupervoxelClustering::getSupervoxelAdjacency (std::multimap pcl::PointCloud::Ptr pcl::SupervoxelClustering::getColoredCloud () const { - pcl::PointCloud::Ptr colored_cloud = boost::make_shared >(); + pcl::PointCloud::Ptr colored_cloud (new pcl::PointCloud); pcl::copyPointCloud (*input_,*colored_cloud); pcl::PointCloud ::iterator i_colored; @@ -578,7 +578,7 @@ pcl::SupervoxelClustering::getColoredCloud () const template pcl::PointCloud::Ptr pcl::SupervoxelClustering::getColoredVoxelCloud () const { - pcl::PointCloud::Ptr colored_cloud = boost::make_shared< pcl::PointCloud > (); + pcl::PointCloud::Ptr colored_cloud (new pcl::PointCloud); for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) { typename PointCloudT::Ptr voxels; @@ -600,7 +600,7 @@ pcl::SupervoxelClustering::getColoredVoxelCloud () const template typename pcl::PointCloud::Ptr pcl::SupervoxelClustering::getVoxelCentroidCloud () const { - typename PointCloudT::Ptr centroid_copy = boost::make_shared (); + typename PointCloudT::Ptr centroid_copy (new PointCloudT); copyPointCloud (*voxel_centroid_cloud_, *centroid_copy); return centroid_copy; } @@ -609,7 +609,7 @@ pcl::SupervoxelClustering::getVoxelCentroidCloud () const template pcl::PointCloud::Ptr pcl::SupervoxelClustering::getLabeledVoxelCloud () const { - pcl::PointCloud::Ptr labeled_voxel_cloud = boost::make_shared< pcl::PointCloud > (); + pcl::PointCloud::Ptr labeled_voxel_cloud (new pcl::PointCloud); for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) { typename PointCloudT::Ptr voxels; @@ -631,7 +631,7 @@ pcl::SupervoxelClustering::getLabeledVoxelCloud () const template pcl::PointCloud::Ptr pcl::SupervoxelClustering::getLabeledCloud () const { - pcl::PointCloud::Ptr labeled_cloud = boost::make_shared >(); + pcl::PointCloud::Ptr labeled_cloud (new pcl::PointCloud); pcl::copyPointCloud (*input_,*labeled_cloud); pcl::PointCloud ::iterator i_labeled; @@ -661,7 +661,7 @@ pcl::SupervoxelClustering::getLabeledCloud () const template pcl::PointCloud::Ptr pcl::SupervoxelClustering::makeSupervoxelNormalCloud (std::map::Ptr > &supervoxel_clusters) { - pcl::PointCloud::Ptr normal_cloud = boost::make_shared > (); + pcl::PointCloud::Ptr normal_cloud (new pcl::PointCloud); normal_cloud->resize (supervoxel_clusters.size ()); typename std::map ::Ptr>::iterator sv_itr,sv_itr_end; sv_itr = supervoxel_clusters.begin (); @@ -730,9 +730,9 @@ pcl::SupervoxelClustering::setNormalImportance (float val) template void pcl::SupervoxelClustering::initializeLabelColors () { - int max_label = getMaxLabel (); + uint32_t max_label = static_cast (getMaxLabel ()); //If we already have enough colors, return - if (label_colors_.size () > max_label) + if (label_colors_.size () > max_label) return; //Otherwise, generate new colors until we have enough @@ -821,9 +821,30 @@ namespace pcl data_.xyz_[1] /= (static_cast (num_points_)); data_.xyz_[2] /= (static_cast (num_points_)); } - + + //Explicit overloads for XYZ types + template<> + void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::addPoint (const pcl::PointXYZ &new_point) + { + ++num_points_; + //Same as before here + data_.xyz_[0] += new_point.x; + data_.xyz_[1] += new_point.y; + data_.xyz_[2] += new_point.z; + } + + template<> void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::computeData () + { + data_.xyz_[0] /= (static_cast (num_points_)); + data_.xyz_[1] /= (static_cast (num_points_)); + data_.xyz_[2] /= (static_cast (num_points_)); + } + } } + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -1016,12 +1037,14 @@ pcl::SupervoxelClustering::SupervoxelHelper::updateCentroid () template void pcl::SupervoxelClustering::SupervoxelHelper::getVoxels (typename pcl::PointCloud::Ptr &voxels) const { - voxels = boost::make_shared > (); + voxels.reset (new pcl::PointCloud); voxels->clear (); voxels->resize (leaves_.size ()); typename pcl::PointCloud::iterator voxel_itr = voxels->begin (); - typename std::set::iterator leaf_itr; - for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr) + //typename std::set::iterator leaf_itr; + for (typename std::set::const_iterator leaf_itr = leaves_.begin (); + leaf_itr != leaves_.end (); + ++leaf_itr, ++voxel_itr) { const VoxelData& leaf_data = (*leaf_itr)->getData (); leaf_data.getPoint (*voxel_itr); @@ -1032,10 +1055,10 @@ pcl::SupervoxelClustering::SupervoxelHelper::getVoxels (typename pcl::Po template void pcl::SupervoxelClustering::SupervoxelHelper::getNormals (typename pcl::PointCloud::Ptr &normals) const { - normals = boost::make_shared > (); + normals.reset (new pcl::PointCloud); normals->clear (); normals->resize (leaves_.size ()); - typename std::set::iterator leaf_itr; + typename std::set::const_iterator leaf_itr; typename pcl::PointCloud::iterator normal_itr = normals->begin (); for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr) { @@ -1050,7 +1073,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::getNeighborLabels (std::set { neighbor_labels.clear (); //For each leaf belonging to this supervoxel - typename std::set::iterator leaf_itr; + typename std::set::const_iterator leaf_itr; for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { //for each neighbor of the leaf diff --git a/segmentation/include/pcl/segmentation/progressive_morphological_filter.h b/segmentation/include/pcl/segmentation/progressive_morphological_filter.h new file mode 100644 index 00000000..5935e8a7 --- /dev/null +++ b/segmentation/include/pcl/segmentation/progressive_morphological_filter.h @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_PROGRESSIVE_MORPHOLOGICAL_FILTER_H_ +#define PCL_PROGRESSIVE_MORPHOLOGICAL_FILTER_H_ + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the Progressive Morphological Filter for segmentation of ground points. + * Description can be found in the article + * "A Progressive Morphological Filter for Removing Nonground Measurements from + * Airborne LIDAR Data" + * by K. Zhang, S. Chen, D. Whitman, M. Shyu, J. Yan, and C. Zhang. + */ + template + class PCL_EXPORTS ProgressiveMorphologicalFilter : public pcl::PCLBase + { + public: + + typedef pcl::PointCloud PointCloud; + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + public: + + /** \brief Constructor that sets default values for member variables. */ + ProgressiveMorphologicalFilter (); + + virtual + ~ProgressiveMorphologicalFilter (); + + /** \brief Get the maximum window size to be used in filtering ground returns. */ + inline int + getMaxWindowSize () const { return (max_window_size_); } + + /** \brief Set the maximum window size to be used in filtering ground returns. */ + inline void + setMaxWindowSize (int max_window_size) { max_window_size_ = max_window_size; } + + /** \brief Get the slope value to be used in computing the height threshold. */ + inline float + getSlope () const { return (slope_); } + + /** \brief Set the slope value to be used in computing the height threshold. */ + inline void + setSlope (float slope) { slope_ = slope; } + + /** \brief Get the maximum height above the parameterized ground surface to be considered a ground return. */ + inline float + getMaxDistance () const { return (max_distance_); } + + /** \brief Set the maximum height above the parameterized ground surface to be considered a ground return. */ + inline void + setMaxDistance (float max_distance) { max_distance_ = max_distance; } + + /** \brief Get the initial height above the parameterized ground surface to be considered a ground return. */ + inline float + getInitialDistance () const { return (initial_distance_); } + + /** \brief Set the initial height above the parameterized ground surface to be considered a ground return. */ + inline void + setInitialDistance (float initial_distance) { initial_distance_ = initial_distance; } + + /** \brief Get the cell size. */ + inline float + getCellSize () const { return (cell_size_); } + + /** \brief Set the cell size. */ + inline void + setCellSize (float cell_size) { cell_size_ = cell_size; } + + /** \brief Get the base to be used in computing progressive window sizes. */ + inline float + getBase () const { return (base_); } + + /** \brief Set the base to be used in computing progressive window sizes. */ + inline void + setBase (float base) { base_ = base; } + + /** \brief Get flag indicating whether or not to exponentially grow window sizes? */ + inline bool + getExponential () const { return (exponential_); } + + /** \brief Set flag indicating whether or not to exponentially grow window sizes? */ + inline void + setExponential (bool exponential) { exponential_ = exponential; } + + /** \brief This method launches the segmentation algorithm and returns indices of + * points determined to be ground returns. + * \param[out] ground indices of points determined to be ground returns. + */ + virtual void + extract (std::vector& ground); + + protected: + + /** \brief Maximum window size to be used in filtering ground returns. */ + int max_window_size_; + + /** \brief Slope value to be used in computing the height threshold. */ + float slope_; + + /** \brief Maximum height above the parameterized ground surface to be considered a ground return. */ + float max_distance_; + + /** \brief Initial height above the parameterized ground surface to be considered a ground return. */ + float initial_distance_; + + /** \brief Cell size. */ + float cell_size_; + + /** \brief Base to be used in computing progressive window sizes. */ + float base_; + + /** \brief Exponentially grow window sizes? */ + bool exponential_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif + diff --git a/segmentation/include/pcl/segmentation/region_3d.h b/segmentation/include/pcl/segmentation/region_3d.h index 98db59c5..52df2e3e 100644 --- a/segmentation/include/pcl/segmentation/region_3d.h +++ b/segmentation/include/pcl/segmentation/region_3d.h @@ -89,6 +89,20 @@ namespace pcl return (count_); } + /** \brief Get the curvature of the region. */ + float + getCurvature () const + { + return (curvature_); + } + + /** \brief Set the curvature of the region. */ + void + setCurvature (float curvature) + { + curvature_ = curvature; + } + protected: /** \brief The centroid of the region. */ Eigen::Vector3f centroid_; @@ -99,6 +113,9 @@ namespace pcl /** \brief The number of points in the region. */ unsigned count_; + /** \brief The mean curvature of the region. */ + float curvature_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/segmentation/include/pcl/segmentation/region_growing.h b/segmentation/include/pcl/segmentation/region_growing.h index e2f30c40..7a5afff8 100644 --- a/segmentation/include/pcl/segmentation/region_growing.h +++ b/segmentation/include/pcl/segmentation/region_growing.h @@ -185,7 +185,7 @@ namespace pcl getSearchMethod () const; /** \brief Allows to set search method that will be used for finding KNN. - * \param[in] search search method to use + * \param[in] tree pointer to a KdTree */ void setSearchMethod (const KdTreePtr& tree); @@ -272,7 +272,6 @@ namespace pcl validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const; /** \brief This function simply assembles the regions from list of point labels. - * \param[out] clusters clusters that were obtained during the segmentation process. * Each cluster is an array of point indices. */ void diff --git a/segmentation/include/pcl/segmentation/region_growing_rgb.h b/segmentation/include/pcl/segmentation/region_growing_rgb.h index d1addc38..e4cee106 100644 --- a/segmentation/include/pcl/segmentation/region_growing_rgb.h +++ b/segmentation/include/pcl/segmentation/region_growing_rgb.h @@ -171,6 +171,7 @@ namespace pcl /** \brief For a given point this function builds a segment to which it belongs and returns this segment. * \param[in] index index of the initial point which will be the seed for growing a segment. + * \param cluster */ virtual void getSegmentFromPoint (int index, pcl::PointIndices& cluster); diff --git a/segmentation/include/pcl/segmentation/sac_segmentation.h b/segmentation/include/pcl/segmentation/sac_segmentation.h index e8e030f6..9db82f2e 100644 --- a/segmentation/include/pcl/segmentation/sac_segmentation.h +++ b/segmentation/include/pcl/segmentation/sac_segmentation.h @@ -197,6 +197,7 @@ namespace pcl /** \brief Set the maximum distance allowed when drawing random samples * \param[in] radius the maximum distance (L2 norm) + * \param search */ inline void setSamplesMaxDist (const double &radius, SearchPtr search) @@ -369,7 +370,8 @@ namespace pcl getNormalDistanceWeight () const { return (distance_weight_); } /** \brief Set the minimum opning angle for a cone model. - * \param oa the opening angle which we need minumum to validate a cone model. + * \param min_angle the opening angle which we need minumum to validate a cone model. + * \param max_angle the opening angle which we need maximum to validate a cone model. */ inline void setMinMaxOpeningAngle (const double &min_angle, const double &max_angle) diff --git a/segmentation/include/pcl/segmentation/supervoxel_clustering.h b/segmentation/include/pcl/segmentation/supervoxel_clustering.h index c3d600c0..09ea59df 100644 --- a/segmentation/include/pcl/segmentation/supervoxel_clustering.h +++ b/segmentation/include/pcl/segmentation/supervoxel_clustering.h @@ -236,10 +236,10 @@ namespace pcl * \param[in] cloud The cloud to be supervoxelize */ virtual void - setInputCloud (typename pcl::PointCloud::ConstPtr cloud); + setInputCloud (const typename pcl::PointCloud::ConstPtr& cloud); /** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud) - * \param[in] cloud The input normals + * \param[in] normal_cloud The input normals */ virtual void setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud); @@ -477,16 +477,20 @@ namespace pcl size_t size () const { return leaves_.size (); } - private: + private: //Stores leaves std::set leaves_; uint32_t label_; VoxelData centroid_; SupervoxelClustering* parent_; - - + public: + //Type VoxelData may have fixed-size Eigen objects inside + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; + //Make boost::ptr_list can access the private class SupervoxelHelper + friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering::SupervoxelHelper *); + typedef boost::ptr_list HelperListT; HelperListT supervoxel_helpers_; diff --git a/segmentation/src/approximate_progressive_morphological_filter.cpp b/segmentation/src/approximate_progressive_morphological_filter.cpp new file mode 100644 index 00000000..6ff3c8ae --- /dev/null +++ b/segmentation/src/approximate_progressive_morphological_filter.cpp @@ -0,0 +1,53 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include +#include +#include +#include + +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE(ApproximateProgressiveMorphologicalFilter, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)) +#else + PCL_INSTANTIATE(ApproximateProgressiveMorphologicalFilter, PCL_XYZ_POINT_TYPES) +#endif + diff --git a/segmentation/src/grabcut.cpp b/segmentation/src/grabcut.cpp deleted file mode 100644 index 48cdd2bc..00000000 --- a/segmentation/src/grabcut.cpp +++ /dev/null @@ -1,845 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * - */ - -#include - -#include -#include -#include -#include -#include - -pcl::segmentation::grabcut::BoykovKolmogorov::BoykovKolmogorov (std::size_t max_nodes) - : flow_value_(0.0) -{ - if (max_nodes > 0) - { - source_edges_.reserve (max_nodes); - target_edges_.reserve (max_nodes); - nodes_.reserve (max_nodes); - } -} - -double -pcl::segmentation::grabcut::BoykovKolmogorov::operator() (int u, int v) const -{ - if ((u < 0) && (v < 0)) return flow_value_; - if (u < 0) { return source_edges_[v]; } - if (v < 0) { return target_edges_[u]; } - capacitated_edge::const_iterator it = nodes_[u].find (v); - if (it == nodes_[u].end ()) return 0.0; - return it->second; -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::preAugmentPaths () -{ - for (int u = 0; u < (int)nodes_.size (); u++) - { - // augment s-u-t paths - if ((source_edges_[u] > 0.0) && (target_edges_[u] > 0.0)) - { - const double cap = std::min (source_edges_[u], target_edges_[u]); - flow_value_ += cap; - source_edges_[u] -= cap; - target_edges_[u] -= cap; - } - - if (source_edges_[u] == 0.0) continue; - - // augment s-u-v-t paths - for (std::map::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) - { - const int v = it->first; - if ((it->second == 0.0) || (target_edges_[v] == 0.0)) continue; - const double w = std::min (it->second, std::min (source_edges_[u], target_edges_[v])); - source_edges_[u] -= w; - target_edges_[v] -= w; - it->second -= w; - nodes_[v][u] += w; - flow_value_ += w; - if (source_edges_[u] == 0.0) break; - } - } -} - -int -pcl::segmentation::grabcut::BoykovKolmogorov::addNodes (size_t n) -{ - int node_id = (int)nodes_.size (); - nodes_.resize (nodes_.size () + n); - source_edges_.resize (nodes_.size (), 0.0); - target_edges_.resize (nodes_.size (), 0.0); - return (node_id); -} - -// void -// pcl::segmentation::grabcut::BoykovKolmogorov::addSourceAndTargetEdges (int u, double source_cap, double sink_cap) -// { -// addSourceEdge (u, source_cap); -// addTargetEdge (u, sink_cap); - -// } - -void -pcl::segmentation::grabcut::BoykovKolmogorov::addSourceEdge (int u, double cap) -{ - assert ((u >= 0) && (u < (int)nodes_.size ())); - if (cap < 0.0) - { - flow_value_ += cap; - target_edges_[u] -= cap; - } - else - source_edges_[u] += cap; -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::addTargetEdge (int u, double cap) -{ - assert ((u >= 0) && (u < (int)nodes_.size ())); - if (cap < 0.0) - { - flow_value_ += cap; - source_edges_[u] -= cap; - } - else - target_edges_[u] += cap; -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::addEdge (int u, int v, double cap_uv, double cap_vu) -{ - assert ((u >= 0) && (u < (int)nodes_.size ())); - assert ((v >= 0) && (v < (int)nodes_.size ())); - assert (u != v); - - capacitated_edge::iterator it = nodes_[u].find (v); - if (it == nodes_[u].end ()) - { - assert (cap_uv + cap_vu >= 0.0); - if (cap_uv < 0.0) - { - nodes_[u].insert (std::make_pair (v, 0.0)); - nodes_[v].insert (std::make_pair (u, cap_vu + cap_uv)); - source_edges_[u] -= cap_uv; - target_edges_[v] -= cap_uv; - flow_value_ += cap_uv; - } - else - { - if (cap_vu < 0.0) - { - nodes_[u].insert (std::make_pair (v, cap_uv + cap_vu)); - nodes_[v].insert (std::make_pair (u, 0.0)); - source_edges_[v] -= cap_vu; - target_edges_[u] -= cap_vu; - flow_value_ += cap_vu; - } - else - { - nodes_[u].insert (std::make_pair (v, cap_uv)); - nodes_[v].insert (std::make_pair (u, cap_vu)); - } - } - } - else - { - capacitated_edge::iterator jt = nodes_[v].find (u); - it->second += cap_uv; - jt->second += cap_vu; - assert (it->second + jt->second >= 0.0); - if (it->second < 0.0) - { - jt->second += it->second; - source_edges_[u] -= it->second; - target_edges_[v] -= it->second; - flow_value_ += it->second; - it->second = 0.0; - } - else - { - if (jt->second < 0.0) - { - it->second += jt->second; - source_edges_[v] -= jt->second; - target_edges_[u] -= jt->second; - flow_value_ += jt->second; - jt->second = 0.0; - } - } - } -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::reset () -{ - flow_value_ = 0.0; - std::fill (source_edges_.begin (), source_edges_.end (), 0.0); - std::fill (target_edges_.begin (), target_edges_.end (), 0.0); - for (int u = 0; u < (int)nodes_.size (); u++) - { - for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) - { - it->second = 0.0; - } - } - std::fill (cut_.begin (), cut_.end (), FREE); - parents_.clear (); - clearActive (); -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::clear () -{ - flow_value_ = 0.0; - source_edges_.clear (); - target_edges_.clear (); - nodes_.clear (); - cut_.clear (); - parents_.clear (); - clearActive (); -} - -double -pcl::segmentation::grabcut::BoykovKolmogorov::solve () -{ - // initialize search tree and active set - cut_.resize (nodes_.size ()); - std::fill (cut_.begin (), cut_.end (), FREE); - parents_.resize (nodes_.size ()); - - clearActive (); - - // pre-augment paths - preAugmentPaths (); - - // initialize search trees - initializeTrees (); - - std::deque orphans; - while (!isActiveSetEmpty ()) - { - const std::pair path = expandTrees (); - augmentPath (path, orphans); - if (!orphans.empty ()) - { - adoptOrphans (orphans); - } - } - return (flow_value_); -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::initializeTrees () -{ - // initialize search tree - for (int u = 0; u < (int)nodes_.size (); u++) - { - if (source_edges_[u] > 0.0) - { - cut_[u] = SOURCE; - parents_[u].first = TERMINAL; - markActive (u); - } - else - { - if (target_edges_[u] > 0.0) - { - cut_[u] = TARGET; - parents_[u].first = TERMINAL; - markActive (u); - } - } - } -} - -std::pair -pcl::segmentation::grabcut::BoykovKolmogorov::expandTrees () -{ - // expand trees looking for augmenting paths - while (!isActiveSetEmpty ()) - { - const int u = active_head_; - - if (cut_[u] == SOURCE) { - for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) - { - if (it->second > 0.0) - { - if (cut_[it->first] == FREE) - { - cut_[it->first] = SOURCE; - parents_[it->first] = std::make_pair (u, std::make_pair (it, nodes_[it->first].find (u))); - markActive (it->first); - } - else - { - if (cut_[it->first] == TARGET) - { - // found augmenting path - return (std::make_pair (u, it->first)); - } - } - } - } - } - else - { - for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) - { - if (cut_[it->first] == TARGET) continue; - if (nodes_[it->first][u] > 0.0) - { - if (cut_[it->first] == FREE) - { - cut_[it->first] = TARGET; - parents_[it->first] = std::make_pair (u, std::make_pair (nodes_[it->first].find (u), it)); - markActive (it->first); - } - else - { - if (cut_[it->first] == SOURCE) - { - // found augmenting path - return (std::make_pair (it->first, u)); - } - } - } - } - } - - // remove node from active set - markInactive (u); - } - - return (std::make_pair (TERMINAL, TERMINAL)); -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::augmentPath (const std::pair& path, std::deque& orphans) -{ - if ((path.first == TERMINAL) && (path.second == TERMINAL)) - return; - - // find path capacity - - // backtrack - const edge_pair e = std::make_pair (nodes_[path.first].find (path.second), - nodes_[path.second].find (path.first)); - double c = e.first->second; - int u = path.first; - while (parents_[u].first != TERMINAL) - { - c = std::min (c, parents_[u].second.first->second); - u = parents_[u].first; - //assert (cut_[u] == SOURCE); - } - c = std::min (c, source_edges_[u]); - - // forward track - u = path.second; - while (parents_[u].first != TERMINAL) - { - c = std::min (c, parents_[u].second.first->second); - u = parents_[u].first; - //assert (cut_[u] == TARGET); - } - c = std::min (c, target_edges_[u]); - - // augment path - flow_value_ += c; - //DRWN_LOG_DEBUG ("path capacity: " << c); - - // backtrack - u = path.first; - while (parents_[u].first != TERMINAL) - { - //nodes_[u][parents_[u].first] += c; - parents_[u].second.second->second += c; - parents_[u].second.first->second -= c; - if (parents_[u].second.first->second == 0.0) - { - orphans.push_front (u); - } - u = parents_[u].first; - } - source_edges_[u] -= c; - if (source_edges_[u] == 0.0) - { - orphans.push_front (u); - } - - // link - e.first->second -= c; - e.second->second += c; - - // forward track - u = path.second; - while (parents_[u].first != TERMINAL) { - //nodes_[parents_[u].first][u] += c; - parents_[u].second.second->second += c; - parents_[u].second.first->second -= c; - if (parents_[u].second.first->second == 0.0) { - orphans.push_back (u); - } - u = parents_[u].first; - } - target_edges_[u] -= c; - if (target_edges_[u] == 0.0) { - orphans.push_back (u); - } -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::adoptOrphans (std::deque& orphans) -{ - // find new parent for orphaned subtree or free it - while (!orphans.empty ()) - { - const int u = orphans.front (); - const char tree_label = cut_[u]; - orphans.pop_front (); - - // can occur if same node is inserted into orphans multiple times - if (tree_label == FREE) continue; - //assert (tree_label != FREE); - - // look for new parent - bool b_free_orphan = true; - for (capacitated_edge::iterator jt = nodes_[u].begin (); jt != nodes_[u].end (); ++jt) { - // skip if different trees - if (cut_[jt->first] != tree_label) continue; - - // check edge capacity - const capacitated_edge::iterator kt = nodes_[jt->first].find (u); - if (((tree_label == TARGET) && (jt->second <= 0.0)) || - ((tree_label == SOURCE) && (kt->second <= 0.0))) - continue; - - // check that u is not an ancestor of jt->first - int v = jt->first; - while ((v != u) && (v != TERMINAL)) - { - v = parents_[v].first; - } - if (v != TERMINAL) continue; - - // add as parent - const edge_pair e = (tree_label == SOURCE) ? std::make_pair (kt, jt) : std::make_pair (jt, kt); - parents_[u] = std::make_pair (jt->first, e); - b_free_orphan = false; - break; - } - - // free the orphan subtree and remove it from the active set - if (b_free_orphan) - { - for (capacitated_edge::const_iterator jt = nodes_[u].begin (); jt != nodes_[u].end (); ++jt) - { - if ((cut_[jt->first] == tree_label) && (parents_[jt->first].first == u)) - { - orphans.push_front (jt->first); - markActive (jt->first); - } - else if (cut_[jt->first] != FREE) - { - markActive (jt->first); - } - } - - // mark inactive and free - markInactive (u); - cut_[u] = FREE; - } - } -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::clearActive () -{ - active_head_ = active_tail_ = TERMINAL; - active_list_.resize (nodes_.size ()); - std::fill (active_list_.begin (), active_list_.end (), std::make_pair (TERMINAL, TERMINAL)); -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::markActive (int u) -{ - if (isActive (u)) return; - - active_list_[u].first = active_tail_; - active_list_[u].second = TERMINAL; - if (active_tail_ == TERMINAL) - active_head_ = u; - else - active_list_[active_tail_].second = u; - active_tail_ = u; -} - -void -pcl::segmentation::grabcut::BoykovKolmogorov::markInactive (int u) -{ - //if (!isActive (u)) return; - - if (u == active_head_) - { - active_head_ = active_list_[u].second; - if (u != active_tail_) - { - active_list_[active_list_[u].second].first = TERMINAL; - } - } - else - if (u == active_tail_) - { - active_tail_ = active_list_[u].first; - active_list_[active_list_[u].first].second = TERMINAL; - } - else - if (active_list_[u].first != TERMINAL) - { - active_list_[active_list_[u].first].second = active_list_[u].second; - active_list_[active_list_[u].second].first = active_list_[u].first; - } - //active_list_[u] = std::make_pair (TERMINAL, TERMINAL); - active_list_[u].first = TERMINAL; -} - -void -pcl::segmentation::grabcut::GaussianFitter::add (const Color &c) -{ - sum_[0] += c.r; sum_[1] += c.g; sum_[2] += c.b; - accumulator_ (0,0) += c.r*c.r; accumulator_ (0,1) += c.r*c.g; accumulator_ (0,2) += c.r*c.b; - accumulator_ (1,0) += c.g*c.r; accumulator_ (1,1) += c.g*c.g; accumulator_ (1,2) += c.g*c.b; - accumulator_ (2,0) += c.b*c.r; accumulator_ (2,1) += c.b*c.g; accumulator_ (2,2) += c.b*c.b; - - ++count_; -} - -// Build the gaussian out of all the added colors -void -pcl::segmentation::grabcut::GaussianFitter::fit (Gaussian& g, std::size_t total_count, bool compute_eigens) const -{ - if (count_==0) - { - g.pi = 0; - } - else - { - const float count_f = static_cast (count_); - - // Compute mean of gaussian - g.mu.r = sum_[0]/count_f; - g.mu.g = sum_[1]/count_f; - g.mu.b = sum_[2]/count_f; - - // Compute covariance matrix - g.covariance (0,0) = accumulator_ (0,0)/count_f - g.mu.r*g.mu.r + epsilon_; - g.covariance (0,1) = accumulator_ (0,1)/count_f - g.mu.r*g.mu.g; - g.covariance (0,2) = accumulator_ (0,2)/count_f - g.mu.r*g.mu.b; - g.covariance (1,0) = accumulator_ (1,0)/count_f - g.mu.g*g.mu.r; - g.covariance (1,1) = accumulator_ (1,1)/count_f - g.mu.g*g.mu.g + epsilon_; - g.covariance (1,2) = accumulator_ (1,2)/count_f - g.mu.g*g.mu.b; - g.covariance (2,0) = accumulator_ (2,0)/count_f - g.mu.b*g.mu.r; - g.covariance (2,1) = accumulator_ (2,1)/count_f - g.mu.b*g.mu.g; - g.covariance (2,2) = accumulator_ (2,2)/count_f - g.mu.b*g.mu.b + epsilon_; - - // Compute determinant of covariance matrix - g.determinant = g.covariance (0,0)*(g.covariance (1,1)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,1)) - - g.covariance (0,1)*(g.covariance (1,0)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,0)) - + g.covariance (0,2)*(g.covariance (1,0)*g.covariance (2,1) - g.covariance (1,1)*g.covariance (2,0)); - - // Compute inverse (cofactor matrix divided by determinant) - g.inverse (0,0) = (g.covariance (1,1)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,1)) / g.determinant; - g.inverse (1,0) = -(g.covariance (1,0)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,0)) / g.determinant; - g.inverse (2,0) = (g.covariance (1,0)*g.covariance (2,1) - g.covariance (1,1)*g.covariance (2,0)) / g.determinant; - g.inverse (0,1) = -(g.covariance (0,1)*g.covariance (2,2) - g.covariance (0,2)*g.covariance (2,1)) / g.determinant; - g.inverse (1,1) = (g.covariance (0,0)*g.covariance (2,2) - g.covariance (0,2)*g.covariance (2,0)) / g.determinant; - g.inverse (2,1) = -(g.covariance (0,0)*g.covariance (2,1) - g.covariance (0,1)*g.covariance (2,0)) / g.determinant; - g.inverse (0,2) = (g.covariance (0,1)*g.covariance (1,2) - g.covariance (0,2)*g.covariance (1,1)) / g.determinant; - g.inverse (1,2) = -(g.covariance (0,0)*g.covariance (1,2) - g.covariance (0,2)*g.covariance (1,0)) / g.determinant; - g.inverse (2,2) = (g.covariance (0,0)*g.covariance (1,1) - g.covariance (0,1)*g.covariance (1,0)) / g.determinant; - - // The weight of the gaussian is the fraction of the number of pixels in this Gaussian to the number - // of pixels in all the gaussians of this GMM. - g.pi = count_f / static_cast (total_count); - - if (compute_eigens) - { - // Compute eigenvalues and vectors using SVD - Eigen::JacobiSVD svd (g.covariance, Eigen::ComputeFullU); - // Store highest eigenvalue - g.eigenvalue = svd.singularValues ()[0]; - // Store corresponding eigenvector - g.eigenvector = svd.matrixU ().col (0); - } - } -} - -float -pcl::segmentation::grabcut::GMM::probabilityDensity (const Color &c) -{ - float result = 0; - - for (std::size_t i=0; i < gaussians_.size (); ++i) - result += gaussians_[i].pi * probabilityDensity (i, c); - - return (result); -} - -float -pcl::segmentation::grabcut::GMM::probabilityDensity (std::size_t i, const Color &c) -{ - float result = 0; - const pcl::segmentation::grabcut::Gaussian &G = gaussians_[i]; - if (G.pi > 0 ) - { - if (G.determinant > 0) - { - float r = c.r - G.mu.r; - float g = c.g - G.mu.g; - float b = c.b - G.mu.b; - - float d = r * (r*G.inverse (0,0) + g*G.inverse (1,0) + b*G.inverse (2,0)) + - g * (r*G.inverse (0,1) + g*G.inverse (1,1) + b*G.inverse (2,1)) + - b * (r*G.inverse (0,2) + g*G.inverse (1,2) + b*G.inverse (2,2)); - - result = static_cast (1.0/(sqrt (G.determinant)) * exp (-0.5*d)); - } - } - - return (result); -} - -void -pcl::segmentation::grabcut::buildGMMs (const Image& image, - const std::vector& indices, - const std::vector& hard_segmentation, - std::vector& components, - GMM& background_GMM, GMM& foreground_GMM) -{ - // Step 3: Build GMMs using Orchard-Bouman clustering algorithm - - // Set up Gaussian Fitters - std::vector back_fitters (background_GMM.getK ()); - std::vector fore_fitters (foreground_GMM.getK ()); - - std::size_t fore_count = 0, back_count = 0; - const int indices_size = static_cast (indices.size ()); - // Initialize the first foreground and background clusters - for (int idx = 0; idx < indices_size; ++idx) - { - components [idx] = 0; - - if (hard_segmentation [idx] == SegmentationForeground) - { - fore_fitters[0].add (image[indices[idx]]); - fore_count++; - } - else - { - back_fitters[0].add (image[indices[idx]]); - back_count++; - } - } - - back_fitters[0].fit (background_GMM[0], back_count, true); - fore_fitters[0].fit (foreground_GMM[0], fore_count, true); - - std::size_t n_back = 0, n_fore = 0; // Which cluster will be split - std::size_t max_K = (background_GMM.getK () > foreground_GMM.getK ()) ? background_GMM.getK () : foreground_GMM.getK (); - - // Compute clusters - for (std::size_t i = 1; i < max_K; ++i) - { - // Reset the fitters for the splitting clusters - back_fitters[n_back] = GaussianFitter (); - fore_fitters[n_fore] = GaussianFitter (); - - // For brevity, get references to the splitting Gaussians - Gaussian& bg = background_GMM[n_back]; - Gaussian& fg = foreground_GMM[n_fore]; - - // Compute splitting points - float split_background = bg.eigenvector[0] * bg.mu.r + bg.eigenvector[1] * bg.mu.g + bg.eigenvector[2] * bg.mu.b; - float split_foreground = fg.eigenvector[0] * fg.mu.r + fg.eigenvector[1] * fg.mu.g + fg.eigenvector[2] * fg.mu.b; - - // Split clusters nBack and nFore, place split portion into cluster i - for (int idx = 0; idx < indices_size; ++idx) - { - const Color &c = image[indices[idx]]; - - // For each pixel - if (i < foreground_GMM.getK () && - hard_segmentation[idx] == SegmentationForeground && - components[idx] == n_fore) - { - if (fg.eigenvector[0] * c.r + fg.eigenvector[1] * c.g + fg.eigenvector[2] * c.b > split_foreground) - { - components[idx] = i; - fore_fitters[i].add (c); - } - else - { - fore_fitters[n_fore].add (c); - } - } - else if (i < background_GMM.getK () && - hard_segmentation[idx] == SegmentationBackground && - components[idx] == n_back) - { - if (bg.eigenvector[0] * c.r + bg.eigenvector[1] * c.g + bg.eigenvector[2] * c.b > split_background) - { - components[idx] = i; - back_fitters[i].add (c); - } - else - { - back_fitters[n_back].add (c); - } - } - } - - // Compute new split Gaussians - back_fitters[n_back].fit (background_GMM[n_back], back_count, true); - fore_fitters[n_fore].fit (foreground_GMM[n_fore], fore_count, true); - - if (i < background_GMM.getK ()) - back_fitters[i].fit (background_GMM[i], back_count, true); - if (i < foreground_GMM.getK ()) - fore_fitters[i].fit (foreground_GMM[i], fore_count, true); - - // Find clusters with highest eigenvalue - n_back = 0; - n_fore = 0; - - for (std::size_t j = 0; j <= i; ++j) - { - if (j < background_GMM.getK () && background_GMM[j].eigenvalue > background_GMM[n_back].eigenvalue) - n_back = j; - - if (j < foreground_GMM.getK () && foreground_GMM[j].eigenvalue > foreground_GMM[n_fore].eigenvalue) - n_fore = j; - } - } - - back_fitters.clear (); - fore_fitters.clear (); -} - -void -pcl::segmentation::grabcut::learnGMMs (const Image& image, - const std::vector& indices, - const std::vector& hard_segmentation, - std::vector& components, - GMM& background_GMM, GMM& foreground_GMM) -{ - const std::size_t indices_size = static_cast (indices.size ()); - // Step 4: Assign each pixel to the component which maximizes its probability - for (std::size_t idx = 0; idx < indices_size; ++idx) - { - const Color &c = image[indices[idx]]; - - if (hard_segmentation[idx] == SegmentationForeground) - { - std::size_t k = 0; - float max = 0; - - for (std::size_t i = 0; i < foreground_GMM.getK (); i++) - { - float p = foreground_GMM.probabilityDensity (i, c); - if (p > max) - { - k = i; - max = p; - } - } - components[idx] = k; - } - else - { - std::size_t k = 0; - float max = 0; - - for (std::size_t i = 0; i < background_GMM.getK (); i++) - { - float p = background_GMM.probabilityDensity (i, c); - if (p > max) - { - k = i; - max = p; - } - } - components[idx] = k; - } - } - - // Step 5: Relearn GMMs from new component assignments - - // Set up Gaussian Fitters - std::vector back_fitters (background_GMM.getK ()); - std::vector fore_fitters (foreground_GMM.getK ()); - - std::size_t fore_counter = 0, back_counter = 0; - for (std::size_t idx = 0; idx < indices_size; ++idx) - { - const Color &c = image [indices [idx]]; - - if (hard_segmentation[idx] == SegmentationForeground) - { - fore_fitters[components[idx]].add (c); - fore_counter++; - } - else - { - back_fitters[components[idx]].add (c); - back_counter++; - } - } - - for (std::size_t i = 0; i < background_GMM.getK (); ++i) - back_fitters[i].fit (background_GMM[i], back_counter, false); - - for (std::size_t i = 0; i < foreground_GMM.getK (); ++i) - fore_fitters[i].fit (foreground_GMM[i], fore_counter, false); - - back_fitters.clear (); - fore_fitters.clear (); -} diff --git a/segmentation/src/grabcut_segmentation.cpp b/segmentation/src/grabcut_segmentation.cpp new file mode 100644 index 00000000..a5f3f206 --- /dev/null +++ b/segmentation/src/grabcut_segmentation.cpp @@ -0,0 +1,857 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include + +#include +#include +#include +#include +#include + +pcl::segmentation::grabcut::BoykovKolmogorov::BoykovKolmogorov (std::size_t max_nodes) + : flow_value_(0.0) +{ + if (max_nodes > 0) + { + source_edges_.reserve (max_nodes); + target_edges_.reserve (max_nodes); + nodes_.reserve (max_nodes); + } +} + +double +pcl::segmentation::grabcut::BoykovKolmogorov::operator() (int u, int v) const +{ + if ((u < 0) && (v < 0)) return flow_value_; + if (u < 0) { return source_edges_[v]; } + if (v < 0) { return target_edges_[u]; } + capacitated_edge::const_iterator it = nodes_[u].find (v); + if (it == nodes_[u].end ()) return 0.0; + return it->second; +} + +double +pcl::segmentation::grabcut::BoykovKolmogorov::getSourceEdgeCapacity (int u) const +{ + return (source_edges_[u]); +} + +double +pcl::segmentation::grabcut::BoykovKolmogorov::getTargetEdgeCapacity (int u) const +{ + return (target_edges_[u]); +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::preAugmentPaths () +{ + for (int u = 0; u < (int)nodes_.size (); u++) + { + // augment s-u-t paths + if ((source_edges_[u] > 0.0) && (target_edges_[u] > 0.0)) + { + const double cap = std::min (source_edges_[u], target_edges_[u]); + flow_value_ += cap; + source_edges_[u] -= cap; + target_edges_[u] -= cap; + } + + if (source_edges_[u] == 0.0) continue; + + // augment s-u-v-t paths + for (std::map::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) + { + const int v = it->first; + if ((it->second == 0.0) || (target_edges_[v] == 0.0)) continue; + const double w = std::min (it->second, std::min (source_edges_[u], target_edges_[v])); + source_edges_[u] -= w; + target_edges_[v] -= w; + it->second -= w; + nodes_[v][u] += w; + flow_value_ += w; + if (source_edges_[u] == 0.0) break; + } + } +} + +int +pcl::segmentation::grabcut::BoykovKolmogorov::addNodes (size_t n) +{ + int node_id = (int)nodes_.size (); + nodes_.resize (nodes_.size () + n); + source_edges_.resize (nodes_.size (), 0.0); + target_edges_.resize (nodes_.size (), 0.0); + return (node_id); +} + +// void +// pcl::segmentation::grabcut::BoykovKolmogorov::addSourceAndTargetEdges (int u, double source_cap, double sink_cap) +// { +// addSourceEdge (u, source_cap); +// addTargetEdge (u, sink_cap); + +// } + +void +pcl::segmentation::grabcut::BoykovKolmogorov::addSourceEdge (int u, double cap) +{ + assert ((u >= 0) && (u < (int)nodes_.size ())); + if (cap < 0.0) + { + flow_value_ += cap; + target_edges_[u] -= cap; + } + else + source_edges_[u] += cap; +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::addTargetEdge (int u, double cap) +{ + assert ((u >= 0) && (u < (int)nodes_.size ())); + if (cap < 0.0) + { + flow_value_ += cap; + source_edges_[u] -= cap; + } + else + target_edges_[u] += cap; +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::addEdge (int u, int v, double cap_uv, double cap_vu) +{ + assert ((u >= 0) && (u < (int)nodes_.size ())); + assert ((v >= 0) && (v < (int)nodes_.size ())); + assert (u != v); + + capacitated_edge::iterator it = nodes_[u].find (v); + if (it == nodes_[u].end ()) + { + assert (cap_uv + cap_vu >= 0.0); + if (cap_uv < 0.0) + { + nodes_[u].insert (std::make_pair (v, 0.0)); + nodes_[v].insert (std::make_pair (u, cap_vu + cap_uv)); + source_edges_[u] -= cap_uv; + target_edges_[v] -= cap_uv; + flow_value_ += cap_uv; + } + else + { + if (cap_vu < 0.0) + { + nodes_[u].insert (std::make_pair (v, cap_uv + cap_vu)); + nodes_[v].insert (std::make_pair (u, 0.0)); + source_edges_[v] -= cap_vu; + target_edges_[u] -= cap_vu; + flow_value_ += cap_vu; + } + else + { + nodes_[u].insert (std::make_pair (v, cap_uv)); + nodes_[v].insert (std::make_pair (u, cap_vu)); + } + } + } + else + { + capacitated_edge::iterator jt = nodes_[v].find (u); + it->second += cap_uv; + jt->second += cap_vu; + assert (it->second + jt->second >= 0.0); + if (it->second < 0.0) + { + jt->second += it->second; + source_edges_[u] -= it->second; + target_edges_[v] -= it->second; + flow_value_ += it->second; + it->second = 0.0; + } + else + { + if (jt->second < 0.0) + { + it->second += jt->second; + source_edges_[v] -= jt->second; + target_edges_[u] -= jt->second; + flow_value_ += jt->second; + jt->second = 0.0; + } + } + } +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::reset () +{ + flow_value_ = 0.0; + std::fill (source_edges_.begin (), source_edges_.end (), 0.0); + std::fill (target_edges_.begin (), target_edges_.end (), 0.0); + for (int u = 0; u < (int)nodes_.size (); u++) + { + for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) + { + it->second = 0.0; + } + } + std::fill (cut_.begin (), cut_.end (), FREE); + parents_.clear (); + clearActive (); +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::clear () +{ + flow_value_ = 0.0; + source_edges_.clear (); + target_edges_.clear (); + nodes_.clear (); + cut_.clear (); + parents_.clear (); + clearActive (); +} + +double +pcl::segmentation::grabcut::BoykovKolmogorov::solve () +{ + // initialize search tree and active set + cut_.resize (nodes_.size ()); + std::fill (cut_.begin (), cut_.end (), FREE); + parents_.resize (nodes_.size ()); + + clearActive (); + + // pre-augment paths + preAugmentPaths (); + + // initialize search trees + initializeTrees (); + + std::deque orphans; + while (!isActiveSetEmpty ()) + { + const std::pair path = expandTrees (); + augmentPath (path, orphans); + if (!orphans.empty ()) + { + adoptOrphans (orphans); + } + } + return (flow_value_); +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::initializeTrees () +{ + // initialize search tree + for (int u = 0; u < (int)nodes_.size (); u++) + { + if (source_edges_[u] > 0.0) + { + cut_[u] = SOURCE; + parents_[u].first = TERMINAL; + markActive (u); + } + else + { + if (target_edges_[u] > 0.0) + { + cut_[u] = TARGET; + parents_[u].first = TERMINAL; + markActive (u); + } + } + } +} + +std::pair +pcl::segmentation::grabcut::BoykovKolmogorov::expandTrees () +{ + // expand trees looking for augmenting paths + while (!isActiveSetEmpty ()) + { + const int u = active_head_; + + if (cut_[u] == SOURCE) { + for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) + { + if (it->second > 0.0) + { + if (cut_[it->first] == FREE) + { + cut_[it->first] = SOURCE; + parents_[it->first] = std::make_pair (u, std::make_pair (it, nodes_[it->first].find (u))); + markActive (it->first); + } + else + { + if (cut_[it->first] == TARGET) + { + // found augmenting path + return (std::make_pair (u, it->first)); + } + } + } + } + } + else + { + for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++) + { + if (cut_[it->first] == TARGET) continue; + if (nodes_[it->first][u] > 0.0) + { + if (cut_[it->first] == FREE) + { + cut_[it->first] = TARGET; + parents_[it->first] = std::make_pair (u, std::make_pair (nodes_[it->first].find (u), it)); + markActive (it->first); + } + else + { + if (cut_[it->first] == SOURCE) + { + // found augmenting path + return (std::make_pair (it->first, u)); + } + } + } + } + } + + // remove node from active set + markInactive (u); + } + + return (std::make_pair (TERMINAL, TERMINAL)); +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::augmentPath (const std::pair& path, std::deque& orphans) +{ + if ((path.first == TERMINAL) && (path.second == TERMINAL)) + return; + + // find path capacity + + // backtrack + const edge_pair e = std::make_pair (nodes_[path.first].find (path.second), + nodes_[path.second].find (path.first)); + double c = e.first->second; + int u = path.first; + while (parents_[u].first != TERMINAL) + { + c = std::min (c, parents_[u].second.first->second); + u = parents_[u].first; + //assert (cut_[u] == SOURCE); + } + c = std::min (c, source_edges_[u]); + + // forward track + u = path.second; + while (parents_[u].first != TERMINAL) + { + c = std::min (c, parents_[u].second.first->second); + u = parents_[u].first; + //assert (cut_[u] == TARGET); + } + c = std::min (c, target_edges_[u]); + + // augment path + flow_value_ += c; + //DRWN_LOG_DEBUG ("path capacity: " << c); + + // backtrack + u = path.first; + while (parents_[u].first != TERMINAL) + { + //nodes_[u][parents_[u].first] += c; + parents_[u].second.second->second += c; + parents_[u].second.first->second -= c; + if (parents_[u].second.first->second == 0.0) + { + orphans.push_front (u); + } + u = parents_[u].first; + } + source_edges_[u] -= c; + if (source_edges_[u] == 0.0) + { + orphans.push_front (u); + } + + // link + e.first->second -= c; + e.second->second += c; + + // forward track + u = path.second; + while (parents_[u].first != TERMINAL) { + //nodes_[parents_[u].first][u] += c; + parents_[u].second.second->second += c; + parents_[u].second.first->second -= c; + if (parents_[u].second.first->second == 0.0) { + orphans.push_back (u); + } + u = parents_[u].first; + } + target_edges_[u] -= c; + if (target_edges_[u] == 0.0) { + orphans.push_back (u); + } +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::adoptOrphans (std::deque& orphans) +{ + // find new parent for orphaned subtree or free it + while (!orphans.empty ()) + { + const int u = orphans.front (); + const char tree_label = cut_[u]; + orphans.pop_front (); + + // can occur if same node is inserted into orphans multiple times + if (tree_label == FREE) continue; + //assert (tree_label != FREE); + + // look for new parent + bool b_free_orphan = true; + for (capacitated_edge::iterator jt = nodes_[u].begin (); jt != nodes_[u].end (); ++jt) { + // skip if different trees + if (cut_[jt->first] != tree_label) continue; + + // check edge capacity + const capacitated_edge::iterator kt = nodes_[jt->first].find (u); + if (((tree_label == TARGET) && (jt->second <= 0.0)) || + ((tree_label == SOURCE) && (kt->second <= 0.0))) + continue; + + // check that u is not an ancestor of jt->first + int v = jt->first; + while ((v != u) && (v != TERMINAL)) + { + v = parents_[v].first; + } + if (v != TERMINAL) continue; + + // add as parent + const edge_pair e = (tree_label == SOURCE) ? std::make_pair (kt, jt) : std::make_pair (jt, kt); + parents_[u] = std::make_pair (jt->first, e); + b_free_orphan = false; + break; + } + + // free the orphan subtree and remove it from the active set + if (b_free_orphan) + { + for (capacitated_edge::const_iterator jt = nodes_[u].begin (); jt != nodes_[u].end (); ++jt) + { + if ((cut_[jt->first] == tree_label) && (parents_[jt->first].first == u)) + { + orphans.push_front (jt->first); + markActive (jt->first); + } + else if (cut_[jt->first] != FREE) + { + markActive (jt->first); + } + } + + // mark inactive and free + markInactive (u); + cut_[u] = FREE; + } + } +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::clearActive () +{ + active_head_ = active_tail_ = TERMINAL; + active_list_.resize (nodes_.size ()); + std::fill (active_list_.begin (), active_list_.end (), std::make_pair (TERMINAL, TERMINAL)); +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::markActive (int u) +{ + if (isActive (u)) return; + + active_list_[u].first = active_tail_; + active_list_[u].second = TERMINAL; + if (active_tail_ == TERMINAL) + active_head_ = u; + else + active_list_[active_tail_].second = u; + active_tail_ = u; +} + +void +pcl::segmentation::grabcut::BoykovKolmogorov::markInactive (int u) +{ + //if (!isActive (u)) return; + + if (u == active_head_) + { + active_head_ = active_list_[u].second; + if (u != active_tail_) + { + active_list_[active_list_[u].second].first = TERMINAL; + } + } + else + if (u == active_tail_) + { + active_tail_ = active_list_[u].first; + active_list_[active_list_[u].first].second = TERMINAL; + } + else + if (active_list_[u].first != TERMINAL) + { + active_list_[active_list_[u].first].second = active_list_[u].second; + active_list_[active_list_[u].second].first = active_list_[u].first; + } + //active_list_[u] = std::make_pair (TERMINAL, TERMINAL); + active_list_[u].first = TERMINAL; +} + +void +pcl::segmentation::grabcut::GaussianFitter::add (const Color &c) +{ + sum_[0] += c.r; sum_[1] += c.g; sum_[2] += c.b; + accumulator_ (0,0) += c.r*c.r; accumulator_ (0,1) += c.r*c.g; accumulator_ (0,2) += c.r*c.b; + accumulator_ (1,0) += c.g*c.r; accumulator_ (1,1) += c.g*c.g; accumulator_ (1,2) += c.g*c.b; + accumulator_ (2,0) += c.b*c.r; accumulator_ (2,1) += c.b*c.g; accumulator_ (2,2) += c.b*c.b; + + ++count_; +} + +// Build the gaussian out of all the added colors +void +pcl::segmentation::grabcut::GaussianFitter::fit (Gaussian& g, std::size_t total_count, bool compute_eigens) const +{ + if (count_==0) + { + g.pi = 0; + } + else + { + const float count_f = static_cast (count_); + + // Compute mean of gaussian + g.mu.r = sum_[0]/count_f; + g.mu.g = sum_[1]/count_f; + g.mu.b = sum_[2]/count_f; + + // Compute covariance matrix + g.covariance (0,0) = accumulator_ (0,0)/count_f - g.mu.r*g.mu.r + epsilon_; + g.covariance (0,1) = accumulator_ (0,1)/count_f - g.mu.r*g.mu.g; + g.covariance (0,2) = accumulator_ (0,2)/count_f - g.mu.r*g.mu.b; + g.covariance (1,0) = accumulator_ (1,0)/count_f - g.mu.g*g.mu.r; + g.covariance (1,1) = accumulator_ (1,1)/count_f - g.mu.g*g.mu.g + epsilon_; + g.covariance (1,2) = accumulator_ (1,2)/count_f - g.mu.g*g.mu.b; + g.covariance (2,0) = accumulator_ (2,0)/count_f - g.mu.b*g.mu.r; + g.covariance (2,1) = accumulator_ (2,1)/count_f - g.mu.b*g.mu.g; + g.covariance (2,2) = accumulator_ (2,2)/count_f - g.mu.b*g.mu.b + epsilon_; + + // Compute determinant of covariance matrix + g.determinant = g.covariance (0,0)*(g.covariance (1,1)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,1)) + - g.covariance (0,1)*(g.covariance (1,0)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,0)) + + g.covariance (0,2)*(g.covariance (1,0)*g.covariance (2,1) - g.covariance (1,1)*g.covariance (2,0)); + + // Compute inverse (cofactor matrix divided by determinant) + g.inverse (0,0) = (g.covariance (1,1)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,1)) / g.determinant; + g.inverse (1,0) = -(g.covariance (1,0)*g.covariance (2,2) - g.covariance (1,2)*g.covariance (2,0)) / g.determinant; + g.inverse (2,0) = (g.covariance (1,0)*g.covariance (2,1) - g.covariance (1,1)*g.covariance (2,0)) / g.determinant; + g.inverse (0,1) = -(g.covariance (0,1)*g.covariance (2,2) - g.covariance (0,2)*g.covariance (2,1)) / g.determinant; + g.inverse (1,1) = (g.covariance (0,0)*g.covariance (2,2) - g.covariance (0,2)*g.covariance (2,0)) / g.determinant; + g.inverse (2,1) = -(g.covariance (0,0)*g.covariance (2,1) - g.covariance (0,1)*g.covariance (2,0)) / g.determinant; + g.inverse (0,2) = (g.covariance (0,1)*g.covariance (1,2) - g.covariance (0,2)*g.covariance (1,1)) / g.determinant; + g.inverse (1,2) = -(g.covariance (0,0)*g.covariance (1,2) - g.covariance (0,2)*g.covariance (1,0)) / g.determinant; + g.inverse (2,2) = (g.covariance (0,0)*g.covariance (1,1) - g.covariance (0,1)*g.covariance (1,0)) / g.determinant; + + // The weight of the gaussian is the fraction of the number of pixels in this Gaussian to the number + // of pixels in all the gaussians of this GMM. + g.pi = count_f / static_cast (total_count); + + if (compute_eigens) + { + // Compute eigenvalues and vectors using SVD + Eigen::JacobiSVD svd (g.covariance, Eigen::ComputeFullU); + // Store highest eigenvalue + g.eigenvalue = svd.singularValues ()[0]; + // Store corresponding eigenvector + g.eigenvector = svd.matrixU ().col (0); + } + } +} + +float +pcl::segmentation::grabcut::GMM::probabilityDensity (const Color &c) +{ + float result = 0; + + for (std::size_t i=0; i < gaussians_.size (); ++i) + result += gaussians_[i].pi * probabilityDensity (i, c); + + return (result); +} + +float +pcl::segmentation::grabcut::GMM::probabilityDensity (std::size_t i, const Color &c) +{ + float result = 0; + const pcl::segmentation::grabcut::Gaussian &G = gaussians_[i]; + if (G.pi > 0 ) + { + if (G.determinant > 0) + { + float r = c.r - G.mu.r; + float g = c.g - G.mu.g; + float b = c.b - G.mu.b; + + float d = r * (r*G.inverse (0,0) + g*G.inverse (1,0) + b*G.inverse (2,0)) + + g * (r*G.inverse (0,1) + g*G.inverse (1,1) + b*G.inverse (2,1)) + + b * (r*G.inverse (0,2) + g*G.inverse (1,2) + b*G.inverse (2,2)); + + result = static_cast (1.0/(sqrt (G.determinant)) * exp (-0.5*d)); + } + } + + return (result); +} + +void +pcl::segmentation::grabcut::buildGMMs (const Image& image, + const std::vector& indices, + const std::vector& hard_segmentation, + std::vector& components, + GMM& background_GMM, GMM& foreground_GMM) +{ + // Step 3: Build GMMs using Orchard-Bouman clustering algorithm + + // Set up Gaussian Fitters + std::vector back_fitters (background_GMM.getK ()); + std::vector fore_fitters (foreground_GMM.getK ()); + + std::size_t fore_count = 0, back_count = 0; + const int indices_size = static_cast (indices.size ()); + // Initialize the first foreground and background clusters + for (int idx = 0; idx < indices_size; ++idx) + { + components [idx] = 0; + + if (hard_segmentation [idx] == SegmentationForeground) + { + fore_fitters[0].add (image[indices[idx]]); + fore_count++; + } + else + { + back_fitters[0].add (image[indices[idx]]); + back_count++; + } + } + + back_fitters[0].fit (background_GMM[0], back_count, true); + fore_fitters[0].fit (foreground_GMM[0], fore_count, true); + + std::size_t n_back = 0, n_fore = 0; // Which cluster will be split + std::size_t max_K = (background_GMM.getK () > foreground_GMM.getK ()) ? background_GMM.getK () : foreground_GMM.getK (); + + // Compute clusters + for (std::size_t i = 1; i < max_K; ++i) + { + // Reset the fitters for the splitting clusters + back_fitters[n_back] = GaussianFitter (); + fore_fitters[n_fore] = GaussianFitter (); + + // For brevity, get references to the splitting Gaussians + Gaussian& bg = background_GMM[n_back]; + Gaussian& fg = foreground_GMM[n_fore]; + + // Compute splitting points + float split_background = bg.eigenvector[0] * bg.mu.r + bg.eigenvector[1] * bg.mu.g + bg.eigenvector[2] * bg.mu.b; + float split_foreground = fg.eigenvector[0] * fg.mu.r + fg.eigenvector[1] * fg.mu.g + fg.eigenvector[2] * fg.mu.b; + + // Split clusters nBack and nFore, place split portion into cluster i + for (int idx = 0; idx < indices_size; ++idx) + { + const Color &c = image[indices[idx]]; + + // For each pixel + if (i < foreground_GMM.getK () && + hard_segmentation[idx] == SegmentationForeground && + components[idx] == n_fore) + { + if (fg.eigenvector[0] * c.r + fg.eigenvector[1] * c.g + fg.eigenvector[2] * c.b > split_foreground) + { + components[idx] = i; + fore_fitters[i].add (c); + } + else + { + fore_fitters[n_fore].add (c); + } + } + else if (i < background_GMM.getK () && + hard_segmentation[idx] == SegmentationBackground && + components[idx] == n_back) + { + if (bg.eigenvector[0] * c.r + bg.eigenvector[1] * c.g + bg.eigenvector[2] * c.b > split_background) + { + components[idx] = i; + back_fitters[i].add (c); + } + else + { + back_fitters[n_back].add (c); + } + } + } + + // Compute new split Gaussians + back_fitters[n_back].fit (background_GMM[n_back], back_count, true); + fore_fitters[n_fore].fit (foreground_GMM[n_fore], fore_count, true); + + if (i < background_GMM.getK ()) + back_fitters[i].fit (background_GMM[i], back_count, true); + if (i < foreground_GMM.getK ()) + fore_fitters[i].fit (foreground_GMM[i], fore_count, true); + + // Find clusters with highest eigenvalue + n_back = 0; + n_fore = 0; + + for (std::size_t j = 0; j <= i; ++j) + { + if (j < background_GMM.getK () && background_GMM[j].eigenvalue > background_GMM[n_back].eigenvalue) + n_back = j; + + if (j < foreground_GMM.getK () && foreground_GMM[j].eigenvalue > foreground_GMM[n_fore].eigenvalue) + n_fore = j; + } + } + + back_fitters.clear (); + fore_fitters.clear (); +} + +void +pcl::segmentation::grabcut::learnGMMs (const Image& image, + const std::vector& indices, + const std::vector& hard_segmentation, + std::vector& components, + GMM& background_GMM, GMM& foreground_GMM) +{ + const std::size_t indices_size = static_cast (indices.size ()); + // Step 4: Assign each pixel to the component which maximizes its probability + for (std::size_t idx = 0; idx < indices_size; ++idx) + { + const Color &c = image[indices[idx]]; + + if (hard_segmentation[idx] == SegmentationForeground) + { + std::size_t k = 0; + float max = 0; + + for (std::size_t i = 0; i < foreground_GMM.getK (); i++) + { + float p = foreground_GMM.probabilityDensity (i, c); + if (p > max) + { + k = i; + max = p; + } + } + components[idx] = k; + } + else + { + std::size_t k = 0; + float max = 0; + + for (std::size_t i = 0; i < background_GMM.getK (); i++) + { + float p = background_GMM.probabilityDensity (i, c); + if (p > max) + { + k = i; + max = p; + } + } + components[idx] = k; + } + } + + // Step 5: Relearn GMMs from new component assignments + + // Set up Gaussian Fitters + std::vector back_fitters (background_GMM.getK ()); + std::vector fore_fitters (foreground_GMM.getK ()); + + std::size_t fore_counter = 0, back_counter = 0; + for (std::size_t idx = 0; idx < indices_size; ++idx) + { + const Color &c = image [indices [idx]]; + + if (hard_segmentation[idx] == SegmentationForeground) + { + fore_fitters[components[idx]].add (c); + fore_counter++; + } + else + { + back_fitters[components[idx]].add (c); + back_counter++; + } + } + + for (std::size_t i = 0; i < background_GMM.getK (); ++i) + back_fitters[i].fit (background_GMM[i], back_counter, false); + + for (std::size_t i = 0; i < foreground_GMM.getK (); ++i) + fore_fitters[i].fit (foreground_GMM[i], fore_counter, false); + + back_fitters.clear (); + fore_fitters.clear (); +} diff --git a/segmentation/src/progressive_morphological_filter.cpp b/segmentation/src/progressive_morphological_filter.cpp new file mode 100644 index 00000000..1974df90 --- /dev/null +++ b/segmentation/src/progressive_morphological_filter.cpp @@ -0,0 +1,53 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include +#include +#include +#include + +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE(ProgressiveMorphologicalFilter, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)) +#else + PCL_INSTANTIATE(ProgressiveMorphologicalFilter, PCL_XYZ_POINT_TYPES) +#endif + diff --git a/segmentation/src/supervoxel_clustering.cpp b/segmentation/src/supervoxel_clustering.cpp index 940e1b1c..5204507f 100644 --- a/segmentation/src/supervoxel_clustering.cpp +++ b/segmentation/src/supervoxel_clustering.cpp @@ -48,16 +48,21 @@ template class pcl::SupervoxelClustering; template class pcl::SupervoxelClustering; +template class pcl::SupervoxelClustering; +typedef pcl::SupervoxelClustering::VoxelData VoxelDataT; typedef pcl::SupervoxelClustering::VoxelData VoxelDataRGBT; typedef pcl::SupervoxelClustering::VoxelData VoxelDataRGBAT; +typedef pcl::octree::OctreePointCloudAdjacencyContainer AdjacencyContainerT; typedef pcl::octree::OctreePointCloudAdjacencyContainer AdjacencyContainerRGBT; typedef pcl::octree::OctreePointCloudAdjacencyContainer AdjacencyContainerRGBAT; +template class pcl::octree::OctreePointCloudAdjacencyContainer; template class pcl::octree::OctreePointCloudAdjacencyContainer; template class pcl::octree::OctreePointCloudAdjacencyContainer; +template class pcl::octree::OctreePointCloudAdjacency; template class pcl::octree::OctreePointCloudAdjacency; template class pcl::octree::OctreePointCloudAdjacency; diff --git a/surface/CMakeLists.txt b/surface/CMakeLists.txt index 9e004c8d..dec7d123 100644 --- a/surface/CMakeLists.txt +++ b/surface/CMakeLists.txt @@ -3,22 +3,22 @@ set(SUBSYS_DESC "Point cloud surface library") set(SUBSYS_DEPS common search kdtree octree) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS qhull) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS qhull) -PCL_ADD_DOC(${SUBSYS_NAME}) +PCL_ADD_DOC("${SUBSYS_NAME}") if(build) if(QHULL_FOUND) include_directories(${QHULL_INCLUDE_DIRS}) set(HULL_INCLUDES - include/pcl/${SUBSYS_NAME}/concave_hull.h - include/pcl/${SUBSYS_NAME}/convex_hull.h - include/pcl/${SUBSYS_NAME}/qhull.h + "include/pcl/${SUBSYS_NAME}/concave_hull.h" + "include/pcl/${SUBSYS_NAME}/convex_hull.h" + "include/pcl/${SUBSYS_NAME}/qhull.h" ) set(HULL_IMPLS - include/pcl/${SUBSYS_NAME}/impl/concave_hull.hpp - include/pcl/${SUBSYS_NAME}/impl/convex_hull.hpp + "include/pcl/${SUBSYS_NAME}/impl/concave_hull.hpp" + "include/pcl/${SUBSYS_NAME}/impl/convex_hull.hpp" ) set(HULL_SOURCES src/concave_hull.cpp @@ -27,16 +27,16 @@ if(build) endif(QHULL_FOUND) if (VTK_FOUND AND NOT ANDROID) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") set(VTK_SMOOTHING_INCLUDES - include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk.h - include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_utils.h - include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_subdivision.h - include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_quadric_decimation.h - include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_smoothing_laplacian.h - include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h) + "include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk.h" + "include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_utils.h" + "include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_subdivision.h" + "include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_quadric_decimation.h" + "include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_smoothing_laplacian.h" + "include/pcl/${SUBSYS_NAME}/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h") set(VTK_SMOOTHING_SOURCE src/vtk_smoothing/vtk_utils.cpp @@ -44,7 +44,12 @@ if(build) src/vtk_smoothing/vtk_mesh_quadric_decimation.cpp src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp) - set(VTK_SMOOTHING_TARGET_LINK_LIBRARIES vtkCommon vtkWidgets vtkGraphics) + + if("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_LESS "6.0") + set(VTK_SMOOTHING_TARGET_LINK_LIBRARIES vtkCommon vtkWidgets vtkGraphics) + else("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_LESS "6.0") + set(VTK_SMOOTHING_TARGET_LINK_LIBRARIES vtkCommonCore vtkCommonDataModel vtkCommonExecutionModel vtkFiltersModeling) + endif("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_LESS "6.0") endif() SET(BUILD_surface_on_nurbs 0 CACHE BOOL "Fitting NURBS to point-clouds using openNURBS" ) @@ -54,31 +59,31 @@ if(build) ENDIF(BUILD_surface_on_nurbs) set(POISSON_INCLUDES - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/allocator.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/binary_node.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/bspline_data.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/factor.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/function_data.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/geometry.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/hash.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/marching_cubes_poisson.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/mat.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/multi_grid_octree_data.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/octree_poisson.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/polynomial.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/ppolynomial.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/sparse_matrix.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/vector.h - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/bspline_data.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/function_data.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/geometry.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/mat.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/multi_grid_octree_data.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/octree_poisson.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/polynomial.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/ppolynomial.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/sparse_matrix.hpp - include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/vector.hpp + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/allocator.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/binary_node.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/bspline_data.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/factor.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/function_data.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/geometry.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/hash.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/marching_cubes_poisson.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/mat.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/multi_grid_octree_data.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/octree_poisson.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/polynomial.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/ppolynomial.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/sparse_matrix.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/vector.h" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/bspline_data.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/function_data.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/geometry.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/mat.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/multi_grid_octree_data.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/octree_poisson.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/polynomial.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/ppolynomial.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/sparse_matrix.hpp" + "include/pcl/${SUBSYS_NAME}/3rdparty/poisson4/vector.hpp" ) set(POISSON_SOURCES src/3rdparty/poisson4/factor.cpp @@ -109,23 +114,23 @@ if(build) ) set(incs - include/pcl/${SUBSYS_NAME}/boost.h - include/pcl/${SUBSYS_NAME}/eigen.h - include/pcl/${SUBSYS_NAME}/ear_clipping.h - include/pcl/${SUBSYS_NAME}/gp3.h - include/pcl/${SUBSYS_NAME}/grid_projection.h - include/pcl/${SUBSYS_NAME}/marching_cubes.h - include/pcl/${SUBSYS_NAME}/marching_cubes_hoppe.h - include/pcl/${SUBSYS_NAME}/marching_cubes_rbf.h - include/pcl/${SUBSYS_NAME}/bilateral_upsampling.h - include/pcl/${SUBSYS_NAME}/mls.h - include/pcl/${SUBSYS_NAME}/organized_fast_mesh.h - include/pcl/${SUBSYS_NAME}/reconstruction.h - include/pcl/${SUBSYS_NAME}/processing.h - include/pcl/${SUBSYS_NAME}/simplification_remove_unused_vertices.h - include/pcl/${SUBSYS_NAME}/surfel_smoothing.h - include/pcl/${SUBSYS_NAME}/texture_mapping.h - include/pcl/${SUBSYS_NAME}/poisson.h + "include/pcl/${SUBSYS_NAME}/boost.h" + "include/pcl/${SUBSYS_NAME}/eigen.h" + "include/pcl/${SUBSYS_NAME}/ear_clipping.h" + "include/pcl/${SUBSYS_NAME}/gp3.h" + "include/pcl/${SUBSYS_NAME}/grid_projection.h" + "include/pcl/${SUBSYS_NAME}/marching_cubes.h" + "include/pcl/${SUBSYS_NAME}/marching_cubes_hoppe.h" + "include/pcl/${SUBSYS_NAME}/marching_cubes_rbf.h" + "include/pcl/${SUBSYS_NAME}/bilateral_upsampling.h" + "include/pcl/${SUBSYS_NAME}/mls.h" + "include/pcl/${SUBSYS_NAME}/organized_fast_mesh.h" + "include/pcl/${SUBSYS_NAME}/reconstruction.h" + "include/pcl/${SUBSYS_NAME}/processing.h" + "include/pcl/${SUBSYS_NAME}/simplification_remove_unused_vertices.h" + "include/pcl/${SUBSYS_NAME}/surfel_smoothing.h" + "include/pcl/${SUBSYS_NAME}/texture_mapping.h" + "include/pcl/${SUBSYS_NAME}/poisson.h" ${HULL_INCLUDES} # ${VTK_SMOOTHING_INCLUDES} # ${POISSON_INCLUDES} @@ -134,47 +139,47 @@ if(build) ) set(impl_incs - include/pcl/${SUBSYS_NAME}/impl/gp3.hpp - include/pcl/${SUBSYS_NAME}/impl/grid_projection.hpp - include/pcl/${SUBSYS_NAME}/impl/marching_cubes.hpp - include/pcl/${SUBSYS_NAME}/impl/marching_cubes_hoppe.hpp - include/pcl/${SUBSYS_NAME}/impl/marching_cubes_rbf.hpp - include/pcl/${SUBSYS_NAME}/impl/bilateral_upsampling.hpp - include/pcl/${SUBSYS_NAME}/impl/mls.hpp - include/pcl/${SUBSYS_NAME}/impl/organized_fast_mesh.hpp - include/pcl/${SUBSYS_NAME}/impl/reconstruction.hpp - include/pcl/${SUBSYS_NAME}/impl/processing.hpp - include/pcl/${SUBSYS_NAME}/impl/surfel_smoothing.hpp - include/pcl/${SUBSYS_NAME}/impl/texture_mapping.hpp - include/pcl/${SUBSYS_NAME}/impl/poisson.hpp + "include/pcl/${SUBSYS_NAME}/impl/gp3.hpp" + "include/pcl/${SUBSYS_NAME}/impl/grid_projection.hpp" + "include/pcl/${SUBSYS_NAME}/impl/marching_cubes.hpp" + "include/pcl/${SUBSYS_NAME}/impl/marching_cubes_hoppe.hpp" + "include/pcl/${SUBSYS_NAME}/impl/marching_cubes_rbf.hpp" + "include/pcl/${SUBSYS_NAME}/impl/bilateral_upsampling.hpp" + "include/pcl/${SUBSYS_NAME}/impl/mls.hpp" + "include/pcl/${SUBSYS_NAME}/impl/organized_fast_mesh.hpp" + "include/pcl/${SUBSYS_NAME}/impl/reconstruction.hpp" + "include/pcl/${SUBSYS_NAME}/impl/processing.hpp" + "include/pcl/${SUBSYS_NAME}/impl/surfel_smoothing.hpp" + "include/pcl/${SUBSYS_NAME}/impl/texture_mapping.hpp" + "include/pcl/${SUBSYS_NAME}/impl/poisson.hpp" ${HULL_IMPLS} ) - set(LIB_NAME pcl_${SUBSYS_NAME}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${VTK_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}) + set(LIB_NAME "pcl_${SUBSYS_NAME}") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" ${VTK_INCLUDE_DIRS} "${CMAKE_CURRENT_SOURCE_DIR}") link_directories(${VTK_LIBRARY_DIRS}) - PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME} ${srcs} ${incs} ${impl_incs} ${VTK_SMOOTHING_INCLUDES} ${POISSON_INCLUDES} ${OPENNURBS_INCLUDES} ${ON_NURBS_INCLUDES}) - target_link_libraries(${LIB_NAME} pcl_common pcl_search pcl_kdtree pcl_octree ${VTK_SMOOTHING_TARGET_LINK_LIBRARIES} ${ON_NURBS_LIBRARIES}) + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs} ${VTK_SMOOTHING_INCLUDES} ${POISSON_INCLUDES} ${OPENNURBS_INCLUDES} ${ON_NURBS_INCLUDES}) + target_link_libraries("${LIB_NAME}" pcl_common pcl_search pcl_kdtree pcl_octree ${VTK_LIBRARIES} ${ON_NURBS_LIBRARIES}) if(QHULL_FOUND) - target_link_libraries(${LIB_NAME} ${QHULL_LIBRARIES}) + target_link_libraries("${LIB_NAME}" ${QHULL_LIBRARIES}) endif(QHULL_FOUND) - PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") # Install include files - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME} ${incs}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/3rdparty/poisson4 ${POISSON_INCLUDES}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/impl ${impl_incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/3rdparty/poisson4" ${POISSON_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) if(BUILD_surface_on_nurbs) add_definitions(-DUNICODE -D_UNICODE) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/3rdparty/opennurbs ${OPENNURBS_INCLUDES}) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/on_nurbs ${ON_NURBS_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/3rdparty/opennurbs" ${OPENNURBS_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/on_nurbs" ${ON_NURBS_INCLUDES}) endif(BUILD_surface_on_nurbs) if (VTK_FOUND AND NOT ANDROID) - PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_NAME}/vtk_smoothing ${VTK_SMOOTHING_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/vtk_smoothing" ${VTK_SMOOTHING_INCLUDES}) endif (VTK_FOUND AND NOT ANDROID) if(WIN32) - target_link_libraries(${LIB_NAME} Rpcrt4.lib) + target_link_libraries("${LIB_NAME}" Rpcrt4.lib) endif(WIN32) endif(build) diff --git a/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp b/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp index a0149b32..7cfe20c1 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp @@ -1948,7 +1948,7 @@ namespace pcl int i , j , d , tIter=0; SparseSymmetricMatrix< Real > _M; - Vector< Real > B , _B , _X; + Vector< Real > B , B_ , X_; AdjacencySetFunction asf; AdjacencyCountFunction acf; double systemTime = 0 , solveTime = 0 , memUsage = 0 , evaluateTime = 0 , gTime = 0, sTime = 0; @@ -2025,14 +2025,14 @@ namespace pcl } // Get the associated constraint vector - _B.Resize( asf.adjacencyCount ); - for( j=0 ; jnodeData.solution; + X_[j] = sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution; } // Get the associated matrix SparseSymmetricMatrix< Real >::internalAllocator.rollBack(); @@ -2040,19 +2040,19 @@ namespace pcl #pragma omp parallel for num_threads( threads ) schedule( static ) for( j=0 ; jnodeData.constraint; + B_[j] += sNodes.treeNodes[asf.adjacencies[j]]->nodeData.constraint; sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.constraint = 0; } // Solve the matrix // Since we don't have the full matrix, the system shouldn't be singular, so we shouldn't have to correct it - iter += SparseSymmetricMatrix< Real >::Solve( _M , _B , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , _X , mrVector , Real(accuracy) , 0 ); + iter += SparseSymmetricMatrix< Real >::Solve( _M , B_ , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , X_ , mrVector , Real(accuracy) , 0 ); if( showResidual ) { double mNorm = 0; for( int i=0 ; i<_M.rows ; i++ ) for( int j=0 ; j<_M.rowSizes[i] ; j++ ) mNorm += _M[i][j].Value * _M[i][j].Value; - double bNorm = _B.Norm( 2 ) , rNorm = ( _B - _M * _X ).Norm( 2 ); + double bNorm = B_.Norm( 2 ) , rNorm = ( B_ - _M * X_ ).Norm( 2 ); printf( "\t\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , _M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter ); } @@ -2061,7 +2061,7 @@ namespace pcl { TreeOctNode* temp=sNodes.treeNodes[ asf.adjacencies[j] ]; while( temp->depth()>sNodes.treeNodes[i]->depth() ) temp=temp->parent; - if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( _X[j] ); + if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( X_[j] ); } systemTime += gTime; solveTime += sTime; diff --git a/surface/include/pcl/surface/bilateral_upsampling.h b/surface/include/pcl/surface/bilateral_upsampling.h index b8984aae..555b2aef 100644 --- a/surface/include/pcl/surface/bilateral_upsampling.h +++ b/surface/include/pcl/surface/bilateral_upsampling.h @@ -145,6 +145,12 @@ namespace pcl void performProcessing (pcl::PointCloud &output); + /** \brief Computes the distance for depth and RGB. + * \param[out] val_exp_depth distance values for depth + * \param[out] val_exp_rgb distance values for RGB */ + void + computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb); + private: int window_size_; float sigma_color_, sigma_depth_; diff --git a/surface/include/pcl/surface/concave_hull.h b/surface/include/pcl/surface/concave_hull.h index 93a5ec1d..3ff21b11 100644 --- a/surface/include/pcl/surface/concave_hull.h +++ b/surface/include/pcl/surface/concave_hull.h @@ -134,8 +134,10 @@ namespace pcl } /** \brief Returns the dimensionality (2 or 3) of the calculated hull. */ - PCL_DEPRECATED (int getDim () const, "[pcl::ConcaveHull::getDim] This method is deprecated. Please use getDimension () instead."); - + PCL_DEPRECATED ("[pcl::ConcaveHull::getDim] This method is deprecated. Please use getDimension () instead.") + int + getDim () const; + /** \brief Returns the dimensionality (2 or 3) of the calculated hull. */ inline int getDimension () const diff --git a/surface/include/pcl/surface/ear_clipping.h b/surface/include/pcl/surface/ear_clipping.h index 3fe77a03..870608ed 100644 --- a/surface/include/pcl/surface/ear_clipping.h +++ b/surface/include/pcl/surface/ear_clipping.h @@ -106,11 +106,10 @@ namespace pcl * \param[in] p the point to check */ bool - isInsideTriangle (const Eigen::Vector2f& u, - const Eigen::Vector2f& v, - const Eigen::Vector2f& w, - const Eigen::Vector2f& p); - + isInsideTriangle (const Eigen::Vector3f& u, + const Eigen::Vector3f& v, + const Eigen::Vector3f& w, + const Eigen::Vector3f& p); /** \brief Compute the cross product between 2D vectors. * \param[in] p1 the first 2D vector diff --git a/surface/include/pcl/surface/impl/bilateral_upsampling.hpp b/surface/include/pcl/surface/impl/bilateral_upsampling.hpp index ce867e9c..b355f235 100644 --- a/surface/include/pcl/surface/impl/bilateral_upsampling.hpp +++ b/surface/include/pcl/surface/impl/bilateral_upsampling.hpp @@ -88,6 +88,9 @@ pcl::BilateralUpsampling::performProcessing (PointCloudOut output.resize (input_->size ()); float nan = std::numeric_limits::quiet_NaN (); + Eigen::MatrixXf val_exp_depth_matrix; + Eigen::VectorXf val_exp_rgb_vector; + computeDistances (val_exp_depth_matrix, val_exp_rgb_vector); for (int x = 0; x < static_cast (input_->width); ++x) for (int y = 0; y < static_cast (input_->height); ++y) @@ -106,13 +109,14 @@ pcl::BilateralUpsampling::performProcessing (PointCloudOut float dx = float (x - x_w), dy = float (y - y_w); - float val_exp_depth = expf (- (dx*dx + dy*dy) / (2.0f * static_cast (sigma_depth_ * sigma_depth_))); + float val_exp_depth = val_exp_depth_matrix(dx+window_size_, dy+window_size_); float d_color = static_cast ( abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) + abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) + abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b)); - float val_exp_rgb = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_)); + + float val_exp_rgb = val_exp_rgb_vector(d_color); if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z)) { @@ -148,6 +152,32 @@ pcl::BilateralUpsampling::performProcessing (PointCloudOut } +template void +pcl::BilateralUpsampling::computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb) +{ + val_exp_depth.resize (2*window_size_+1,2*window_size_+1); + val_exp_rgb.resize (3*255); + + int j = 0; + for (int dx = -window_size_; dx < window_size_+1; ++dx) + { + int i = 0; + for (int dy = -window_size_; dy < window_size_+1; ++dy) + { + float val_exp = expf (- (dx*dx + dy*dy) / (2.0f * static_cast (sigma_depth_ * sigma_depth_))); + val_exp_depth(i,j) = val_exp; + i++; + } + j++; + } + + for (int d_color = 0; d_color < 3*255; d_color++) + { + float val_exp = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_)); + val_exp_rgb(d_color) = val_exp; + } +} + #define PCL_INSTANTIATE_BilateralUpsampling(T,OutT) template class PCL_EXPORTS pcl::BilateralUpsampling; diff --git a/surface/include/pcl/surface/impl/concave_hull.hpp b/surface/include/pcl/surface/impl/concave_hull.hpp index f05ae97b..7900104a 100644 --- a/surface/include/pcl/surface/impl/concave_hull.hpp +++ b/surface/include/pcl/surface/impl/concave_hull.hpp @@ -209,7 +209,7 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: if (exitcode != 0) { - PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%zu)!\n", getClassName ().c_str (), cloud_transformed.points.size ()); + PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%lu)!\n", getClassName ().c_str (), cloud_transformed.points.size ()); //check if it fails because of NaN values... if (!cloud_transformed.is_dense) diff --git a/surface/include/pcl/surface/impl/convex_hull.hpp b/surface/include/pcl/surface/impl/convex_hull.hpp index d335d353..c1552b50 100644 --- a/surface/include/pcl/surface/impl/convex_hull.hpp +++ b/surface/include/pcl/surface/impl/convex_hull.hpp @@ -190,7 +190,7 @@ pcl::ConvexHull::performReconstruction2D (PointCloud &hull, std::vecto // 0 if no error from qhull or it doesn't find any vertices if (exitcode != 0 || qh num_vertices == 0) { - PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), indices_->size ()); + PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), indices_->size ()); hull.points.resize (0); hull.width = hull.height = 0; @@ -324,7 +324,7 @@ pcl::ConvexHull::performReconstruction3D ( // 0 if no error from qhull if (exitcode != 0) { - PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), input_->points.size ()); + PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), input_->points.size ()); hull.points.resize (0); hull.width = hull.height = 0; diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index 6a407684..9e915b65 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -1039,7 +1039,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

0) PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn); @@ -1051,7 +1051,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

size ()); + PCL_DEBUG ("Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ()); return (true); } diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index 6b3a459e..374dc24e 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -116,16 +117,21 @@ pcl::MovingLeastSquares::process (PointCloudOut &output) boost::uniform_real uniform_distrib (-tmp, tmp); rng_uniform_distribution_.reset (new boost::variate_generator > (rng_alg_, uniform_distrib)); + mls_results_.resize (1); // Need to have a reference to a single dummy result. + break; } case (VOXEL_GRID_DILATION): case (DISTINCT_CLOUD): { - mls_results_.resize (input_->size ()); - break; + mls_results_.resize (input_->size ()); + break; } default: - break; + { + mls_results_.resize (1); // Need to have a reference to a single dummy result. + break; + } } // Perform the actual surface reconstruction @@ -466,6 +472,8 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & // \note resize is irrelevant for a radiusSearch (). std::vector nn_indices; std::vector nn_sqr_dists; + + size_t mls_result_index = 0; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) @@ -485,7 +493,11 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it int index = (*indices_)[cp]; - computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]); + + if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD) + mls_result_index = index; // otherwise we give it a dummy location. + + computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]); // Copy all information from the input cloud to the output points (not doing any interpolation) @@ -543,7 +555,12 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu // Get a plane approximating the local surface's tangent and project point onto it int index = (*indices_)[cp]; - this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); + size_t mls_result_index = 0; + + if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD) + mls_result_index = index; // otherwise we give it a dummy location. + + this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[mls_result_index]); // Copy all information from the input cloud to the output points (not doing any interpolation) for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) @@ -752,13 +769,8 @@ template void pcl::MovingLeastSquares::copyMissingFields (const PointInT &point_in, PointOutT &point_out) const { - typedef typename pcl::traits::fieldList::type FieldListInput; - typedef typename pcl::traits::fieldList::type FieldListOutput; - typedef typename pcl::intersect::type FieldList; - PointOutT temp = point_out; - pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, - point_out)); + copyPoint (point_in, point_out); point_out.x = temp.x; point_out.y = temp.y; point_out.z = temp.z; diff --git a/surface/include/pcl/surface/impl/texture_mapping.hpp b/surface/include/pcl/surface/impl/texture_mapping.hpp index eb542439..6d8dfff9 100644 --- a/surface/include/pcl/surface/impl/texture_mapping.hpp +++ b/surface/include/pcl/surface/impl/texture_mapping.hpp @@ -1042,12 +1042,29 @@ pcl::TextureMapping::getPointUVCoordinates(const pcl::PointXYZ &pt, co // compute image center and dimension double sizeX = cam.width; double sizeY = cam.height; - double cx = sizeX / 2.0; - double cy = sizeY / 2.0; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = sizeX / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = sizeY / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h > 0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; // project point on camera's image plane - UV_coordinates.x = static_cast ((cam.focal_length * (pt.x / pt.z) + cx) / sizeX); //horizontal - UV_coordinates.y = 1.0f - static_cast ((cam.focal_length * (pt.y / pt.z) + cy) / sizeY); //vertical + UV_coordinates.x = static_cast ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal + UV_coordinates.y = 1.0f - static_cast ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical // point is visible! if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0) diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 9de582c2..8aeb9a2f 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -184,6 +184,7 @@ namespace pcl getSqrGaussParam () const { return (sqr_gauss_param_); } /** \brief Set the upsampling method to be used + * \param method * \note Options are: * NONE - no upsampling will be done, only the input points will be projected to their own * MLS surfaces * * DISTINCT_CLOUD - will project the points of the distinct cloud to the closest point on @@ -193,7 +194,7 @@ namespace pcl * parameters * * RANDOM_UNIFORM_DENSITY - the local plane of each input point will be sampled using an * uniform random distribution such that the density of points is - * constant throughout the cloud - given by the \ref \ref desired_num_points_in_radius_ + * constant throughout the cloud - given by the \ref desired_num_points_in_radius_ * parameter * * VOXEL_GRID_DILATION - the input cloud will be inserted into a voxel grid with voxels of * size \ref voxel_size_; this voxel grid will be dilated \ref dilation_iteration_num_ @@ -449,9 +450,9 @@ namespace pcl } /** \brief Smooth a given point and its neighborghood using Moving Least Squares. - * \param[in] index the inex of the query point in the \ref input cloud - * \param[in] nn_indices the set of nearest neighbors indices for \ref pt - * \param[in] nn_sqr_dists the set of nearest neighbors squared distances for \ref pt + * \param[in] index the inex of the query point in the input cloud + * \param[in] nn_indices the set of nearest neighbors indices for pt + * \param[in] nn_sqr_dists the set of nearest neighbors squared distances for pt * \param[out] projected_points the set of points projected points around the query point * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned, * in the case of the other upsampling methods, multiple points will be returned) @@ -473,11 +474,11 @@ namespace pcl * the MLS surface of the input point * \param[in] u_disp the u coordinate of the sample point in the local plane of the query point * \param[in] v_disp the v coordinate of the sample point in the local plane of the query point - * \param[in] u the axis corresponding to the u-coordinates of the local plane of the query point - * \param[in] v the axis corresponding to the v-coordinates of the local plane of the query point - * \param[in] plane_normal the normal to the local plane of the query point + * \param[in] u_axis the axis corresponding to the u-coordinates of the local plane of the query point + * \param[in] v_axis the axis corresponding to the v-coordinates of the local plane of the query point + * \param[in] n_axis + * \param mean * \param[in] curvature the curvature of the surface at the query point - * \param[in] query_point the absolute 3D position of the query point * \param[in] c_vec the coefficients of the polynomial fit on the MLS surface of the query point * \param[in] num_neighbors the number of neighbors of the query point in the input cloud * \param[out] result_point the absolute 3D position of the resulting projected point @@ -547,6 +548,9 @@ namespace pcl using MovingLeastSquares::nr_coeff_; using MovingLeastSquares::order_; using MovingLeastSquares::compute_normals_; + using MovingLeastSquares::upsample_method_; + using MovingLeastSquares::VOXEL_GRID_DILATION; + using MovingLeastSquares::DISTINCT_CLOUD; typedef pcl::PointCloud NormalCloud; typedef pcl::PointCloud::Ptr NormalCloudPtr; diff --git a/surface/include/pcl/surface/poisson.h b/surface/include/pcl/surface/poisson.h index a13452fd..5b2b1ffa 100644 --- a/surface/include/pcl/surface/poisson.h +++ b/surface/include/pcl/surface/poisson.h @@ -253,4 +253,8 @@ namespace pcl }; } +#ifdef PCL_NO_PRECOMPILE +#include +#endif + #endif // PCL_SURFACE_POISSON_H_ diff --git a/surface/include/pcl/surface/texture_mapping.h b/surface/include/pcl/surface/texture_mapping.h index e706326a..17bddcc2 100644 --- a/surface/include/pcl/surface/texture_mapping.h +++ b/surface/include/pcl/surface/texture_mapping.h @@ -50,12 +50,26 @@ namespace pcl namespace texture_mapping { - /** \brief Structure to store camera pose and focal length. */ + /** \brief Structure to store camera pose and focal length. + * + * One can assign a value to focal_length, to be used along + * both camera axes or, optionally, axis-specific values + * (focal_length_w and focal_length_h). Optionally, one can + * also specify center-of-focus using parameters + * center_w and center_h. If the center-of-focus is not + * specified, it will be set to the geometric center of + * the camera, as defined by the width and height parameters. + */ struct Camera { - Camera () : pose (), focal_length (), height (), width (), texture_file () {} + Camera () : pose (), focal_length (), focal_length_w (-1), focal_length_h (-1), + center_w (-1), center_h (-1), height (), width (), texture_file () {} Eigen::Affine3f pose; double focal_length; + double focal_length_w; // optional + double focal_length_h; // optinoal + double center_w; // optional + double center_h; // optional double height; double width; std::string texture_file; @@ -187,11 +201,25 @@ namespace pcl // compute image center and dimension double sizeX = cam.width; double sizeY = cam.height; - double cx = (sizeX) / 2.0; - double cy = (sizeY) / 2.0; - - double focal_x = cam.focal_length; - double focal_y = cam.focal_length; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = (sizeX) / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = (sizeY) / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h>0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; // project point on image frame UV_coordinates[0] = static_cast ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal @@ -298,7 +326,7 @@ namespace pcl /** \brief Segment and texture faces by camera visibility. Face-based segmentation. * \details With N camera, faces will be arranged into N+1 groups: 1 for each camera, plus 1 for faces not visible from any camera. * The mesh will also contain uv coordinates for each face - * \param[in/out] tex_mesh input mesh that needs sorting. Should contain only 1 sub-mesh. + * \param mesh input mesh that needs sorting. Should contain only 1 sub-mesh. * \param[in] cameras vector containing the cameras used for texture mapping. */ void @@ -335,7 +363,7 @@ namespace pcl * \param[out] radius the radius of the circumscribed circle. */ inline void - getTriangleCircumcenterAndSize (const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circomcenter, double &radius); + getTriangleCircumcenterAndSize (const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius); /** \brief Returns the centroid of a triangle and the corresponding circumscribed circle's radius. diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h index 20582b2a..eae4fcb5 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h @@ -56,7 +56,7 @@ namespace pcl MeshQuadricDecimationVTK (); /** \brief Set the percentage of faces that should be removed. - * \param[in] float the factor + * \param[in] factor the factor */ inline void setTargetReductionFactor (float factor) diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h index e103095a..98e63faf 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h @@ -117,7 +117,7 @@ namespace pcl }; /** \brief Turn on/off smoothing along sharp interior edges. - * \param[in] status decision whether to enable/disable smoothing along sharp interior edges + * \param[in] feature_edge_smoothing whether to enable/disable smoothing along sharp interior edges */ inline void setFeatureEdgeSmoothing (bool feature_edge_smoothing) diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h index 83b39569..b25bb941 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h @@ -116,7 +116,7 @@ namespace pcl } /** \brief Turn on/off smoothing along sharp interior edges. - * \param[in] status decision whether to enable/disable smoothing along sharp interior edges + * \param[in] feature_edge_smoothing whether to enable/disable smoothing along sharp interior edges */ inline void setFeatureEdgeSmoothing (bool feature_edge_smoothing) diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_utils.h b/surface/include/pcl/surface/vtk_smoothing/vtk_utils.h index 9a128a97..c28adbf4 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_utils.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_utils.h @@ -50,11 +50,13 @@ namespace pcl public: /** \brief Convert a PCL PolygonMesh to a VTK vtkPolyData. * \param[in] triangles PolygonMesh to be converted to vtkPolyData, stored in the object. + * \param[out] triangles_out_vtk */ static int convertToVTK (const pcl::PolygonMesh &triangles, vtkSmartPointer &triangles_out_vtk); /** \brief Convert the vtkPolyData object back to PolygonMesh. + * \param[in] vtk_polygons * \param[out] triangles the PolygonMesh to store the vtkPolyData in. */ static void diff --git a/surface/src/ear_clipping.cpp b/surface/src/ear_clipping.cpp index 01ae1e10..dc61a02b 100644 --- a/surface/src/ear_clipping.cpp +++ b/surface/src/ear_clipping.cpp @@ -68,8 +68,13 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output) { const int n_vertices = static_cast (vertices.vertices.size ()); - if (n_vertices <= 3) + if (n_vertices < 3) return; + else if (n_vertices == 3) + { + output.polygons.push_back( vertices ); + return; + } std::vector remaining_vertices (n_vertices); if (area (vertices.vertices) > 0) // clockwise? @@ -109,19 +114,36 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output) float pcl::EarClipping::area (const std::vector& vertices) { - int n = static_cast (vertices.size ()); - float area = 0.0f; - Eigen::Vector2f prev_p, cur_p; - for (int prev = n - 1, cur = 0; cur < n; prev = cur++) - { - prev_p[0] = points_->points[vertices[prev]].x; - prev_p[1] = points_->points[vertices[prev]].y; - cur_p[0] = points_->points[vertices[cur]].x; - cur_p[1] = points_->points[vertices[cur]].y; + //if the polygon is projected onto the xy-plane, the area of the polygon is determined + //by the trapeze formula of Gauss. However this fails, if the projection is one 'line'. + //Therefore the following implementation determines the area of the flat polygon in 3D-space + //using Stoke's law: http://code.activestate.com/recipes/578276-3d-polygon-area/ + + int n = static_cast (vertices.size ()); + float area = 0.0f; + Eigen::Vector3f prev_p, cur_p; + Eigen::Vector3f total (0,0,0); + Eigen::Vector3f unit_normal; + + if (n > 3) + { + for (int prev = n - 1, cur = 0; cur < n; prev = cur++) + { + prev_p = points_->points[vertices[prev]].getVector3fMap(); + cur_p = points_->points[vertices[cur]].getVector3fMap(); - area += crossProduct (prev_p, cur_p); - } - return (area * 0.5f); + total += prev_p.cross( cur_p ); + } + + //unit_normal is unit normal vector of plane defined by the first three points + prev_p = points_->points[vertices[1]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap(); + cur_p = points_->points[vertices[2]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap(); + unit_normal = (prev_p.cross(cur_p)).normalized(); + + area = total.dot( unit_normal ); + } + + return area * 0.5f; } @@ -129,31 +151,27 @@ pcl::EarClipping::area (const std::vector& vertices) bool pcl::EarClipping::isEar (int u, int v, int w, const std::vector& vertices) { - Eigen::Vector2f p_u, p_v, p_w; - p_u[0] = points_->points[vertices[u]].x; - p_u[1] = points_->points[vertices[u]].y; - p_v[0] = points_->points[vertices[v]].x; - p_v[1] = points_->points[vertices[v]].y; - p_w[0] = points_->points[vertices[w]].x; - p_w[1] = points_->points[vertices[w]].y; + Eigen::Vector3f p_u, p_v, p_w; + p_u = points_->points[vertices[u]].getVector3fMap(); + p_v = points_->points[vertices[v]].getVector3fMap(); + p_w = points_->points[vertices[w]].getVector3fMap(); - // Avoid flat triangles. - // FIXME: triangulation would fail if all the triangles are flat in the X-Y axis const float eps = 1e-15f; - Eigen::Vector2f p_uv, p_uw; + Eigen::Vector3f p_uv, p_uw; p_uv = p_v - p_u; p_uw = p_w - p_u; - if (crossProduct (p_uv, p_uw) < eps) + + // Avoid flat triangles. + if ((p_uv.cross(p_uw)).norm() < eps) return (false); - Eigen::Vector2f p; + Eigen::Vector3f p; // Check if any other vertex is inside the triangle. for (int k = 0; k < static_cast (vertices.size ()); k++) { if ((k == u) || (k == v) || (k == w)) continue; - p[0] = points_->points[vertices[k]].x; - p[1] = points_->points[vertices[k]].y; + p = points_->points[vertices[k]].getVector3fMap(); if (isInsideTriangle (p_u, p_v, p_w, p)) return (false); @@ -163,23 +181,30 @@ pcl::EarClipping::isEar (int u, int v, int w, const std::vector& verti ///////////////////////////////////////////////////////////////////////////////////////////// bool -pcl::EarClipping::isInsideTriangle (const Eigen::Vector2f& u, - const Eigen::Vector2f& v, - const Eigen::Vector2f& w, - const Eigen::Vector2f& p) +pcl::EarClipping::isInsideTriangle (const Eigen::Vector3f& u, + const Eigen::Vector3f& v, + const Eigen::Vector3f& w, + const Eigen::Vector3f& p) { - // Check first side. - if (crossProduct (w - v, p - v) < 0) - return (false); - - // Check second side. - if (crossProduct (v - u, p - u) < 0) - return (false); - - // Check third side. - if (crossProduct (u - w, p - w) < 0) - return (false); - - return (true); + // see http://www.blackpawn.com/texts/pointinpoly/default.html + // Barycentric Coordinates + Eigen::Vector3f v0 = w - u; + Eigen::Vector3f v1 = v - u; + Eigen::Vector3f v2 = p - u; + + // Compute dot products + float dot00 = v0.dot(v0); + float dot01 = v0.dot(v1); + float dot02 = v0.dot(v2); + float dot11 = v1.dot(v1); + float dot12 = v1.dot(v2); + + // Compute barycentric coordinates + float invDenom = 1 / (dot00 * dot11 - dot01 * dot01); + float a = (dot11 * dot02 - dot01 * dot12) * invDenom; + float b = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // Check if point is in triangle + return (a >= 0) && (b >= 0) && (a + b < 1); } diff --git a/surface/src/on_nurbs/sequential_fitter.cpp b/surface/src/on_nurbs/sequential_fitter.cpp index a9849970..e88d1eca 100644 --- a/surface/src/on_nurbs/sequential_fitter.cpp +++ b/surface/src/on_nurbs/sequential_fitter.cpp @@ -483,13 +483,13 @@ SequentialFitter::grow (float max_dist, float max_angle, unsigned min_length, un if (unsigned (this->m_data.boundary.size ()) != num_bnd) { - printf ("[SequentialFitter::grow] %zu %u\n", this->m_data.boundary.size (), num_bnd); + printf ("[SequentialFitter::grow] %lu %u\n", this->m_data.boundary.size (), num_bnd); throw std::runtime_error ("[SequentialFitter::grow] size of boundary and boundary parameters do not match."); } if (this->m_boundary_indices->indices.size () != num_bnd) { - printf ("[SequentialFitter::grow] %zu %u\n", this->m_boundary_indices->indices.size (), num_bnd); + printf ("[SequentialFitter::grow] %lu %u\n", this->m_boundary_indices->indices.size (), num_bnd); throw std::runtime_error ("[SequentialFitter::grow] size of boundary indices and boundary parameters do not match."); } diff --git a/surface/src/vtk_smoothing/vtk_mesh_quadric_decimation.cpp b/surface/src/vtk_smoothing/vtk_mesh_quadric_decimation.cpp index 9485106a..a10e68ab 100644 --- a/surface/src/vtk_smoothing/vtk_mesh_quadric_decimation.cpp +++ b/surface/src/vtk_smoothing/vtk_mesh_quadric_decimation.cpp @@ -39,6 +39,7 @@ #include #include +#include #include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -59,7 +60,11 @@ pcl::MeshQuadricDecimationVTK::performProcessing (pcl::PolygonMesh &output) // Apply the VTK algorithm vtkSmartPointer vtk_quadric_decimation_filter = vtkSmartPointer::New(); vtk_quadric_decimation_filter->SetTargetReduction (target_reduction_factor_); +#if VTK_MAJOR_VERSION < 6 vtk_quadric_decimation_filter->SetInput (vtk_polygons_); +#else + vtk_quadric_decimation_filter->SetInputData (vtk_polygons_); +#endif vtk_quadric_decimation_filter->Update (); vtk_polygons_ = vtk_quadric_decimation_filter->GetOutput (); diff --git a/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp b/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp index ac6ced46..11340065 100644 --- a/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp +++ b/surface/src/vtk_smoothing/vtk_mesh_smoothing_laplacian.cpp @@ -39,6 +39,7 @@ #include #include +#include #include @@ -51,7 +52,11 @@ pcl::MeshSmoothingLaplacianVTK::performProcessing (pcl::PolygonMesh &output) // Apply the VTK algorithm vtkSmartPointer vtk_smoother = vtkSmoothPolyDataFilter::New (); +#if VTK_MAJOR_VERSION < 6 vtk_smoother->SetInput (vtk_polygons_); +#else + vtk_smoother->SetInputData (vtk_polygons_); +#endif vtk_smoother->SetNumberOfIterations (num_iter_); if (convergence_ != 0.0f) vtk_smoother->SetConvergence (convergence_); diff --git a/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp b/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp index d01f155d..67873674 100644 --- a/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp +++ b/surface/src/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.cpp @@ -39,6 +39,7 @@ #include #include +#include #include @@ -51,7 +52,11 @@ pcl::MeshSmoothingWindowedSincVTK::performProcessing (pcl::PolygonMesh &output) // Apply the VTK algorithm vtkSmartPointer vtk_smoother = vtkWindowedSincPolyDataFilter::New (); +#if VTK_MAJOR_VERSION < 6 vtk_smoother->SetInput (vtk_polygons_); +#else + vtk_smoother->SetInputData (vtk_polygons_); +#endif vtk_smoother->SetNumberOfIterations (num_iter_); vtk_smoother->SetPassBand (pass_band_); vtk_smoother->SetNormalizeCoordinates (normalize_coordinates_); diff --git a/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp b/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp index d6309e3f..7debf28b 100644 --- a/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp +++ b/surface/src/vtk_smoothing/vtk_mesh_subdivision.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -78,7 +79,11 @@ pcl::MeshSubdivisionVTK::performProcessing (pcl::PolygonMesh &output) break; } +#if VTK_MAJOR_VERSION < 6 vtk_subdivision_filter->SetInput (vtk_polygons_); +#else + vtk_subdivision_filter->SetInputData (vtk_polygons_); +#endif vtk_subdivision_filter->Update (); vtk_polygons_ = vtk_subdivision_filter->GetOutput (); diff --git a/surface/src/vtk_smoothing/vtk_utils.cpp b/surface/src/vtk_smoothing/vtk_utils.cpp index 9a4e7dc6..7c47a797 100644 --- a/surface/src/vtk_smoothing/vtk_utils.cpp +++ b/surface/src/vtk_smoothing/vtk_utils.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -63,7 +64,11 @@ pcl::VTKUtils::convertToVTK (const pcl::PolygonMesh &triangles, vtkSmartPointer< mesh2vtk (triangles, vtk_polygons); vtkSmartPointer vtk_triangles = vtkTriangleFilter::New (); +#if VTK_MAJOR_VERSION < 6 vtk_triangles->SetInput (vtk_polygons); +#else + vtk_triangles->SetInputData (vtk_polygons); +#endif vtk_triangles->Update(); triangles_out_vtk = vtk_triangles->GetOutput (); diff --git a/surface/surface.doxy b/surface/surface.doxy index 82ff8c80..2996988f 100644 --- a/surface/surface.doxy +++ b/surface/surface.doxy @@ -13,7 +13,7 @@ composed of multiple scans that are not aligned perfectly. The complexity of the surface estimation can be adjusted, and normals can be estimated in the same step if needed. -\image html http://www.pointclouds.org/documentation/tutorials/_images/resampling_1.png +\image html http://www.pointclouds.org/documentation/tutorials/_images/resampling_1.jpg Meshing is a general way to create a surface out of points, and currently there are two algorithms provided: a very fast triangulation of the original diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0e10301f..d8027ebc 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -2,7 +2,7 @@ set(SUBSYS_NAME global_tests) set(SUBSYS_DESC "Point cloud library global unit tests") if(BUILD_visualization) - include (${VTK_USE_FILE}) + include("${VTK_USE_FILE}") set(SUBSYS_DEPS common sample_consensus io kdtree features filters geometry keypoints search surface registration segmentation octree recognition people outofcore visualization) set(OPT_DEPS vtk) else() @@ -11,14 +11,15 @@ endif() set(DEFAULT OFF) set(build TRUE) -PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ${DEFAULT} ${REASON}) -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS ${OPT_DEPS}) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${OPT_DEPS}) if(build) - include_directories(${PCL_SOURCE_DIR}/test/gtest-1.6.0/include) - include_directories(${PCL_SOURCE_DIR}/test/gtest-1.6.0/) - add_library(pcl_gtest STATIC gtest-1.6.0/src/gtest-all.cc) + find_package(Gtest REQUIRED) + include_directories(SYSTEM ${GTEST_INCLUDE_DIRS} ${GTEST_SRC_DIR}) + + add_library(pcl_gtest STATIC ${GTEST_SRC_DIR}/src/gtest-all.cc) if( MSVC11 ) # VS2012 doesn't correctly support variadic templates yet add_definitions("-D_VARIADIC_MAX=10") @@ -27,6 +28,8 @@ if(build) enable_testing() include_directories(${PCL_INCLUDE_DIRS}) + add_custom_target(tests "${CMAKE_CTEST_COMMAND}" "-V" VERBATIM) + add_subdirectory(common) add_subdirectory(features) add_subdirectory(filters) @@ -39,52 +42,52 @@ if(build) add_subdirectory(search) add_subdirectory(keypoints) add_subdirectory(surface) + add_subdirectory(sample_consensus) - PCL_ADD_TEST(a_recognition_ism_test test_recognition_ism + PCL_ADD_TEST(a_bearing_angle_image_test test_bearing_angle_image + FILES test_bearing_angle_image.cpp + LINK_WITH pcl_gtest pcl_common pcl_io) + + PCL_ADD_TEST(a_recognition_ism_test test_recognition_ism FILES test_recognition_ism.cpp LINK_WITH pcl_gtest pcl_io pcl_features - ARGUMENTS ${PCL_SOURCE_DIR}/test/ism_train.pcd ${PCL_SOURCE_DIR}/test/ism_test.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/ism_train.pcd" "${PCL_SOURCE_DIR}/test/ism_test.pcd") PCL_ADD_TEST(search test_search FILES test_search.cpp LINK_WITH pcl_gtest pcl_search pcl_io pcl_kdtree - ARGUMENTS ${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd") - PCL_ADD_TEST(a_sample_consensus_test test_sample_consensus - FILES test_sample_consensus.cpp - LINK_WITH pcl_gtest pcl_io pcl_sample_consensus pcl_kdtree pcl_features - ARGUMENTS ${PCL_SOURCE_DIR}/test/sac_plane_test.pcd) - PCL_ADD_TEST(a_transforms_test test_transforms FILES test_transforms.cpp LINK_WITH pcl_gtest pcl_io - ARGUMENTS ${PCL_SOURCE_DIR}/test/bun0.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd") PCL_ADD_TEST(a_segmentation_test test_segmentation FILES test_segmentation.cpp LINK_WITH pcl_gtest pcl_io pcl_segmentation pcl_features pcl_kdtree pcl_search pcl_common - ARGUMENTS ${PCL_SOURCE_DIR}/test/bun0.pcd ${PCL_SOURCE_DIR}/test/car6.pcd ${PCL_SOURCE_DIR}/test/colored_cloud.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/car6.pcd" "${PCL_SOURCE_DIR}/test/colored_cloud.pcd") PCL_ADD_TEST(test_non_linear test_non_linear FILES test_non_linear.cpp LINK_WITH pcl_gtest pcl_common pcl_io pcl_sample_consensus pcl_segmentation pcl_kdtree pcl_search - ARGUMENTS ${PCL_SOURCE_DIR}/test/noisy_slice_displaced.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/noisy_slice_displaced.pcd") PCL_ADD_TEST(a_recognition_cg_test test_recognition_cg FILES test_recognition_cg.cpp LINK_WITH pcl_gtest pcl_common pcl_io pcl_kdtree pcl_features pcl_recognition pcl_keypoints - ARGUMENTS ${PCL_SOURCE_DIR}/test/milk.pcd ${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/milk.pcd" "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd") PCL_ADD_TEST(a_people_detection_test test_people_detection FILES test_people_groundBasedPeopleDetectionApp.cpp LINK_WITH pcl_gtest pcl_common pcl_io pcl_kdtree pcl_search pcl_features pcl_sample_consensus pcl_filters pcl_io pcl_segmentation pcl_people - ARGUMENTS ${PCL_SOURCE_DIR}/people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml ${PCL_SOURCE_DIR}/test/five_people.pcd ) + ARGUMENTS "${PCL_SOURCE_DIR}/people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml" "${PCL_SOURCE_DIR}/test/five_people.pcd") if(BUILD_visualization AND (NOT UNIX OR (UNIX AND DEFINED ENV{DISPLAY}))) PCL_ADD_TEST(a_visualization_test test_visualization FILES test_visualization.cpp LINK_WITH pcl_gtest pcl_io pcl_visualization pcl_features - ARGUMENTS ${PCL_SOURCE_DIR}/test/bunny.pcd) + ARGUMENTS "${PCL_SOURCE_DIR}/test/bunny.pcd") endif() endif(build) diff --git a/test/common/CMakeLists.txt b/test/common/CMakeLists.txt index bafa5fd8..30b863a8 100644 --- a/test/common/CMakeLists.txt +++ b/test/common/CMakeLists.txt @@ -3,6 +3,8 @@ PCL_ADD_TEST(common_test_wrappers test_wrappers FILES test_wrappers.cpp LINK_WIT PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest) PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_centroid test_centroid FILES test_centroid.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_int test_plane_intersection FILES test_plane_intersection.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_pca test_pca FILES test_pca.cpp LINK_WITH pcl_gtest pcl_common) #PCL_ADD_TEST(common_spring test_spring FILES test_spring.cpp LINK_WITH pcl_gtest pcl_common) @@ -13,5 +15,6 @@ PCL_ADD_TEST(common_eigen test_eigen FILES test_eigen.cpp LINK_WITH pcl_gtest pc PCL_ADD_TEST(common_intensity test_intensity FILES test_intensity.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_generator test_generator FILES test_generator.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_io test_common_io FILES test_io.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_copy_make_borders test_copy_make_borders FILES test_copy_make_borders.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_point_type_conversion test_common_point_type_conversion FILES test_point_type_conversion.cpp LINK_WITH pcl_gtest pcl_common) diff --git a/test/common/test_centroid.cpp b/test/common/test_centroid.cpp new file mode 100644 index 00000000..3e2d2dd3 --- /dev/null +++ b/test/common/test_centroid.cpp @@ -0,0 +1,1001 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace pcl; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, compute3DCentroidFloat) +{ + pcl::PointIndices pindices; + std::vector indices; + PointXYZ point; + PointCloud cloud; + Eigen::Vector4f centroid; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (centroid [3], 1); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + EXPECT_EQ (centroid [3], 1.0); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (centroid [3], 1); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + indices.push_back (8); // add the NaN + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + EXPECT_EQ (centroid [3], 1.0); + + pindices.indices = indices; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + EXPECT_EQ (centroid [3], 1.0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, compute3DCentroidDouble) +{ + pcl::PointIndices pindices; + std::vector indices; + PointXYZ point; + PointCloud cloud; + Eigen::Vector4d centroid; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (centroid [3], 1); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (centroid [3], 1); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + indices.push_back (8); // add the NaN + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + EXPECT_EQ (centroid [3], 1.0); + + pindices.indices = indices; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + EXPECT_EQ (centroid [3], 1.0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, compute3DCentroidCloudIterator) +{ + pcl::PointIndices pindices; + std::vector indices; + PointXYZ point; + PointCloud cloud; + Eigen::Vector4f centroid_f; + + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + + ConstCloudIterator it (cloud, indices); + + EXPECT_EQ (compute3DCentroid (it, centroid_f), 4); + + EXPECT_EQ (centroid_f[0], 0.0f); + EXPECT_EQ (centroid_f[1], 1.0f); + EXPECT_EQ (centroid_f[2], 0.0f); + EXPECT_EQ (centroid_f[3], 1.0f); + + Eigen::Vector4d centroid_d; + it.reset (); + EXPECT_EQ (compute3DCentroid (it, centroid_d), 4); + + EXPECT_EQ (centroid_d[0], 0.0); + EXPECT_EQ (centroid_d[1], 1.0); + EXPECT_EQ (centroid_d[2], 0.0); + EXPECT_EQ (centroid_d[3], 1.0); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeCovarianceMatrix) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Vector4f centroid; + Eigen::Matrix3f covariance_matrix; + + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 8); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 8); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 8); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 4); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 4); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 0; + + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 8); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 8); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 8); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 4); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 4); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeCovarianceMatrixNormalized) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Vector4f centroid; + Eigen::Matrix3f covariance_matrix; + + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8); + + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4); + + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 0; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeDemeanedCovariance) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Matrix3f covariance_matrix; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 0); + + cloud.clear (); + indices.clear (); + + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeMeanAndCovariance) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f centroid; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 0); + + cloud.clear (); + indices.clear (); + + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8); + + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 1); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 1); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, CentroidPoint) +{ + PointXYZ p1; p1.getVector3fMap () << 1, 2, 3; + PointXYZ p2; p2.getVector3fMap () << 3, 2, 1; + PointXYZ p3; p3.getVector3fMap () << 5, 5, 5; + + // Zero points (get should have no effect) + { + CentroidPoint centroid; + PointXYZ c (100, 100, 100); + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (100, 100, 100), c); + } + // Single point + { + CentroidPoint centroid; + centroid.add (p1); + PointXYZ c; + centroid.get (c); + EXPECT_XYZ_EQ (p1, c); + } + // Multiple points + { + CentroidPoint centroid; + centroid.add (p1); + centroid.add (p2); + centroid.add (p3); + PointXYZ c; + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (3, 3, 3), c); + } + + // Retrieve centroid into a different point type + { + CentroidPoint centroid; + centroid.add (p1); + PointXYZRGB c; c.rgba = 0x00FFFFFF; + centroid.get (c); + EXPECT_XYZ_EQ (p1, c); + EXPECT_EQ (0x00FFFFFF, c.rgba); + } + + // Centroid with XYZ and RGB + { + PointXYZRGB cp1; cp1.getVector3fMap () << 4, 2, 4; cp1.rgba = 0xFF330000; + PointXYZRGB cp2; cp2.getVector3fMap () << 2, 4, 2; cp2.rgba = 0xFF003300; + PointXYZRGB cp3; cp3.getVector3fMap () << 3, 3, 3; cp3.rgba = 0xFF000033; + CentroidPoint centroid; + centroid.add (cp1); + centroid.add (cp2); + centroid.add (cp3); + PointXYZRGB c; + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (3, 3, 3), c); + EXPECT_EQ (0xFF111111, c.rgba); + } + + // Centroid with normal and curavture + { + Normal np1; np1.getNormalVector4fMap () << 1, 0, 0, 0; np1.curvature = 0.2; + Normal np2; np2.getNormalVector4fMap () << -1, 0, 0, 0; np2.curvature = 0.1; + Normal np3; np3.getNormalVector4fMap () << 0, 1, 0, 0; np3.curvature = 0.9; + CentroidPoint centroid; + centroid.add (np1); + centroid.add (np2); + centroid.add (np3); + Normal c; + centroid.get (c); + EXPECT_NORMAL_EQ (np3, c); + EXPECT_FLOAT_EQ (0.4, c.curvature); + } + + // Centroid with XYZ and intensity + { + PointXYZI ip1; ip1.getVector3fMap () << 1, 2, 3; ip1.intensity = 0.8; + PointXYZI ip2; ip2.getVector3fMap () << 3, 2, 1; ip2.intensity = 0.2; + PointXYZI ip3; ip3.getVector3fMap () << 5, 5, 5; ip3.intensity = 0.2; + CentroidPoint centroid; + centroid.add (ip1); + centroid.add (ip2); + centroid.add (ip3); + PointXYZI c; + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (3, 3, 3), c); + EXPECT_FLOAT_EQ (0.4, c.intensity); + } + + // Centroid with label + { + Label lp1; lp1.label = 1; + Label lp2; lp2.label = 1; + Label lp3; lp3.label = 2; + CentroidPoint