vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e

modules/surface_matching/CMakeLists.txt

@@ -0,0 +1,2 @@
+set(the_description "3D point features")
+ocv_define_module(surface_matching opencv_core opencv_flann WRAP python)

modules/surface_matching/PATENTS.txt

@@ -0,0 +1,29 @@
+The following patents have been issued for methods embodied in this
+software: "Recognition and pose determination of 3D objects in 3D scenes
+using geometric point pair descriptors and the generalized Hough
+Transform", Bertram Heinrich Drost, Markus Ulrich, EP Patent 2385483
+(Nov. 21, 2012), assignee: MVTec Software GmbH, 81675 Muenchen
+(Germany); "Recognition and pose determination of 3D objects in 3D
+scenes", Bertram Heinrich Drost, Markus Ulrich, US Patent 8830229 (Sept.
+9, 2014), assignee: MVTec Software GmbH, 81675 Muenchen (Germany).
+Further patents are pending. For further details, contact MVTec Software
+GmbH (info@mvtec.com).
+
+Note that restrictions imposed by these patents (and possibly others)
+exist independently of and may be in conflict with the freedoms granted
+in OpenCV license, which refers to copyright of the program, not patents
+for any methods that it implements. Both copyright and patent law must
+be obeyed to legally use and redistribute this program and it is not the
+purpose of this license to induce you to infringe any patents or other
+property right claims or to contest validity of any such claims.  If you
+redistribute or use the program, then this license merely protects you
+from committing copyright infringement.  It does not protect you from
+committing patent infringement.  So, before you do anything with this
+program, make sure that you have permission to do so not merely in terms
+of copyright, but also in terms of patent law.
+
+Please note that this license is not to be understood as a guarantee
+either.  If you use the program according to this license, but in
+conflict with patent law, it does not mean that the licensor will refund
+you for any losses that you incur if you are sued for your patent
+infringement.

modules/surface_matching/README.md

@@ -0,0 +1,5 @@
+Point Pair Features for 3D Surface Matching
+===========================================
+
+Implements 3d object detection and localization using multimodal point pair features.
+http://docs.opencv.org/3.0-beta/modules/surface_matching/doc/surface_matching.html

modules/surface_matching/doc/surface_matching.bib

@@ -0,0 +1,23 @@
+@inproceedings{drost2010,
+  title={3d object detection and localization using multimodal point pair features},
+  author={Drost, Bertram and Ilic, Slobodan},
+  booktitle={3D Imaging, Modeling, Processing, Visualization and Transmission (3DIMPVT), 2012 Second International Conference on},
+  pages={9--16},
+  year={2012},
+  organization={IEEE}
+}
+@inproceedings{pickyicp,
+  title={A refined ICP algorithm for robust 3-D correspondence estimation},
+  author={Zin{\ss}er, Timo and Schmidt, Jochen and Niemann, Heinrich},
+  booktitle={Image Processing, 2003. ICIP 2003. Proceedings. 2003 International Conference on},
+  volume={2},
+  pages={II--695},
+  year={2003},
+  organization={IEEE}
+}
+@article{koklimlow,
+  title={Linear least-squares optimization for point-to-plane icp surface registration},
+  author={Low, Kok-Lim},
+  journal={Chapel Hill, University of North Carolina},
+  year={2004}
+}

modules/surface_matching/include/opencv2/surface_matching.hpp

@@ -0,0 +1,402 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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 __OPENCV_SURFACE_MATCHING_HPP__
+#define __OPENCV_SURFACE_MATCHING_HPP__
+
+#include "surface_matching/ppf_match_3d.hpp"
+#include "surface_matching/icp.hpp"
+
+/** @defgroup surface_matching Surface Matching
+
+Note about the License and Patents
+-----------------------------------
+
+The following patents have been issued for methods embodied in this
+software: "Recognition and pose determination of 3D objects in 3D scenes
+using geometric point pair descriptors and the generalized Hough
+Transform", Bertram Heinrich Drost, Markus Ulrich, EP Patent 2385483
+(Nov. 21, 2012), assignee: MVTec Software GmbH, 81675 Muenchen
+(Germany); "Recognition and pose determination of 3D objects in 3D
+scenes", Bertram Heinrich Drost, Markus Ulrich, US Patent 8830229 (Sept.
+9, 2014), assignee: MVTec Software GmbH, 81675 Muenchen (Germany).
+Further patents are pending. For further details, contact MVTec Software
+GmbH (info@mvtec.com).
+
+Note that restrictions imposed by these patents (and possibly others)
+exist independently of and may be in conflict with the freedoms granted
+in this license, which refers to copyright of the program, not patents
+for any methods that it implements.  Both copyright and patent law must
+be obeyed to legally use and redistribute this program and it is not the
+purpose of this license to induce you to infringe any patents or other
+property right claims or to contest validity of any such claims.  If you
+redistribute or use the program, then this license merely protects you
+from committing copyright infringement.  It does not protect you from
+committing patent infringement.  So, before you do anything with this
+program, make sure that you have permission to do so not merely in terms
+of copyright, but also in terms of patent law.
+
+Please note that this license is not to be understood as a guarantee
+either.  If you use the program according to this license, but in
+conflict with patent law, it does not mean that the licensor will refund
+you for any losses that you incur if you are sued for your patent
+infringement.
+
+
+Introduction to Surface Matching
+--------------------------------
+
+Cameras and similar devices with the capability of sensation of 3D structure are becoming more
+common. Thus, using depth and intensity information for matching 3D objects (or parts) are of
+crucial importance for computer vision. Applications range from industrial control to guiding
+everyday actions for visually impaired people. The task in recognition and pose estimation in range
+images aims to identify and localize a queried 3D free-form object by matching it to the acquired
+database.
+
+From an industrial perspective, enabling robots to automatically locate and pick up randomly placed
+and oriented objects from a bin is an important challenge in factory automation, replacing tedious
+and heavy manual labor. A system should be able to recognize and locate objects with a predefined
+shape and estimate the position with the precision necessary for a gripping robot to pick it up.
+This is where vision guided robotics takes the stage. Similar tools are also capable of guiding
+robots (and even people) through unstructured environments, leading to automated navigation. These
+properties make 3D matching from point clouds a ubiquitous necessity. Within this context, I will
+now describe the OpenCV implementation of a 3D object recognition and pose estimation algorithm
+using 3D features.
+
+Surface Matching Algorithm Through 3D Features
+----------------------------------------------
+
+The state of the algorithms in order to achieve the task 3D matching is heavily based on
+@cite drost2010, which is one of the first and main practical methods presented in this area. The
+approach is composed of extracting 3D feature points randomly from depth images or generic point
+clouds, indexing them and later in runtime querying them efficiently. Only the 3D structure is
+considered, and a trivial hash table is used for feature queries.
+
+While being fully aware that utilization of the nice CAD model structure in order to achieve a smart
+point sampling, I will be leaving that aside now in order to respect the generalizability of the
+methods (Typically for such algorithms training on a CAD model is not needed, and a point cloud
+would be sufficient). Below is the outline of the entire algorithm:
+
+![Outline of the Algorithm](img/outline.jpg)
+
+As explained, the algorithm relies on the extraction and indexing of point pair features, which are
+defined as follows:
+
+\f[\bf{{F}}(\bf{{m1}}, \bf{{m2}}) = (||\bf{{d}}||_2, <(\bf{{n1}},\bf{{d}}), <(\bf{{n2}},\bf{{d}}), <(\bf{{n1}},\bf{{n2}}))\f]
+
+where \f$\bf{{m1}}\f$ and \f$\bf{{m2}}\f$ are feature two selected points on the model (or scene),
+\f$\bf{{d}}\f$ is the difference vector, \f$\bf{{n1}}\f$ and \f$\bf{{n2}}\f$ are the normals at \f$\bf{{m1}}\f$ and
+\f$\bf{m2}\f$. During the training stage, this vector is quantized, indexed. In the test stage, same
+features are extracted from the scene and compared to the database. With a few tricks like
+separation of the rotational components, the pose estimation part can also be made efficient (check
+the reference for more details). A Hough-like voting and clustering is employed to estimate the
+object pose. To cluster the poses, the raw pose hypotheses are sorted in decreasing order of the
+number of votes. From the highest vote, a new cluster is created. If the next pose hypothesis is
+close to one of the existing clusters, the hypothesis is added to the cluster and the cluster center
+is updated as the average of the pose hypotheses within the cluster. If the next hypothesis is not
+close to any of the clusters, it creates a new cluster. The proximity testing is done with fixed
+thresholds in translation and rotation. Distance computation and averaging for translation are
+performed in the 3D Euclidean space, while those for rotation are performed using quaternion
+representation. After clustering, the clusters are sorted in decreasing order of the total number of
+votes which determines confidence of the estimated poses.
+
+This pose is further refined using \f$ICP\f$ in order to obtain the final pose.
+
+PPF presented above depends largely on robust computation of angles between 3D vectors. Even though
+not reported in the paper, the naive way of doing this (\f$\theta = cos^{-1}({\bf{a}}\cdot{\bf{b}})\f$
+remains numerically unstable. A better way to do this is then use inverse tangents, like:
+
+\f[<(\bf{n1},\bf{n2})=tan^{-1}(||{\bf{n1}  \wedge \bf{n2}}||_2, \bf{n1} \cdot \bf{n2})\f]
+
+Rough Computation of Object Pose Given PPF
+------------------------------------------
+
+Let me summarize the following notation:
+
+-   \f$p^i_m\f$: \f$i^{th}\f$ point of the model (\f$p^j_m\f$ accordingly)
+-   \f$n^i_m\f$: Normal of the \f$i^{th}\f$ point of the model (\f$n^j_m\f$ accordingly)
+-   \f$p^i_s\f$: \f$i^{th}\f$ point of the scene (\f$p^j_s\f$ accordingly)
+-   \f$n^i_s\f$: Normal of the \f$i^{th}\f$ point of the scene (\f$n^j_s\f$ accordingly)
+-   \f$T_{m\rightarrow g}\f$: The transformation required to translate \f$p^i_m\f$ to the origin and rotate
+    its normal \f$n^i_m\f$ onto the \f$x\f$-axis.
+-   \f$R_{m\rightarrow g}\f$: Rotational component of \f$T_{m\rightarrow g}\f$.
+-   \f$t_{m\rightarrow g}\f$: Translational component of \f$T_{m\rightarrow g}\f$.
+-   \f$(p^i_m)^{'}\f$: \f$i^{th}\f$ point of the model transformed by \f$T_{m\rightarrow g}\f$. (\f$(p^j_m)^{'}\f$
+    accordingly).
+-   \f${\bf{R_{m\rightarrow g}}}\f$: Axis angle representation of rotation \f$R_{m\rightarrow g}\f$.
+-   \f$\theta_{m\rightarrow g}\f$: The angular component of the axis angle representation
+    \f${\bf{R_{m\rightarrow g}}}\f$.
+
+The transformation in a point pair feature is computed by first finding the transformation
+\f$T_{m\rightarrow g}\f$ from the first point, and applying the same transformation to the second one.
+Transforming each point, together with the normal, to the ground plane leaves us with an angle to
+find out, during a comparison with a new point pair.
+
+We could now simply start writing
+
+\f[(p^i_m)^{'} = T_{m\rightarrow g} p^i_m\f]
+
+where
+
+\f[T_{m\rightarrow g} = -t_{m\rightarrow g}R_{m\rightarrow g}\f]
+
+Note that this is nothing but a stacked transformation. The translational component
+\f$t_{m\rightarrow g}\f$ reads
+
+\f[t_{m\rightarrow g} = -R_{m\rightarrow g}p^i_m\f]
+
+and the rotational being
+
+\f[\theta_{m\rightarrow g} = \cos^{-1}(n^i_m \cdot {\bf{x}})\\
+ {\bf{R_{m\rightarrow g}}} = n^i_m \wedge {\bf{x}}\f]
+
+in axis angle format. Note that bold refers to the vector form. After this transformation, the
+feature vectors of the model are registered onto the ground plane X and the angle with respect to
+\f$x=0\f$ is called \f$\alpha_m\f$. Similarly, for the scene, it is called \f$\alpha_s\f$.
+
+### Hough-like Voting Scheme
+
+As shown in the outline, PPF (point pair features) are extracted from the model, quantized, stored
+in the hashtable and indexed, during the training stage. During the runtime however, the similar
+operation is perfomed on the input scene with the exception that this time a similarity lookup over
+the hashtable is performed, instead of an insertion. This lookup also allows us to compute a
+transformation to the ground plane for the scene pairs. After this point, computing the rotational
+component of the pose reduces to computation of the difference \f$\alpha=\alpha_m-\alpha_s\f$. This
+component carries the cue about the object pose. A Hough-like voting scheme is performed over the
+local model coordinate vector and \f$\alpha\f$. The highest poses achieved for every scene point lets us
+recover the object pose.
+
+### Source Code for PPF Matching
+
+~~~{cpp}
+// pc is the loaded point cloud of the model
+// (Nx6) and pcTest is a loaded point cloud of
+// the scene (Mx6)
+ppf_match_3d::PPF3DDetector detector(0.03, 0.05);
+detector.trainModel(pc);
+vector<Pose3DPtr> results;
+detector.match(pcTest, results, 1.0/10.0, 0.05);
+cout << "Poses: " << endl;
+// print the poses
+for (size_t i=0; i<results.size(); i++)
+{
+    Pose3DPtr pose = results[i];
+    cout << "Pose Result " << i << endl;
+    pose->printPose();
+}
+~~~
+
+Pose Registration via ICP
+-------------------------
+
+The matching process terminates with the attainment of the pose. However, due to the multiple
+matching points, erroneous hypothesis, pose averaging and etc. such pose is very open to noise and
+many times is far from being perfect. Although the visual results obtained in that stage are
+pleasing, the quantitative evaluation shows \f$~10\f$ degrees variation (error), which is an acceptable
+level of matching. Many times, the requirement might be set well beyond this margin and it is
+desired to refine the computed pose.
+
+Furthermore, in typical RGBD scenes and point clouds, 3D structure can capture only less than half
+of the model due to the visibility in the scene. Therefore, a robust pose refinement algorithm,
+which can register occluded and partially visible shapes quickly and correctly is not an unrealistic
+wish.
+
+At this point, a trivial option would be to use the well known iterative closest point algorithm .
+However, utilization of the basic ICP leads to slow convergence, bad registration, outlier
+sensitivity and failure to register partial shapes. Thus, it is definitely not suited to the
+problem. For this reason, many variants have been proposed . Different variants contribute to
+different stages of the pose estimation process.
+
+ICP is composed of \f$6\f$ stages and the improvements I propose for each stage is summarized below.
+
+### Sampling
+
+To improve convergence speed and computation time, it is common to use less points than the model
+actually has. However, sampling the correct points to register is an issue in itself. The naive way
+would be to sample uniformly and hope to get a reasonable subset. More smarter ways try to identify
+the critical points, which are found to highly contribute to the registration process. Gelfand et.
+al. exploit the covariance matrix in order to constrain the eigenspace, so that a set of points
+which affect both translation and rotation are used. This is a clever way of subsampling, which I
+will optionally be using in the implementation.
+
+### Correspondence Search
+
+As the name implies, this step is actually the assignment of the points in the data and the model in
+a closest point fashion. Correct assignments will lead to a correct pose, where wrong assignments
+strongly degrade the result. In general, KD-trees are used in the search of nearest neighbors, to
+increase the speed. However this is not an optimality guarantee and many times causes wrong points
+to be matched. Luckily the assignments are corrected over iterations.
+
+To overcome some of the limitations, Picky ICP @cite pickyicp and BC-ICP (ICP using bi-unique
+correspondences) are two well-known methods. Picky ICP first finds the correspondences in the
+old-fashioned way and then among the resulting corresponding pairs, if more than one scene point
+\f$p_i\f$ is assigned to the same model point \f$m_j\f$, it selects \f$p_i\f$ that corresponds to the minimum
+distance. BC-ICP on the other hand, allows multiple correspondences first and then resolves the
+assignments by establishing bi-unique correspondences. It also defines a novel no-correspondence
+outlier, which intrinsically eases the process of identifying outliers.
+
+For reference, both methods are used. Because P-ICP is a bit faster, with not-so-significant
+performance drawback, it will be the method of choice in refinment of correspondences.
+
+### Weighting of Pairs
+
+In my implementation, I currently do not use a weighting scheme. But the common approaches involve
+*normal compatibility* (\f$w_i=n^1_i\cdot n^2_j\f$) or assigning lower weights to point pairs with
+greater distances (\f$w=1-\frac{||dist(m_i,s_i)||_2}{dist_{max}}\f$).
+
+### Rejection of Pairs
+
+The rejections are done using a dynamic thresholding based on a robust estimate of the standard
+deviation. In other words, in each iteration, I find the MAD estimate of the Std. Dev. I denote this
+as \f$mad_i\f$. I reject the pairs with distances \f$d_i>\tau mad_i\f$. Here \f$\tau\f$ is the threshold of
+rejection and by default set to \f$3\f$. The weighting is applied prior to Picky refinement, explained
+in the previous stage.
+
+### Error Metric
+
+As described in , a linearization of point to plane as in @cite koklimlow error metric is used. This
+both speeds up the registration process and improves convergence.
+
+### Minimization
+
+Even though many non-linear optimizers (such as Levenberg Mardquardt) are proposed, due to the
+linearization in the previous step, pose estimation reduces to solving a linear system of equations.
+This is what I do exactly using cv::solve with DECOMP_SVD option.
+
+### ICP Algorithm
+
+Having described the steps above, here I summarize the layout of the ICP algorithm.
+
+#### Efficient ICP Through Point Cloud Pyramids
+
+While the up-to-now-proposed variants deal well with some outliers and bad initializations, they
+require significant number of iterations. Yet, multi-resolution scheme can help reducing the number
+of iterations by allowing the registration to start from a coarse level and propagate to the lower
+and finer levels. Such approach both improves the performances and enhances the runtime.
+
+The search is done through multiple levels, in a hierarchical fashion. The registration starts with
+a very coarse set of samples of the model. Iteratively, the points are densified and sought. After
+each iteration the previously estimated pose is used as an initial pose and refined with the ICP.
+
+#### Visual Results
+
+##### Results on Synthetic Data
+
+In all of the results, the pose is initiated by PPF and the rest is left as:
+\f$[\theta_x, \theta_y, \theta_z, t_x, t_y, t_z]=[0]\f$
+
+### Source Code for Pose Refinement Using ICP
+
+~~~{cpp}
+ICP icp(200, 0.001f, 2.5f, 8);
+// Using the previously declared pc and pcTest
+// This will perform registration for every pose
+// contained in results
+icp.registerModelToScene(pc, pcTest, results);
+
+// results now contain the refined poses
+~~~
+
+Results
+-------
+
+This section is dedicated to the results of surface matching (point-pair-feature matching and a
+following ICP refinement):
+
+![Several matches of a single frog model using ppf + icp](img/gsoc_forg_matches.jpg)
+
+Matches of different models for Mian dataset is presented below:
+
+![Matches of different models for Mian dataset](img/snapshot27.jpg)
+
+You might checkout the video on [youTube here](http://www.youtube.com/watch?v=uFnqLFznuZU).
+
+A Complete Sample
+-----------------
+
+### Parameter Tuning
+
+Surface matching module treats its parameters relative to the model diameter (diameter of the axis
+parallel bounding box), whenever it can. This makes the parameters independent from the model size.
+This is why, both model and scene cloud were subsampled such that all points have a minimum distance
+of \f$RelativeSamplingStep*DimensionRange\f$, where \f$DimensionRange\f$ is the distance along a given
+dimension. All three dimensions are sampled in similar manner. For example, if
+\f$RelativeSamplingStep\f$ is set to 0.05 and the diameter of model is 1m (1000mm), the points sampled
+from the object's surface will be approximately 50 mm apart. From another point of view, if the
+sampling RelativeSamplingStep is set to 0.05, at most \f$20x20x20 = 8000\f$ model points are generated
+(depending on how the model fills in the volume). Consequently this results in at most 8000x8000
+pairs. In practice, because the models are not uniformly distributed over a rectangular prism, much
+less points are to be expected. Decreasing this value, results in more model points and thus a more
+accurate representation. However, note that number of point pair features to be computed is now
+quadratically increased as the complexity is O(N\^2). This is especially a concern for 32 bit
+systems, where large models can easily overshoot the available memory. Typically, values in the
+range of 0.025 - 0.05 seem adequate for most of the applications, where the default value is 0.03.
+(Note that there is a difference in this paremeter with the one presented in @cite drost2010 . In
+@cite drost2010 a uniform cuboid is used for quantization and model diameter is used for reference of
+sampling. In my implementation, the cuboid is a rectangular prism, and each dimension is quantized
+independently. I do not take reference from the diameter but along the individual dimensions.
+
+It would very wise to remove the outliers from the model and prepare an ideal model initially. This
+is because, the outliers directly affect the relative computations and degrade the matching
+accuracy.
+
+During runtime stage, the scene is again sampled by \f$RelativeSamplingStep\f$, as described above.
+However this time, only a portion of the scene points are used as reference. This portion is
+controlled by the parameter \f$RelativeSceneSampleStep\f$, where
+\f$SceneSampleStep = (int)(1.0/RelativeSceneSampleStep)\f$. In other words, if the
+\f$RelativeSceneSampleStep = 1.0/5.0\f$, the subsampled scene will once again be uniformly sampled to
+1/5 of the number of points. Maximum value of this parameter is 1 and increasing this parameter also
+increases the stability, but decreases the speed. Again, because of the initial scene-independent
+relative sampling, fine tuning this parameter is not a big concern. This would only be an issue when
+the model shape occupies a volume uniformly, or when the model shape is condensed in a tiny place
+within the quantization volume (e.g. The octree representation would have too much empty cells).
+
+\f$RelativeDistanceStep\f$ acts as a step of discretization over the hash table. The point pair features
+are quantized to be mapped to the buckets of the hashtable. This discretization involves a
+multiplication and a casting to the integer. Adjusting RelativeDistanceStep in theory controls the
+collision rate. Note that, more collisions on the hashtable results in less accurate estimations.
+Reducing this parameter increases the affect of quantization but starts to assign non-similar point
+pairs to the same bins. Increasing it however, wanes the ability to group the similar pairs.
+Generally, because during the sampling stage, the training model points are selected uniformly with
+a distance controlled by RelativeSamplingStep, RelativeDistanceStep is expected to equate to this
+value. Yet again, values in the range of 0.025-0.05 are sensible. This time however, when the model
+is dense, it is not advised to decrease this value. For noisy scenes, the value can be increased to
+improve the robustness of the matching against noisy points.
+
+*/
+
+#endif

