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: Koen Buys
35 */
36
37 #include <pcl/gpu/people/label_common.h>
38 #include <pcl/gpu/people/probability_processor.h>
39 #include <pcl/console/print.h>
40 #include "internal.h"
41
ProbabilityProcessor()42 pcl::gpu::people::ProbabilityProcessor::ProbabilityProcessor()
43 {
44 PCL_DEBUG("[pcl::gpu::people::ProbabilityProcessor] : (D) : Constructor called\n");
45 impl_.reset (new device::ProbabilityProc());
46 }
47
48 /** \brief This will merge the votes from the different trees into one final vote, including probabilistic's **/
49 void
SelectLabel(const Depth & depth,Labels & labels,pcl::device::LabelProbability & probabilities)50 pcl::gpu::people::ProbabilityProcessor::SelectLabel (const Depth& depth, Labels& labels, pcl::device::LabelProbability& probabilities)
51 {
52 PCL_DEBUG("[pcl::gpu::people::ProbabilityProcessor::SelectLabel] : (D) : Called\n");
53 impl_->CUDA_SelectLabel(depth, labels, probabilities);
54 }
55
56 /** \brief This will combine two probabilities according their weight **/
57 void
CombineProb(const Depth & depth,pcl::device::LabelProbability & probIn1,float weight1,pcl::device::LabelProbability & probIn2,float weight2,pcl::device::LabelProbability & probOut)58 pcl::gpu::people::ProbabilityProcessor::CombineProb ( const Depth& depth, pcl::device::LabelProbability& probIn1, float weight1,
59 pcl::device::LabelProbability& probIn2, float weight2, pcl::device::LabelProbability& probOut)
60 {
61 impl_->CUDA_CombineProb(depth, probIn1, weight1, probIn2, weight2, probOut);
62 }
63
64 /** \brief This will sum a probability multiplied with it's weight **/
65 void
WeightedSumProb(const Depth & depth,pcl::device::LabelProbability & probIn,float weight,pcl::device::LabelProbability & probOut)66 pcl::gpu::people::ProbabilityProcessor::WeightedSumProb ( const Depth& depth, pcl::device::LabelProbability& probIn, float weight, pcl::device::LabelProbability& probOut)
67 {
68 impl_->CUDA_WeightedSumProb(depth, probIn, weight, probOut);
69 }
70
71 /** \brief This will do a GaussianBlur over the LabelProbability **/
72 int
GaussianBlur(const Depth & depth,pcl::device::LabelProbability & probIn,DeviceArray<float> & kernel,pcl::device::LabelProbability & probOut)73 pcl::gpu::people::ProbabilityProcessor::GaussianBlur( const Depth& depth,
74 pcl::device::LabelProbability& probIn,
75 DeviceArray<float>& kernel,
76 pcl::device::LabelProbability& probOut)
77 {
78 return impl_->CUDA_GaussianBlur( depth, probIn, kernel, probOut);
79 }
80
81 /** \brief This will do a GaussianBlur over the LabelProbability **/
82 int
GaussianBlur(const Depth & depth,pcl::device::LabelProbability & probIn,DeviceArray<float> & kernel,pcl::device::LabelProbability & probTemp,pcl::device::LabelProbability & probOut)83 pcl::gpu::people::ProbabilityProcessor::GaussianBlur( const Depth& depth,
84 pcl::device::LabelProbability& probIn,
85 DeviceArray<float>& kernel,
86 pcl::device::LabelProbability& probTemp,
87 pcl::device::LabelProbability& probOut)
88 {
89 return impl_->CUDA_GaussianBlur( depth, probIn, kernel, probTemp, probOut);
90 }
91
92 /** \brief This will create a Gaussian Kernel **/
93 float*
CreateGaussianKernel(float sigma,int kernelSize)94 pcl::gpu::people::ProbabilityProcessor::CreateGaussianKernel ( float sigma,
95 int kernelSize)
96 {
97 float* f;
98 f = static_cast<float*> (malloc(kernelSize * sizeof(float)));
99 float sigma_sq = static_cast<float> (std::pow (sigma,2.f));
100 float mult = static_cast<float> (1/sqrt (2*M_PI*sigma_sq));
101 int mid = static_cast<int> (std::floor (static_cast<float> (kernelSize)/2.f));
102
103 // Create a symmetric kernel, could also be solved in CUDA kernel but let's do it here :D
104 float sum = 0;
105 for(int i = 0; i < kernelSize; i++)
106 {
107 f[i] = static_cast<float> (mult * std::exp (-(pow (i-mid,2.f)/2*sigma_sq)));
108 sum += f[i];
109 }
110
111 // Normalize f
112 for(int i = 0; i < kernelSize; i++)
113 {
114 f[i] /=sum;
115 }
116
117 return f;
118 }
119