Loading...
Searching...
No Matches
organized.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-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 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.
*
* $Id$
*
*/
#pragma once
#include <pcl/search/organized.h>
#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <pcl/common/projection_matrix.h> // for getCameraMatrixFromProjectionMatrix, ...
#include <Eigen/Eigenvalues>
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const PointT &query,
const double radius,
Indices &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
// NAN test
assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
// search window
unsigned left, right, top, bottom;
//unsigned x, y, idx;
float squared_distance;
const float squared_radius = radius * radius;
k_indices.clear ();
k_sqr_distances.clear ();
this->getProjectedRadiusSearchBox (query, squared_radius, left, right, top, bottom);
// iterate over search box
if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->size ()))
max_nn = static_cast<unsigned int> (input_->size ());
k_indices.reserve (max_nn);
k_sqr_distances.reserve (max_nn);
unsigned yEnd = (bottom + 1) * input_->width + right + 1;
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)
{
if (!mask_[idx] || !isFinite ((*input_)[idx]))
continue;
float dist_x = (*input_)[idx].x - query.x;
float dist_y = (*input_)[idx].y - query.y;
float dist_z = (*input_)[idx].z - query.z;
squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
//squared_distance = ((*input_)[idx].getVector3fMap () - query.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)
{
if (sorted_results_)
this->sortResults (k_indices, k_sqr_distances);
return (max_nn);
}
}
}
}
if (sorted_results_)
this->sortResults (k_indices, k_sqr_distances);
return (static_cast<int> (k_indices.size ()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
int k,
Indices &k_indices,
std::vector<float> &k_sqr_distances) const
{
assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
if (k < 1)
{
k_indices.clear ();
k_sqr_distances.clear ();
return (0);
}
Eigen::Vector3f queryvec (query.x, query.y, query.z);
// project query point on the image plane
//Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
int xBegin = static_cast<int>(q [0] / q [2] + 0.5f);
int yBegin = static_cast<int>(q [1] / q [2] + 0.5f);
int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
int yEnd = yBegin + 1;
// the search window. This is supposed to shrink within the iterations
unsigned left = 0;
unsigned right = input_->width - 1;
unsigned top = 0;
unsigned bottom = input_->height - 1;
std::vector <Entry> results; // sorted from smallest to largest distance
results.reserve (k);
// add point laying on the projection of the query point.
if (xBegin >= 0 &&
xBegin < static_cast<int> (input_->width) &&
yBegin >= 0 &&
yBegin < static_cast<int> (input_->height))
testPoint (query, k, results, yBegin * input_->width + xBegin);
else // point lys
{
// find the box that touches the image border -> don't waste time evaluating boxes that are completely outside the image!
int dist = std::numeric_limits<int>::max ();
if (xBegin < 0)
dist = -xBegin;
else if (xBegin >= static_cast<int> (input_->width))
dist = xBegin - static_cast<int> (input_->width);
if (yBegin < 0)
dist = std::min (dist, -yBegin);
else if (yBegin >= static_cast<int> (input_->height))
dist = std::min (dist, yBegin - static_cast<int> (input_->height));
xBegin -= dist;
xEnd += dist;
yBegin -= dist;
yEnd += dist;
}
// stop used as isChanged as well as stop.
bool stop = false;
do
{
// increment box size
--xBegin;
++xEnd;
--yBegin;
++yEnd;
// the range in x-direction which intersects with the image width
int xFrom = xBegin;
int xTo = xEnd;
clipRange (xFrom, xTo, 0, input_->width);
// if x-extend is not 0
if (xTo > xFrom)
{
// if upper line of the rectangle is visible and x-extend is not 0
if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
{
index_t idx = yBegin * input_->width + xFrom;
index_t idxTo = idx + xTo - xFrom;
for (; idx < idxTo; ++idx)
stop = testPoint (query, k, results, idx) || stop;
}
// the row yEnd does NOT belong to the box -> last row = yEnd - 1
// if lower line of the rectangle is visible
if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
{
index_t idx = (yEnd - 1) * input_->width + xFrom;
index_t idxTo = idx + xTo - xFrom;
for (; idx < idxTo; ++idx)
stop = testPoint (query, k, results, idx) || stop;
}
// skip first row and last row (already handled above)
int yFrom = yBegin + 1;
int yTo = yEnd - 1;
clipRange (yFrom, yTo, 0, input_->height);
// if we have lines in between that are also visible
if (yFrom < yTo)
{
if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
{
index_t idx = yFrom * input_->width + xBegin;
index_t idxTo = yTo * input_->width + xBegin;
for (; idx < idxTo; idx += input_->width)
stop = testPoint (query, k, results, idx) || stop;
}
if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
{
index_t idx = yFrom * input_->width + xEnd - 1;
index_t idxTo = yTo * input_->width + xEnd - 1;
for (; idx < idxTo; idx += input_->width)
stop = testPoint (query, k, results, idx) || stop;
}
}
// stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
if (stop)
getProjectedRadiusSearchBox (query, results.back ().distance, left, right, top, bottom);
}
// now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
stop = (static_cast<int> (left) >= xBegin && static_cast<int> (left) < xEnd &&
static_cast<int> (right) >= xBegin && static_cast<int> (right) < xEnd &&
static_cast<int> (top) >= yBegin && static_cast<int> (top) < yEnd &&
static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);
} while (!stop);
const auto results_size = results.size ();
k_indices.resize (results_size);
k_sqr_distances.resize (results_size);
std::size_t idx = 0;
for(const auto& result : results)
{
k_indices [idx] = result.index;
k_sqr_distances [idx] = result.distance;
++idx;
}
return (static_cast<int> (results_size));
}
////////////////////////////////////////////////////////////////////////////////////////////
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 queryvec (point.x, point.y, point.z);
//Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
Eigen::Vector3f q (KR_ * queryvec + 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
{
float y1 = static_cast<float> ((b - std::sqrt (det)) / a);
float y2 = static_cast<float> ((b + std::sqrt (det)) / a);
min = std::min (static_cast<int> (std::floor (y1)), static_cast<int> (std::floor (y2)));
max = std::max (static_cast<int> (std::ceil (y1)), static_cast<int> (std::ceil (y2)));
minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
maxY = static_cast<unsigned> (std::max (std::min (static_cast<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
{
float x1 = static_cast<float> ((b - std::sqrt (det)) / a);
float x2 = static_cast<float> ((b + std::sqrt (det)) / a);
min = std::min (static_cast<int> (std::floor (x1)), static_cast<int> (std::floor (x2)));
max = std::max (static_cast<int> (std::ceil (x1)), static_cast<int> (std::ceil (x2)));
minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const
{
pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
{
// internally we calculate with double but store the result into float matrices.
projection_matrix_.setZero ();
if (input_->height == 1 || input_->width == 1)
{
PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
return false;
}
const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, static_cast<unsigned>(1));
const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, static_cast<unsigned>(1));
Indices indices;
indices.reserve (input_->size () >> (pyramid_level_ << 1));
for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
{
for (unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip)
{
if (!mask_ [idx2])
continue;
indices.push_back (idx2);
}
}
double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);
PCL_DEBUG_STREAM("[pcl::" << this->getName () << "::estimateProjectionMatrix] projection matrix=" << std::endl << projection_matrix_ << std::endl << "residual_sqr=" << residual_sqr << std::endl);
if (std::abs (residual_sqr) > eps_ * static_cast<float>(indices.size ()))
{
PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not from a projective device!\nResidual (MSE) %g, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ());
return false;
}
// get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
// and R being the rotation matrix
KR_ = projection_matrix_.topLeftCorner <3, 3> ();
// precalculate KR * KR^T needed by calculations during nn-search
KR_KRT_ = KR_ * KR_.transpose ();
// final test: project a few points at known image coordinates and test if the projected coordinates are close
for(std::size_t i=0; i<11; ++i) {
const std::size_t test_index = input_->size()*i/11u;
if (!mask_[test_index])
continue;
const auto& test_point = (*input_)[test_index];
pcl::PointXY q;
if (!projectPoint(test_point, q) || std::abs(q.x-test_index%input_->width)>1 || std::abs(q.y-test_index/input_->width)>1) {
PCL_WARN ("[pcl::%s::estimateProjectionMatrix] Input dataset does not seem to be from a projective device! (point %zu (%g,%g,%g) projected to pixel coordinates (%g,%g), but actual pixel coordinates are (%zu,%zu))\n",
this->getName ().c_str (), test_index, test_point.x, test_point.y, test_point.z, q.x, q.y, static_cast<std::size_t>(test_index%input_->width), static_cast<std::size_t>(test_index/input_->width));
return false;
}
}
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
pcl::search::OrganizedNeighbor<PointT>::projectPoint (const PointT& point, pcl::PointXY& q) const
{
Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
q.x = projected [0] / projected [2];
q.y = projected [1] / projected [2];
return (projected[2] != 0);
}
#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
pcl::search::OrganizedNeighbor::estimateProjectionMatrix
bool estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
Definition organized.hpp:333
pcl::search::OrganizedNeighbor::radiusSearch
int radiusSearch(const PointT &p_q, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all neighbors of query point that are within a given radius.
Definition organized.hpp:49
pcl::search::OrganizedNeighbor::nearestKSearch
int nearestKSearch(const PointT &p_q, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for a given query point.
Definition organized.hpp:114
pcl::search::OrganizedNeighbor::computeCameraMatrix
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
Definition organized.hpp:326
pcl::search::OrganizedNeighbor::projectPoint
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Definition organized.hpp:394
pcl::search::OrganizedNeighbor::getProjectedRadiusSearchBox
void getProjectedRadiusSearchBox(const PointT &point, float squared_radius, unsigned &minX, unsigned &minY, unsigned &maxX, unsigned &maxY) const
Obtain a search box in 2D from a sphere with a radius in 3D.
Definition organized.hpp:269
pcl::projectPoint
void projectPoint(const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
Project a point on a planar model given by a set of normalized coefficients.
Definition sac_model_plane.h:62
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::getCameraMatrixFromProjectionMatrix
PCL_EXPORTS void getCameraMatrixFromProjectionMatrix(const Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, Eigen::Matrix3f &camera_matrix)
Determines the camera matrix from the given projection matrix.
pcl::PointXY
A 2D point structure representing Euclidean xy coordinates.
Definition point_types.hpp:745
pcl::PointXY::y
float y
Definition point_types.hpp:752
pcl::PointXY::x
float x
Definition point_types.hpp:751
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623