modules/surface_matching/include/opencv2/surface_matching/icp.hpp

@@ -0,0 +1,170 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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
+ *
+ * @brief  Implementation of ICP (Iterative Closest Point) Algorithm
+ * @author Tolga Birdal <tbirdal AT gmail.com>
+ */
+
+#ifndef __OPENCV_SURFACE_MATCHING_ICP_HPP__
+#define __OPENCV_SURFACE_MATCHING_ICP_HPP__
+
+#include <opencv2/core.hpp>
+
+#include "pose_3d.hpp"
+#include <vector>
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+//! @addtogroup surface_matching
+//! @{
+
+/**
+* @brief This class implements a very efficient and robust variant of the iterative closest point (ICP) algorithm.
+* The task is to register a 3D model (or point cloud) against a set of noisy target data. The variants are put together
+* by myself after certain tests. The task is to be able to match partial, noisy point clouds in cluttered scenes, quickly.
+* You will find that my emphasis is on the performance, while retaining the accuracy.
+* This implementation is based on Tolga Birdal's MATLAB implementation in here:
+* http://www.mathworks.com/matlabcentral/fileexchange/47152-icp-registration-using-efficient-variants-and-multi-resolution-scheme
+* The main contributions come from:
+* 1. Picky ICP:
+* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2003/Zinsser03-ARI.pdf
+* 2. Efficient variants of the ICP Algorithm:
+* http://docs.happycoders.org/orgadoc/graphics/imaging/fasticp_paper.pdf
+* 3. Geometrically Stable Sampling for the ICP Algorithm: https://graphics.stanford.edu/papers/stabicp/stabicp.pdf
+* 4. Multi-resolution registration:
+* http://www.cvl.iis.u-tokyo.ac.jp/~oishi/Papers/Alignment/Jost_MultiResolutionICP_3DIM03.pdf
+* 5. Linearization of Point-to-Plane metric by Kok Lim Low:
+* https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
+*/
+class CV_EXPORTS_W ICP
+{
+public:
+
+  CV_WRAP enum
+  {
+    ICP_SAMPLING_TYPE_UNIFORM = 0,
+    ICP_SAMPLING_TYPE_GELFAND = 1
+  };
+
+  CV_WRAP ICP()
+  {
+    m_tolerance = 0.005f;
+    m_rejectionScale = 2.5f;
+    m_maxIterations = 250;
+    m_numLevels = 6;
+    m_sampleType = ICP_SAMPLING_TYPE_UNIFORM;
+    m_numNeighborsCorr = 1;
+  }
+
+  virtual ~ICP() { }
+
+  /**
+     *  \brief ICP constructor with default arguments.
+     *  @param [in] iterations
+     *  @param [in] tolerence Controls the accuracy of registration at each iteration of ICP.
+     *  @param [in] rejectionScale Robust outlier rejection is applied for robustness. This value
+            actually corresponds to the standard deviation coefficient. Points with
+            rejectionScale * &sigma are ignored during registration.
+     *  @param [in] numLevels Number of pyramid levels to proceed. Deep pyramids increase speed but
+            decrease accuracy. Too coarse pyramids might have computational overhead on top of the
+            inaccurate registrtaion. This parameter should be chosen to optimize a balance. Typical
+            values range from 4 to 10.
+     *  @param [in] sampleType Currently this parameter is ignored and only uniform sampling is
+            applied. Leave it as 0.
+     *  @param [in] numMaxCorr Currently this parameter is ignored and only PickyICP is applied. Leave it as 1.
+     */
+  CV_WRAP ICP(const int iterations, const float tolerence = 0.05f, const float rejectionScale = 2.5f, const int numLevels = 6, const int sampleType = ICP::ICP_SAMPLING_TYPE_UNIFORM, const int numMaxCorr = 1)
+  {
+    m_tolerance = tolerence;
+    m_numNeighborsCorr = numMaxCorr;
+    m_rejectionScale = rejectionScale;
+    m_maxIterations = iterations;
+    m_numLevels = numLevels;
+    m_sampleType = sampleType;
+  }
+
+  /**
+     *  \brief Perform registration
+     *
+     *  @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
+     *  CV_32F is the only supported data type.
+     *  @param [in] dstPC The input point cloud for the scene. It is assumed that the model is registered on the scene. Scene remains static. Expected to have the normals (Nx6). Currently, CV_32F is the only supported data type.
+     *  @param [out] residual The output registration error.
+     *  @param [out] pose Transformation between srcPC and dstPC.
+     *  \return On successful termination, the function returns 0.
+     *
+     *  \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
+     */
+  CV_WRAP int registerModelToScene(const Mat& srcPC, const Mat& dstPC, CV_OUT double& residual, CV_OUT Matx44d& pose);
+
+  /**
+     *  \brief Perform registration with multiple initial poses
+     *
+     *  @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
+     *  CV_32F is the only supported data type.
+     *  @param [in] dstPC The input point cloud for the scene. Currently, CV_32F is the only supported data type.
+     *  @param [in,out] poses Input poses to start with but also list output of poses.
+     *  \return On successful termination, the function returns 0.
+     *
+     *  \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
+     */
+  CV_WRAP int registerModelToScene(const Mat& srcPC, const Mat& dstPC, CV_IN_OUT std::vector<Pose3DPtr>& poses);
+
+private:
+  float m_tolerance;
+  int m_maxIterations;
+  float m_rejectionScale;
+  int m_numNeighborsCorr;
+  int m_numLevels;
+  int m_sampleType;
+
+};
+
+//! @}
+
+} // namespace ppf_match_3d
+
+} // namespace cv
+
+#endif

modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp

@@ -0,0 +1,188 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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
+@author Tolga Birdal <tbirdal AT gmail.com>
+*/
+
+#ifndef __OPENCV_SURFACE_MATCHING_POSE3D_HPP__
+#define __OPENCV_SURFACE_MATCHING_POSE3D_HPP__
+
+#include "opencv2/core/cvstd.hpp" // cv::Ptr
+#include <vector>
+#include <string>
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+//! @addtogroup surface_matching
+//! @{
+
+class Pose3D;
+typedef Ptr<Pose3D> Pose3DPtr;
+
+class PoseCluster3D;
+typedef Ptr<PoseCluster3D> PoseCluster3DPtr;
+
+/**
+* @brief Class, allowing the storage of a pose. The data structure stores both
+* the quaternions and the matrix forms. It supports IO functionality together with
+* various helper methods to work with poses
+*
+*/
+class CV_EXPORTS_W Pose3D
+{
+public:
+  CV_WRAP Pose3D()
+  {
+    alpha=0;
+    modelIndex=0;
+    numVotes=0;
+    residual = 0;
+
+    pose = Matx44d::all(0);
+  }
+
+  CV_WRAP Pose3D(double Alpha, size_t ModelIndex=0, size_t NumVotes=0)
+  {
+    alpha = Alpha;
+    modelIndex = ModelIndex;
+    numVotes = NumVotes;
+    residual=0;
+
+    pose = Matx44d::all(0);
+  }
+
+  /**
+   *  \brief Updates the pose with the new one
+   *  \param [in] NewPose New pose to overwrite
+   */
+  CV_WRAP void updatePose(Matx44d& NewPose);
+
+  /**
+   *  \brief Updates the pose with the new one
+   */
+  CV_WRAP void updatePose(Matx33d& NewR, Vec3d& NewT);
+
+  /**
+   *  \brief Updates the pose with the new one, but this time using quaternions to represent rotation
+   */
+  CV_WRAP void updatePoseQuat(Vec4d& Q, Vec3d& NewT);
+
+  /**
+   *  \brief Left multiplies the existing pose in order to update the transformation
+   *  \param [in] IncrementalPose New pose to apply
+   */
+  CV_WRAP void appendPose(Matx44d& IncrementalPose);
+  CV_WRAP void printPose();
+
+  Pose3DPtr clone();
+
+  int writePose(FILE* f);
+  int readPose(FILE* f);
+  int writePose(const std::string& FileName);
+  int readPose(const std::string& FileName);
+
+  virtual ~Pose3D() {}
+
+  CV_PROP double alpha, residual;
+  CV_PROP size_t modelIndex, numVotes;
+  CV_PROP Matx44d pose;
+  CV_PROP double angle;
+  CV_PROP Vec3d t;
+  CV_PROP Vec4d q;
+};
+
+/**
+* @brief When multiple poses (see Pose3D) are grouped together (contribute to the same transformation)
+* pose clusters occur. This class is a general container for such groups of poses. It is possible to store,
+* load and perform IO on these poses.
+*/
+class CV_EXPORTS_W PoseCluster3D
+{
+public:
+  PoseCluster3D()
+  {
+    numVotes=0;
+    id=0;
+  }
+
+  PoseCluster3D(Pose3DPtr newPose)
+  {
+    poseList.clear();
+    poseList.push_back(newPose);
+    numVotes=newPose->numVotes;
+    id=0;
+  }
+
+  PoseCluster3D(Pose3DPtr newPose, int newId)
+  {
+    poseList.push_back(newPose);
+    this->numVotes = newPose->numVotes;
+    this->id = newId;
+  }
+
+  virtual ~PoseCluster3D()
+  {}
+
+  /**
+   *  \brief Adds a new pose to the cluster. The pose should be "close" to the mean poses
+   *  in order to preserve the consistency
+   *  \param [in] newPose Pose to add to the cluster
+   */
+  void addPose(Pose3DPtr newPose);
+
+  int writePoseCluster(FILE* f);
+  int readPoseCluster(FILE* f);
+  int writePoseCluster(const std::string& FileName);
+  int readPoseCluster(const std::string& FileName);
+
+  std::vector<Pose3DPtr> poseList;
+  size_t numVotes;
+  int id;
+};
+
+//! @}
+
+} // namespace ppf_match_3d
+} // namespace cv
+
+#endif
+

modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp

@@ -0,0 +1,153 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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
+@author Tolga Birdal <tbirdal AT gmail.com>
+*/
+
+#ifndef __OPENCV_SURFACE_MATCHING_HELPERS_HPP__
+#define __OPENCV_SURFACE_MATCHING_HELPERS_HPP__
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+//! @addtogroup surface_matching
+//! @{
+
+/**
+ *  @brief Load a PLY file
+ *  @param [in] fileName The PLY model to read
+ *  @param [in] withNormals Flag wheather the input PLY contains normal information,
+ *  and whether it should be loaded or not
+ *  @return Returns the matrix on successful load
+ */
+CV_EXPORTS_W Mat loadPLYSimple(const char* fileName, int withNormals = 0);
+
+/**
+ *  @brief Write a point cloud to PLY file
+ *  @param [in] PC Input point cloud
+ *  @param [in] fileName The PLY model file to write
+*/
+CV_EXPORTS_W void writePLY(Mat PC, const char* fileName);
+
+/**
+*  @brief Used for debbuging pruposes, writes a point cloud to a PLY file with the tip
+*  of the normal vectors as visible red points
+*  @param [in] PC Input point cloud
+*  @param [in] fileName The PLY model file to write
+*/
+CV_EXPORTS_W void writePLYVisibleNormals(Mat PC, const char* fileName);
+
+Mat samplePCUniform(Mat PC, int sampleStep);
+Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& indices);
+
+/**
+ *  Sample a point cloud using uniform steps
+ *  @param [in] pc Input point cloud
+ *  @param [in] xrange X components (min and max) of the bounding box of the model
+ *  @param [in] yrange Y components (min and max) of the bounding box of the model
+ *  @param [in] zrange Z components (min and max) of the bounding box of the model
+ *  @param [in] sample_step_relative The point cloud is sampled such that all points
+ *  have a certain minimum distance. This minimum distance is determined relatively using
+ *  the parameter sample_step_relative.
+ *  @param [in] weightByCenter The contribution of the quantized data points can be weighted
+ *  by the distance to the origin. This parameter enables/disables the use of weighting.
+ *  @return Sampled point cloud
+*/
+CV_EXPORTS_W Mat samplePCByQuantization(Mat pc, Vec2f& xrange, Vec2f& yrange, Vec2f& zrange, float sample_step_relative, int weightByCenter=0);
+
+void computeBboxStd(Mat pc, Vec2f& xRange, Vec2f& yRange, Vec2f& zRange);
+
+void* indexPCFlann(Mat pc);
+void destroyFlann(void* flannIndex);
+void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances);
+void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors);
+
+Mat normalizePCCoeff(Mat pc, float scale, float* Cx, float* Cy, float* Cz, float* MinVal, float* MaxVal);
+Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal, float MaxVal);
+
+/**
+ *  Transforms the point cloud with a given a homogeneous 4x4 pose matrix (in double precision)
+ *  @param [in] pc Input point cloud (CV_32F family). Point clouds with 3 or 6 elements per
+ *  row are expected. In the case where the normals are provided, they are also rotated to be
+ *  compatible with the entire transformation
+ *  @param [in] Pose 4x4 pose matrix, but linearized in row-major form.
+ *  @return Transformed point cloud
+*/
+CV_EXPORTS_W Mat transformPCPose(Mat pc, const Matx44d& Pose);
+
+/**
+ *  Generate a random 4x4 pose matrix
+ *  @param [out] Pose The random pose
+*/
+CV_EXPORTS_W void getRandomPose(Matx44d& Pose);
+
+/**
+ *  Adds a uniform noise in the given scale to the input point cloud
+ *  @param [in] pc Input point cloud (CV_32F family).
+ *  @param [in] scale Input scale of the noise. The larger the scale, the more noisy the output
+*/
+CV_EXPORTS_W Mat addNoisePC(Mat pc, double scale);
+
+/**
+ *  @brief Compute the normals of an arbitrary point cloud
+ *  computeNormalsPC3d uses a plane fitting approach to smoothly compute
+ *  local normals. Normals are obtained through the eigenvector of the covariance
+ *  matrix, corresponding to the smallest eigen value.
+ *  If PCNormals is provided to be an Nx6 matrix, then no new allocation
+ *  is made, instead the existing memory is overwritten.
+ *  @param [in] PC Input point cloud to compute the normals for.
+ *  @param [out] PCNormals Output point cloud
+ *  @param [in] NumNeighbors Number of neighbors to take into account in a local region
+ *  @param [in] FlipViewpoint Should normals be flipped to a viewing direction?
+ *  @param [in] viewpoint
+ *  @return Returns 0 on success
+ */
+CV_EXPORTS_W int computeNormalsPC3d(const Mat& PC, CV_OUT Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3f& viewpoint);
+
+//! @}
+
+} // namespace ppf_match_3d
+} // namespace cv
+
+#endif

modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp

