// --------------------------------------------------------------------
static matrix_type _create_coefficients_mat(
const size_t K,
const size_t row_padding)
{
matrix_type c_mat (K + K + row_padding, K);
for (size_t k = 0; k < K; k++)
{
c_mat(k, k) = value_type(-1.0);
c_mat(K + k, k) = value_type(+1.0);
}
for (size_t r = K + K; r < c_mat.get_height(); r++)
for (size_t c = 0; c < c_mat.get_width(); c++)
c_mat(r, c) = value_type(1.0);
return c_mat;
}
void trmm(enum AMPBLAS_ORDER order, enum AMPBLAS_SIDE side, enum AMPBLAS_UPLO uplo, enum AMPBLAS_TRANSPOSE transa, enum AMPBLAS_DIAG diag, int m, int n, value_type alpha, const value_type* a, int lda, value_type* b, int ldb)
{
// recursive order adjustment
if (order == AmpblasRowMajor)
{
trmm(AmpblasColMajor, side == AmpblasLeft ? AmpblasRight : AmpblasLeft, uplo == AmpblasUpper ? AmpblasLower : AmpblasUpper, transa, diag, m, n, alpha, a, lda, b, ldb);
return;
}
// quick return
if (m == 0 && n == 0)
return;
// derived parameters
int k = (side == AmpblasLeft ? m : n);
// argument check
if (m < 0)
argument_error("trmm", 6);
if (n < 0)
argument_error("trmm", 7);
if (a == nullptr)
argument_error("trmm", 9);
if (lda < k)
argument_error("trmm", 10);
if (b == nullptr)
argument_error("trmm", 11);
if (ldb < m)
argument_error("trmm", 12);
// create views
auto a_mat = make_matrix_view(k, k, a, lda);
auto b_mat = make_matrix_view(m, n, b, ldb);
auto b_mat_const = make_matrix_view(m, n, const_cast<const value_type*>(b), ldb);
// fill with zeros if alpha is zero
if (alpha == value_type())
{
ampblas::_detail::fill(get_current_accelerator_view(), b_mat.extent, value_type(), b_mat);
return;
}
// workspace
concurrency::array<value_type,2> c(n,m);
concurrency::array_view<value_type,2> c_mat(c);
c_mat.discard_data();
// forward to tuning routine
ampblas::trmm(get_current_accelerator_view(), cast(side), cast(uplo), cast(transa), cast(diag), m, n, alpha, a_mat, b_mat_const, c_mat);
// copy workspace to answer
copy(c_mat, b_mat);
}
template<typename PointT, typename PointNT> void
pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices)
{
if (!initCompute ())
return;
//--- Part A from the paper
// Set up matrix F
Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
{
f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
(*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
}
// Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
Eigen::Matrix<double, 6, 6> c_mat (f_mat * f_mat.transpose ());
Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
eigen_solver.compute (c_mat, true);
Eigen::MatrixXcd complex_eigenvectors = eigen_solver.eigenvectors ();
Eigen::Matrix<double, 6, 6> x;
for (size_t i = 0; i < 6; ++i)
for (size_t j = 0; j < 6; ++j)
x (i, j) = real (complex_eigenvectors (i, j));
//--- Part B from the paper
/// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
std::vector<size_t> candidate_indices;
candidate_indices.resize (indices_->size ());
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
candidate_indices[p_i] = p_i;
// Compute the v 6-vectors
typedef Eigen::Matrix<double, 6, 1> Vector6d;
std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
v.resize (candidate_indices.size ());
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
{
v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
(*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> ();
v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
}
// Set up the lists to be sorted
std::vector<std::list<std::pair<int, double> > > L;
L.resize (6);
for (size_t i = 0; i < 6; ++i)
{
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
L[i].push_back (std::make_pair (p_i, fabs (v[p_i].dot (x.block<6, 1> (0, i)))));
// Sort in decreasing order
L[i].sort (sort_dot_list_function);
}
// Initialize the 6 t's
std::vector<double> t (6, 0.0);
sampled_indices.resize (num_samples_);
std::vector<bool> point_sampled (candidate_indices.size (), false);
// Now select the actual points
for (size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
{
// Find the most unconstrained dimension, i.e., the minimum t
size_t min_t_i = 0;
for (size_t i = 0; i < 6; ++i)
{
if (t[min_t_i] > t[i])
min_t_i = i;
}
// Add the point from the top of the list corresponding to the dimension to the set of samples
while (point_sampled [L[min_t_i].front ().first])
L[min_t_i].pop_front ();
sampled_indices[sample_i] = L[min_t_i].front ().first;
point_sampled[L[min_t_i].front ().first] = true;
L[min_t_i].pop_front ();
// Update the running totals
for (size_t i = 0; i < 6; ++i)
{
double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
t[i] += val * val;
}
}
// Remap the sampled_indices to the input_ cloud
for (size_t i = 0; i < sampled_indices.size (); ++i)
sampled_indices[i] = (*indices_)[candidate_indices[sampled_indices[i]]];
}
