// -------------------------------------------------------------------- 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]]]; }