@@ -0,0 +1,179 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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.
+//
+
+/**
+** ppf_match_3d : Interfaces for matching 3d surfaces in 3d scenes. This module implements the algorithm from Bertram Drost and Slobodan Ilic.
+** Use: Read a 3D model, load a 3D scene and match the model to the scene
+**
+**
+**  Creation - 2014
+**      Author: Tolga Birdal (tbirdal@gmail.com)
+**
+** Refer to the following research paper for more information:
+**  B. Drost, Markus Ulrich, N. Navab, S. Ilic
+Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
+IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR), San Francisco, California (USA), June 2010.
+***/
+
+/** @file
+@author Tolga Birdal  <tbirdal AT gmail.com>
+*/
+
+
+#ifndef __OPENCV_SURFACE_MATCHING_PPF_MATCH_3D_HPP__
+#define __OPENCV_SURFACE_MATCHING_PPF_MATCH_3D_HPP__
+
+#include <opencv2/core.hpp>
+
+#include <vector>
+#include "pose_3d.hpp"
+#include "t_hash_int.hpp"
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+//! @addtogroup surface_matching
+//! @{
+
+/**
+  * @brief Struct, holding a node in the hashtable
+  */
+typedef struct THash
+{
+  int id;
+  int i, ppfInd;
+} THash;
+
+/**
+  * @brief Class, allowing the load and matching 3D models.
+  * Typical Use:
+  * @code
+  * // Train a model
+  * ppf_match_3d::PPF3DDetector detector(0.05, 0.05);
+  * detector.trainModel(pc);
+  * // Search the model in a given scene
+  * vector<Pose3DPtr> results;
+  * detector.match(pcTest, results, 1.0/5.0,0.05);
+  * @endcode
+  */
+class CV_EXPORTS_W PPF3DDetector
+{
+public:
+
+  /**
+   * \brief Empty constructor. Sets default arguments
+   */
+  CV_WRAP PPF3DDetector();
+
+  /**
+    * Constructor with arguments
+    * @param [in] relativeSamplingStep Sampling distance relative to the object's diameter. Models are first sampled uniformly in order to improve efficiency. Decreasing this value leads to a denser model, and a more accurate pose estimation but the larger the model, the slower the training. Increasing the value leads to a less accurate pose computation but a smaller model and faster model generation and matching. Beware of the memory consumption when using small values.
+    * @param [in] relativeDistanceStep The discretization distance of the point pair distance relative to the model's diameter. This value has a direct impact on the hashtable. Using small values would lead to too fine discretization, and thus ambiguity in the bins of hashtable. Too large values would lead to no discrimination over the feature vectors and different point pair features would be assigned to the same bin. This argument defaults to the value of RelativeSamplingStep. For noisy scenes, the value can be increased to improve the robustness of the matching against noisy points.
+    * @param [in] numAngles Set the discretization of the point pair orientation as the number of subdivisions of the angle. This value is the equivalent of RelativeDistanceStep for the orientations. Increasing the value increases the precision of the matching but decreases the robustness against incorrect normal directions. Decreasing the value decreases the precision of the matching but increases the robustness against incorrect normal directions. For very noisy scenes where the normal directions can not be computed accurately, the value can be set to 25 or 20.
+    */
+  CV_WRAP PPF3DDetector(const double relativeSamplingStep, const double relativeDistanceStep=0.05, const double numAngles=30);
+
+  virtual ~PPF3DDetector();
+
+  /**
+    *  Set the parameters for the search
+    *  @param [in] positionThreshold Position threshold controlling the similarity of translations. Depends on the units of calibration/model.
+    *  @param [in] rotationThreshold Position threshold controlling the similarity of rotations. This parameter can be perceived as a threshold over the difference of angles
+    *  @param [in] useWeightedClustering The algorithm by default clusters the poses without weighting. A non-zero value would indicate that the pose clustering should take into account the number of votes as the weights and perform a weighted averaging instead of a simple one.
+    */
+  void setSearchParams(const double positionThreshold=-1, const double rotationThreshold=-1, const bool useWeightedClustering=false);
+
+  /**
+    *  \brief Trains a new model.
+    *
+    *  @param [in] Model The input point cloud with normals (Nx6)
+    *
+    *  \details Uses the parameters set in the constructor to downsample and learn a new model. When the model is learnt, the instance gets ready for calling "match".
+    */
+  CV_WRAP void trainModel(const Mat& Model);
+
+  /**
+    *  \brief Matches a trained model across a provided scene.
+    *
+    *  @param [in] scene Point cloud for the scene
+    *  @param [out] results List of output poses
+    *  @param [in] relativeSceneSampleStep The ratio of scene points to be used for the matching after sampling with relativeSceneDistance. For example, if this value is set to 1.0/5.0, every 5th point from the scene is used for pose estimation. This parameter allows an easy trade-off between speed and accuracy of the matching. Increasing the value leads to less points being used and in turn to a faster but less accurate pose computation. Decreasing the value has the inverse effect.
+    *  @param [in] relativeSceneDistance Set the distance threshold relative to the diameter of the model. This parameter is equivalent to relativeSamplingStep in the training stage. This parameter acts like a prior sampling with the relativeSceneSampleStep parameter.
+    */
+  CV_WRAP void match(const Mat& scene, CV_OUT std::vector<Pose3DPtr> &results, const double relativeSceneSampleStep=1.0/5.0, const double relativeSceneDistance=0.03);
+
+  void read(const FileNode& fn);
+  void write(FileStorage& fs) const;
+
+protected:
+
+  double angle_step, angle_step_radians, distance_step;
+  double sampling_step_relative, angle_step_relative, distance_step_relative;
+  Mat sampled_pc, ppf;
+  int num_ref_points;
+  hashtable_int* hash_table;
+  THash* hash_nodes;
+
+  double position_threshold, rotation_threshold;
+  bool use_weighted_avg;
+
+  int scene_sample_step;
+
+  void clearTrainingModels();
+
+private:
+  void computePPFFeatures(const Vec3d& p1, const Vec3d& n1,
+                          const Vec3d& p2, const Vec3d& n2,
+                          Vec4d& f);
+
+  bool matchPose(const Pose3D& sourcePose, const Pose3D& targetPose);
+
+  void clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses, std::vector<Pose3DPtr> &finalPoses);
+
+  bool trained;
+};
+
+//! @}
+
+} // namespace ppf_match_3d
+
+} // namespace cv
+#endif

modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp

@@ -0,0 +1,113 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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
+@author Tolga Birdal <tbirdal AT gmail.com>
+*/
+
+#ifndef __OPENCV_SURFACE_MATCHING_T_HASH_INT_HPP__
+#define __OPENCV_SURFACE_MATCHING_T_HASH_INT_HPP__
+
+#include <stdio.h>
+#include <stdlib.h>
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+//! @addtogroup surface_matching
+//! @{
+
+typedef uint KeyType;
+
+typedef struct hashnode_i
+{
+  KeyType key;
+  void *data;
+  struct hashnode_i *next;
+} hashnode_i ;
+
+typedef struct HSHTBL_i
+{
+  size_t size;
+  struct hashnode_i **nodes;
+  size_t (*hashfunc)(uint);
+} hashtable_int;
+
+
+/** @brief Round up to the next highest power of 2
+
+from http://www-graphics.stanford.edu/~seander/bithacks.html
+*/
+inline static uint next_power_of_two(uint value)
+{
+
+  --value;
+  value |= value >> 1;
+  value |= value >> 2;
+  value |= value >> 4;
+  value |= value >> 8;
+  value |= value >> 16;
+  ++value;
+
+  return value;
+}
+
+hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(uint));
+void hashtableDestroy(hashtable_int *hashtbl);
+int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data);
+int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data);
+int hashtableRemove(hashtable_int *hashtbl, KeyType key);
+void *hashtableGet(hashtable_int *hashtbl, KeyType key);
+hashnode_i* hashtableGetBucketHashed(hashtable_int *hashtbl, KeyType key);
+int hashtableResize(hashtable_int *hashtbl, size_t size);
+hashtable_int *hashtable_int_clone(hashtable_int *hashtbl);
+hashtable_int *hashtableRead(FILE* f);
+int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f);
+void hashtablePrint(hashtable_int *hashtbl);
+
+//! @}
+
+} // namespace ppf_match_3d
+
+} // namespace cv
+#endif
+
+

modules/surface_matching/misc/python/pyopencv_ppf_match_3d.hpp

@@ -0,0 +1,17 @@
+#ifdef HAVE_OPENCV_SURFACE_MATCHING
+
+template<> struct pyopencvVecConverter<ppf_match_3d::Pose3DPtr >
+{
+    static bool to(PyObject* obj, std::vector<ppf_match_3d::Pose3DPtr >& value, const ArgInfo& info)
+    {
+        return pyopencv_to_generic_vec(obj, value, info);
+    }
+
+    static PyObject* from(const std::vector<ppf_match_3d::Pose3DPtr >& value)
+    {
+        return pyopencv_from_generic_vec(value);
+    }
+};
+
+typedef std::vector<ppf_match_3d::Pose3DPtr> vector_Pose3DPtr;
+#endif

modules/surface_matching/samples/ppf_icp.py

@@ -0,0 +1,52 @@
+import cv2 as cv
+import numpy as np
+
+def rotation(theta):
+    tx, ty, tz = theta
+
+    Rx = np.array([[1, 0, 0], [0, np.cos(tx), -np.sin(tx)], [0, np.sin(tx), np.cos(tx)]])
+    Ry = np.array([[np.cos(ty), 0, -np.sin(ty)], [0, 1, 0], [np.sin(ty), 0, np.cos(ty)]])
+    Rz = np.array([[np.cos(tz), -np.sin(tz), 0], [np.sin(tz), np.cos(tz), 0], [0, 0, 1]])
+
+    return np.dot(Rx, np.dot(Ry, Rz))
+
+width = 20
+height = 10
+max_deg = np.pi / 12
+
+cloud, rotated_cloud = [None]*3, [None]*3
+retval, residual, pose = [None]*3, [None]*3, [None]*3
+noise = np.random.normal(0.0, 0.1, height * width * 3).reshape((-1, 3))
+noise2 = np.random.normal(0.0, 1.0, height * width)
+
+x, y = np.meshgrid(
+    range(-width//2, width//2),
+    range(-height//2, height//2),
+    sparse=False, indexing='xy'
+)
+z = np.zeros((height, width))
+
+cloud[0] = np.dstack((x, y, z)).reshape((-1, 3)).astype(np.float32)
+cloud[1] = noise.astype(np.float32) + cloud[0]
+cloud[2] = cloud[1]
+cloud[2][:, 2] += noise2.astype(np.float32)
+
+R = rotation([
+    0, #np.random.uniform(-max_deg, max_deg),
+    np.random.uniform(-max_deg, max_deg),
+    0, #np.random.uniform(-max_deg, max_deg)
+])
+t = np.zeros((3, 1))
+Rt = np.vstack((
+    np.hstack((R, t)),
+    np.array([0, 0, 0, 1])
+)).astype(np.float32)
+
+icp = cv.ppf_match_3d_ICP(100)
+
+I = np.eye(4)
+print("Unaligned error:\t%.6f" % np.linalg.norm(I - Rt))
+for i in range(3):
+    rotated_cloud[i] = np.matmul(Rt[0:3,0:3], cloud[i].T).T + Rt[:3,3].T
+    retval[i], residual[i], pose[i] = icp.registerModelToScene(rotated_cloud[i], cloud[i])
+    print("ICP error:\t\t%.6f" % np.linalg.norm(I - np.matmul(pose[0], Rt)))

modules/surface_matching/samples/ppf_load_match.cpp

@@ -0,0 +1,159 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#include "opencv2/surface_matching.hpp"
+#include <iostream>
+#include "opencv2/surface_matching/ppf_helpers.hpp"
+#include "opencv2/core/utility.hpp"
+
+using namespace std;
+using namespace cv;
+using namespace ppf_match_3d;
+
+static void help(const string& errorMessage)
+{
+    cout << "Program init error : "<< errorMessage << endl;
+    cout << "\nUsage : ppf_matching [input model file] [input scene file]"<< endl;
+    cout << "\nPlease start again with new parameters"<< endl;
+}
+
+int main(int argc, char** argv)
+{
+    // welcome message
+    cout << "****************************************************" << endl;
+    cout << "* Surface Matching demonstration : demonstrates the use of surface matching"
+             " using point pair features." << endl;
+    cout << "* The sample loads a model and a scene, where the model lies in a different"
+             " pose than the training.\n* It then trains the model and searches for it in the"
+             " input scene. The detected poses are further refined by ICP\n* and printed to the "
+             " standard output." << endl;
+    cout << "****************************************************" << endl;
+    
+    if (argc < 3)
+    {
+        help("Not enough input arguments");
+        exit(1);
+    }
+    
+#if (defined __x86_64__ || defined _M_X64)
+    cout << "Running on 64 bits" << endl;
+#else
+    cout << "Running on 32 bits" << endl;
+#endif
+    
+#ifdef _OPENMP
+    cout << "Running with OpenMP" << endl;
+#else
+    cout << "Running without OpenMP and without TBB" << endl;
+#endif
+    
+    string modelFileName = (string)argv[1];
+    string sceneFileName = (string)argv[2];
+    
+    Mat pc = loadPLYSimple(modelFileName.c_str(), 1);
+    
+    // Now train the model
+    cout << "Training..." << endl;
+    int64 tick1 = cv::getTickCount();
+    ppf_match_3d::PPF3DDetector detector(0.025, 0.05);
+    detector.trainModel(pc);
+    int64 tick2 = cv::getTickCount();
+    cout << endl << "Training complete in "
+         << (double)(tick2-tick1)/ cv::getTickFrequency()
+         << " sec" << endl << "Loading model..." << endl;
+         
+    // Read the scene
+    Mat pcTest = loadPLYSimple(sceneFileName.c_str(), 1);
+    
+    // Match the model to the scene and get the pose
+    cout << endl << "Starting matching..." << endl;
+    vector<Pose3DPtr> results;
+    tick1 = cv::getTickCount();
+    detector.match(pcTest, results, 1.0/40.0, 0.05);
+    tick2 = cv::getTickCount();
+    cout << endl << "PPF Elapsed Time " <<
+         (tick2-tick1)/cv::getTickFrequency() << " sec" << endl;
+
+    //check results size from match call above
+    size_t results_size = results.size();
+    cout << "Number of matching poses: " << results_size;
+    if (results_size == 0) {
+        cout << endl << "No matching poses found. Exiting." << endl;
+        exit(0);
+    }
+
+    // Get only first N results - but adjust to results size if num of results are less than that specified by N
+    size_t N = 2;
+    if (results_size < N) {
+        cout << endl << "Reducing matching poses to be reported (as specified in code): "
+             << N << " to the number of matches found: " << results_size << endl;
+        N = results_size;
+    }
+    vector<Pose3DPtr> resultsSub(results.begin(),results.begin()+N);
+    
+    // Create an instance of ICP
+    ICP icp(100, 0.005f, 2.5f, 8);
+    int64 t1 = cv::getTickCount();
+    
+    // Register for all selected poses
+    cout << endl << "Performing ICP on " << N << " poses..." << endl;
+    icp.registerModelToScene(pc, pcTest, resultsSub);
+    int64 t2 = cv::getTickCount();
+    
+    cout << endl << "ICP Elapsed Time " <<
+         (t2-t1)/cv::getTickFrequency() << " sec" << endl;
+         
+    cout << "Poses: " << endl;
+    // debug first five poses
+    for (size_t i=0; i<resultsSub.size(); i++)
+    {
+        Pose3DPtr result = resultsSub[i];
+        cout << "Pose Result " << i << endl;
+        result->printPose();
+        if (i==0)
+        {
+            Mat pct = transformPCPose(pc, result->pose);
+            writePLY(pct, "para6700PCTrans.ply");
+        }
+    }
+    
+    return 0;
+    
+}

modules/surface_matching/samples/ppf_load_match.py

@@ -0,0 +1,32 @@
+import cv2 as cv
+
+N = 2
+modelname = "parasaurolophus_6700"
+scenename = "rs1_normals"
+
+detector = cv.ppf_match_3d_PPF3DDetector(0.025, 0.05)
+
+print('Loading model...')
+pc = cv.ppf_match_3d.loadPLYSimple("data/%s.ply" % modelname, 1)
+
+
+print('Training...')
+detector.trainModel(pc)
+
+print('Loading scene...')
+pcTest = cv.ppf_match_3d.loadPLYSimple("data/%s.ply" % scenename, 1)
+
+print('Matching...')
+results = detector.match(pcTest, 1.0/40.0, 0.05)
+
+print('Performing ICP...')
+icp = cv.ppf_match_3d_ICP(100)
+_, results = icp.registerModelToScene(pc, pcTest, results[:N])
+
+print("Poses: ")
+for i, result in enumerate(results):
+    #result.printPose()
+    print("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n%s\n" % (result.modelIndex, result.numVotes, result.residual, result.pose))
+    if i == 0:
+        pct = cv.ppf_match_3d.transformPCPose(pc, result.pose)
+        cv.ppf_match_3d.writePLY(pct, "%sPCTrans.ply" % modelname)

modules/surface_matching/samples/ppf_normal_computation.cpp

@@ -0,0 +1,78 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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 <iostream>
+#include "opencv2/surface_matching.hpp"
+#include "opencv2/surface_matching/ppf_helpers.hpp"
+
+using namespace std;
+
+static void help(const string& errorMessage)
+{
+  cout << "Program init error : " << errorMessage << endl;
+  cout << "\nUsage : ppf_normal_computation [input model file] [output model file]" << endl;
+  cout << "\nPlease start again with new parameters" << endl;
+}
+
+int main(int argc, char** argv)
+{
+  if (argc < 3)
+  {
+    help("Not enough input arguments");
+    exit(1);
+  }
+
+  string modelFileName = (string)argv[1];
+  string outputFileName = (string)argv[2];
+  cv::Mat points, pointsAndNormals;
+
+  cout << "Loading points\n";
+  cv::ppf_match_3d::loadPLYSimple(modelFileName.c_str(), 1).copyTo(points);
+
+  cout << "Computing normals\n";
+  cv::Vec3d viewpoint(0, 0, 0);
+  cv::ppf_match_3d::computeNormalsPC3d(points, pointsAndNormals, 6, false, viewpoint);
+
+  std::cout << "Writing points\n";
+  cv::ppf_match_3d::writePLY(pointsAndNormals, outputFileName.c_str());
+  //the following function can also be used for debugging purposes
+  //cv::ppf_match_3d::writePLYVisibleNormals(pointsAndNormals, outputFileName.c_str());
+
+  std::cout << "Done\n";
+  return 0;
+}

modules/surface_matching/src/c_utils.hpp

@@ -0,0 +1,337 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#ifndef __OPENCV_SURFACE_MATCHING_UTILS_HPP_
+#define __OPENCV_SURFACE_MATCHING_UTILS_HPP_
+
+#include <cmath>
+#include <cstdio>
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+const float EPS = 1.192092896e-07F;        /* smallest such that 1.0+FLT_EPSILON != 1.0 */
+
+#ifndef M_PI
+#define M_PI  3.1415926535897932384626433832795
+#endif
+
+static inline void TNormalize3(Vec3d& v)
+{
+  double norm = cv::norm(v);
+  if (norm > EPS)
+  {
+    v *= 1.0 / norm;
+  }
+}
+
+/**
+ *  \brief Calculate angle between two normalized vectors
+ *
+ *  \param [in] a normalized vector
+ *  \param [in] b normalized vector
+ *  \return angle between a and b vectors in radians
+ */
+static inline double TAngle3Normalized(const Vec3d& a, const Vec3d& b)
+{
+  /*
+   angle = atan2(a dot b, |a x b|) # Bertram (accidental mistake)
+   angle = atan2(|a x b|, a dot b) # Tolga Birdal (correction)
+   angle = acos(a dot b)           # Hamdi Sahloul (simplification, a & b are normalized)
+  */
+
+  return acos(a.dot(b));
+}
+
+static inline void rtToPose(const Matx33d& R, const Vec3d& t, Matx44d& Pose)
+{
+  Matx34d P;
+  hconcat(R, t, P);
+  vconcat(P, Matx14d(0, 0, 0, 1), Pose);
+}
+
+static inline void poseToR(const Matx44d& Pose, Matx33d& R)
+{
+  Mat(Pose).rowRange(0, 3).colRange(0, 3).copyTo(R);
+}
+
+static inline void poseToRT(const Matx44d& Pose, Matx33d& R, Vec3d& t)
+{
+  poseToR(Pose, R);
+  Mat(Pose).rowRange(0, 3).colRange(3, 4).copyTo(t);
+}
+
+/**
+ *  \brief Axis angle to rotation
+ */
+static inline void aaToR(const Vec3d& axis, double angle, Matx33d& R)
+{
+  const double sinA = sin(angle);
+  const double cosA = cos(angle);
+  const double cos1A = (1 - cosA);
+  uint i, j;
+
+  Mat(cosA * Matx33d::eye()).copyTo(R);
+
+  for (i = 0; i < 3; i++)
+    for (j = 0; j < 3; j++)
+    {
+      if (i != j)
+      {
+        // Symmetry skew matrix
+        R(i, j) += (((i + 1) % 3 == j) ? -1 : 1) * sinA * axis[3 - i - j];
+      }
+      R(i, j) += cos1A * axis[i] * axis[j];
+    }
+}
+
+/**
+ *  \brief Compute a rotation in order to rotate around X direction
+ */
+static inline void getUnitXRotation(double angle, Matx33d& Rx)
+{
+  const double sx = sin(angle);
+  const double cx = cos(angle);
+
+  Mat(Rx.eye()).copyTo(Rx);
+  Rx(1, 1) = cx;
+  Rx(1, 2) = -sx;
+  Rx(2, 1) = sx;
+  Rx(2, 2) = cx;
+}
+
+/**
+*  \brief Compute a rotation in order to rotate around Y direction
+*/
+static inline void getUnitYRotation(double angle, Matx33d& Ry)
+{
+  const double sy = sin(angle);
+  const double cy = cos(angle);
+
+  Mat(Ry.eye()).copyTo(Ry);
+  Ry(0, 0) = cy;
+  Ry(0, 2) = sy;
+  Ry(2, 0) = -sy;
+  Ry(2, 2) = cy;
+}
+
+/**
+*  \brief Compute a rotation in order to rotate around Z direction
+*/
+static inline void getUnitZRotation(double angle, Matx33d& Rz)
+{
+  const double sz = sin(angle);
+  const double cz = cos(angle);
+
+  Mat(Rz.eye()).copyTo(Rz);
+  Rz(0, 0) = cz;
+  Rz(0, 1) = -sz;
+  Rz(1, 0) = sz;
+  Rz(1, 1) = cz;
+}
+
+/**
+*  \brief Convert euler representation to rotation matrix
+*
+*  \param [in] euler RPY angles
+*  \param [out] R 3x3 Rotation matrix
+*/
+static inline void eulerToDCM(const Vec3d& euler, Matx33d& R)
+{
+  Matx33d Rx, Ry, Rz;
+
+  getUnitXRotation(euler[0], Rx);
+  getUnitYRotation(euler[1], Ry);
+  getUnitZRotation(euler[2], Rz);
+
+  Mat(Rx * (Ry * Rz)).copyTo(R);
+}
+
+
+/**
+ *  \brief Compute the transformation needed to rotate n1 onto x axis and p1 to origin
+ */
+static inline void computeTransformRT(const Vec3d& p1, const Vec3d& n1, Matx33d& R, Vec3d& t)
+{
+  // dot product with x axis
+  double angle = acos(n1[0]);
+
+  // cross product with x axis
+  Vec3d axis(0, n1[2], -n1[1]);
+
+  // we try to project on the ground plane but it's already parallel
+  if (n1[1] == 0 && n1[2] == 0)
+  {
+    axis[1] = 1;
+    axis[2] = 0;
+  }
+  else
+  {
+    TNormalize3(axis);
+  }
+
+  aaToR(axis, angle, R);
+  t = -R * p1;
+}
+
+/**
+ *  \brief Flip a normal to the viewing direction
+ *
+ *  \param [in] point Scene point
+ *  \param [in] vp view direction
+ *  \param [in] n normal
+ */
+static inline void flipNormalViewpoint(const Vec3f& point, const Vec3f& vp, Vec3f& n)
+{
+  float cos_theta;
+
+  // See if we need to flip any plane normals
+  Vec3f diff = vp - point;
+
+  // Dot product between the (viewpoint - point) and the plane normal
+  cos_theta = diff.dot(n);
+
+  // Flip the plane normal
+  if (cos_theta < 0)
+  {
+    n *= -1;
+  }
+}
+
+/**
+ *  \brief Convert a rotation matrix to axis angle representation
+ *
+ *  \param [in] R Rotation matrix
+ *  \param [out] axis Axis vector
+ *  \param [out] angle Angle in radians
+ */
+static inline void dcmToAA(Matx33d& R, Vec3d& axis, double *angle)
+{
+  Mat(Vec3d(R(2, 1) - R(2, 1),
+            R(0, 2) - R(2, 0),
+            R(1, 0) - R(0, 1))).copyTo(axis);
+  TNormalize3(axis);
+  *angle = acos(0.5 * (cv::trace(R) - 1.0));
+}
+
+/**
+ *  \brief Convert axis angle representation to rotation matrix
+ *
+ *  \param [in] axis Axis Vector
+ *  \param [in] angle Angle (In radians)
+ *  \param [out] R 3x3 Rotation matrix
+ */
+static inline void aaToDCM(const Vec3d& axis, double angle, Matx33d& R)
+{
+  uint i, j;
+  Matx33d n = Matx33d::all(0);
+
+  for (i = 0; i < 3; i++)
+    for (j = 0; j < 3; j++)
+      if (i != j)
+        n(i, j) = (((i + 1) % 3 == j) ? -1 : 1) * axis[3 - i - j];
+  Mat(Matx33d::eye() + sin(angle) * n + cos(angle) * n * n).copyTo(R);
+}
+
+/**
+ *  \brief Convert a discrete cosine matrix to quaternion
+ *
+ *  \param [in] R Rotation Matrix
+ *  \param [in] q Quaternion
+ */
+static inline void dcmToQuat(Matx33d& R, Vec4d& q)
+{
+  double tr = cv::trace(R);
+  Vec3d v(R(0, 0), R(1, 1), R(2, 2));
+  int idx = tr > 0.0 ? 3 : (int)(std::max_element(v.val, v.val + 3) - v.val);
+  double norm4 = q[(idx + 1) % 4] = 1.0 + (tr > 0.0 ? tr : 2 * R(idx, idx) - tr);
+  int i, prev, next, step = idx % 2 ? 1 : -1, curr = 3;
+  for (i = 0; i < 3; i++)
+  {
+    curr = (curr + step) % 4;
+    next = (curr + 1) % 3, prev = (curr + 2) % 3;
+    q[(idx + i + 2) % 4] = R(next, prev) + (tr > 0.0 || idx == curr ? -1 : 1) * R(prev, next);
+  }
+  q *= 0.5 / sqrt(norm4);
+}
+
+/**
+ *  \brief Convert quaternion to a discrete cosine matrix
+ *
+ *  \param [in] q Quaternion (w is at first element)
+ *  \param [in] R Rotation Matrix
+ *
+ */
+static inline void quatToDCM(Vec4d& q, Matx33d& R)
+{
+  Vec4d sq = q.mul(q);
+
+  double tmp1, tmp2;
+
+  R(0, 0) = sq[0] + sq[1] - sq[2] - sq[3]; // since norm(q) = 1
+  R(1, 1) = sq[0] - sq[1] + sq[2] - sq[3];
+  R(2, 2) = sq[0] - sq[1] - sq[2] + sq[3];
+
+  tmp1 = q[1] * q[2];
+  tmp2 = q[3] * q[0];
+
+  R(0, 1) = 2.0 * (tmp1 + tmp2);
+  R(1, 0) = 2.0 * (tmp1 - tmp2);
+
+  tmp1 = q[1] * q[3];
+  tmp2 = q[2] * q[0];
+
+  R(0, 2) = 2.0 * (tmp1 - tmp2);
+  R(2, 0) = 2.0 * (tmp1 + tmp2);
+
+  tmp1 = q[2] * q[3];
+  tmp2 = q[1] * q[0];
+
+  R(1, 2) = 2.0 * (tmp1 + tmp2);
+  R(2, 1) = 2.0 * (tmp1 - tmp2);
+}
+
+} // namespace ppf_match_3d
+
+} // namespace cv
+
+#endif

