pair_features.hpp
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
*/
#ifndef PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_
#define PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_
#include <pcl/gpu/utils/device/vector_math.hpp>
#include <pcl/gpu/features/device/rodrigues.hpp>
namespace pcl
{
namespace device
{
__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)
{
f1 = f2 = f3 = f4 = 0.0f;
float3 dp2p1 = p2 - p1;
f4 = norm(dp2p1);
if (f4 == 0.f)
return false;
float3 n1_copy = n1, n2_copy = n2;
float angle1 = dot(n1_copy, dp2p1) / f4;
float angle2 = dot(n2_copy, dp2p1) / f4;
if (std::acos (std::abs (angle1)) > std::acos (std::abs (angle2)))
{
// switch p1 and p2
n1_copy = n2;
n2_copy = n1;
dp2p1 *= (-1);
f3 = -angle2;
}
else
f3 = angle1;
// Create a Darboux frame coordinate system u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
float3 v = cross(dp2p1, n1_copy);
float v_norm = norm(v);
if (v_norm == 0.0f)
return false;
// Normalize v
v *= 1.f/v_norm;
// Do not have to normalize w - it is a unit vector by construction
f2 = dot(v, n2_copy);
float3 w = cross(n1_copy, v);
// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
f1 = std::atan2 (dot(w, n2_copy), dot(n1_copy, n2_copy)); // @todo optimize this
return true;
}
__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)
{
float3 dp2p1 = p2 - p1;
f4 = norm(dp2p1);
if (f4 == 0.0f)
{
f1 = f2 = f3 = f4 = f5 = f6 = f7 = 0.0f;
return false;
}
float3 n1_copy = n1, n2_copy = n2;
float angle1 = dot(n1_copy, dp2p1) / f4;
f3 = angle1;
// Create a Darboux frame coordinate system u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
float3 v = cross(dp2p1, n1_copy);
float v_norm = norm(v);
if (v_norm == 0.0f)
{
f1 = f2 = f3 = f4 = f5 = f6 = f7 = 0.0f;
return false;
}
// Normalize v
v *= 1.f/v_norm;
float3 w = cross(n1_copy, v);
// Do not have to normalize w - it is a unit vector by construction
f2 = dot(v, n2_copy);
// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
f1 = std::atan2 (dot(w, n2_copy), dot (n1_copy, n2_copy));
// everything before was standard 4D-Darboux frame feature pair
// now, for the experimental color stuff
f5 = ((float) ((colors1 ) & 0xFF)) / ((colors2 ) & 0xFF);
f6 = ((float) ((colors1 >> 8) & 0xFF)) / ((colors2 >> 8) & 0xFF);
f7 = ((float) ((colors1 >> 16) & 0xFF)) / ((colors2 >> 16) & 0xFF);
// make sure the ratios are in the [-1, 1] interval
if (f5 > 1.f) f5 = - 1.f / f5;
if (f6 > 1.f) f6 = - 1.f / f6;
if (f7 > 1.f) f7 = - 1.f / f7;
return true;
}
__device__ __host__ __forceinline__
void computeRGBPairFeatures_RGBOnly (const int& colors1, const int& colors2, float &f5, float &f6, float &f7)
{
f5 = ((float) ((colors1 ) & 0xFF)) / ((colors2 ) & 0xFF);
f6 = ((float) ((colors1 >> 8) & 0xFF)) / ((colors2 >> 8) & 0xFF);
f7 = ((float) ((colors1 >> 16) & 0xFF)) / ((colors2 >> 16) & 0xFF);
// make sure the ratios are in the [-1, 1] interval
if (f5 > 1.f) f5 = - 1.f / f5;
if (f6 > 1.f) f6 = - 1.f / f6;
if (f7 > 1.f) f7 = - 1.f / f7;
}
__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)
{
float3 delta = p2 - p1;
f4 = norm (delta);
delta.x /= f4;
delta.y /= f4;
delta.z /= f4;
f1 = dot(n1, delta);
f2 = dot(n2, delta);
f3 = dot(n1, n2);
return true;
}
__device__ __host__ __forceinline__ void computeAlfaM(const float3& model_reference_point, const float3& model_reference_normal,
const float3& model_point, float& alpha_m)
{
float acos_value = std::acos (model_reference_normal.x);
//float3 cross_vector = cross(model_reference_normal, Eigen::Vector3f::UnitX);
float3 cross_vector = make_float3(0, model_reference_normal.z, - model_reference_normal.y);
float3 cross_vector_norm = normalized(cross_vector);
//Eigen::AngleAxisf rotation_mg (acos_value, cross_vector_norm);
//Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
float3 row1, row2, row3; // == rotation_mg
AngleAxisf(acos_value, cross_vector_norm, row1, row2, row3);
float3 translation;
//translation.x = row1.x * -model_reference_point.x + row1.y * -model_reference_point.y + row1.z * -model_reference_point.z;
translation.y = row2.x * -model_reference_point.x + row2.y * -model_reference_point.y + row2.z * -model_reference_point.z;
translation.z = row3.x * -model_reference_point.x + row3.y * -model_reference_point.y + row3.z * -model_reference_point.z;
float3 model_point_transformed;// = transform_mg * model_point;
//model_point_transformed.x = translation.x + row1.x * model_point.x + row1.y * model_point.y + row1.z * model_point.z;
model_point_transformed.y = translation.y + row2.x * model_point.x + row2.y * model_point.y + row2.z * model_point.z;
model_point_transformed.z = translation.z + row3.x * model_point.x + row3.y * model_point.y + row3.z * model_point.z;
float angle = std::atan2 ( -model_point_transformed.z, model_point_transformed.y);
if (sinf(angle) * model_point_transformed.z < 0.0f)
//if (angle * model_point_transformed.z < 0.ff)
angle *= -1;
alpha_m = -angle;
}
}
}
#endif /* PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_ */
pcl::device::computePPFPairFeature
__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)
Definition: pair_features.hpp:160
pcl::device::normalized
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:101
pcl::device::computePairFeatures
__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)
Definition: pair_features.hpp:48
pcl::device::computeAlfaM
__device__ __host__ __forceinline__ void computeAlfaM(const float3 &model_reference_point, const float3 &model_reference_normal, const float3 &model_point, float &alpha_m)
Definition: pair_features.hpp:179
pcl::device::dot
__device__ __forceinline__ float dot(const float3 &v1, const float3 &v2)
Definition: utils.hpp:59
pcl::device::norm
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
Definition: vector_operations.hpp:60
pcl::device::computeRGBPairFeatures
__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)
Definition: pair_features.hpp:95
pcl::device::computeRGBPairFeatures_RGBOnly
__device__ __host__ __forceinline__ void computeRGBPairFeatures_RGBOnly(const int &colors1, const int &colors2, float &f5, float &f6, float &f7)
Definition: pair_features.hpp:148
pcl::device::cross
__device__ __host__ __forceinline__ float3 cross(const float3 &v1, const float3 &v2)
Definition: utils.hpp:107
pcl::device::AngleAxisf
__device__ __host__ __forceinline__ void AngleAxisf(float angle, const float3 &r, float3 &row1, float3 &row2, float3 &row3)
Definition: rodrigues.hpp:47
pcl
Definition: convolution.h:46