Point Cloud Library (PCL) 1.12.1
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
43namespace 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