template <typename PointT> inline PointT
pcl::transformPoint (const PointT &point, const Eigen::Affine3f &transform)
{
PointT ret = point;
ret.getVector3fMap () = transform * point.getVector3fMap ();
return ret;
}
/** \brief Calculate centroid of voxel.
* \param[out] centroid_arg the resultant centroid of the voxel
*/
void
getCentroid (PointT& centroid_arg) const
{
using namespace pcl::common;
if (point_counter_)
{
centroid_arg.getVector3fMap() = (pt_ / static_cast<double> (point_counter_)).cast<float>();
centroid_arg.getNormalVector3fMap() = n_.normalized().cast<float>();
centroid_arg.r = static_cast<unsigned char>(r_ / point_counter_);
centroid_arg.g = static_cast<unsigned char>(g_ / point_counter_);
centroid_arg.b = static_cast<unsigned char>(b_ / point_counter_);
}
else
{
centroid_arg.x = std::numeric_limits<float>::quiet_NaN();
centroid_arg.y = std::numeric_limits<float>::quiet_NaN();
centroid_arg.z = std::numeric_limits<float>::quiet_NaN();
centroid_arg.normal_x = std::numeric_limits<float>::quiet_NaN();
centroid_arg.normal_y = std::numeric_limits<float>::quiet_NaN();
centroid_arg.normal_z = std::numeric_limits<float>::quiet_NaN();
centroid_arg.r = 0;
centroid_arg.g = 0;
centroid_arg.b = 0;
}
}
void images_to_cloud(cv::Mat& depth, cv::Mat& rgb, CloudT::Ptr& cloud, const Eigen::Matrix3f& K)
{
int rows = depth.rows;
int cols = depth.cols;
cloud->reserve(rows*cols);
Eigen::Vector3f pe;
PointT pc;
cv::Vec3b prgb;
Eigen::ColPivHouseholderQR<Eigen::Matrix3f> dec(K);
for (int x = 0; x < cols; ++x) {
for (int y = 0; y < rows; ++y) {
uint16_t d = depth.at<uint16_t>(y, x);
if (d == 0) {
continue;
}
float df = float(d)/1000.0f;
pe(0) = x;
pe(1) = y;
pe(2) = 1.0f;
pe = dec.solve(pe);
pe *= df/pe(2);
pc.getVector3fMap() = pe;
prgb = rgb.at<cv::Vec3b>(y, x);
pc.r = prgb[2];
pc.g = prgb[1];
pc.b = prgb[0];
cloud->push_back(pc);
//cout << p.getVector3fMap().transpose() << endl;
}
}
//object_retrieval obr("/random");
//obr.visualize_cloud(cloud);
}
template <typename PointT, typename Scalar> inline PointT
pcl::transformPoint (const PointT &point,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
ret.getVector3fMap () = transform * point.getVector3fMap ();
return (ret);
}
template<typename PointT> inline void
pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
input.getVector3fMap ()+= mean_.head<3> ();
}
template<typename PointT> inline void
pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
}
template<typename PointT> bool
pcl::BoxClipper3D<PointT>::clipPoint3D (const PointT& point) const
{
Eigen::Vector4f point_coordinates (transformation_.matrix ()
* point.getVector4fMap ());
return (point_coordinates.array ().abs () <= 1).all ();
}
template <typename PointT> bool
PCLVisualizer::addSphere (const PointT ¢er, double radius, double r, double g, double b, const std::string &id, int viewport)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
PCL_WARN ("[addSphere] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
vtkSmartPointer<vtkDataSet> data = createSphere (center.getVector4fMap (), radius);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetInterpolationToGouraud ();
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetProperty ()->SetColor (r, g, b);
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const PointT &p_q,
const double radius,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
if (projection_matrix_.coeffRef (0) == 0)
{
PCL_ERROR ("[pcl::%s::radiusSearch] Invalid projection matrix. Probably input dataset was not organized!\n", getName ().c_str ());
return (0);
}
// NAN test
if (!pcl_isfinite (p_q.x) || !pcl_isfinite (p_q.y) || !pcl_isfinite (p_q.z))
return (0);
// search window
unsigned left, right, top, bottom;
//unsigned x, y, idx;
float squared_distance, squared_radius;
k_indices.clear ();
k_sqr_distances.clear ();
squared_radius = radius * radius;
this->getProjectedRadiusSearchBox (p_q, squared_radius, left, right, top, bottom);
// iterate over search box
if (max_nn == 0 || max_nn >= (unsigned int)input_->points.size ())
max_nn = input_->points.size ();
k_indices.reserve (max_nn);
k_sqr_distances.reserve (max_nn);
unsigned yEnd = (bottom + 1) * input_->width + right + 1;
register unsigned idx = top * input_->width + left;
unsigned skip = input_->width - right + left - 1;
unsigned xEnd = idx - left + right + 1;
for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
{
for (; idx < xEnd; ++idx)
{
squared_distance = (input_->points[idx].getVector3fMap () - p_q.getVector3fMap ()).squaredNorm ();
if (squared_distance <= squared_radius)
{
k_indices.push_back (idx);
k_sqr_distances.push_back (squared_distance);
// already done ?
if (k_indices.size () == max_nn)
return max_nn;
}
}
}
return (k_indices.size ());
}
void
drawBoundingBox(PointCloudT::Ptr cloud,
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
int z)
{
//Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);
//Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
//Eigen::ComputeEigenvectors);
eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
// eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
// (covariance,Eigen::ComputeEigenvectors);
eigDx = eigen_solver.eigenvectors();
eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));
//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
p2w.block<3,3>(0,0) = eigDx.transpose();
p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
//pcl::PointCloud<PointT> cPoints;
pcl::transformPointCloud(*cloud, cPoints, p2w);
//PointT min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
const Eigen::Quaternionf qfinal(eigDx);
const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
//viewer->addPointCloud(cloud);
viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));
}
template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
pcl::CropHull<PointT>::isPointIn2DPolyWithVertIndices (
const PointT& point, const Vertices& verts, const PointCloud& cloud)
{
bool in_poly = false;
double x1, x2, y1, y2;
const int nr_poly_points = static_cast<const int>(verts.vertices.size ());
double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
for (int i = 0; i < nr_poly_points; i++)
{
const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
if (xnew > xold)
{
x1 = xold;
x2 = xnew;
y1 = yold;
y2 = ynew;
}
else
{
x1 = xnew;
x2 = xold;
y1 = ynew;
y2 = yold;
}
if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
(point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
{
in_poly = !in_poly;
}
xold = xnew;
yold = ynew;
}
return (in_poly);
}
void PC_to_Mat(PointCloudT::Ptr &cloud, cv::Mat &result){
if (cloud->isOrganized()) {
std::cout << "PointCloud is organized..." << std::endl;
result = cv::Mat(cloud->height, cloud->width, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<result.rows; h++) {
for (int w=0; w<result.cols; w++) {
PointT point = cloud->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
result.at<cv::Vec3b>(h,w)[0] = rgb[2];
result.at<cv::Vec3b>(h,w)[1] = rgb[1];
result.at<cv::Vec3b>(h,w)[2] = rgb[0];
}
}
}
}
}
template<typename PointT> void
pcl::BoxClipper3D<PointT>::transformPoint (const PointT& pointIn, PointT& pointOut) const
{
const Eigen::Vector4f& point = pointIn.getVector4fMap ();
pointOut.getVector4fMap () = transformation_ * point;
// homogeneous value might not be 1
if (point [3] != 1)
{
// homogeneous component might be uninitialized -> invalid
if (point [3] != 0)
{
pointOut.x += (1 - point [3]) * transformation_.data () [ 9];
pointOut.y += (1 - point [3]) * transformation_.data () [10];
pointOut.z += (1 - point [3]) * transformation_.data () [11];
}
else
{
pointOut.x += transformation_.data () [ 9];
pointOut.y += transformation_.data () [10];
pointOut.z += transformation_.data () [11];
}
}
}
template<typename PointT> bool
pcl::CropHull<PointT>::rayTriangleIntersect (const PointT& point,
const Eigen::Vector3f& ray,
const Vertices& verts,
const PointCloud& cloud)
{
// Algorithm here is adapted from:
// http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
//
// Original copyright notice:
// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
//
assert (verts.vertices.size () == 3);
const Eigen::Vector3f p = point.getVector3fMap ();
const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
const Eigen::Vector3f u = b - a;
const Eigen::Vector3f v = c - a;
const Eigen::Vector3f n = u.cross (v);
const float n_dot_ray = n.dot (ray);
if (std::fabs (n_dot_ray) < 1e-9)
return (false);
const float r = n.dot (a - p) / n_dot_ray;
if (r < 0)
return (false);
const Eigen::Vector3f w = p + r * ray - a;
const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
const float s = s_numerator / denominator;
if (s < 0 || s > 1)
return (false);
const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
const float t = t_numerator / denominator;
if (t < 0 || s+t > 1)
return (false);
return (true);
}
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point,
float squared_radius,
unsigned &minX,
unsigned &maxX,
unsigned &minY,
unsigned &maxY) const
{
Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
int min, max;
// a and c are multiplied by two already => - 4ac -> - ac
float det = b * b - a * c;
if (det < 0)
{
minY = 0;
maxY = input_->height - 1;
}
else
{
min = std::min ((int) floor ((b - sqrt (det)) / a), (int) floor ((b + sqrt (det)) / a));
max = std::max ((int) ceil ((b - sqrt (det)) / a), (int) ceil ((b + sqrt (det)) / a));
minY = (unsigned) std::min ((int) input_->height - 1, std::max (0, min));
maxY = (unsigned) std::max (std::min ((int) input_->height - 1, max), 0);
}
b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
det = b * b - a * c;
if (det < 0)
{
minX = 0;
maxX = input_->width - 1;
}
else
{
min = std::min ((int) floor ((b - sqrt (det)) / a), (int) floor ((b + sqrt (det)) / a));
max = std::max ((int) ceil ((b - sqrt (det)) / a), (int) ceil ((b + sqrt (det)) / a));
minX = (unsigned) std::min ((int)input_->width - 1, std::max (0, min));
maxX = (unsigned) std::max (std::min ((int)input_->width - 1, max), 0);
}
}
double ColorizeDistanceFromPlane::distanceToConvexes(
const PointT& p,
const std::vector<ConvexPolygon::Ptr>& convexes)
{
Eigen::Vector3f v = p.getVector3fMap();
double min_distance = DBL_MAX;
for (size_t i = 0; i < convexes.size(); i++) {
ConvexPolygon::Ptr convex = convexes[i];
if (!only_projectable_ || convex->isProjectableInside(v)) {
double d = convex->distanceToPoint(v);
if (d < min_distance) {
min_distance = d;
}
}
}
return min_distance;
}
void segmentation_callback(const std_msgs::String::ConstPtr& msg)
{
data_summary.load(data_path);
boost::filesystem::path sweep_xml(msg->data);
boost::filesystem::path surfel_path = sweep_xml.parent_path() / "surfel_map.pcd";
SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT);
pcl::io::loadPCDFile(surfel_path.string(), *surfel_cloud);
CloudT::Ptr cloud(new CloudT);
NormalCloudT::Ptr normals(new NormalCloudT);
cloud->reserve(surfel_cloud->size());
normals->reserve(surfel_cloud->size());
for (const SurfelT& s : surfel_cloud->points) {
if (s.confidence < threshold) {
continue;
}
PointT p;
p.getVector3fMap() = s.getVector3fMap();
p.rgba = s.rgba;
NormalT n;
n.getNormalVector3fMap() = s.getNormalVector3fMap();
cloud->push_back(p);
normals->push_back(n);
}
// we might want to save the map and normals here
boost::filesystem::path cloud_path = sweep_xml.parent_path() / "cloud.pcd";
boost::filesystem::path normals_path = sweep_xml.parent_path() / "normals.pcd";
pcl::io::savePCDFileBinary(cloud_path.string(), *cloud);
pcl::io::savePCDFileBinary(normals_path.string(), *normals);
supervoxel_segmentation ss(0.02f, 0.2f, 0.4f, false); // do not filter
Graph* g;
Graph* convex_g;
vector<CloudT::Ptr> supervoxels;
vector<CloudT::Ptr> convex_segments;
map<size_t, size_t> indices;
std::tie(g, convex_g, supervoxels, convex_segments, indices) = ss.compute_convex_oversegmentation(cloud, normals, false);
#if VISUALIZE
CloudT::Ptr colored_segments(new CloudT);
colored_segments->reserve(cloud->size());
int counter = 0;
for (CloudT::Ptr& c : convex_segments) {
for (PointT p : c->points) {
p.r = colormap[counter%24][0];
p.g = colormap[counter%24][1];
p.b = colormap[counter%24][2];
colored_segments->push_back(p);
}
++counter;
}
//dynamic_object_retrieval::visualize(colored_segments);
sensor_msgs::PointCloud2 vis_msg;
pcl::toROSMsg(*colored_segments, vis_msg);
vis_msg.header.frame_id = "/map";
vis_cloud_pub.publish(vis_msg);
#endif
boost::filesystem::path segments_path = sweep_xml.parent_path() / "convex_segments";
boost::filesystem::create_directory(segments_path);
ss.save_graph(*convex_g, (segments_path / "graph.cereal").string());
delete g;
delete convex_g;
dynamic_object_retrieval::sweep_summary summary;
summary.nbr_segments = convex_segments.size();
int i = 0;
vector<string> segment_paths;
for (CloudT::Ptr& c : convex_segments) {
std::stringstream ss;
ss << "segment" << std::setw(4) << std::setfill('0') << i;
boost::filesystem::path segment_path = segments_path / (ss.str() + ".pcd");
segment_paths.push_back(segment_path.string());
pcl::io::savePCDFileBinary(segment_path.string(), *c);
summary.segment_indices.push_back(i); // counter
++i;
}
summary.save(segments_path);
std_msgs::String done_msg;
done_msg.data = msg->data;
pub.publish(done_msg);
data_summary.nbr_sweeps++;
data_summary.nbr_convex_segments += convex_segments.size();
data_summary.index_convex_segment_paths.insert(data_summary.index_convex_segment_paths.end(),
segment_paths.begin(), segment_paths.end());
data_summary.save(data_path);
}
void startRecording(string PCDfilepath, string TRJfilepath, pcl::visualization::PCLVisualizer *viewer, PointCloudT::Ptr &cloud, float dist, bool& new_cloud_available_flag)
{
// Writer::startWriting();
while (flag3)
{
if (new_cloud_available_flag && cloud_mutex.try_lock()) // if a new cloud is available
{
new_cloud_available_flag = false;
// Perform people detection on the new cloud:
std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters
people_detector.setInputCloud(cloud);
people_detector.setGround(ground_coeffs); // set floor coefficients
people_detector.compute(clusters); // perform people detection
ground_coeffs = people_detector.getGround(); // get updated floor coefficients
// Draw cloud and people bounding boxes in the viewer:
viewer->removeAllPointClouds();
viewer->removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer->addPointCloud<PointT>(cloud, rgb, "input_cloud");
unsigned int k = 0;
centroids_curr.clear();
for (std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
if (it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold
{
Eigen::Vector3f centroid_coords; //vector containing persons centroid coordinates
centroid_coords = it->getCenter(); // calculate centroid coordinates
int person_on_screen; // person number displayed on screen
person_on_screen = if_same_person(centroids_prev, centroid_coords, dist); // check if current person existed in prev frame
// add coordinates to vector containing people from current frame
float x = centroid_coords[0]; // extract x coordinate of centroid
float y = centroid_coords[1]; // extract y coordinate of centroid
float z = centroid_coords[2]; // extract z coorfinate of centroid
PointT pp;
pp.getVector3fMap() = it->getCenter();
Eigen::Vector4f centroid_coords_person;
centroid_coords_person[0] = x;
centroid_coords_person[1] = y;
centroid_coords_person[2] = z;
centroid_coords_person[3] = (float)person_on_screen;
centroids_curr.push_back(centroid_coords_person);
// draw theoretical person bounding box in the PCL viewer:
it->drawTBoundingBox(*viewer, k); // draw persons bounding box
cout << "tesst";
//creating text to display near person box
string tekst = "person ";
string Result;
ostringstream convert;
convert << person_on_screen;
Result = convert.str();
tekst = tekst + Result;
viewer->addText3D(tekst, pp, 0.08); // display text
k++;
cout << "-------------";
}
}
if (k == 0)
{
empty_in_row++;
cout << "Empty in a row: " << empty_in_row;
}
else empty_in_row = 0;
if (empty_in_row == 3) {
cout << "Czyszcze wektor przechowujacy dane o postaciach";
centroids_prev.clear();
}
if (k > 0)centroids_prev = centroids_curr;
std::cout << k << " people found" << std::endl;
viewer->spinOnce();
// Writer::write(cloud, clusters);
cloud_mutex.unlock();
}
}
}
template <typename PointT> float
pcl16::search::BruteForce<PointT>::getDistSqr (
const PointT& point1, const PointT& point2) const
{
return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
}
template <typename PointT> void
pcl16::approximatePolygon2D (const typename pcl16::PointCloud<PointT>::VectorType &polygon,
typename pcl16::PointCloud<PointT>::VectorType &approx_polygon,
float threshold, bool refine, bool closed)
{
approx_polygon.clear ();
if (polygon.size () < 3)
return;
std::vector<std::pair<unsigned, unsigned> > intervals;
std::pair<unsigned,unsigned> interval (0, 0);
if (closed)
{
float max_distance = .0f;
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
(polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.second = idx;
}
}
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
(polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.first = idx;
}
}
if (max_distance < threshold * threshold)
return;
intervals.push_back (interval);
std::swap (interval.first, interval.second);
intervals.push_back (interval);
}
else
{
interval.first = 0;
interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
intervals.push_back (interval);
}
std::vector<unsigned> result;
// recursively refine
while (!intervals.empty ())
{
std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
line_x *= linelen;
line_y *= linelen;
line_d *= linelen;
float max_distance = 0.0;
unsigned first_index = currentInterval.first + 1;
unsigned max_index = 0;
// => 0-crossing
if (currentInterval.first > currentInterval.second)
{
for (unsigned idx = first_index; idx < polygon.size(); idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
first_index = 0;
}
for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
if (max_distance > threshold)
{
std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
currentInterval.second = max_index;
intervals.push_back (interval);
}
else
{
result.push_back (currentInterval.second);
intervals.pop_back ();
}
}
approx_polygon.reserve (result.size ());
if (refine)
{
std::vector<Eigen::Vector3f> lines (result.size ());
std::reverse (result.begin (), result.end ());
for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
{
unsigned nIdx = rIdx + 1;
if (nIdx == result.size ())
nIdx = 0;
Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
unsigned pIdx = result[rIdx];
unsigned num_points = 0;
if (pIdx > result[nIdx])
{
num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
for (; pIdx < polygon.size (); ++pIdx)
{
covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
centroid [0] += polygon [pIdx].x;
centroid [1] += polygon [pIdx].y;
}
pIdx = 0;
}
num_points += result[nIdx] - pIdx;
for (; pIdx < result[nIdx]; ++pIdx)
{
covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
centroid [0] += polygon [pIdx].x;
centroid [1] += polygon [pIdx].y;
}
covariance.coeffRef (2) = covariance.coeff (1);
float norm = 1.0f / float (num_points);
centroid *= norm;
covariance *= norm;
covariance.coeffRef (0) -= centroid [0] * centroid [0];
covariance.coeffRef (1) -= centroid [0] * centroid [1];
covariance.coeffRef (3) -= centroid [1] * centroid [1];
float eval;
Eigen::Vector2f normal;
eigen22 (covariance, eval, normal);
// select the one which is more "parallel" to the original line
Eigen::Vector2f direction;
direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
direction.normalize ();
if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
{
std::swap (normal [0], normal [1]);
normal [0] = -normal [0];
}
// needs to be on the left side of the edge
if (direction [0] * normal [1] < direction [1] * normal [0])
normal *= -1.0;
lines [rIdx].head<2> () = normal;
lines [rIdx] [2] = -normal.dot (centroid);
}
float threshold2 = threshold * threshold;
for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
{
unsigned nIdx = rIdx + 1;
if (nIdx == result.size ())
nIdx = 0;
Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
vertex /= vertex [2];
vertex [2] = 0.0;
PointT point;
// test whether we need another edge since the intersection point is too far away from the original vertex
Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
pq [2] = 0.0;
float distance = pq.squaredNorm ();
if (distance > threshold2)
{
// test whether the old point is inside the new polygon or outside
if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
(pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
{
float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];
point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
approx_polygon.push_back (point);
vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
}
}
point.getVector3fMap () = vertex;
approx_polygon.push_back (point);
}
}
else
{
// we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
approx_polygon.push_back (polygon [*it]);
}
}
template<typename PointT, typename LeafT, typename BranchT> float
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::pointSquaredDist (const PointT & pointA,
const PointT & pointB) const
{
return (pointA.getVector3fMap () - pointB.getVector3fMap ()).squaredNorm ();
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> float
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist (const PointT & point_a,
const PointT & point_b) const
{
return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
}
