Point Cloud Library (PCL)  1.14.1-dev
pair_features.hpp
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
35 */
36 
37 #ifndef PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_
38 #define PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_
39 
40 #include <pcl/gpu/utils/device/vector_math.hpp>
41 #include <pcl/gpu/features/device/rodrigues.hpp>
42 
43 namespace pcl
44 {
45  namespace device
46  {
47  __device__ __host__ __forceinline__
48  bool computePairFeatures (const float3& p1, const float3& n1, const float3& p2, const float3& n2, float &f1, float &f2, float &f3, float &f4)
49  {
50  f1 = f2 = f3 = f4 = 0.0f;
51 
52  float3 dp2p1 = p2 - p1;
53  f4 = norm(dp2p1);
54 
55  if (f4 == 0.f)
56  return false;
57 
58  float3 n1_copy = n1, n2_copy = n2;
59  float angle1 = dot(n1_copy, dp2p1) / f4;
60 
61 
62  float angle2 = dot(n2_copy, dp2p1) / f4;
63  if (std::acos (std::abs (angle1)) > std::acos (std::abs (angle2)))
64  {
65  // switch p1 and p2
66  n1_copy = n2;
67  n2_copy = n1;
68  dp2p1 *= (-1);
69  f3 = -angle2;
70  }
71  else
72  f3 = angle1;
73 
74  // Create a Darboux frame coordinate system u-v-w
75  // u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
76  float3 v = cross(dp2p1, n1_copy);
77  float v_norm = norm(v);
78  if (v_norm == 0.0f)
79  return false;
80 
81  // Normalize v
82  v *= 1.f/v_norm;
83 
84  // Do not have to normalize w - it is a unit vector by construction
85  f2 = dot(v, n2_copy);
86 
87  float3 w = cross(n1_copy, v);
88  // Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
89  f1 = std::atan2 (dot(w, n2_copy), dot(n1_copy, n2_copy)); // @todo optimize this
90 
91  return true;
92  }
93 
94  __device__ __host__ __forceinline__
95  bool computeRGBPairFeatures (const float3& p1, const float3& n1, const int& colors1, const float3& p2, const float3& n2, const int& colors2,
96  float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
97  {
98  float3 dp2p1 = p2 - p1;
99  f4 = norm(dp2p1);
100 
101  if (f4 == 0.0f)
102  {
103  f1 = f2 = f3 = f4 = f5 = f6 = f7 = 0.0f;
104  return false;
105  }
106 
107  float3 n1_copy = n1, n2_copy = n2;
108  float angle1 = dot(n1_copy, dp2p1) / f4;
109 
110  f3 = angle1;
111 
112  // Create a Darboux frame coordinate system u-v-w
113  // u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
114  float3 v = cross(dp2p1, n1_copy);
115  float v_norm = norm(v);
116  if (v_norm == 0.0f)
117  {
118  f1 = f2 = f3 = f4 = f5 = f6 = f7 = 0.0f;
119  return false;
120  }
121  // Normalize v
122  v *= 1.f/v_norm;
123 
124  float3 w = cross(n1_copy, v);
125  // Do not have to normalize w - it is a unit vector by construction
126 
127  f2 = dot(v, n2_copy);
128 
129  // Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
130  f1 = std::atan2 (dot(w, n2_copy), dot (n1_copy, n2_copy));
131 
132  // everything before was standard 4D-Darboux frame feature pair
133  // now, for the experimental color stuff
134 
135  f5 = ((float) ((colors1 ) & 0xFF)) / ((colors2 ) & 0xFF);
136  f6 = ((float) ((colors1 >> 8) & 0xFF)) / ((colors2 >> 8) & 0xFF);
137  f7 = ((float) ((colors1 >> 16) & 0xFF)) / ((colors2 >> 16) & 0xFF);
138 
139  // make sure the ratios are in the [-1, 1] interval
140  if (f5 > 1.f) f5 = - 1.f / f5;
141  if (f6 > 1.f) f6 = - 1.f / f6;
142  if (f7 > 1.f) f7 = - 1.f / f7;
143 
144  return true;
145  }
146 
147  __device__ __host__ __forceinline__
148  void computeRGBPairFeatures_RGBOnly (const int& colors1, const int& colors2, float &f5, float &f6, float &f7)
149  {
150  f5 = ((float) ((colors1 ) & 0xFF)) / ((colors2 ) & 0xFF);
151  f6 = ((float) ((colors1 >> 8) & 0xFF)) / ((colors2 >> 8) & 0xFF);
152  f7 = ((float) ((colors1 >> 16) & 0xFF)) / ((colors2 >> 16) & 0xFF);
153 
154  // make sure the ratios are in the [-1, 1] interval
155  if (f5 > 1.f) f5 = - 1.f / f5;
156  if (f6 > 1.f) f6 = - 1.f / f6;
157  if (f7 > 1.f) f7 = - 1.f / f7;
158  }
159 
160  __device__ __host__ __forceinline__ bool computePPFPairFeature(const float3& p1, const float3& n1, const float3& p2, const float3& n2,
161  float& f1, float& f2, float& f3, float& f4)
162  {
163  float3 delta = p2 - p1;
164 
165  f4 = norm (delta);
166 
167  delta.x /= f4;
168  delta.y /= f4;
169  delta.z /= f4;
170 
171  f1 = dot(n1, delta);
172  f2 = dot(n2, delta);
173  f3 = dot(n1, n2);
174 
175  return true;
176  }
177 
178 
179  __device__ __host__ __forceinline__ void computeAlfaM(const float3& model_reference_point, const float3& model_reference_normal,
180  const float3& model_point, float& alpha_m)
181  {
182  float acos_value = std::acos (model_reference_normal.x);
183 
184  //float3 cross_vector = cross(model_reference_normal, Eigen::Vector3f::UnitX);
185  float3 cross_vector = make_float3(0, model_reference_normal.z, - model_reference_normal.y);
186  float3 cross_vector_norm = normalized(cross_vector);
187 
188  //Eigen::AngleAxisf rotation_mg (acos_value, cross_vector_norm);
189  //Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
190 
191  float3 row1, row2, row3; // == rotation_mg
192  AngleAxisf(acos_value, cross_vector_norm, row1, row2, row3);
193 
194  float3 translation;
195  //translation.x = row1.x * -model_reference_point.x + row1.y * -model_reference_point.y + row1.z * -model_reference_point.z;
196  translation.y = row2.x * -model_reference_point.x + row2.y * -model_reference_point.y + row2.z * -model_reference_point.z;
197  translation.z = row3.x * -model_reference_point.x + row3.y * -model_reference_point.y + row3.z * -model_reference_point.z;
198 
199  float3 model_point_transformed;// = transform_mg * model_point;
200  //model_point_transformed.x = translation.x + row1.x * model_point.x + row1.y * model_point.y + row1.z * model_point.z;
201  model_point_transformed.y = translation.y + row2.x * model_point.x + row2.y * model_point.y + row2.z * model_point.z;
202  model_point_transformed.z = translation.z + row3.x * model_point.x + row3.y * model_point.y + row3.z * model_point.z;
203 
204 
205  float angle = std::atan2 ( -model_point_transformed.z, model_point_transformed.y);
206 
207  if (sinf(angle) * model_point_transformed.z < 0.0f)
208  //if (angle * model_point_transformed.z < 0.ff)
209  angle *= -1;
210  alpha_m = -angle;
211  }
212  }
213 }
214 
215 #endif /* PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_ */
__device__ __host__ __forceinline__ bool computePPFPairFeature(const float3 &p1, const float3 &n1, const float3 &p2, const float3 &n2, float &f1, float &f2, float &f3, float &f4)
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:101
__device__ __host__ __forceinline__ bool computePairFeatures(const float3 &p1, const float3 &n1, const float3 &p2, const float3 &n2, float &f1, float &f2, float &f3, float &f4)
__device__ __host__ __forceinline__ void computeAlfaM(const float3 &model_reference_point, const float3 &model_reference_normal, const float3 &model_point, float &alpha_m)
__device__ __forceinline__ float dot(const float3 &v1, const float3 &v2)
Definition: utils.hpp:59
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
__device__ __host__ __forceinline__ bool computeRGBPairFeatures(const float3 &p1, const float3 &n1, const int &colors1, const float3 &p2, const float3 &n2, const int &colors2, float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
__device__ __host__ __forceinline__ void computeRGBPairFeatures_RGBOnly(const int &colors1, const int &colors2, float &f5, float &f6, float &f7)
__device__ __host__ __forceinline__ float3 cross(const float3 &v1, const float3 &v2)
Definition: utils.hpp:107
__device__ __host__ __forceinline__ void AngleAxisf(float angle, const float3 &r, float3 &row1, float3 &row2, float3 &row3)
Definition: rodrigues.hpp:47