modules/surface_matching/src/hash_murmur.hpp

@@ -0,0 +1,84 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#ifndef __OPENCV_SURFACE_MATCHING_HASH_MURMUR_HPP_
+#define __OPENCV_SURFACE_MATCHING_HASH_MURMUR_HPP_
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+
+#if defined(_MSC_VER)
+
+#define FORCE_INLINE    inline static
+
+#include <stdlib.h>
+
+#define ROTL32(x,y)     _rotl(x,y)
+#define ROTL64(x,y)     _rotl64(x,y)
+
+#else
+//#define FORCE_INLINE __attribute__((always_inline))
+#define FORCE_INLINE inline static
+
+/* gcc recognises this code and generates a rotate instruction for CPUs with one */
+#define ROTL32(x,r)  (((uint32_t)x << r) | ((uint32_t)x >> (32 - r)))
+
+inline static long long ROTL64 ( long long x, int8_t r )
+{
+    return (x << r) | (x >> (64 - r));
+}
+
+#endif // !defined(_MSC_VER)
+
+#if (defined __x86_64__ || defined _M_X64)
+#include "hash_murmur64.hpp"
+#define murmurHash hashMurmurx64
+#else
+#include "hash_murmur86.hpp"
+#define murmurHash hashMurmurx86
+#endif
+}
+}
+
+
+#endif

modules/surface_matching/src/hash_murmur64.hpp

@@ -0,0 +1,168 @@
+/*
+Copyright (c) <2014> SMHasher
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+#ifndef __OPENCV_HASH_MURMUR64_HPP_
+#define __OPENCV_HASH_MURMUR64_HPP_
+
+//-----------------------------------------------------------------------------
+// Block read - if your platform needs to do endian-swapping or can only
+// handle aligned reads, do the conversion here
+
+FORCE_INLINE uint getblock ( const uint * p, int i )
+{
+  return p[i];
+}
+
+//----------
+// Finalization mix - force all bits of a hash block to avalanche
+
+// avalanches all bits to within 0.25% bias
+
+FORCE_INLINE uint fmix32 ( uint h )
+{
+  h ^= h >> 16;
+  h *= 0x85ebca6b;
+  h ^= h >> 13;
+  h *= 0xc2b2ae35;
+  h ^= h >> 16;
+
+  return h;
+}
+
+//-----------------------------------------------------------------------------
+
+FORCE_INLINE void bmix32 ( uint & h1, uint & k1, uint & c1, uint & c2 )
+{
+  k1 *= c1;
+  k1  = ROTL32(k1,11);
+  k1 *= c2;
+  h1 ^= k1;
+
+  h1 = h1*3+0x52dce729;
+
+  c1 = c1*5+0x7b7d159c;
+  c2 = c2*5+0x6bce6396;
+}
+
+//-----------------------------------------------------------------------------
+
+FORCE_INLINE void bmix32 ( uint & h1, uint & h2, uint & k1, uint & k2, uint & c1, uint & c2 )
+{
+  k1 *= c1;
+  k1  = ROTL32(k1,11);
+  k1 *= c2;
+  h1 ^= k1;
+  h1 += h2;
+
+  h2 = ROTL32(h2,17);
+
+  k2 *= c2;
+  k2  = ROTL32(k2,11);
+  k2 *= c1;
+  h2 ^= k2;
+  h2 += h1;
+
+  h1 = h1*3+0x52dce729;
+  h2 = h2*3+0x38495ab5;
+
+  c1 = c1*5+0x7b7d159c;
+  c2 = c2*5+0x6bce6396;
+}
+
+//----------
+
+FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const uint seed, void * out )
+{
+  const uchar * data = (const uchar*)key;
+  const int nblocks = len / 8;
+
+  uint h1 = 0x8de1c3ac ^ seed;
+  uint h2 = 0xbab98226 ^ seed;
+
+  uint c1 = 0x95543787;
+  uint c2 = 0x2ad7eb25;
+
+  //----------
+  // body
+
+  const uint * blocks = (const uint *)(data + nblocks*8);
+
+  for (int i = -nblocks; i; i++)
+  {
+    uint k1 = getblock(blocks,i*2+0);
+    uint k2 = getblock(blocks,i*2+1);
+
+    bmix32(h1,h2,k1,k2,c1,c2);
+  }
+
+  //----------
+  // tail
+
+  const uchar * tail = (const uchar*)(data + nblocks*8);
+
+  uint k1 = 0;
+  uint k2 = 0;
+
+  switch (len & 7)
+  {
+  case 7:
+    k2 ^= tail[6] << 16;
+    /* fallthrough */
+  case 6:
+    k2 ^= tail[5] << 8;
+    /* fallthrough */
+  case 5:
+    k2 ^= tail[4] << 0;
+    /* fallthrough */
+  case 4:
+    k1 ^= tail[3] << 24;
+    /* fallthrough */
+  case 3:
+    k1 ^= tail[2] << 16;
+    /* fallthrough */
+  case 2:
+    k1 ^= tail[1] << 8;
+    /* fallthrough */
+  case 1:
+    k1 ^= tail[0] << 0;
+    bmix32(h1,h2,k1,k2,c1,c2);
+  };
+
+  //----------
+  // finalization
+
+  h2 ^= len;
+
+  h1 += h2;
+  h2 += h1;
+
+  h1 = fmix32(h1);
+  h2 = fmix32(h2);
+
+  h1 += h2;
+  h2 += h1;
+
+  ((uint*)out)[0] = h1;
+  ((uint*)out)[1] = h2;
+}
+
+
+#endif

modules/surface_matching/src/hash_murmur86.hpp

@@ -0,0 +1,276 @@
+/*-----------------------------------------------------------------------------
+* MurmurHash3 was written by Austin Appleby, and is placed in the public
+* domain.
+*
+* This implementation was written by Shane Day, and is also public domain.
+*
+* This is a portable ANSI C implementation of MurmurHash3_x86_32 (Murmur3A)
+* with support for progressive processing.
+*/
+
+#include "cvconfig.h"
+
+/* ------------------------------------------------------------------------- */
+/* Determine what native type to use for uint32_t */
+
+/* We can't use the name 'uint32_t' here because it will conflict with
+* any version provided by the system headers or application. */
+
+#if !defined(ulong)
+#define ulong unsigned long
+#endif
+
+/* First look for special cases */
+#if defined(_MSC_VER)
+#define MH_UINT32 ulong
+#endif
+
+/* If the compiler says it's C99 then take its word for it */
+#if !defined(MH_UINT32) && ( \
+ defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L )
+#include <stdint.h>
+#define MH_UINT32 uint32_t
+#endif
+
+/* Otherwise try testing against max value macros from limit.h */
+#if !defined(MH_UINT32)
+#include  <limits.h>
+#if   (USHRT_MAX == 0xffffffffUL)
+#define MH_UINT32 ushort
+#elif (UINT_MAX == 0xffffffffUL)
+#define MH_UINT32 uint
+#elif (ULONG_MAX == 0xffffffffUL)
+#define MH_UINT32 ulong
+#endif
+#endif
+
+#if !defined(MH_UINT32)
+#error Unable to determine type name for u32-bit int
+#endif
+
+/* I'm yet to work on a platform where 'uchar' is not 8 bits */
+#define MH_UINT8  uchar
+
+void PMurHash32_Process(MH_UINT32 *ph1, MH_UINT32 *pcarry, const void *key, int len);
+MH_UINT32 PMurHash32_Result(MH_UINT32 h1, MH_UINT32 carry, MH_UINT32 total_length);
+MH_UINT32 PMurHash32(MH_UINT32 seed, const void *key, int len);
+void hashMurmurx86 ( const void * key, const int len, const uint seed, void * out );
+
+/* I used ugly type names in the header to avoid potential conflicts with
+* application or system typedefs & defines. Since I'm not including any more
+* headers below here I can rename these so that the code reads like C99 */
+#undef uint32_t
+#define uint32_t MH_UINT32
+#undef uint8_t
+#define uint8_t  MH_UINT8
+
+/* MSVC warnings we choose to ignore */
+#if defined(_MSC_VER)
+#pragma warning(disable: 4127) /* conditional expression is constant */
+#endif
+
+/*-----------------------------------------------------------------------------
+* Endianess, misalignment capabilities and util macros
+*
+* The following 3 macros are defined in this section. The other macros defined
+* are only needed to help derive these 3.
+*
+* READ_UINT32(x)   Read a little endian u32-bit int
+* UNALIGNED_SAFE   Defined if READ_UINT32 works on non-word boundaries
+* ROTL32(x,r)      Rotate x left by r bits
+*/
+
+#if (defined(_M_IX86) || defined(__i386__) || defined(__i386) || defined(i386))
+# define UNALIGNED_SAFE 1
+#endif
+
+#ifndef UNALIGNED_SAFE
+# define UNALIGNED_SAFE 1
+#elif defined(UNALIGNED_SAFE) && !UNALIGNED_SAFE == 0
+# undef UNALIGNED_SAFE
+#endif
+
+/* Now find best way we can to READ_UINT32 */
+#ifndef WORDS_BIGENDIAN
+# define READ_UINT32(ptr)   (*((uint32_t*)(ptr)))
+#elif defined(WORDS_BIGENDIAN) \
+    && defined(__GNUC__) && (__GNUC__>4 || (__GNUC__==4 && __GNUC_MINOR__>=3))
+# define READ_UINT32(ptr)   (__builtin_bswap32(*((uint32_t*)(ptr))))
+#endif
+
+#ifndef READ_UINT32
+/* Unknown endianess so last resort is to read individual bytes */
+# define READ_UINT32(ptr)   (ptr[0]|ptr[1]<<8|ptr[2]<<16|ptr[3]<<24)
+# undef UNALIGNED_SAFE
+# define UNALIGNED_SAFE 1
+#endif
+
+/*-----------------------------------------------------------------------------
+* Core murmurhash algorithm macros */
+
+#define C1  (0xcc9e2d51)
+#define C2  (0x1b873593)
+
+/* This is the main processing body of the algorithm. It operates
+* on each full 32-bits of input. */
+#define DOBLOCK(h1, k1) do{ \
+ k1 *= C1; \
+      k1 = ROTL32(k1,15); \
+           k1 *= C2; \
+                 \
+                 h1 ^= k1; \
+                       h1 = ROTL32(h1,13); \
+                            h1 = h1*5+0xe6546b64; \
+                                 }while (0)
+
+
+/* Append unaligned bytes to carry, forcing hash churn if we have 4 bytes */
+/* cnt=bytes to process, h1=name of h1 var, c=carry, n=bytes in c, ptr/len=payload */
+#define DOBYTES(cnt, h1, c, n, ptr, len) do{ \
+                                  int _i = cnt; \
+                                                                           while (_i--){    \
+        c = c>>8 | *ptr++<<24;    \
+        n++;    len--;    \
+        if (n==4)        {            \
+                        DOBLOCK(h1, c);            \
+                        n = 0;            \
+                    } \
+            }}while (0)
+
+/*---------------------------------------------------------------------------*/
+
+/* Main hashing function. Initialise carry to 0 and h1 to 0 or an initial seed
+* if wanted. Both ph1 and pcarry are required arguments. */
+void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int len)
+{
+    uint32_t h1 = *ph1;
+    uint32_t c = *pcarry;
+
+    const uint8_t *ptr = (uint8_t*)                         key;
+    const uint8_t *end;
+
+    /* Extract carry count from low 2 bits of c value */
+    int n = c & 3;
+
+#if defined(UNALIGNED_SAFE)
+    /* This CPU handles unaligned word access */
+
+    /* Consume any carry bytes */
+    int i = (4-n) & 3;
+    if (i && i <= len)
+    {
+        DOBYTES(i, h1, c, n, ptr, len);
+    }
+
+    /* Process 32-bit chunks */
+    end = ptr + len/4*4;
+    for ( ; ptr < end ; ptr+=4)
+    {
+        uint32_t k1 = READ_UINT32(ptr);
+        DOBLOCK(h1, k1);
+    }
+
+#else /*UNALIGNED_SAFE*/
+    /* This CPU does not handle unaligned word access */
+
+    /* Consume enough so that the next data byte is word aligned */
+    int i = -(long)ptr & 3;
+    if (i && i <= len)
+    {
+        DOBYTES(i, h1, c, n, ptr, len);
+    }
+
+    /* We're now aligned. Process in aligned blocks. Specialise for each possible carry count */
+    end = ptr + len/4*4;
+    switch (n)
+    {
+            /* how many bytes in c */
+        case 0: /* c=[----]  w=[3210]  b=[3210]=w            c'=[----] */
+            for ( ; ptr < end ; ptr+=4)
+            {
+                uint32_t k1 = READ_UINT32(ptr);
+                DOBLOCK(h1, k1);
+            }
+            break;
+        case 1: /* c=[0---]  w=[4321]  b=[3210]=c>>24|w<<8   c'=[4---] */
+            for ( ; ptr < end ; ptr+=4)
+            {
+                uint32_t k1 = c>>24;
+                c = READ_UINT32(ptr);
+                k1 |= c<<8;
+                DOBLOCK(h1, k1);
+            }
+            break;
+        case 2: /* c=[10--]  w=[5432]  b=[3210]=c>>16|w<<16  c'=[54--] */
+            for ( ; ptr < end ; ptr+=4)
+            {
+                uint32_t k1 = c>>16;
+                c = READ_UINT32(ptr);
+                k1 |= c<<16;
+                DOBLOCK(h1, k1);
+            }
+            break;
+        case 3: /* c=[210-]  w=[6543]  b=[3210]=c>>8|w<<24   c'=[654-] */
+            for ( ; ptr < end ; ptr+=4)
+            {
+                uint32_t k1 = c>>8;
+                c = READ_UINT32(ptr);
+                k1 |= c<<24;
+                DOBLOCK(h1, k1);
+            }
+    }
+#endif /*UNALIGNED_SAFE*/
+
+    /* Advance over whole 32-bit chunks, possibly leaving 1..3 bytes */
+    len -= len/4*4;
+
+    /* Append any remaining bytes into carry */
+    DOBYTES(len, h1, c, n, ptr, len);
+
+    /* Copy out new running hash and carry */
+    *ph1 = h1;
+    *pcarry = (c & ~0xff) | n;
+}
+
+/*---------------------------------------------------------------------------*/
+
+/* Finalize a hash. To match the original Murmur3A the total_length must be provided */
+uint32_t PMurHash32_Result(uint32_t h, uint32_t carry, uint32_t total_length)
+{
+    uint32_t k1;
+    int n = carry & 3;
+    if (n)
+    {
+        k1 = carry >> (4-n)*8;
+        k1 *= C1;
+        k1 = ROTL32(k1,15);
+        k1 *= C2;
+        h ^= k1;
+    }
+    h ^= total_length;
+
+    /* fmix */
+    h ^= h >> 16;
+    h *= 0x85ebca6b;
+    h ^= h >> 13;
+    h *= 0xc2b2ae35;
+    h ^= h >> 16;
+
+    return h;
+}
+
+/*---------------------------------------------------------------------------*/
+
+/* Murmur3A compatible all-at-once */
+uint32_t PMurHash32(uint32_t seed, const void *key, int len)
+{
+    uint32_t h1=seed, carry=0;
+    PMurHash32_Process(&h1, &carry, key, len);
+    return PMurHash32_Result(h1, carry, len);
+}
+
+void hashMurmurx86 ( const void * key, const int len, const uint seed, void * out )
+{
+    *(uint*)out = PMurHash32 (seed, key, len);
+}
+

