/**
* \return Returns the weights for the covariance of the points as a vector
*/
WeightVector covariance_weights_vector() const noexcept
{
const int point_count = count_points();
WeightVector weight_vec(point_count);
for (int i = 0; i < point_count; ++i)
{
weight_vec(i) = weights_(i).w_cov;
}
return weight_vec;
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
const PointCloudIn &input,
const std::vector<int> &nn_indices,
std::vector<float> &nn_sqr_dists,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals)
{
// Compute the plane coefficients
//pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
//pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix);
// Compute the 3x3 covariance matrix
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
Eigen::Vector4f model_coefficients;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
model_coefficients.head<3> () = eigen_vector;
model_coefficients[3] = 0;
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Projected query point
Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap ();
float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
point -= distance * model_coefficients.head<3> ();
float curvature = covariance_matrix.trace ();
// Compute the curvature surface change
if (curvature != 0)
curvature = fabsf (eigen_value / curvature);
// Get a copy of the plane normal easy access
Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> ();
// Vector in which the polynomial coefficients will be put
Eigen::VectorXd c_vec;
// Local coordinate system (Darboux frame)
Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f);
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
{
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
}
// Allocate matrices and vectors to hold the data used for the polynomial fit
Eigen::VectorXd weight_vec (nn_indices.size ());
Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
Eigen::VectorXd f_vec (nn_indices.size ());
Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
// Get local coordinate system (Darboux frame)
v = plane_normal.unitOrthogonal ();
u = plane_normal.cross (v);
// Go through neighbors, transform them in the local coordinate system,
// save height and the evaluation of the polynome's terms
double u_coord, v_coord, u_pow, v_pow;
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
// (re-)compute weights
weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
// transforming coordinates
u_coord = de_meaned[ni].dot (u);
v_coord = de_meaned[ni].dot (v);
f_vec (ni) = de_meaned[ni].dot (plane_normal);
// compute the polynomial's terms at the current point
int j = 0;
u_pow = 1;
for (int ui = 0; ui <= order_; ++ui)
{
v_pow = 1;
for (int vi = 0; vi <= order_ - ui; ++vi)
{
P (j++, ni) = u_pow * v_pow;
v_pow *= v_coord;
}
u_pow *= u_coord;
}
}
// Computing coefficients
P_weight = P * weight_vec.asDiagonal ();
P_weight_Pt = P_weight * P.transpose ();
c_vec = P_weight * f_vec;
P_weight_Pt.llt ().solveInPlace (c_vec);
}
switch (upsample_method_)
{
case (NONE):
{
Eigen::Vector3d normal = plane_normal;
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
{
point += (c_vec[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
}
PointOutT aux;
aux.x = point[0];
aux.y = point[1];
aux.z = point[2];
projected_points.push_back (aux);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
break;
}
case (SAMPLE_LOCAL_PLANE):
{
// Uniformly sample a circle around the query point using the radius and step parameters
for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
{
PointOutT projected_point;
pcl::Normal projected_normal;
projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
static_cast<int> (nn_indices.size ()),
projected_point, projected_normal);
projected_points.push_back (projected_point);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
}
break;
}
case (RANDOM_UNIFORM_DENSITY):
{
// Compute the local point density and add more samples if necessary
int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
// Just add the query point, because the density is good
if (num_points_to_add <= 0)
{
// Just add the current point
Eigen::Vector3d normal = plane_normal;
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
{
// Projection onto MLS surface along Darboux normal to the height at (0,0)
point += (c_vec[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
}
PointOutT aux;
aux.x = point[0];
aux.y = point[1];
aux.z = point[2];
projected_points.push_back (aux);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
}
else
{
// Sample the local plane
for (int num_added = 0; num_added < num_points_to_add;)
{
float u_disp = (*rng_uniform_distribution_) (),
v_disp = (*rng_uniform_distribution_) ();
// Check if inside circle; if not, try another coin flip
if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
continue;
PointOutT projected_point;
pcl::Normal projected_normal;
projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
static_cast<int> (nn_indices.size ()),
projected_point, projected_normal);
projected_points.push_back (projected_point);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
num_added ++;
}
}
break;
}
case (VOXEL_GRID_DILATION):
{
// Take all point pairs and sample space between them in a grid-fashion
// \note consider only point pairs with increasing indices
MLSResult result (plane_normal, u, v, c_vec, static_cast<int> (nn_indices.size ()), curvature);
mls_results_[index] = result;
break;
}
}
}
/*
* Fits a weighted cubic regression on predictor(s)
*
* @param contrast - want to predict this value per snp
* @param strength - covariate of choice
* @param weights - weight of data points for this genotype
* @param Predictor - output, prediction function coefficients
* @param Predicted - output, predicted contrast per snp
*/
void
FitWeightedCubic(const std::vector<double> &contrast,
const std::vector<double> &strength,
const std::vector<double> &weights,
std::vector<double> &Predictor,
std::vector<double> &Predicted) {
// Singular value decomposition method
unsigned int i;
unsigned int nobs;
unsigned int npred;
npred = 3+1;
nobs= contrast.size();
// convert double into doubles to match newmat
vector<Real> tmp_vec(nobs);
Real* tmp_ptr = &tmp_vec[0];
vector<Real> obs_vec(nobs);
Real *obs_ptr = &obs_vec[0];
vector<Real> weight_vec(nobs);
Matrix covarMat(nobs,npred);
ColumnVector observedVec(nobs);
// fill in the data
// modified by weights
for (i=0; i<nobs; i++)
weight_vec[i] = sqrt(weights[i]);
// load data - 1s into col 1 of matrix
for (i=0; i<nobs; i++)
tmp_vec[i] = weight_vec[i];
covarMat.Column(1) << tmp_ptr;
for (i=0; i<nobs; i++)
tmp_vec[i] *= strength[i];
covarMat.Column(2) << tmp_ptr;
for (i=0; i<nobs; i++)
tmp_vec[i] *= strength[i];
covarMat.Column(3) << tmp_ptr;
for (i=0; i<nobs; i++)
tmp_vec[i] *= strength[i];
covarMat.Column(4) << tmp_ptr;
for (i=0; i<nobs; i++)
obs_vec[i] = contrast[i]*weight_vec[i];
observedVec << obs_ptr;
// do SVD
Matrix U, V;
DiagonalMatrix D;
ColumnVector Fitted(nobs);
ColumnVector A(npred);
SVD(covarMat,D,U,V);
Fitted = U.t() * observedVec;
A = V * ( D.i() * Fitted );
// this predicts "0" for low weights
// because of weighted regression
Fitted = U * Fitted;
// this is the predictor
Predictor.resize(npred);
for (i=0; i<npred; i++)
Predictor[i] = A.element(i);
// export data back to doubles
// and therefore this predicts "0" for low-weighted points
// which is >not< the desired outcome!!!!
// instead we need to predict all points at once
// >unweighted< as output
vector<double> Goofy;
Predicted.resize(nobs);
for (i = 0; i < nobs; ++i) {
Goofy.resize(npred);
Goofy[0] = 1;
Goofy[1] = strength[i];
Goofy[2] = strength[i]*Goofy[1];
Goofy[3] = strength[i]*Goofy[2];
Predicted[i] = vprod(Goofy,Predictor);
}
}
//' rcpp_make_compact_graph
//'
//' Removes nodes and edges from a graph that are not needed for routing
//'
//' @param graph graph to be processed
//' @param quiet If TRUE, display progress
//' @return \code{Rcpp::List} containing one \code{data.frame} with the compact
//' graph, one \code{data.frame} with the original graph and one
//' \code{data.frame} containing information about the relating edge ids of the
//' original and compact graph.
//'
//' @noRd
// [[Rcpp::export]]
Rcpp::List rcpp_make_compact_graph (Rcpp::DataFrame graph, bool quiet)
{
vertex_map_t vertices;
edge_map_t edge_map;
std::unordered_map <osm_id_t, int> components;
int largest_component;
vert2edge_map_t vert2edge_map;
if (!quiet)
{
Rcpp::Rcout << "Constructing graph ... ";
Rcpp::Rcout.flush ();
}
graph_from_df (graph, vertices, edge_map, vert2edge_map);
if (!quiet)
{
Rcpp::Rcout << std::endl << "Determining connected components ... ";
Rcpp::Rcout.flush ();
}
get_largest_graph_component (vertices, components, largest_component);
if (!quiet)
{
Rcpp::Rcout << std::endl << "Removing intermediate nodes ... ";
Rcpp::Rcout.flush ();
}
vertex_map_t vertices2 = vertices;
edge_map_t edge_map2 = edge_map;
contract_graph (vertices2, edge_map2, vert2edge_map);
if (!quiet)
{
Rcpp::Rcout << std::endl << "Mapping compact to original graph ... ";
Rcpp::Rcout.flush ();
}
int nedges = edge_map2.size ();
// These vectors are all for the contracted graph:
Rcpp::StringVector from_vec (nedges), to_vec (nedges),
highway_vec (nedges);
Rcpp::NumericVector from_lat_vec (nedges), from_lon_vec (nedges),
to_lat_vec (nedges), to_lon_vec (nedges), dist_vec (nedges),
weight_vec (nedges), edgeid_vec (nedges);
unsigned int map_size = 0; // size of edge map contracted -> original
unsigned int en = 0;
std::map <int, osm_edge_t> edge_ordered;
for (auto e = edge_map2.begin (); e != edge_map2.end (); ++e)
edge_ordered.emplace (e->first, e->second);
for (auto e = edge_ordered.begin (); e != edge_ordered.end (); ++e)
{
osm_id_t from = e->second.get_from_vertex ();
osm_id_t to = e->second.get_to_vertex ();
osm_vertex_t from_vtx = vertices2.at (from);
osm_vertex_t to_vtx = vertices2.at (to);
from_vec (en) = from;
to_vec (en) = to;
highway_vec (en) = e->second.highway;
dist_vec (en) = e->second.dist;
weight_vec (en) = e->second.weight;
from_lat_vec (en) = from_vtx.getLat ();
from_lon_vec (en) = from_vtx.getLon ();
to_lat_vec (en) = to_vtx.getLat ();
to_lon_vec (en) = to_vtx.getLon ();
edgeid_vec (en) = e->second.getID ();
map_size += e->second.is_replacement_for ().size ();
en++;
}
Rcpp::NumericVector edge_id_orig (map_size), edge_id_comp (map_size);
int pos = 0;
for (auto e = edge_ordered.begin (); e != edge_ordered.end (); ++e)
{
int eid = e->second.getID ();
std::set <int> edges = e->second.is_replacement_for ();
for (auto ei: edges)
{
edge_id_comp (pos) = eid;
edge_id_orig (pos++) = ei;
}
}
Rcpp::DataFrame compact = Rcpp::DataFrame::create (
Rcpp::Named ("from_id") = from_vec,
Rcpp::Named ("to_id") = to_vec,
Rcpp::Named ("edge_id") = edgeid_vec,
Rcpp::Named ("d") = dist_vec,
Rcpp::Named ("d_weighted") = weight_vec,
Rcpp::Named ("from_lat") = from_lat_vec,
Rcpp::Named ("from_lon") = from_lon_vec,
Rcpp::Named ("to_lat") = to_lat_vec,
Rcpp::Named ("to_lon") = to_lon_vec,
Rcpp::Named ("highway") = highway_vec);
Rcpp::DataFrame rel = Rcpp::DataFrame::create (
Rcpp::Named ("id_compact") = edge_id_comp,
Rcpp::Named ("id_original") = edge_id_orig);
if (!quiet)
Rcpp::Rcout << std::endl;
return Rcpp::List::create (
Rcpp::Named ("compact") = compact,
Rcpp::Named ("original") = graph,
Rcpp::Named ("map") = rel);
}
template <typename PointT> void
pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
int index,
const std::vector<int> &nn_indices,
double search_radius,
int polynomial_order,
boost::function<double(const double)> weight_func)
{
// Compute the plane coefficients
EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
Eigen::Vector4d xyz_centroid;
// Estimate the XYZ centroid
pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid);
// Compute the 3x3 covariance matrix
pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix);
EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
Eigen::Vector4d model_coefficients (0, 0, 0, 0);
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
model_coefficients.head<3> ().matrix () = eigen_vector;
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Projected query point
valid = true;
query_point = cloud.points[index].getVector3fMap ().template cast<double> ();
double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
mean = query_point - distance * model_coefficients.head<3> ();
curvature = covariance_matrix.trace ();
// Compute the curvature surface change
if (curvature != 0)
curvature = std::abs (eigen_value / curvature);
// Get a copy of the plane normal easy access
plane_normal = model_coefficients.head<3> ();
// Local coordinate system (Darboux frame)
v_axis = plane_normal.unitOrthogonal ();
u_axis = plane_normal.cross (v_axis);
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
num_neighbors = static_cast<int> (nn_indices.size ());
order = polynomial_order;
if (order > 1)
{
int nr_coeff = (order + 1) * (order + 2) / 2;
if (num_neighbors >= nr_coeff)
{
// Note: The max_sq_radius parameter is only used if weight_func was not defined
double max_sq_radius = 1;
if (weight_func == 0)
{
max_sq_radius = search_radius * search_radius;
weight_func = boost::bind (&pcl::MLSResult::computeMLSWeight, this, _1, max_sq_radius);
}
// Allocate matrices and vectors to hold the data used for the polynomial fit
Eigen::VectorXd weight_vec (num_neighbors);
Eigen::MatrixXd P (nr_coeff, num_neighbors);
Eigen::VectorXd f_vec (num_neighbors);
Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
for (size_t ni = 0; ni < (size_t) num_neighbors; ++ni)
{
de_meaned[ni][0] = cloud.points[nn_indices[ni]].x - mean[0];
de_meaned[ni][1] = cloud.points[nn_indices[ni]].y - mean[1];
de_meaned[ni][2] = cloud.points[nn_indices[ni]].z - mean[2];
weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
}
// Go through neighbors, transform them in the local coordinate system,
// save height and the evaluation of the polynome's terms
double u_coord, v_coord, u_pow, v_pow;
for (size_t ni = 0; ni < (size_t) num_neighbors; ++ni)
{
// Transforming coordinates
u_coord = de_meaned[ni].dot (u_axis);
v_coord = de_meaned[ni].dot (v_axis);
f_vec (ni) = de_meaned[ni].dot (plane_normal);
// Compute the polynomial's terms at the current point
int j = 0;
u_pow = 1;
for (int ui = 0; ui <= order; ++ui)
{
v_pow = 1;
for (int vi = 0; vi <= order - ui; ++vi)
{
P (j++, ni) = u_pow * v_pow;
v_pow *= v_coord;
}
u_pow *= u_coord;
}
}
// Computing coefficients
P_weight = P * weight_vec.asDiagonal ();
P_weight_Pt = P_weight * P.transpose ();
c_vec = P_weight * f_vec;
P_weight_Pt.llt ().solveInPlace (c_vec);
}
}
}
