diff options
Diffstat (limited to 'thirdparty1/linux/include/opencv2/surface_matching.hpp')
| -rw-r--r-- | thirdparty1/linux/include/opencv2/surface_matching.hpp | 402 |
1 files changed, 402 insertions, 0 deletions
diff --git a/thirdparty1/linux/include/opencv2/surface_matching.hpp b/thirdparty1/linux/include/opencv2/surface_matching.hpp new file mode 100644 index 0000000..6cf5f9d --- /dev/null +++ b/thirdparty1/linux/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: + + + +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): + + + +Matches of different models for Mian dataset is presented below: + + + +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 |