modules/surface_matching/src/icp.cpp

@@ -0,0 +1,487 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#include "precomp.hpp"
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+static void subtractColumns(Mat srcPC, Vec3d& mean)
+{
+  int height = srcPC.rows;
+
+  for (int i=0; i<height; i++)
+  {
+    float *row = srcPC.ptr<float>(i);
+    {
+      row[0]-=(float)mean[0];
+      row[1]-=(float)mean[1];
+      row[2]-=(float)mean[2];
+    }
+  }
+}
+
+// as in PCA
+static void computeMeanCols(Mat srcPC, Vec3d& mean)
+{
+  int height = srcPC.rows;
+
+  double mean1=0, mean2 = 0, mean3 = 0;
+
+  for (int i=0; i<height; i++)
+  {
+    const float *row = srcPC.ptr<float>(i);
+    {
+      mean1 += (double)row[0];
+      mean2 += (double)row[1];
+      mean3 += (double)row[2];
+    }
+  }
+
+  mean1/=(double)height;
+  mean2/=(double)height;
+  mean3/=(double)height;
+
+  mean[0] = mean1;
+  mean[1] = mean2;
+  mean[2] = mean3;
+}
+
+// as in PCA
+/*static void subtractMeanFromColumns(Mat srcPC, Vec3d& mean)
+{
+    computeMeanCols(srcPC, mean);
+    subtractColumns(srcPC, mean);
+}*/
+
+// compute the average distance to the origin
+static double computeDistToOrigin(Mat srcPC)
+{
+  int height = srcPC.rows;
+  double dist = 0;
+
+  for (int i=0; i<height; i++)
+  {
+    const float *row = srcPC.ptr<float>(i);
+    dist += sqrt(row[0]*row[0]+row[1]*row[1]+row[2]*row[2]);
+  }
+
+  return dist;
+}
+
+// From numerical receipes: Finds the median of an array
+static float medianF(float arr[], int n)
+{
+  int low, high ;
+  int median;
+  int middle, ll, hh;
+
+  low = 0 ;
+  high = n-1 ;
+  median = (low + high) >>1;
+  for (;;)
+  {
+    if (high <= low) /* One element only */
+      return arr[median] ;
+
+    if (high == low + 1)
+    {
+      /* Two elements only */
+      if (arr[low] > arr[high])
+        std::swap(arr[low], arr[high]) ;
+      return arr[median] ;
+    }
+
+    /* Find median of low, middle and high items; swap into position low */
+    middle = (low + high) >>1;
+    if (arr[middle] > arr[high])
+      std::swap(arr[middle], arr[high]) ;
+    if (arr[low] > arr[high])
+      std::swap(arr[low], arr[high]) ;
+    if (arr[middle] > arr[low])
+      std::swap(arr[middle], arr[low]) ;
+
+    /* Swap low item (now in position middle) into position (low+1) */
+    std::swap(arr[middle], arr[low+1]) ;
+
+    /* Nibble from each end towards middle, swapping items when stuck */
+    ll = low + 1;
+    hh = high;
+    for (;;)
+    {
+      do
+        ll++;
+      while (arr[low] > arr[ll]) ;
+      do
+        hh--;
+      while (arr[hh]  > arr[low]) ;
+
+      if (hh < ll)
+        break;
+
+      std::swap(arr[ll], arr[hh]) ;
+    }
+
+    /* Swap middle item (in position low) back into correct position */
+    std::swap(arr[low], arr[hh]) ;
+
+    /* Re-set active partition */
+    if (hh <= median)
+      low = ll;
+    if (hh >= median)
+      high = hh - 1;
+  }
+}
+
+static float getRejectionThreshold(float* r, int m, float outlierScale)
+{
+  float* t=(float*)calloc(m, sizeof(float));
+  int i=0;
+  float s=0, medR, threshold;
+
+  memcpy(t, r, m*sizeof(float));
+  medR=medianF(t, m);
+
+  for (i=0; i<m; i++)
+    t[i] = (float)fabs((double)r[i]-(double)medR);
+
+  s = 1.48257968f * medianF(t, m);
+
+  threshold = (outlierScale*s+medR);
+
+  free(t);
+  return threshold;
+}
+
+// Kok Lim Low's linearization
+static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Vec3d& rpy, Vec3d& t)
+{
+  //Mat sub = Dst - Src;
+  Mat A = Mat(Src.rows, 6, CV_64F);
+  Mat b = Mat(Src.rows, 1, CV_64F);
+  Mat rpy_t;
+
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
+  for (int i=0; i<Src.rows; i++)
+  {
+    const Vec3d srcPt(Src.ptr<double>(i));
+    const Vec3d dstPt(Dst.ptr<double>(i));
+    const Vec3d normals(Dst.ptr<double>(i) + 3);
+    const Vec3d sub = dstPt - srcPt;
+    const Vec3d axis = srcPt.cross(normals);
+
+    *b.ptr<double>(i) = sub.dot(normals);
+    hconcat(axis.reshape<1, 3>(), normals.reshape<1, 3>(), A.row(i));
+  }
+
+  cv::solve(A, b, rpy_t, DECOMP_SVD);
+  rpy_t.rowRange(0, 3).copyTo(rpy);
+  rpy_t.rowRange(3, 6).copyTo(t);
+}
+
+static void getTransformMat(Vec3d& euler, Vec3d& t, Matx44d& Pose)
+{
+  Matx33d R;
+  eulerToDCM(euler, R);
+  rtToPose(R, t, Pose);
+}
+
+/* Fast way to look up the duplicates
+duplicates is pre-allocated
+make sure that the max element in array will not exceed maxElement
+*/
+static hashtable_int* getHashtable(int* data, size_t length, int numMaxElement)
+{
+  hashtable_int* hashtable = hashtableCreate(static_cast<size_t>(numMaxElement*2), 0);
+  for (size_t i = 0; i < length; i++)
+  {
+    const KeyType key = (KeyType)data[i];
+    hashtableInsertHashed(hashtable, key+1, reinterpret_cast<void*>(i+1));
+  }
+
+  return hashtable;
+}
+
+// source point clouds are assumed to contain their normals
+int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, Matx44d& pose)
+{
+  int n = srcPC.rows;
+  CV_CheckGT(n, 0, "");
+
+  const bool useRobustReject = m_rejectionScale>0;
+
+  Mat srcTemp = srcPC.clone();
+  Mat dstTemp = dstPC.clone();
+  Vec3d meanSrc, meanDst;
+  computeMeanCols(srcTemp, meanSrc);
+  computeMeanCols(dstTemp, meanDst);
+  Vec3d meanAvg = 0.5 * (meanSrc + meanDst);
+  subtractColumns(srcTemp, meanAvg);
+  subtractColumns(dstTemp, meanAvg);
+
+  double distSrc = computeDistToOrigin(srcTemp);
+  double distDst = computeDistToOrigin(dstTemp);
+
+  double scale = (double)n / ((distSrc + distDst)*0.5);
+
+  srcTemp(cv::Range(0, srcTemp.rows), cv::Range(0,3)) *= scale;
+  dstTemp(cv::Range(0, dstTemp.rows), cv::Range(0,3)) *= scale;
+
+  Mat srcPC0 = srcTemp;
+  Mat dstPC0 = dstTemp;
+
+  // initialize pose
+  pose = Matx44d::eye();
+
+  Mat M = Mat::eye(4,4,CV_64F);
+
+  double tempResidual = 0;
+
+
+  // walk the pyramid
+  for (int level = m_numLevels-1; level >=0; level--)
+  {
+    const int numSamples = divUp(n, 1 << level);
+    const double TolP = m_tolerance*(double)(level+1)*(level+1);
+    const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1));
+
+    // Obtain the sampled point clouds for this level: Also rotates the normals
+    Mat srcPCT = transformPCPose(srcPC0, pose);
+
+    const int sampleStep = cvRound((double)n/(double)numSamples);
+
+    srcPCT = samplePCUniform(srcPCT, sampleStep);
+    /*
+    Tolga Birdal thinks that downsampling the scene points might decrease the accuracy.
+    Hamdi Sahloul, however, noticed that accuracy increased (pose residual decreased slightly).
+    */
+    Mat dstPCS = samplePCUniform(dstPC0, sampleStep);
+    void* flann = indexPCFlann(dstPCS);
+
+    double fval_old=9999999999;
+    double fval_perc=0;
+    double fval_min=9999999999;
+    Mat Src_Moved = srcPCT.clone();
+
+    int i=0;
+
+    size_t numElSrc = (size_t)Src_Moved.rows;
+    int sizesResult[2] = {(int)numElSrc, 1};
+    float* distances = new float[numElSrc];
+    int* indices = new int[numElSrc];
+
+    Mat Indices(2, sizesResult, CV_32S, indices, 0);
+    Mat Distances(2, sizesResult, CV_32F, distances, 0);
+
+    // use robust weighting for outlier treatment
+    int* indicesModel = new int[numElSrc];
+    int* indicesScene = new int[numElSrc];
+
+    int* newI = new int[numElSrc];
+    int* newJ = new int[numElSrc];
+
+    Matx44d PoseX = Matx44d::eye();
+
+    while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
+    {
+      uint di=0, selInd = 0;
+
+      queryPCFlann(flann, Src_Moved, Indices, Distances);
+
+      for (di=0; di<numElSrc; di++)
+      {
+        newI[di] = di;
+        newJ[di] = indices[di];
+      }
+
+      if (useRobustReject)
+      {
+        int numInliers = 0;
+        float threshold = getRejectionThreshold(distances, Distances.rows, m_rejectionScale);
+        Mat acceptInd = Distances<threshold;
+
+        uchar *accPtr = (uchar*)acceptInd.data;
+        for (int l=0; l<acceptInd.rows; l++)
+        {
+          if (accPtr[l])
+          {
+            newI[numInliers] = l;
+            newJ[numInliers] = indices[l];
+            numInliers++;
+          }
+        }
+        numElSrc=numInliers;
+      }
+
+      // Step 2: Picky ICP
+      // Among the resulting corresponding pairs, if more than one scene point p_i
+      // is assigned to the same model point m_j, then select p_i that corresponds
+      // to the minimum distance
+
+      hashtable_int* duplicateTable = getHashtable(newJ, numElSrc, dstPCS.rows);
+
+      for (di=0; di<duplicateTable->size; di++)
+      {
+        hashnode_i *node = duplicateTable->nodes[di];
+
+        if (node)
+        {
+          // select the first node
+          size_t idx = reinterpret_cast<size_t>(node->data)-1;
+          int dup = (int)node->key-1;
+          size_t minIdxD = idx;
+          float minDist = distances[idx];
+
+          while ( node )
+          {
+            idx = reinterpret_cast<size_t>(node->data)-1;
+
+            if (distances[idx] < minDist)
+            {
+              minDist = distances[idx];
+              minIdxD = idx;
+            }
+
+            node = node->next;
+          }
+
+          indicesModel[ selInd ] = newI[ minIdxD ];
+          indicesScene[ selInd ] = dup ;
+          selInd++;
+        }
+      }
+
+      hashtableDestroy(duplicateTable);
+
+      if (selInd >= 6)
+      {
+
+        Mat Src_Match = Mat(selInd, srcPCT.cols, CV_64F);
+        Mat Dst_Match = Mat(selInd, srcPCT.cols, CV_64F);
+
+        for (di=0; di<selInd; di++)
+        {
+          const int indModel = indicesModel[di];
+          const int indScene = indicesScene[di];
+          const float *srcPt = srcPCT.ptr<float>(indModel);
+          const float *dstPt = dstPCS.ptr<float>(indScene);
+          double *srcMatchPt = Src_Match.ptr<double>(di);
+          double *dstMatchPt = Dst_Match.ptr<double>(di);
+          int ci=0;
+
+          for (ci=0; ci<srcPCT.cols; ci++)
+          {
+            srcMatchPt[ci] = (double)srcPt[ci];
+            dstMatchPt[ci] = (double)dstPt[ci];
+          }
+        }
+
+        Vec3d rpy, t;
+        minimizePointToPlaneMetric(Src_Match, Dst_Match, rpy, t);
+        if (cvIsNaN(cv::trace(rpy)) || cvIsNaN(cv::norm(t)))
+          break;
+        getTransformMat(rpy, t, PoseX);
+        Src_Moved = transformPCPose(srcPCT, PoseX);
+
+        double fval = cv::norm(Src_Match, Dst_Match)/(double)(Src_Moved.rows);
+
+        // Calculate change in error between iterations
+        fval_perc=fval/fval_old;
+
+        // Store error value
+        fval_old=fval;
+
+        if (fval < fval_min)
+          fval_min = fval;
+      }
+      else
+        break;
+
+      i++;
+
+    }
+
+    pose = PoseX * pose;
+    residual = tempResidual;
+
+    delete[] newI;
+    delete[] newJ;
+    delete[] indicesModel;
+    delete[] indicesScene;
+    delete[] distances;
+    delete[] indices;
+
+    tempResidual = fval_min;
+    destroyFlann(flann);
+  }
+
+  Matx33d Rpose;
+  Vec3d Cpose;
+  poseToRT(pose, Rpose, Cpose);
+  Cpose = Cpose / scale + meanAvg - Rpose * meanAvg;
+  rtToPose(Rpose, Cpose, pose);
+
+  residual = tempResidual;
+
+  return 0;
+}
+
+// source point clouds are assumed to contain their normals
+int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3DPtr>& poses)
+{
+  #if defined _OPENMP
+  #pragma omp parallel for
+  #endif
+  for (int i=0; i<(int)poses.size(); i++)
+  {
+    Matx44d poseICP = Matx44d::eye();
+    Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
+    registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
+    poses[i]->appendPose(poseICP);
+  }
+  return 0;
+}
+
+} // namespace ppf_match_3d
+
+} // namespace cv

modules/surface_matching/src/pose_3d.cpp

