308 lines
10 KiB
C++
308 lines
10 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2013-, Open Perception, 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 the copyright holder(s) 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.
|
|
*
|
|
* person_classifier.hpp
|
|
* Created on: Nov 30, 2012
|
|
* Author: Matteo Munaro
|
|
*/
|
|
|
|
#include <pcl/people/person_classifier.h>
|
|
|
|
#ifndef PCL_PEOPLE_PERSON_CLASSIFIER_HPP_
|
|
#define PCL_PEOPLE_PERSON_CLASSIFIER_HPP_
|
|
|
|
template <typename PointT>
|
|
pcl::people::PersonClassifier<PointT>::PersonClassifier () = default;
|
|
|
|
template <typename PointT>
|
|
pcl::people::PersonClassifier<PointT>::~PersonClassifier () = default;
|
|
|
|
template <typename PointT> bool
|
|
pcl::people::PersonClassifier<PointT>::loadSVMFromFile (const std::string& svm_filename)
|
|
{
|
|
std::string line;
|
|
std::ifstream SVM_file;
|
|
SVM_file.open(svm_filename.c_str());
|
|
|
|
getline (SVM_file,line); // read window_height line
|
|
std::size_t tok_pos = line.find_first_of(':', 0); // search for token ":"
|
|
window_height_ = std::atoi(line.substr(tok_pos+1, std::string::npos - tok_pos-1).c_str());
|
|
|
|
getline (SVM_file,line); // read window_width line
|
|
tok_pos = line.find_first_of(':', 0); // search for token ":"
|
|
window_width_ = std::atoi(line.substr(tok_pos+1, std::string::npos - tok_pos-1).c_str());
|
|
|
|
getline (SVM_file,line); // read SVM_offset line
|
|
tok_pos = line.find_first_of(':', 0); // search for token ":"
|
|
SVM_offset_ = std::atof(line.substr(tok_pos+1, std::string::npos - tok_pos-1).c_str());
|
|
|
|
getline (SVM_file,line); // read SVM_weights line
|
|
tok_pos = line.find_first_of('[', 0); // search for token "["
|
|
std::size_t tok_end_pos = line.find_first_of(']', 0); // search for token "]" , end of SVM weights
|
|
while (tok_pos < tok_end_pos) // while end of SVM_weights is not reached
|
|
{
|
|
std::size_t prev_tok_pos = tok_pos;
|
|
tok_pos = line.find_first_of(',', prev_tok_pos+1); // search for token ","
|
|
SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str()));
|
|
}
|
|
SVM_file.close();
|
|
|
|
if (SVM_weights_.empty ())
|
|
{
|
|
PCL_ERROR ("[pcl::people::PersonClassifier::loadSVMFromFile] Invalid SVM file!\n");
|
|
return (false);
|
|
}
|
|
return (true);
|
|
}
|
|
|
|
template <typename PointT> void
|
|
pcl::people::PersonClassifier<PointT>::setSVM (int window_height, int window_width, std::vector<float> SVM_weights, float SVM_offset)
|
|
{
|
|
window_height_ = window_height;
|
|
window_width_ = window_width;
|
|
SVM_weights_ = SVM_weights;
|
|
SVM_offset_ = SVM_offset;
|
|
}
|
|
|
|
template <typename PointT> void
|
|
pcl::people::PersonClassifier<PointT>::getSVM (int& window_height, int& window_width, std::vector<float>& SVM_weights, float& SVM_offset)
|
|
{
|
|
window_height = window_height_;
|
|
window_width = window_width_;
|
|
SVM_weights = SVM_weights_;
|
|
SVM_offset = SVM_offset_;
|
|
}
|
|
|
|
template <typename PointT> void
|
|
pcl::people::PersonClassifier<PointT>::resize (PointCloudPtr& input_image,
|
|
PointCloudPtr& output_image,
|
|
int width,
|
|
int height)
|
|
{
|
|
PointT new_point;
|
|
new_point.r = 0;
|
|
new_point.g = 0;
|
|
new_point.b = 0;
|
|
|
|
// Allocate the vector of points:
|
|
output_image->points.resize(width*height, new_point);
|
|
output_image->height = height;
|
|
output_image->width = width;
|
|
|
|
// Compute scale factor:
|
|
float scale1 = static_cast<float>(height) / static_cast<float>(input_image->height);
|
|
float scale2 = static_cast<float>(width) / static_cast<float>(input_image->width);
|
|
|
|
Eigen::Matrix3f T_inv;
|
|
T_inv << 1/scale1, 0, 0,
|
|
0, 1/scale2, 0,
|
|
0, 0, 1;
|
|
|
|
Eigen::Vector3f A;
|
|
int c1, c2, f1, f2;
|
|
PointT g1, g2, g3, g4;
|
|
float w1, w2;
|
|
for (int i = 0; i < height; i++) // for every row
|
|
{
|
|
for (int j = 0; j < width; j++) // for every column
|
|
{
|
|
A = T_inv * Eigen::Vector3f(i, j, 1);
|
|
c1 = std::ceil(A(0));
|
|
f1 = std::floor(A(0));
|
|
c2 = std::ceil(A(1));
|
|
f2 = std::floor(A(1));
|
|
|
|
if ( (f1 < 0) ||
|
|
(c1 < 0) ||
|
|
(f1 >= static_cast<int> (input_image->height)) ||
|
|
(c1 >= static_cast<int> (input_image->height)) ||
|
|
(f2 < 0) ||
|
|
(c2 < 0) ||
|
|
(f2 >= static_cast<int> (input_image->width)) ||
|
|
(c2 >= static_cast<int> (input_image->width)))
|
|
{ // if out of range, continue
|
|
continue;
|
|
}
|
|
|
|
g1 = (*input_image)(f2, c1);
|
|
g3 = (*input_image)(f2, f1);
|
|
g4 = (*input_image)(c2, f1);
|
|
g2 = (*input_image)(c2, c1);
|
|
|
|
w1 = (A(0) - f1);
|
|
w2 = (A(1) - f2);
|
|
new_point.r = static_cast<int>((1 - w1) * ((1 - w2) * g1.r + w2 * g4.r) + w1 * ((1 - w2) * g3.r + w2 * g4.r));
|
|
new_point.g = static_cast<int>((1 - w1) * ((1 - w2) * g1.g + w2 * g4.g) + w1 * ((1 - w2) * g3.g + w2 * g4.g));
|
|
new_point.b = static_cast<int>((1 - w1) * ((1 - w2) * g1.b + w2 * g4.b) + w1 * ((1 - w2) * g3.b + w2 * g4.b));
|
|
|
|
// Insert the point in the output image:
|
|
(*output_image)(j,i) = new_point;
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename PointT> void
|
|
pcl::people::PersonClassifier<PointT>::copyMakeBorder (PointCloudPtr& input_image,
|
|
PointCloudPtr& output_image,
|
|
int xmin,
|
|
int ymin,
|
|
int width,
|
|
int height)
|
|
{
|
|
PointT black_point;
|
|
black_point.r = 0;
|
|
black_point.g = 0;
|
|
black_point.b = 0;
|
|
output_image->points.resize(height*width, black_point);
|
|
output_image->width = width;
|
|
output_image->height = height;
|
|
|
|
int x_start_in = std::max(0, xmin);
|
|
int x_end_in = std::min(static_cast<int>(input_image->width-1), xmin+width-1);
|
|
int y_start_in = std::max(0, ymin);
|
|
int y_end_in = std::min(static_cast<int>(input_image->height-1), ymin+height-1);
|
|
|
|
int x_start_out = std::max(0, -xmin);
|
|
//int x_end_out = x_start_out + (x_end_in - x_start_in);
|
|
int y_start_out = std::max(0, -ymin);
|
|
//int y_end_out = y_start_out + (y_end_in - y_start_in);
|
|
|
|
for (int i = 0; i < (y_end_in - y_start_in + 1); i++)
|
|
{
|
|
for (int j = 0; j < (x_end_in - x_start_in + 1); j++)
|
|
{
|
|
(*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i);
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename PointT> double
|
|
pcl::people::PersonClassifier<PointT>::evaluate (float height_person,
|
|
float xc,
|
|
float yc,
|
|
PointCloudPtr& image)
|
|
{
|
|
if (SVM_weights_.empty ())
|
|
{
|
|
PCL_ERROR ("[pcl::people::PersonClassifier::evaluate] SVM has not been set!\n");
|
|
return (-1000);
|
|
}
|
|
|
|
int height = std::floor((height_person * window_height_) / (0.75 * window_height_) + 0.5); // std::floor(i+0.5) = round(i)
|
|
int width = std::floor((height_person * window_width_) / (0.75 * window_height_) + 0.5);
|
|
int xmin = std::floor(xc - width / 2 + 0.5);
|
|
int ymin = std::floor(yc - height / 2 + 0.5);
|
|
double confidence;
|
|
|
|
if (height > 0)
|
|
{
|
|
// If near the border, fill with black:
|
|
PointCloudPtr box(new PointCloud);
|
|
copyMakeBorder(image, box, xmin, ymin, width, height);
|
|
|
|
// Make the image match the correct size (used in the training stage):
|
|
PointCloudPtr sample(new PointCloud);
|
|
resize(box, sample, window_width_, window_height_);
|
|
|
|
// Convert the image to array of float:
|
|
float* sample_float = new float[sample->width * sample->height * 3];
|
|
int delta = sample->height * sample->width;
|
|
for (std::uint32_t row = 0; row < sample->height; row++)
|
|
{
|
|
for (std::uint32_t col = 0; col < sample->width; col++)
|
|
{
|
|
sample_float[row + sample->height * col] = (static_cast<float> ((*sample)(col, row).r))/255; //ptr[col * 3 + 2];
|
|
sample_float[row + sample->height * col + delta] = (static_cast<float> ((*sample)(col, row).g))/255; //ptr[col * 3 + 1];
|
|
sample_float[row + sample->height * col + delta * 2] = static_cast<float> (((*sample)(col, row).b))/255; //ptr[col * 3];
|
|
}
|
|
}
|
|
|
|
// Calculate HOG descriptor:
|
|
pcl::people::HOG hog;
|
|
float *descriptor = new float[SVM_weights_.size()];
|
|
std::fill_n(descriptor, SVM_weights_.size(), 0.0f);
|
|
hog.compute(sample_float, descriptor);
|
|
|
|
// Calculate confidence value by dot product:
|
|
confidence = 0.0;
|
|
for(std::size_t i = 0; i < SVM_weights_.size(); i++)
|
|
{
|
|
confidence += SVM_weights_[i] * descriptor[i];
|
|
}
|
|
// Confidence correction:
|
|
confidence -= SVM_offset_;
|
|
|
|
delete[] descriptor;
|
|
delete[] sample_float;
|
|
}
|
|
else
|
|
{
|
|
confidence = std::numeric_limits<double>::quiet_NaN();
|
|
}
|
|
|
|
return confidence;
|
|
}
|
|
|
|
template <typename PointT> double
|
|
pcl::people::PersonClassifier<PointT>::evaluate (PointCloudPtr& image,
|
|
Eigen::Vector3f& bottom,
|
|
Eigen::Vector3f& top,
|
|
Eigen::Vector3f& centroid,
|
|
bool vertical)
|
|
{
|
|
float pixel_height;
|
|
float pixel_width;
|
|
|
|
if (!vertical)
|
|
{
|
|
pixel_height = bottom(1) - top(1);
|
|
pixel_width = pixel_height / 2.0f;
|
|
}
|
|
else
|
|
{
|
|
pixel_width = top(0) - bottom(0);
|
|
pixel_height = pixel_width / 2.0f;
|
|
}
|
|
float pixel_xc = centroid(0);
|
|
float pixel_yc = centroid(1);
|
|
|
|
if (!vertical)
|
|
{
|
|
return (evaluate(pixel_height, pixel_xc, pixel_yc, image));
|
|
}
|
|
return (evaluate(pixel_width, pixel_yc, image->height-pixel_xc+1, image));
|
|
}
|
|
#endif /* PCL_PEOPLE_PERSON_CLASSIFIER_HPP_ */
|