//************This method could be used only when there are many steps, otherwise it might be removed*******************************************//
//Create a combined mxb matrix for trajectories from A to B to A
Eigen::MatrixXf CreateCombinedMXBMatrix(Eigen::MatrixXf matrixA, Eigen::MatrixXf matrixB, float increment, int i, int j, int offset)
{
//Create a matrix with zmp trajectory from A to B
Eigen::MatrixXf mxbMatrixBA = MXB(matrixA.row(i), matrixB.row(j), increment, offset);
Eigen::MatrixXf noZMPMovement(mxbMatrixBA.rows(), mxbMatrixBA.cols());
for(int i = 0; i < mxbMatrixBA.rows(); i++)
{
noZMPMovement.row(i) = mxbMatrixBA.bottomRows(1);
}
Eigen::MatrixXf tempMatrix(2*mxbMatrixBA.rows(), mxbMatrixBA.cols());
tempMatrix << mxbMatrixBA, noZMPMovement;
mxbMatrixBA.swap(tempMatrix);
//Append both step trajectories
if(matrixA.rows() > i+1)
{
//Create a matrix with zmp trajectory from B to A
Eigen::MatrixXf mxbMatrixAB = MXB(matrixB.row(j), matrixA.row(i+1), increment, offset);
for(int i = 0; i < mxbMatrixAB.rows(); i++)
{
noZMPMovement.row(i) = mxbMatrixAB.bottomRows(1);
}
Eigen::MatrixXf tempMatrix2(2*mxbMatrixAB.rows(), mxbMatrixAB.cols());
tempMatrix2 << mxbMatrixAB, noZMPMovement;
mxbMatrixAB.swap(tempMatrix2);
Eigen::MatrixXf mxbMatrix(mxbMatrixBA.rows()+mxbMatrixAB.rows(), mxbMatrixBA.cols());
mxbMatrix << mxbMatrixBA, mxbMatrixAB;
return mxbMatrix;
}
else
{
return mxbMatrixBA;
}
}
template <typename PointT> void
pcl::ApproximateProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
{
bool segmentation_is_possible = initCompute ();
if (!segmentation_is_possible)
{
deinitCompute ();
return;
}
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
std::vector<int> half_sizes;
int iteration = 0;
int half_size = 0.0f;
float window_size = 0.0f;
float height_threshold = 0.0f;
while (window_size < max_window_size_)
{
// Determine the initial window size.
if (exponential_)
half_size = static_cast<int> (std::pow (static_cast<float> (base_), iteration));
else
half_size = (iteration+1) * base_;
window_size = 2 * half_size + 1;
// Calculate the height threshold to be used in the next iteration.
if (iteration == 0)
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
// Enforce max distance on height threshold
if (height_threshold > max_distance_)
height_threshold = max_distance_;
half_sizes.push_back (half_size);
window_sizes.push_back (window_size);
height_thresholds.push_back (height_threshold);
iteration++;
}
// setup grid based on scale and extents
Eigen::Vector4f global_max, global_min;
pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
float xextent = global_max.x () - global_min.x ();
float yextent = global_max.y () - global_min.y ();
int rows = static_cast<int> (std::floor (yextent / cell_size_) + 1);
int cols = static_cast<int> (std::floor (xextent / cell_size_) + 1);
Eigen::MatrixXf A (rows, cols);
A.setConstant (std::numeric_limits<float>::quiet_NaN ());
Eigen::MatrixXf Z (rows, cols);
Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
Eigen::MatrixXf Zf (rows, cols);
Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (int i = 0; i < input_->points.size (); ++i)
{
// ...then test for lower points within the cell
PointT p = input_->points[i];
int row = std::floor(p.y - global_min.y ());
int col = std::floor(p.x - global_min.x ());
if (p.z < A (row, col) || pcl_isnan (A (row, col)))
{
A (row, col) = p.z;
}
}
// Ground indices are initially limited to those points in the input cloud we
// wish to process
ground = *indices_;
// Progressively filter ground returns using morphological open
for (int i = 0; i < window_sizes.size (); ++i)
{
PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
i, height_thresholds[i], window_sizes[i], half_sizes[i]);
// Limit filtering to those points currently considered ground returns
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
// Apply the morphological opening operation at the current window size.
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (int row = 0; row < rows; ++row)
{
int rs, re;
rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
for (int col = 0; col < cols; ++col)
{
int cs, ce;
cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
float min_coeff = std::numeric_limits<float>::max ();
for (int j = rs; j < (re + 1); ++j)
{
for (int k = cs; k < (ce + 1); ++k)
{
if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
{
if (A (j, k) < min_coeff)
min_coeff = A (j, k);
}
}
}
if (min_coeff != std::numeric_limits<float>::max ())
Z(row, col) = min_coeff;
}
}
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (int row = 0; row < rows; ++row)
{
int rs, re;
rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
for (int col = 0; col < cols; ++col)
{
int cs, ce;
cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
float max_coeff = -std::numeric_limits<float>::max ();
for (int j = rs; j < (re + 1); ++j)
{
for (int k = cs; k < (ce + 1); ++k)
{
if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
{
if (Z (j, k) > max_coeff)
max_coeff = Z (j, k);
}
}
}
if (max_coeff != -std::numeric_limits<float>::max ())
Zf (row, col) = max_coeff;
}
}
// Find indices of the points whose difference between the source and
// filtered point clouds is less than the current height threshold.
std::vector<int> pt_indices;
for (boost::int32_t p_idx = 0; p_idx < ground.size (); ++p_idx)
{
PointT p = cloud->points[p_idx];
int erow = static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
int ecol = static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
float diff = p.z - Zf (erow, ecol);
if (diff < height_thresholds[i])
pt_indices.push_back (ground[p_idx]);
}
A.swap (Zf);
// Ground is now limited to pt_indices
ground.swap (pt_indices);
PCL_DEBUG ("ground now has %d points\n", ground.size ());
}
deinitCompute ();
}