@@ -0,0 +1,317 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#include "precomp.hpp"
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+void Pose3D::updatePose(Matx44d& NewPose)
+{
+  Matx33d R;
+
+  pose = NewPose;
+  poseToRT(pose, R, t);
+
+  // compute the angle
+  const double trace = cv::trace(R);
+
+  if (fabs(trace - 3) <= EPS)
+  {
+    angle = 0;
+  }
+  else
+    if (fabs(trace + 1) <= EPS)
+    {
+      angle = M_PI;
+    }
+    else
+    {
+      angle = ( acos((trace - 1)/2) );
+    }
+
+  // compute the quaternion
+  dcmToQuat(R, q);
+}
+
+void Pose3D::updatePose(Matx33d& NewR, Vec3d& NewT)
+{
+  rtToPose(NewR, NewT, pose);
+
+  // compute the angle
+  const double trace = cv::trace(NewR);
+
+  if (fabs(trace - 3) <= EPS)
+  {
+    angle = 0;
+  }
+  else
+    if (fabs(trace + 1) <= EPS)
+    {
+      angle = M_PI;
+    }
+    else
+    {
+      angle = ( acos((trace - 1)/2) );
+    }
+
+  // compute the quaternion
+  dcmToQuat(NewR, q);
+}
+
+void Pose3D::updatePoseQuat(Vec4d& Q, Vec3d& NewT)
+{
+  Matx33d NewR;
+
+  quatToDCM(Q, NewR);
+  q = Q;
+
+  rtToPose(NewR, NewT, pose);
+
+  // compute the angle
+  const double trace = cv::trace(NewR);
+
+  if (fabs(trace - 3) <= EPS)
+  {
+    angle = 0;
+  }
+  else
+  {
+    if (fabs(trace + 1) <= EPS)
+    {
+      angle = M_PI;
+    }
+    else
+    {
+      angle = ( acos((trace - 1)/2) );
+    }
+  }
+}
+
+
+void Pose3D::appendPose(Matx44d& IncrementalPose)
+{
+  Matx33d R;
+  Matx44d PoseFull = IncrementalPose * this->pose;
+
+  poseToRT(PoseFull, R, t);
+
+  // compute the angle
+  const double trace = cv::trace(R);
+
+  if (fabs(trace - 3) <= EPS)
+  {
+    angle = 0;
+  }
+  else
+    if (fabs(trace + 1) <= EPS)
+    {
+      angle = M_PI;
+    }
+    else
+    {
+      angle = ( acos((trace - 1)/2) );
+    }
+
+  // compute the quaternion
+  dcmToQuat(R, q);
+
+  pose = PoseFull;
+}
+
+Pose3DPtr Pose3D::clone()
+{
+  Ptr<Pose3D> new_pose(new Pose3D(alpha, modelIndex, numVotes));
+
+  new_pose->pose = this->pose;
+  new_pose->q = q;
+  new_pose->t = t;
+  new_pose->angle = angle;
+
+  return new_pose;
+}
+
+void Pose3D::printPose()
+{
+  printf("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n", (uint)this->modelIndex, (uint)this->numVotes, this->residual);
+  std::cout << this->pose << std::endl;
+}
+
+int Pose3D::writePose(FILE* f)
+{
+  int POSE_MAGIC = 7673;
+  fwrite(&POSE_MAGIC, sizeof(int), 1, f);
+  fwrite(&angle, sizeof(double), 1, f);
+  fwrite(&numVotes, sizeof(int), 1, f);
+  fwrite(&modelIndex, sizeof(int), 1, f);
+  fwrite(pose.val, sizeof(double)*16, 1, f);
+  fwrite(t.val, sizeof(double)*3, 1, f);
+  fwrite(q.val, sizeof(double)*4, 1, f);
+  fwrite(&residual, sizeof(double), 1, f);
+  return 0;
+}
+
+int Pose3D::readPose(FILE* f)
+{
+  int POSE_MAGIC = 7673, magic;
+
+  size_t status = fread(&magic, sizeof(int), 1, f);
+  if (status && magic == POSE_MAGIC)
+  {
+    status = fread(&angle, sizeof(double), 1, f);
+    status = fread(&numVotes, sizeof(int), 1, f);
+    status = fread(&modelIndex, sizeof(int), 1, f);
+    status = fread(pose.val, sizeof(double)*16, 1, f);
+    status = fread(t.val, sizeof(double)*3, 1, f);
+    status = fread(q.val, sizeof(double)*4, 1, f);
+    status = fread(&residual, sizeof(double), 1, f);
+    return 0;
+  }
+
+  return -1;
+}
+
+int Pose3D::writePose(const std::string& FileName)
+{
+  FILE* f = fopen(FileName.c_str(), "wb");
+
+  if (!f)
+    return -1;
+
+  int status = writePose(f);
+
+  fclose(f);
+  return status;
+}
+
+int Pose3D::readPose(const std::string& FileName)
+{
+  FILE* f = fopen(FileName.c_str(), "rb");
+
+  if (!f)
+    return -1;
+
+  int status = readPose(f);
+
+  fclose(f);
+  return status;
+}
+
+
+void PoseCluster3D::addPose(Pose3DPtr newPose)
+{
+  poseList.push_back(newPose);
+  this->numVotes += newPose->numVotes;
+};
+
+int PoseCluster3D::writePoseCluster(FILE* f)
+{
+  int POSE_CLUSTER_MAGIC_IO = 8462597;
+  fwrite(&POSE_CLUSTER_MAGIC_IO, sizeof(int), 1, f);
+  fwrite(&id, sizeof(int), 1, f);
+  fwrite(&numVotes, sizeof(int), 1, f);
+
+  int numPoses = (int)poseList.size();
+  fwrite(&numPoses, sizeof(int), 1, f);
+
+  for (int i=0; i<numPoses; i++)
+    poseList[i]->writePose(f);
+
+  return 0;
+}
+
+int PoseCluster3D::readPoseCluster(FILE* f)
+{
+  // The magic values are only used to check the files
+  int POSE_CLUSTER_MAGIC_IO = 8462597;
+  int magic=0, numPoses=0;
+  size_t status;
+  status = fread(&magic, sizeof(int), 1, f);
+
+  if (!status || magic!=POSE_CLUSTER_MAGIC_IO)
+    return -1;
+
+  status = fread(&id, sizeof(int), 1, f);
+  status = fread(&numVotes, sizeof(int), 1, f);
+  status = fread(&numPoses, sizeof(int), 1, f);
+
+  poseList.clear();
+  poseList.resize(numPoses);
+  for (size_t i=0; i<poseList.size(); i++)
+  {
+    poseList[i] = Pose3DPtr(new Pose3D());
+    poseList[i]->readPose(f);
+  }
+
+  fclose(f);
+  return 0;
+}
+
+int PoseCluster3D::writePoseCluster(const std::string& FileName)
+{
+  FILE* f = fopen(FileName.c_str(), "wb");
+
+  if (!f)
+    return -1;
+
+  int status = writePoseCluster(f);
+
+  fclose(f);
+  return status;
+}
+
+int PoseCluster3D::readPoseCluster(const std::string& FileName)
+{
+  FILE* f = fopen(FileName.c_str(), "rb");
+
+  if (!f)
+    return -1;
+
+  int status = readPoseCluster(f);
+
+  fclose(f);
+  return status;
+}
+
+} // namespace ppf_match_3d
+
+} // namespace cv

modules/surface_matching/src/ppf_helpers.cpp

@@ -0,0 +1,754 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#include "precomp.hpp"
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+typedef cv::flann::L2<float> Distance_32F;
+typedef cv::flann::GenericIndex< Distance_32F > FlannIndex;
+
+void shuffle(int *array, size_t n);
+Mat genRandomMat(int rows, int cols, double mean, double stddev, int type);
+void getRandQuat(Vec4d& q);
+void getRandomRotation(Matx33d& R);
+void meanCovLocalPC(const Mat& pc, const int point_count, Matx33d& CovMat, Vec3d& Mean);
+void meanCovLocalPCInd(const Mat& pc, const int* Indices, const int point_count, Matx33d& CovMat, Vec3d& Mean);
+
+static std::vector<std::string> split(const std::string &text, char sep) {
+  std::vector<std::string> tokens;
+  std::size_t start = 0, end = 0;
+  while ((end = text.find(sep, start)) != std::string::npos) {
+    tokens.push_back(text.substr(start, end - start));
+    start = end + 1;
+  }
+  tokens.push_back(text.substr(start));
+  return tokens;
+}
+
+
+
+Mat loadPLYSimple(const char* fileName, int withNormals)
+{
+  Mat cloud;
+  int numVertices = 0;
+  int numCols = 3;
+  int has_normals = 0;
+
+  std::ifstream ifs(fileName);
+
+  if (!ifs.is_open())
+    CV_Error(Error::StsError, String("Error opening input file: ") + String(fileName) + "\n");
+
+  std::string str;
+  while (str.substr(0, 10) != "end_header")
+  {
+    std::vector<std::string> tokens = split(str,' ');
+    if (tokens.size() == 3)
+    {
+      if (tokens[0] == "element" && tokens[1] == "vertex")
+      {
+        numVertices = atoi(tokens[2].c_str());
+      }
+      else if (tokens[0] == "property")
+      {
+        if (tokens[2] == "nx" || tokens[2] == "normal_x")
+        {
+          has_normals = -1;
+          numCols += 3;
+        }
+        else if (tokens[2] == "r" || tokens[2] == "red")
+        {
+          //has_color = true;
+          numCols += 3;
+        }
+        else if (tokens[2] == "a" || tokens[2] == "alpha")
+        {
+          //has_alpha = true;
+          numCols += 1;
+        }
+      }
+    }
+    else if (tokens.size() > 1 && tokens[0] == "format" && tokens[1] != "ascii")
+      CV_Error(Error::StsBadArg, String("Cannot read file, only ascii ply format is currently supported..."));
+    std::getline(ifs, str);
+  }
+  withNormals &= has_normals;
+
+  cloud = Mat(numVertices, withNormals ? 6 : 3, CV_32FC1);
+
+  for (int i = 0; i < numVertices; i++)
+  {
+    float* data = cloud.ptr<float>(i);
+    int col = 0;
+    for (; col < (withNormals ? 6 : 3); ++col)
+    {
+      ifs >> data[col];
+    }
+    for (; col < numCols; ++col)
+    {
+      float tmp;
+      ifs >> tmp;
+    }
+    if (withNormals)
+    {
+      // normalize to unit norm
+      double norm = sqrt(data[3]*data[3] + data[4]*data[4] + data[5]*data[5]);
+      if (norm>0.00001)
+      {
+        data[3]/=static_cast<float>(norm);
+        data[4]/=static_cast<float>(norm);
+        data[5]/=static_cast<float>(norm);
+      }
+    }
+  }
+
+  //cloud *= 5.0f;
+  return cloud;
+}
+
+void writePLY(Mat PC, const char* FileName)
+{
+  std::ofstream outFile( FileName );
+
+  if ( !outFile.is_open() )
+    CV_Error(Error::StsError, String("Error opening output file: ") + String(FileName) + "\n");
+
+  ////
+  // Header
+  ////
+
+  const int pointNum = ( int ) PC.rows;
+  const int vertNum  = ( int ) PC.cols;
+
+  outFile << "ply" << std::endl;
+  outFile << "format ascii 1.0" << std::endl;
+  outFile << "element vertex " << pointNum << std::endl;
+  outFile << "property float x" << std::endl;
+  outFile << "property float y" << std::endl;
+  outFile << "property float z" << std::endl;
+  if (vertNum==6)
+  {
+    outFile << "property float nx" << std::endl;
+    outFile << "property float ny" << std::endl;
+    outFile << "property float nz" << std::endl;
+  }
+  outFile << "end_header" << std::endl;
+
+  ////
+  // Points
+  ////
+
+  for ( int pi = 0; pi < pointNum; ++pi )
+  {
+    const float* point = PC.ptr<float>(pi);
+
+    outFile << point[0] << " " << point[1] << " " << point[2];
+
+    if (vertNum==6)
+    {
+      outFile<<" " << point[3] << " "<<point[4]<<" "<<point[5];
+    }
+
+    outFile << std::endl;
+  }
+
+  return;
+}
+
+void writePLYVisibleNormals(Mat PC, const char* FileName)
+{
+  std::ofstream outFile(FileName);
+
+  if (!outFile.is_open())
+    CV_Error(Error::StsError, String("Error opening output file: ") + String(FileName) + "\n");
+
+  ////
+  // Header
+  ////
+
+  const int pointNum = (int)PC.rows;
+  const int vertNum = (int)PC.cols;
+  const bool hasNormals = vertNum == 6;
+
+  outFile << "ply" << std::endl;
+  outFile << "format ascii 1.0" << std::endl;
+  outFile << "element vertex " << (hasNormals? 2*pointNum:pointNum) << std::endl;
+  outFile << "property float x" << std::endl;
+  outFile << "property float y" << std::endl;
+  outFile << "property float z" << std::endl;
+  if (hasNormals)
+  {
+    outFile << "property uchar red" << std::endl;
+    outFile << "property uchar green" << std::endl;
+    outFile << "property uchar blue" << std::endl;
+  }
+  outFile << "end_header" << std::endl;
+
+  ////
+  // Points
+  ////
+
+  for (int pi = 0; pi < pointNum; ++pi)
+  {
+    const float* point = PC.ptr<float>(pi);
+
+    outFile << point[0] << " " << point[1] << " " << point[2];
+
+    if (hasNormals)
+    {
+      outFile << " 127 127 127" << std::endl;
+      outFile << point[0] + point[3] << " " << point[1] + point[4] << " " << point[2] + point[5];
+      outFile << " 255 0 0";
+    }
+
+    outFile << std::endl;
+  }
+
+  return;
+}
+
+Mat samplePCUniform(Mat PC, int sampleStep)
+{
+  int numRows = PC.rows/sampleStep;
+  Mat sampledPC = Mat(numRows, PC.cols, PC.type());
+
+  int c=0;
+  for (int i=0; i<PC.rows && c<numRows; i+=sampleStep)
+  {
+    PC.row(i).copyTo(sampledPC.row(c++));
+  }
+
+  return sampledPC;
+}
+
+Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int> &indices)
+{
+  int numRows = cvRound((double)PC.rows/(double)sampleStep);
+  indices.resize(numRows);
+  Mat sampledPC = Mat(numRows, PC.cols, PC.type());
+
+  int c=0;
+  for (int i=0; i<PC.rows && c<numRows; i+=sampleStep)
+  {
+    indices[c] = i;
+    PC.row(i).copyTo(sampledPC.row(c++));
+  }
+
+  return sampledPC;
+}
+
+void* indexPCFlann(Mat pc)
+{
+  Mat dest_32f;
+  pc.colRange(0,3).copyTo(dest_32f);
+  return new FlannIndex(dest_32f, cvflann::KDTreeSingleIndexParams(8));
+}
+
+void destroyFlann(void* flannIndex)
+{
+  delete ((FlannIndex*)flannIndex);
+}
+
+// For speed purposes this function assumes that PC, Indices and Distances are created with continuous structures
+void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances)
+{
+  queryPCFlann(flannIndex, pc, indices, distances, 1);
+}
+
+void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors)
+{
+  Mat obj_32f;
+  pc.colRange(0, 3).copyTo(obj_32f);
+  ((FlannIndex*)flannIndex)->knnSearch(obj_32f, indices, distances, numNeighbors, cvflann::SearchParams(32));
+}
+
+// uses a volume instead of an octree
+// TODO: Right now normals are required.
+// This is much faster than sample_pc_octree
+Mat samplePCByQuantization(Mat pc, Vec2f& xrange, Vec2f& yrange, Vec2f& zrange, float sampleStep, int weightByCenter)
+{
+  std::vector< std::vector<int> > map;
+
+  int numSamplesDim = (int)(1.0/sampleStep);
+
+  float xr = xrange[1] - xrange[0];
+  float yr = yrange[1] - yrange[0];
+  float zr = zrange[1] - zrange[0];
+
+  int numPoints = 0;
+
+  map.resize((numSamplesDim+1)*(numSamplesDim+1)*(numSamplesDim+1));
+
+  // OpenMP might seem like a good idea, but it didn't speed this up for me
+  //#pragma omp parallel for
+  for (int i=0; i<pc.rows; i++)
+  {
+    const float* point = pc.ptr<float>(i);
+
+    // quantize a point
+    const int xCell =(int) ((float)numSamplesDim*(point[0]-xrange[0])/xr);
+    const int yCell =(int) ((float)numSamplesDim*(point[1]-yrange[0])/yr);
+    const int zCell =(int) ((float)numSamplesDim*(point[2]-zrange[0])/zr);
+    const int index = xCell*numSamplesDim*numSamplesDim+yCell*numSamplesDim+zCell;
+
+    /*#pragma omp critical
+        {*/
+    map[index].push_back(i);
+    //  }
+  }
+
+  for (unsigned int i=0; i<map.size(); i++)
+  {
+    numPoints += (map[i].size()>0);
+  }
+
+  Mat pcSampled = Mat(numPoints, pc.cols, CV_32F);
+  int c = 0;
+
+  for (unsigned int i=0; i<map.size(); i++)
+  {
+    double px=0, py=0, pz=0;
+    double nx=0, ny=0, nz=0;
+
+    std::vector<int> curCell = map[i];
+    int cn = (int)curCell.size();
+    if (cn>0)
+    {
+      if (weightByCenter)
+      {
+        int xCell, yCell, zCell;
+        double xc, yc, zc;
+        double weightSum = 0 ;
+        zCell = i % numSamplesDim;
+        yCell = ((i-zCell)/numSamplesDim) % numSamplesDim;
+        xCell = ((i-zCell-yCell*numSamplesDim)/(numSamplesDim*numSamplesDim));
+
+        xc = ((double)xCell+0.5) * (double)xr/numSamplesDim + (double)xrange[0];
+        yc = ((double)yCell+0.5) * (double)yr/numSamplesDim + (double)yrange[0];
+        zc = ((double)zCell+0.5) * (double)zr/numSamplesDim + (double)zrange[0];
+
+        for (int j=0; j<cn; j++)
+        {
+          const int ptInd = curCell[j];
+          float* point = pc.ptr<float>(ptInd);
+          const double dx = point[0]-xc;
+          const double dy = point[1]-yc;
+          const double dz = point[2]-zc;
+          const double d = sqrt(dx*dx+dy*dy+dz*dz);
+          double w = 0;
+
+          if (d>EPS)
+          {
+            // it is possible to use different weighting schemes.
+            // inverse weigthing was just good for me
+            // exp( - (distance/h)**2 )
+            //const double w = exp(-d*d);
+            w = 1.0/d;
+          }
+
+          //float weights[3]={1,1,1};
+          px += w*(double)point[0];
+          py += w*(double)point[1];
+          pz += w*(double)point[2];
+          nx += w*(double)point[3];
+          ny += w*(double)point[4];
+          nz += w*(double)point[5];
+
+          weightSum+=w;
+        }
+        px/=(double)weightSum;
+        py/=(double)weightSum;
+        pz/=(double)weightSum;
+        nx/=(double)weightSum;
+        ny/=(double)weightSum;
+        nz/=(double)weightSum;
+      }
+      else
+      {
+        for (int j=0; j<cn; j++)
+        {
+          const int ptInd = curCell[j];
+          float* point = pc.ptr<float>(ptInd);
+
+          px += (double)point[0];
+          py += (double)point[1];
+          pz += (double)point[2];
+          nx += (double)point[3];
+          ny += (double)point[4];
+          nz += (double)point[5];
+        }
+
+        px/=(double)cn;
+        py/=(double)cn;
+        pz/=(double)cn;
+        nx/=(double)cn;
+        ny/=(double)cn;
+        nz/=(double)cn;
+
+      }
+
+      float *pcData = pcSampled.ptr<float>(c);
+      pcData[0]=(float)px;
+      pcData[1]=(float)py;
+      pcData[2]=(float)pz;
+
+      // normalize the normals
+      double norm = sqrt(nx*nx+ny*ny+nz*nz);
+
+      if (norm>EPS)
+      {
+        pcData[3]=(float)(nx/norm);
+        pcData[4]=(float)(ny/norm);
+        pcData[5]=(float)(nz/norm);
+      }
+      else
+      {
+        pcData[3]=0.0f;
+        pcData[4]=0.0f;
+        pcData[5]=0.0f;
+      }
+      //#pragma omp atomic
+      c++;
+
+      curCell.clear();
+    }
+  }
+
+  map.clear();
+  return pcSampled;
+}
+
+void shuffle(int *array, size_t n)
+{
+  size_t i;
+  for (i = 0; i < n - 1; i++)
+  {
+    size_t j = i + rand() / (RAND_MAX / (n - i) + 1);
+    int t = array[j];
+    array[j] = array[i];
+    array[i] = t;
+  }
+}
+
+// compute the standard bounding box
+void computeBboxStd(Mat pc, Vec2f& xRange, Vec2f& yRange, Vec2f& zRange)
+{
+  Mat pcPts = pc.colRange(0, 3);
+  int num = pcPts.rows;
+
+  float* points = (float*)pcPts.data;
+
+  xRange[0] = points[0];
+  xRange[1] = points[0];
+  yRange[0] = points[1];
+  yRange[1] = points[1];
+  zRange[0] = points[2];
+  zRange[1] = points[2];
+
+  for  ( int  ind = 0; ind < num; ind++ )
+  {
+    const float* row = (float*)(pcPts.data + (ind * pcPts.step));
+    const float x = row[0];
+    const float y = row[1];
+    const float z = row[2];
+
+    if (x<xRange[0])
+      xRange[0]=x;
+    if (x>xRange[1])
+      xRange[1]=x;
+
+    if (y<yRange[0])
+      yRange[0]=y;
+    if (y>yRange[1])
+      yRange[1]=y;
+
+    if (z<zRange[0])
+      zRange[0]=z;
+    if (z>zRange[1])
+      zRange[1]=z;
+  }
+}
+
+Mat normalizePCCoeff(Mat pc, float scale, float* Cx, float* Cy, float* Cz, float* MinVal, float* MaxVal)
+{
+  double minVal=0, maxVal=0;
+
+  Mat x,y,z, pcn;
+  pc.col(0).copyTo(x);
+  pc.col(1).copyTo(y);
+  pc.col(2).copyTo(z);
+
+  float cx = (float) cv::mean(x)[0];
+  float cy = (float) cv::mean(y)[0];
+  float cz = (float) cv::mean(z)[0];
+
+  cv::minMaxIdx(pc, &minVal, &maxVal);
+
+  x=x-cx;
+  y=y-cy;
+  z=z-cz;
+  pcn.create(pc.rows, 3, CV_32FC1);
+  x.copyTo(pcn.col(0));
+  y.copyTo(pcn.col(1));
+  z.copyTo(pcn.col(2));
+
+  cv::minMaxIdx(pcn, &minVal, &maxVal);
+  pcn=(float)scale*(pcn)/((float)maxVal-(float)minVal);
+
+  *MinVal=(float)minVal;
+  *MaxVal=(float)maxVal;
+  *Cx=(float)cx;
+  *Cy=(float)cy;
+  *Cz=(float)cz;
+
+  return pcn;
+}
+
+Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal, float MaxVal)
+{
+  Mat x,y,z, pcn;
+  pc.col(0).copyTo(x);
+  pc.col(1).copyTo(y);
+  pc.col(2).copyTo(z);
+
+  x=x-Cx;
+  y=y-Cy;
+  z=z-Cz;
+  pcn.create(pc.rows, 3, CV_32FC1);
+  x.copyTo(pcn.col(0));
+  y.copyTo(pcn.col(1));
+  z.copyTo(pcn.col(2));
+
+  pcn=(float)scale*(pcn)/((float)MaxVal-(float)MinVal);
+
+  return pcn;
+}
+
+Mat transformPCPose(Mat pc, const Matx44d& Pose)
+{
+  Mat pct = Mat(pc.rows, pc.cols, CV_32F);
+
+  Matx33d R;
+  Vec3d t;
+  poseToRT(Pose, R, t);
+
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
+  for (int i=0; i<pc.rows; i++)
+  {
+    const float *pcData = pc.ptr<float>(i);
+    const Vec3f n1(&pcData[3]);
+
+    Vec4d p = Pose * Vec4d(pcData[0], pcData[1], pcData[2], 1);
+    Vec3d p2(p.val);
+
+    // p2[3] should normally be 1
+    if (fabs(p[3]) > EPS)
+    {
+      Mat((1.0 / p[3]) * p2).reshape(1, 1).convertTo(pct.row(i).colRange(0, 3), CV_32F);
+    }
+
+    // If the point cloud has normals,
+    // then rotate them as well
+    if (pc.cols == 6)
+    {
+      Vec3d n(n1), n2;
+
+      n2 = R * n;
+      double nNorm = cv::norm(n2);
+
+      if (nNorm > EPS)
+      {
+        Mat((1.0 / nNorm) * n2).reshape(1, 1).convertTo(pct.row(i).colRange(3, 6), CV_32F);
+      }
+    }
+  }
+
+  return pct;
+}
+
+Mat genRandomMat(int rows, int cols, double mean, double stddev, int type)
+{
+  Mat meanMat = mean*Mat::ones(1,1,type);
+  Mat sigmaMat= stddev*Mat::ones(1,1,type);
+  RNG rng(time(0));
+  Mat matr(rows, cols,type);
+  rng.fill(matr, RNG::NORMAL, meanMat, sigmaMat);
+
+  return matr;
+}
+
+void getRandQuat(Vec4d& q)
+{
+  q[0] = (float)rand()/(float)(RAND_MAX);
+  q[1] = (float)rand()/(float)(RAND_MAX);
+  q[2] = (float)rand()/(float)(RAND_MAX);
+  q[3] = (float)rand()/(float)(RAND_MAX);
+
+  q *= 1.0 / cv::norm(q);
+  q[0]=fabs(q[0]);
+}
+
+void getRandomRotation(Matx33d& R)
+{
+  Vec4d q;
+  getRandQuat(q);
+  quatToDCM(q, R);
+}
+
+void getRandomPose(Matx44d& Pose)
+{
+  Matx33d R;
+  Vec3d t;
+
+  srand((unsigned int)time(0));
+  getRandomRotation(R);
+
+  t[0] = (float)rand()/(float)(RAND_MAX);
+  t[1] = (float)rand()/(float)(RAND_MAX);
+  t[2] = (float)rand()/(float)(RAND_MAX);
+
+  rtToPose(R,t,Pose);
+}
+
+Mat addNoisePC(Mat pc, double scale)
+{
+  Mat randT = genRandomMat(pc.rows,pc.cols,0,scale,CV_32FC1);
+  return randT + pc;
+}
+
+/*
+The routines below use the eigenvectors of the local covariance matrix
+to compute the normals of a point cloud.
+The algorithm uses FLANN and Joachim Kopp's fast 3x3 eigenvector computations
+to improve accuracy and increase speed
+Also, view point flipping as in point cloud library is implemented
+*/
+
+void meanCovLocalPC(const Mat& pc, const int point_count, Matx33d& CovMat, Vec3d& Mean)
+{
+  cv::calcCovarMatrix(pc.rowRange(0, point_count), CovMat, Mean, cv::COVAR_NORMAL | cv::COVAR_ROWS);
+  CovMat *= 1.0 / (point_count - 1);
+}
+
+void meanCovLocalPCInd(const Mat& pc, const int* Indices, const int point_count, Matx33d& CovMat, Vec3d& Mean)
+{
+  int i, j, k;
+
+  CovMat = Matx33d::all(0);
+  Mean = Vec3d::all(0);
+  for (i = 0; i < point_count; ++i)
+  {
+    const float* cloud = pc.ptr<float>(Indices[i]);
+    for (j = 0; j < 3; ++j)
+    {
+      for (k = 0; k < 3; ++k)
+        CovMat(j, k) += cloud[j] * cloud[k];
+      Mean[j] += cloud[j];
+    }
+  }
+  Mean *= 1.0 / point_count;
+  CovMat *= 1.0 / point_count;
+
+  for (j = 0; j < 3; ++j)
+    for (k = 0; k < 3; ++k)
+      CovMat(j, k) -= Mean[j] * Mean[k];
+}
+
+int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3f& viewpoint)
+{
+  int i;
+
+  if (PC.cols!=3 && PC.cols!=6) // 3d data is expected
+  {
+    //return -1;
+    CV_Error(cv::Error::BadImageSize, "PC should have 3 or 6 elements in its columns");
+  }
+
+  PCNormals.create(PC.rows, 6, CV_32F);
+  Mat PCInput = PCNormals.colRange(0, 3);
+  Mat Distances(PC.rows, NumNeighbors, CV_32F);
+  Mat Indices(PC.rows, NumNeighbors, CV_32S);
+
+  PC.rowRange(0, PC.rows).colRange(0, 3).copyTo(PCNormals.rowRange(0, PC.rows).colRange(0, 3));
+
+  void* flannIndex = indexPCFlann(PCInput);
+
+  queryPCFlann(flannIndex, PCInput, Indices, Distances, NumNeighbors);
+  destroyFlann(flannIndex);
+  flannIndex = 0;
+
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
+  for (i=0; i<PC.rows; i++)
+  {
+    Matx33d C;
+    Vec3d mu;
+    const int* indLocal = Indices.ptr<int>(i);
+
+    // compute covariance matrix
+    meanCovLocalPCInd(PCNormals, indLocal, NumNeighbors, C, mu);
+
+    // eigenvectors of covariance matrix
+    Mat eigVect, eigVal;
+    eigen(C, eigVal, eigVect);
+    eigVect.row(2).convertTo(PCNormals.row(i).colRange(3, 6), CV_32F);
+
+    if (FlipViewpoint)
+    {
+      Vec3f nr(PCNormals.ptr<float>(i) + 3);
+      Vec3f pci(PCNormals.ptr<float>(i));
+      flipNormalViewpoint(pci, viewpoint, nr);
+      Mat(nr).reshape(1, 1).copyTo(PCNormals.row(i).colRange(3, 6));
+    }
+  }
+
+  return 1;
+}
+
+} // namespace ppf_match_3d
+
+} // namespace cv

modules/surface_matching/src/ppf_match_3d.cpp

@@ -0,0 +1,613 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#include "precomp.hpp"
+#include "hash_murmur.hpp"
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+
+static const size_t PPF_LENGTH = 5;
+
+// routines for assisting sort
+static bool pose3DPtrCompare(const Pose3DPtr& a, const Pose3DPtr& b)
+{
+  CV_Assert(!a.empty() && !b.empty());
+  return ( a->numVotes > b->numVotes );
+}
+
+static int sortPoseClusters(const PoseCluster3DPtr& a, const PoseCluster3DPtr& b)
+{
+  CV_Assert(!a.empty() && !b.empty());
+  return ( a->numVotes > b->numVotes );
+}
+
+// simple hashing
+/*static int hashPPFSimple(const Vec4d& f, const double AngleStep, const double DistanceStep)
+{
+  Vec4i key(
+      (int)(f[0] / AngleStep),
+      (int)(f[1] / AngleStep),
+      (int)(f[2] / AngleStep),
+      (int)(f[3] / DistanceStep));
+
+  int hashKey = d.val[0] | (d.val[1] << 8) | (d.val[2] << 16) | (d.val[3] << 24);
+  return hashKey;
+}*/
+
+// quantize ppf and hash it for proper indexing
+static KeyType hashPPF(const Vec4d& f, const double AngleStep, const double DistanceStep)
+{
+  Vec4i key(
+      (int)(f[0] / AngleStep),
+      (int)(f[1] / AngleStep),
+      (int)(f[2] / AngleStep),
+      (int)(f[3] / DistanceStep));
+  KeyType hashKey[2] = {0, 0};  // hashMurmurx64() fills two values
+
+  murmurHash(key.val, 4*sizeof(int), 42, &hashKey[0]);
+  return hashKey[0];
+}
+
+/*static size_t hashMurmur(uint key)
+{
+  size_t hashKey=0;
+  hashMurmurx86((void*)&key, 4, 42, &hashKey);
+  return hashKey;
+}*/
+
+static double computeAlpha(const Vec3d& p1, const Vec3d& n1, const Vec3d& p2)
+{
+  Vec3d Tmg, mpt;
+  Matx33d R;
+  double alpha;
+
+  computeTransformRT(p1, n1, R, Tmg);
+  mpt = Tmg + R * p2;
+  alpha=atan2(-mpt[2], mpt[1]);
+
+  if ( alpha != alpha)
+  {
+    return 0;
+  }
+
+  if (sin(alpha)*mpt[2]<0.0)
+    alpha=-alpha;
+
+  return (-alpha);
+}
+
+PPF3DDetector::PPF3DDetector()
+{
+  sampling_step_relative = 0.05;
+  distance_step_relative = 0.05;
+  scene_sample_step = (int)(1/0.04);
+  angle_step_relative = 30;
+  angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
+  angle_step = angle_step_radians;
+  trained = false;
+
+  hash_table = NULL;
+  hash_nodes = NULL;
+
+  setSearchParams();
+}
+
+PPF3DDetector::PPF3DDetector(const double RelativeSamplingStep, const double RelativeDistanceStep, const double NumAngles)
+{
+  sampling_step_relative = RelativeSamplingStep;
+  distance_step_relative = RelativeDistanceStep;
+  angle_step_relative = NumAngles;
+  angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
+  //SceneSampleStep = 1.0/RelativeSceneSampleStep;
+  angle_step = angle_step_radians;
+  trained = false;
+
+  hash_table = NULL;
+  hash_nodes = NULL;
+
+  setSearchParams();
+}
+
+void PPF3DDetector::setSearchParams(const double positionThreshold, const double rotationThreshold, const bool useWeightedClustering)
+{
+  if (positionThreshold<0)
+    position_threshold = sampling_step_relative;
+  else
+    position_threshold = positionThreshold;
+
+  if (rotationThreshold<0)
+    rotation_threshold = ((360/angle_step) / 180.0 * M_PI);
+  else
+    rotation_threshold = rotationThreshold;
+
+  use_weighted_avg = useWeightedClustering;
+}
+
+// compute per point PPF as in paper
+void PPF3DDetector::computePPFFeatures(const Vec3d& p1, const Vec3d& n1,
+                                       const Vec3d& p2, const Vec3d& n2,
+                                       Vec4d& f)
+{
+  Vec3d d(p2 - p1);
+  f[3] = cv::norm(d);
+  if (f[3] <= EPS)
+    return;
+  d *= 1.0 / f[3];
+
+  f[0] = TAngle3Normalized(n1, d);
+  f[1] = TAngle3Normalized(n2, d);
+  f[2] = TAngle3Normalized(n1, n2);
+}
+
+void PPF3DDetector::clearTrainingModels()
+{
+  if (this->hash_nodes)
+  {
+    free(this->hash_nodes);
+    this->hash_nodes=0;
+  }
+
+  if (this->hash_table)
+  {
+    hashtableDestroy(this->hash_table);
+    this->hash_table=0;
+  }
+}
+
+PPF3DDetector::~PPF3DDetector()
+{
+  clearTrainingModels();
+}
+
+// TODO: Check all step sizes to be positive
+void PPF3DDetector::trainModel(const Mat &PC)
+{
+  CV_Assert(PC.type() == CV_32F || PC.type() == CV_32FC1);
+
+  // compute bbox
+  Vec2f xRange, yRange, zRange;
+  computeBboxStd(PC, xRange, yRange, zRange);
+
+  // compute sampling step from diameter of bbox
+  float dx = xRange[1] - xRange[0];
+  float dy = yRange[1] - yRange[0];
+  float dz = zRange[1] - zRange[0];
+  float diameter = sqrt ( dx * dx + dy * dy + dz * dz );
+
+  float distanceStep = (float)(diameter * sampling_step_relative);
+
+  Mat sampled = samplePCByQuantization(PC, xRange, yRange, zRange, (float)sampling_step_relative,0);
+
+  int size = sampled.rows*sampled.rows;
+
+  hashtable_int* hashTable = hashtableCreate(size, NULL);
+
+  int numPPF = sampled.rows*sampled.rows;
+  ppf = Mat(numPPF, PPF_LENGTH, CV_32FC1);
+
+  // TODO: Maybe I could sample 1/5th of them here. Check the performance later.
+  int numRefPoints = sampled.rows;
+
+  // pre-allocate the hash nodes
+  hash_nodes = (THash*)calloc(numRefPoints*numRefPoints, sizeof(THash));
+
+  // TODO : This can easily be parallelized. But we have to lock hashtable_insert.
+  // I realized that performance drops when this loop is parallelized (unordered
+  // inserts into the hashtable
+  // But it is still there to be investigated. For now, I leave this unparallelized
+  // since this is just a training part.
+  for (int i=0; i<numRefPoints; i++)
+  {
+    const Vec3f p1(sampled.ptr<float>(i));
+    const Vec3f n1(sampled.ptr<float>(i) + 3);
+
+    //printf("///////////////////// NEW REFERENCE ////////////////////////\n");
+    for (int j=0; j<numRefPoints; j++)
+    {
+      // cannot compute the ppf with myself
+      if (i!=j)
+      {
+        const Vec3f p2(sampled.ptr<float>(j));
+        const Vec3f n2(sampled.ptr<float>(j) + 3);
+
+        Vec4d f = Vec4d::all(0);
+        computePPFFeatures(p1, n1, p2, n2, f);
+        KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
+        double alpha = computeAlpha(p1, n1, p2);
+        uint ppfInd = i*numRefPoints+j;
+
+        THash* hashNode = &hash_nodes[i*numRefPoints+j];
+        hashNode->id = hashValue;
+        hashNode->i = i;
+        hashNode->ppfInd = ppfInd;
+
+        hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
+
+        Mat(f).reshape(1, 1).convertTo(ppf.row(ppfInd).colRange(0, 4), CV_32F);
+        ppf.ptr<float>(ppfInd)[4] = (float)alpha;
+      }
+    }
+  }
+
+  angle_step = angle_step_radians;
+  distance_step = distanceStep;
+  hash_table = hashTable;
+  num_ref_points = numRefPoints;
+  sampled_pc = sampled;
+  trained = true;
+}
+
+
+
+///////////////////////// MATCHING ////////////////////////////////////////
+
+
+bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose)
+{
+  // translational difference
+  Vec3d dv = targetPose.t - sourcePose.t;
+  double dNorm = cv::norm(dv);
+
+  const double phi = fabs ( sourcePose.angle - targetPose.angle );
+
+  return (phi<this->rotation_threshold && dNorm < this->position_threshold);
+}
+
+void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses, std::vector<Pose3DPtr> &finalPoses)
+{
+  std::vector<PoseCluster3DPtr> poseClusters;
+
+  finalPoses.clear();
+
+  // sort the poses for stability
+  std::sort(poseList.begin(), poseList.end(), pose3DPtrCompare);
+
+  for (int i=0; i<numPoses; i++)
+  {
+    Pose3DPtr pose = poseList[i];
+    bool assigned = false;
+
+    // search all clusters
+    for (size_t j=0; j<poseClusters.size() && !assigned; j++)
+    {
+      const Pose3DPtr poseCenter = poseClusters[j]->poseList[0];
+      if (matchPose(*pose, *poseCenter))
+      {
+        poseClusters[j]->addPose(pose);
+        assigned = true;
+      }
+    }
+
+    if (!assigned)
+    {
+      poseClusters.push_back(PoseCluster3DPtr(new PoseCluster3D(pose)));
+    }
+  }
+
+  // sort the clusters so that we could output multiple hypothesis
+  std::sort(poseClusters.begin(), poseClusters.end(), sortPoseClusters);
+
+  finalPoses.resize(poseClusters.size());
+
+  // TODO: Use MinMatchScore
+
+  if (use_weighted_avg)
+  {
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
+    // uses weighting by the number of votes
+    for (int i=0; i<static_cast<int>(poseClusters.size()); i++)
+    {
+      // We could only average the quaternions. So I will make use of them here
+      Vec4d qAvg = Vec4d::all(0);
+      Vec3d tAvg = Vec3d::all(0);
+
+      // Perform the final averaging
+      PoseCluster3DPtr curCluster = poseClusters[i];
+      std::vector<Pose3DPtr> curPoses = curCluster->poseList;
+      int curSize = (int)curPoses.size();
+      size_t numTotalVotes = 0;
+
+      for (int j=0; j<curSize; j++)
+        numTotalVotes += curPoses[j]->numVotes;
+
+      double wSum=0;
+
+      for (int j=0; j<curSize; j++)
+      {
+        const double w = (double)curPoses[j]->numVotes / (double)numTotalVotes;
+
+        qAvg += w * curPoses[j]->q;
+        tAvg += w * curPoses[j]->t;
+        wSum += w;
+      }
+
+      tAvg *= 1.0 / wSum;
+      qAvg *= 1.0 / wSum;
+
+      // normalize quantonion
+      const double qNorm = cv::norm(qAvg);
+      if (qNorm > EPS)
+      {
+        qAvg *= 1.0 / qNorm;
+      }
+
+      curPoses[0]->updatePoseQuat(qAvg, tAvg);
+      curPoses[0]->numVotes=curCluster->numVotes;
+
+      finalPoses[i]=curPoses[0]->clone();
+    }
+  }
+  else
+  {
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
+    for (int i=0; i<static_cast<int>(poseClusters.size()); i++)
+    {
+      // We could only average the quaternions. So I will make use of them here
+      Vec4d qAvg = Vec4d::all(0);
+      Vec3d tAvg = Vec3d::all(0);
+
+      // Perform the final averaging
+      PoseCluster3DPtr curCluster = poseClusters[i];
+      std::vector<Pose3DPtr> curPoses = curCluster->poseList;
+      const int curSize = (int)curPoses.size();
+
+      for (int j=0; j<curSize; j++)
+      {
+        qAvg += curPoses[j]->q;
+        tAvg += curPoses[j]->t;
+      }
+
+      tAvg *= 1.0 / curSize;
+      qAvg *= 1.0 / curSize;
+
+      // normalize quantonion
+      const double qNorm = cv::norm(qAvg);
+      if (qNorm > EPS)
+      {
+        qAvg *= 1.0 / qNorm;
+      }
+
+      curPoses[0]->updatePoseQuat(qAvg, tAvg);
+      curPoses[0]->numVotes=curCluster->numVotes;
+
+      finalPoses[i]=curPoses[0]->clone();
+    }
+  }
+
+  poseClusters.clear();
+}
+
+void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const double relativeSceneSampleStep, const double relativeSceneDistance)
+{
+  if (!trained)
+  {
+    throw cv::Exception(cv::Error::StsError, "The model is not trained. Cannot match without training", __FUNCTION__, __FILE__, __LINE__);
+  }
+
+  CV_Assert(pc.type() == CV_32F || pc.type() == CV_32FC1);
+  CV_Assert(relativeSceneSampleStep<=1 && relativeSceneSampleStep>0);
+
+  scene_sample_step = (int)(1.0/relativeSceneSampleStep);
+
+  //int numNeighbors = 10;
+  int numAngles = (int) (floor (2 * M_PI / angle_step));
+  float distanceStep = (float)distance_step;
+  uint n = num_ref_points;
+  std::vector<Pose3DPtr> poseList;
+  int sceneSamplingStep = scene_sample_step;
+
+  // compute bbox
+  Vec2f xRange, yRange, zRange;
+  computeBboxStd(pc, xRange, yRange, zRange);
+
+  // sample the point cloud
+  /*float dx = xRange[1] - xRange[0];
+  float dy = yRange[1] - yRange[0];
+  float dz = zRange[1] - zRange[0];
+  float diameter = sqrt ( dx * dx + dy * dy + dz * dz );
+  float distanceSampleStep = diameter * RelativeSceneDistance;*/
+  Mat sampled = samplePCByQuantization(pc, xRange, yRange, zRange, (float)relativeSceneDistance, 0);
+
+  // allocate the accumulator : Moved this to the inside of the loop
+  /*#if !defined (_OPENMP)
+     uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint));
+  #endif*/
+
+  poseList.reserve((sampled.rows/sceneSamplingStep)+4);
+
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
+  for (int i = 0; i < sampled.rows; i += sceneSamplingStep)
+  {
+    uint refIndMax = 0, alphaIndMax = 0;
+    uint maxVotes = 0;
+
+    const Vec3f p1(sampled.ptr<float>(i));
+    const Vec3f n1(sampled.ptr<float>(i) + 3);
+    Vec3d tsg = Vec3d::all(0);
+    Matx33d Rsg = Matx33d::all(0), RInv = Matx33d::all(0);
+
+    uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint));
+    computeTransformRT(p1, n1, Rsg, tsg);
+
+    // Tolga Birdal's notice:
+    // As a later update, we might want to look into a local neighborhood only
+    // To do this, simply search the local neighborhood by radius look up
+    // and collect the neighbors to compute the relative pose
+
+    for (int j = 0; j < sampled.rows; j ++)
+    {
+      if (i!=j)
+      {
+        const Vec3f p2(sampled.ptr<float>(j));
+        const Vec3f n2(sampled.ptr<float>(j) + 3);
+        Vec3d p2t;
+        double alpha_scene;
+
+        Vec4d f = Vec4d::all(0);
+        computePPFFeatures(p1, n1, p2, n2, f);
+        KeyType hashValue = hashPPF(f, angle_step, distanceStep);
+
+        p2t = tsg + Rsg * Vec3d(p2);
+
+        alpha_scene=atan2(-p2t[2], p2t[1]);
+
+        if ( alpha_scene != alpha_scene)
+        {
+          continue;
+        }
+
+        if (sin(alpha_scene)*p2t[2]<0.0)
+          alpha_scene=-alpha_scene;
+
+        alpha_scene=-alpha_scene;
+
+        hashnode_i* node = hashtableGetBucketHashed(hash_table, (hashValue));
+
+        while (node)
+        {
+          THash* tData = (THash*) node->data;
+          int corrI = (int)tData->i;
+          int ppfInd = (int)tData->ppfInd;
+          float* ppfCorrScene = ppf.ptr<float>(ppfInd);
+          double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
+          double alpha = alpha_model - alpha_scene;
+
+          /*  Tolga Birdal's note: Map alpha to the indices:
+                  atan2 generates results in (-pi pi]
+                  That's why alpha should be in range [-2pi 2pi]
+                  So the quantization would be :
+                  numAngles * (alpha+2pi)/(4pi)
+                  */
+
+          //printf("%f\n", alpha);
+          int alpha_index = (int)(numAngles*(alpha + 2*M_PI) / (4*M_PI));
+
+          uint accIndex = corrI * numAngles + alpha_index;
+
+          accumulator[accIndex]++;
+          node = node->next;
+        }
+      }
+    }
+
+    // Maximize the accumulator
+    for (uint k = 0; k < n; k++)
+    {
+      for (int j = 0; j < numAngles; j++)
+      {
+        const uint accInd = k*numAngles + j;
+        const uint accVal = accumulator[ accInd ];
+        if (accVal > maxVotes)
+        {
+          maxVotes = accVal;
+          refIndMax = k;
+          alphaIndMax = j;
+        }
+
+#if !defined (_OPENMP)
+        accumulator[accInd ] = 0;
+#endif
+      }
+    }
+
+    // invert Tsg : Luckily rotation is orthogonal: Inverse = Transpose.
+    // We are not required to invert.
+    Vec3d tInv, tmg;
+    Matx33d Rmg;
+    RInv = Rsg.t();
+    tInv = -RInv * tsg;
+
+    Matx44d TsgInv;
+    rtToPose(RInv, tInv, TsgInv);
+
+    // TODO : Compute pose
+    const Vec3f pMax(sampled_pc.ptr<float>(refIndMax));
+    const Vec3f nMax(sampled_pc.ptr<float>(refIndMax) + 3);
+
+    computeTransformRT(pMax, nMax, Rmg, tmg);
+
+    Matx44d Tmg;
+    rtToPose(Rmg, tmg, Tmg);
+
+    // convert alpha_index to alpha
+    int alpha_index = alphaIndMax;
+    double alpha = (alpha_index*(4*M_PI))/numAngles-2*M_PI;
+
+    // Equation 2:
+    Matx44d Talpha;
+    Matx33d R;
+    Vec3d t = Vec3d::all(0);
+    getUnitXRotation(alpha, R);
+    rtToPose(R, t, Talpha);
+
+    Matx44d rawPose = TsgInv * (Talpha * Tmg);
+
+    Pose3DPtr pose(new Pose3D(alpha, refIndMax, maxVotes));
+    pose->updatePose(rawPose);
+    #if defined (_OPENMP)
+    #pragma omp critical
+    #endif
+    {
+      poseList.push_back(pose);
+    }
+
+    free(accumulator);
+  }
+
+  // TODO : Make the parameters relative if not arguments.
+  //double MinMatchScore = 0.5;
+
+  int numPosesAdded = sampled.rows/sceneSamplingStep;
+
+  clusterPoses(poseList, numPosesAdded, results);
+}
+
+} // namespace ppf_match_3d
+
+} // namespace cv

modules/surface_matching/src/precomp.hpp

@@ -0,0 +1,68 @@
+/*
+
+By downloading, copying, installing or using the software you agree to this
+license. If you do not agree to this license, do not download, install,
+copy or use the software.
+
+
+                          License Agreement
+               For Open Source Computer Vision Library
+                       (3-clause BSD License)
+
+Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+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 names of the copyright holders nor the names of the 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 copyright holders 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 __OPENCV_SURFACE_MATCHING_PRECOMP_HPP__
+#define __OPENCV_SURFACE_MATCHING_PRECOMP_HPP__
+
+#include "opencv2/surface_matching/ppf_match_3d.hpp"
+#include "opencv2/surface_matching/icp.hpp"
+#include "opencv2/surface_matching/ppf_helpers.hpp"
+
+#include <string>
+#include <cstdio>
+#include <cstdlib>
+#include <math.h>
+#include <ctime>
+
+#include <fstream>
+#include <iostream>
+#include <algorithm>
+
+#if defined (_OPENMP)
+#include<omp.h>
+#endif
+
+#include <sstream>  // flann dependency, needed in precomp now
+#include "opencv2/flann.hpp"
+
+#include "c_utils.hpp"
+
+#endif /* __OPENCV_SURFACE_MATCHING_PRECOMP_HPP__ */

modules/surface_matching/src/t_hash_int.cpp

@@ -0,0 +1,372 @@
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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: Tolga Birdal <tbirdal AT gmail.com>
+
+#include "precomp.hpp"
+
+namespace cv
+{
+namespace ppf_match_3d
+{
+// This magic value is just
+#define T_HASH_MAGIC 427462442
+
+size_t hash( uint a);
+
+// default hash function
+size_t hash( uint a)
+{
+    a = (a+0x7ed55d16) + (a<<12);
+    a = (a^0xc761c23c) ^ (a>>19);
+    a = (a+0x165667b1) + (a<<5);
+    a = (a+0xd3a2646c) ^ (a<<9);
+    a = (a+0xfd7046c5) + (a<<3);
+    a = (a^0xb55a4f09) ^ (a>>16);
+    return a;
+}
+
+hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(uint))
+{
+    hashtable_int *hashtbl;
+
+    if (size < 16)
+    {
+        size = 16;
+    }
+    else
+    {
+        size = (size_t)next_power_of_two((uint)size);
+    }
+
+    hashtbl=(hashtable_int*)malloc(sizeof(hashtable_int));
+    if (!hashtbl)
+        return NULL;
+
+    hashtbl->nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*));
+    if (!hashtbl->nodes)
+    {
+        free(hashtbl);
+        return NULL;
+    }
+
+    hashtbl->size=size;
+
+    if (hashfunc)
+        hashtbl->hashfunc=hashfunc;
+    else
+        hashtbl->hashfunc=hash;
+
+    return hashtbl;
+}
+
+
+void hashtableDestroy(hashtable_int *hashtbl)
+{
+    size_t n;
+    struct hashnode_i *node, *oldnode;
+
+    for (n=0; n<hashtbl->size; ++n)
+    {
+        node=hashtbl->nodes[n];
+        while (node)
+        {
+            oldnode=node;
+            node=node->next;
+            free(oldnode);
+        }
+    }
+    free(hashtbl->nodes);
+    free(hashtbl);
+}
+
+
+int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data)
+{
+    struct hashnode_i *node;
+    size_t hash=hashtbl->hashfunc(key)%hashtbl->size;
+
+
+    /* fpruintf(stderr, "hashtbl_insert() key=%s, hash=%d, data=%s\n", key, hash, (char*)data);*/
+
+    node=hashtbl->nodes[hash];
+    while (node)
+    {
+        if (node->key!= key)
+        {
+            node->data=data;
+            return 0;
+        }
+        node=node->next;
+    }
+
+
+    node=(hashnode_i*)malloc(sizeof(struct hashnode_i));
+    if (!node)
+        return -1;
+    node->key=key;
+
+    node->data=data;
+    node->next=hashtbl->nodes[hash];
+    hashtbl->nodes[hash]=node;
+
+
+    return 0;
+}
+
+int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data)
+{
+    struct hashnode_i *node;
+    size_t hash = key % hashtbl->size;
+
+
+    /* fpruintf(stderr, "hashtbl_insert() key=%s, hash=%d, data=%s\n", key, hash, (char*)data);*/
+
+    node=hashtbl->nodes[hash];
+    while (node)
+    {
+        if (node->key!= key)
+        {
+            node->data=data;
+            return 0;
+        }
+        node=node->next;
+    }
+
+    node=(hashnode_i*)malloc(sizeof(struct hashnode_i));
+    if (!node)
+        return -1;
+
+    node->key=key;
+
+    node->data=data;
+    node->next=hashtbl->nodes[hash];
+    hashtbl->nodes[hash]=node;
+
+
+    return 0;
+}
+
+
+int hashtableRemove(hashtable_int *hashtbl, KeyType key)
+{
+    struct hashnode_i *node, *prevnode=NULL;
+    size_t hash=hashtbl->hashfunc(key)%hashtbl->size;
+
+    node=hashtbl->nodes[hash];
+    while (node)
+    {
+        if (node->key==key)
+        {
+            if (prevnode)
+                prevnode->next=node->next;
+            else
+                hashtbl->nodes[hash]=node->next;
+            free(node);
+            return 0;
+        }
+        prevnode=node;
+        node=node->next;
+    }
+
+    return -1;
+}
+
+
+void *hashtableGet(hashtable_int *hashtbl, KeyType key)
+{
+    struct hashnode_i *node;
+    size_t hash=hashtbl->hashfunc(key)%hashtbl->size;
+
+    /* fprintf(stderr, "hashtbl_get() key=%s, hash=%d\n", key, hash);*/
+
+    node=hashtbl->nodes[hash];
+    while (node)
+    {
+        if (node->key==key)
+            return node->data;
+        node=node->next;
+    }
+
+    return NULL;
+}
+
+hashnode_i* hashtableGetBucketHashed(hashtable_int *hashtbl, KeyType key)
+{
+    size_t hash = key % hashtbl->size;
+
+    return hashtbl->nodes[hash];
+}
+
+int hashtableResize(hashtable_int *hashtbl, size_t size)
+{
+    hashtable_int newtbl;
+    size_t n;
+    struct hashnode_i *node,*next;
+
+    newtbl.size=size;
+    newtbl.hashfunc=hashtbl->hashfunc;
+
+    newtbl.nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*));
+    if (!newtbl.nodes)
+        return -1;
+
+    for (n=0; n<hashtbl->size; ++n)
+    {
+        for (node=hashtbl->nodes[n]; node; node=next)
+        {
+            next = node->next;
+            hashtableInsert(&newtbl, node->key, node->data);
+            hashtableRemove(hashtbl, node->key);
+
+        }
+    }
+
+    free(hashtbl->nodes);
+    hashtbl->size=newtbl.size;
+    hashtbl->nodes=newtbl.nodes;
+
+    return 0;
+}
+
+
+int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f)
+{
+    size_t hashMagic=T_HASH_MAGIC;
+    size_t n=hashtbl->size;
+    size_t i;
+
+    fwrite(&hashMagic, sizeof(size_t),1, f);
+    fwrite(&n, sizeof(size_t),1, f);
+    fwrite(&dataSize, sizeof(size_t),1, f);
+
+    for (i=0; i<hashtbl->size; i++)
+    {
+        struct hashnode_i* node=hashtbl->nodes[i];
+        size_t noEl=0;
+
+        while (node)
+        {
+            noEl++;
+            node=node->next;
+        }
+
+        fwrite(&noEl, sizeof(size_t),1, f);
+
+        node=hashtbl->nodes[i];
+        while (node)
+        {
+            fwrite(&node->key, sizeof(KeyType), 1, f);
+            fwrite(&node->data, dataSize, 1, f);
+            node=node->next;
+        }
+    }
+
+    return 1;
+}
+
+
+void hashtablePrint(hashtable_int *hashtbl)
+{
+    size_t n;
+    struct hashnode_i *node,*next;
+
+    for (n=0; n<hashtbl->size; ++n)
+    {
+        for (node=hashtbl->nodes[n]; node; node=next)
+        {
+            next = node->next;
+            std::cout<<"Key : "<<node->key<<", Data : "<<node->data<<std::endl;
+        }
+    }
+}
+
+hashtable_int *hashtableRead(FILE* f)
+{
+    size_t hashMagic = 0;
+    size_t n = 0, status;
+    hashtable_int *hashtbl = 0;
+
+    status = fread(&hashMagic, sizeof(size_t),1, f);
+    if (status && hashMagic==T_HASH_MAGIC)
+    {
+        size_t i;
+        size_t dataSize;
+        status = fread(&n, sizeof(size_t),1, f);
+        status = fread(&dataSize, sizeof(size_t),1, f);
+
+        hashtbl=hashtableCreate(n, hash);
+
+        for (i=0; i<hashtbl->size; i++)
+        {
+            size_t j=0;
+            status = fread(&n, sizeof(size_t),1, f);
+
+            for (j=0; j<n; j++)
+            {
+                int key=0;
+                void* data=0;
+                status = fread(&key, sizeof(KeyType), 1, f);
+
+                if (dataSize>sizeof(void*))
+                {
+                    data=malloc(dataSize);
+                    if (!data)
+                    {
+                        hashtableDestroy(hashtbl);
+                        return NULL;
+                    }
+                    status = fread(data, dataSize, 1, f);
+                }
+                else
+                    status = fread(&data, dataSize, 1, f);
+
+                hashtableInsert(hashtbl, key, data);
+                //free(key);
+            }
+        }
+    }
+    else
+        return 0;
+
+    return hashtbl;
+}
+
+} // namespace ppf_match_3d
+
+} // namespace cv