void findOverlaps(const PCRGB::Ptr& target_pc, const PCRGB::Ptr& source_pc,
pcl::IndicesPtr& target_inds_list, pcl::IndicesPtr& source_inds_list,
double icp_radius=0.03, bool use_rgb=false, double color_weight=0.001)
{
KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree->setInputCloud(target_pc);
if(use_rgb){
boost::shared_ptr<pcl::DefaultPointRepresentation<PRGB> const>
pt_rep(new pcl::DefaultPointRepresentation<PRGB>(color_weight, color_weight, color_weight));
kd_tree->setPointRepresentation(pt_rep);
}
vector<bool> target_inds(target_pc->points.size(), false);
vector<bool> source_inds(source_pc->points.size(), false);
for(uint32_t i=0;i<source_pc->points.size();i++) {
vector<int> inds;
vector<float> dists;
kd_tree->radiusSearch(*source_pc, i, icp_radius, inds, dists);
for(uint32_t j=0;j<inds.size();j++)
target_inds[inds[j]] = true;
if(inds.size() > 0)
source_inds[i] = true;
}
for(uint32_t i=0;i<target_pc->points.size();i++)
if(target_inds[i])
target_inds_list->push_back(i);
for(uint32_t i=0;i<source_pc->points.size();i++)
if(source_inds[i])
source_inds_list->push_back(i);
}
void createPriorCloud(const PCRGB& in_pc, const vector<hrl_phri_2011::ForceProcessed::Ptr>& fps, PCRGB& out_pc)
{
double prior_sigma;
ros::param::param<double>("~prior_sigma", prior_sigma, 0.01);
pcl::KdTreeFLANN<PRGB> kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree.setInputCloud(in_pc.makeShared());
vector<int> inds1(1), inds2;
vector<float> dists1(1), dists2;
vector<double> pdf(in_pc.size());
double normal, Z = 0;
for(uint32_t i=0;i<fps.size();i++) {
PRGB pt;
pt.x = fps[i]->tool_frame.transform.translation.x;
pt.y = fps[i]->tool_frame.transform.translation.y;
pt.z = fps[i]->tool_frame.transform.translation.z;
inds1[0] = -1;
kd_tree.nearestKSearch(pt, 1, inds1, dists1);
if(inds1[0] == -1)
continue;
inds2.clear(); dists2.clear();
kd_tree.radiusSearch(pt, 3 * prior_sigma, inds2, dists2);
for(size_t j=0;j<inds2.size();j++) {
normal = NORMAL(dists2[j], prior_sigma);
pdf[inds2[j]] += normal;
Z += normal;
}
}
colorPCHSL(in_pc, pdf, out_pc, 0);
}
void marginalEllipsoid(const PCRGB& pc, const vector<hrl_phri_2011::ForceProcessed::Ptr>& fps,
const PCRGB& fpc, PCRGB& pc_ell, double sigma)
{
pcl::KdTreeFLANN<PRGB> kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree.setInputCloud(fpc.makeShared());
double h, s, l, normal, normal_sum, val_sum, val;
int k = 5;
vector<int> finds;
vector<float> fdists;
for(uint32_t i=0;i<pc.points.size();i++) {
extractHSL(pc_ell.points[i].rgb, h, s, l);
finds.resize(k, 0); fdists.resize(k, 10000);
kd_tree.nearestKSearch(pc, i, 1, finds, fdists);
normal_sum = 0; val_sum = 0;
for(uint32_t j=0;j<finds.size();j++) {
if(sqrt(fdists[j]) > sigma * 3)
continue;
if(fps[finds[j]]->force_normal < 0.2)
continue;
normal = NORMAL(fdists[j], sigma);
val_sum += normal * fps[finds[j]]->force_normal;
normal_sum += normal;
}
if(normal_sum == 0)
continue;
val = val_sum / normal_sum;
s = 20 * val;
if(s < 0)
s = 0;
if(s > 100)
s = 100;
writeHSL(0, s, l, pc_ell.points[i].rgb);
}
}
int main(int argc, char* argv[])
{
int nt, nd;
float dist;
float **points, *point;
kd_node tree, near;
sf_file inp, out;
sf_init(argc,argv);
inp = sf_input("in");
out = sf_output("out");
if (SF_FLOAT != sf_gettype(inp)) sf_error("Need float input");
if (!sf_histint(inp,"n1",&nd)) sf_error("No n1= in input");
if (!sf_histint(inp,"n2",&nt)) sf_error("No n2= in input");
sf_putint(out,"n2",1);
points = sf_floatalloc2(nd,nt);
sf_floatread(points[0],nd*nt,inp);
tree = kd_tree(points,nt,nd);
point = sf_floatalloc(nd);
if (!sf_getfloats("point",point,nd)) sf_error("Need point=");
dist = SF_HUGE;
kd_nearest(tree, point, 0, nd, &near, &dist);
sf_floatwrite(kd_coord(near),nd,out);
exit(0);
}
void sphereTrim(const PCRGB::Ptr& pc_in, PCRGB::Ptr& pc_out, uint32_t ind, double radius)
{
KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree->setInputCloud(pc_in);
vector<int> inds;
vector<float> dists;
kd_tree->radiusSearch(*pc_in, ind, radius, inds, dists);
for(uint32_t j=0;j<inds.size();j++)
COPY_PT_INTO_CLOUD(pc_in, pc_out, inds[j]);
}
int main(int argc, char ** argv)
{
if(argc < 3) {
cerr << "Usage: " << argv[0] << " sample_data.csv kdtree.log"<< endl;
exit(-1);
}
vector<Point> sample_points;
import_data(argv[1], sample_points);
KDTree kd_tree(sample_points);
kd_tree.save(argv[2]);
return 0;
}
void projectEllipsoidDense(Ellipsoid& ell, double ell_height, const PCRGB& pc,
int num_lat, int num_lon, double sigma, int k)
{
pcl::KdTreeFLANN<PRGB> kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree.setInputCloud(pc.makeShared());
PCRGB pc_dense;
vector<int> inds;
vector<float> dists;
double h, s, l, h_sum, s_sum, l_sum, normal, normal_sum;
double x, y, z;
double lat = 0, lon = 0;
for(int i=0;i<num_lat;i++) {
lat += PI / num_lat;
lon = 0;
for(int j=0;j<num_lon;j++) {
lon += 2 * PI / num_lon;
PRGB pt;
ell.ellipsoidalToCart(lat, lon, ell_height, x, y, z);
pt.x = x; pt.y = y; pt.z = z;
inds.clear();
dists.clear();
kd_tree.nearestKSearch(pt, k, inds, dists);
normal_sum = 0; h_sum = 0; s_sum = 0; l_sum = 0;
for(uint32_t inds_i=0;inds_i<inds.size();inds_i++) {
extractHSL(pc.points[inds[inds_i]].rgb, h, s, l);
normal = NORMAL(dists[inds_i], sigma);
normal_sum += normal;
h_sum += normal * h; s_sum += normal * s; l_sum += normal * l;
}
writeHSL(h_sum / normal_sum, s_sum / normal_sum, l_sum / normal_sum, pt.rgb);
pc_dense.points.push_back(pt);
}
}
pc_dense.header.frame_id = "/base_link";
pubLoop(pc_dense, "/proj_head");
}
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
VolMagick::Volume signedDistanceFunction(const boost::shared_ptr<Geometry>& geom,
const VolMagick::Dimension& dim,
const VolMagick::BoundingBox& bbox)
{
int dimx = dim[0], dimy = dim[1], dimz = dim[2];
// read the annotated input file and
Mesh mesh;
cerr << "Reading input mesh ";
read_labeled_mesh(mesh, geom);
cerr << "done." << endl;
// build a bounding box around the input and store the
// origin, span etc.
// vector<double> bbox;
// construct_bbox(mesh, bbox);
VolMagick::BoundingBox box(bbox);
if(box.isNull())
{
geom->GetReadyToDrawWire(); //make sure we have calculated extents for this geometry already
box[0] = geom->m_Min[0];
box[1] = geom->m_Min[1];
box[2] = geom->m_Min[2];
box[3] = geom->m_Max[0];
box[4] = geom->m_Max[1];
box[5] = geom->m_Max[2];
}
// construct a kd-tree of all the non-isolated mesh_vertices.
vector<VECTOR3> points;
vector<Point> pts;
for(int i = 0; i < mesh.get_nv(); i ++)
{
if( mesh.vert_list[i].iso() ) continue;
Point p = mesh.vert_list[i].point();
pts.push_back(p);
points.push_back(VECTOR3(CGAL::to_double(p.x()),
CGAL::to_double(p.y()),
CGAL::to_double(p.z())));
}
KdTree kd_tree(points, 20);
kd_tree.setNOfNeighbours(1);
// Now perform a reconstruction to build a tetrahedralized solid
// with in-out marked.
Triangulation triang;
recon(pts, triang);
// assign weight to each triangle.
vector<double> weights;
// assign_sdf_weight(mesh, weights); // comment out for uniform weight.
VolMagick::Volume vol;
cerr << "SDF " << endl;
try
{
vol.dimension(VolMagick::Dimension(dimx,dimy,dimz));
vol.voxelType(VolMagick::Float);
vol.boundingBox(box);
for(unsigned int k=0; k<vol.ZDim(); k++)
{
for(unsigned int j=0; j<vol.YDim(); j++)
{
for(unsigned int i=0; i<vol.XDim(); i++)
{
double x = vol.XMin() + i*vol.XSpan();
double y = vol.YMin() + j*vol.YSpan();
double z = vol.ZMin() + k*vol.ZSpan();
double fn_val = sdf(Point(x,y,z), mesh, weights, kd_tree, triang);
vol(i,j,k, fn_val);
}
}
fprintf(stderr,
"%5.2f %%\r",
(float(k)/float(vol.ZDim()-1))*100.0);
}
vol.desc("multi_sdf");
}
catch(VolMagick::Exception &e)
{
cerr << e.what() << endl;
}
catch(std::exception &e)
{
cerr << e.what() << endl;
}
cerr << endl << "done." << endl;
return vol;
}
void
NearestNodeLocator::findNodes()
{
Moose::perf_log.push("NearestNodeLocator::findNodes()", "Execution");
/**
* If this is the first time through we're going to build up a "neighborhood" of nodes
* surrounding each of the slave nodes. This will speed searching later.
*/
const std::map<dof_id_type, std::vector<dof_id_type>> & node_to_elem_map = _mesh.nodeToElemMap();
if (_first)
{
_first = false;
// Trial slave nodes are all the nodes on the slave side
// We only keep the ones that are either on this processor or are likely
// to interact with elements on this processor (ie nodes owned by this processor
// are in the "neighborhood" of the slave node
std::vector<dof_id_type> trial_slave_nodes;
std::vector<dof_id_type> trial_master_nodes;
// Build a bounding box. No reason to consider nodes outside of our inflated BB
std::unique_ptr<BoundingBox> my_inflated_box = nullptr;
const std::vector<Real> & inflation = _mesh.getGhostedBoundaryInflation();
// This means there was a user specified inflation... so we can build a BB
if (inflation.size() > 0)
{
BoundingBox my_box = MeshTools::create_local_bounding_box(_mesh);
Point distance;
for (unsigned int i = 0; i < inflation.size(); ++i)
distance(i) = inflation[i];
my_inflated_box =
libmesh_make_unique<BoundingBox>(my_box.first - distance, my_box.second + distance);
}
// Data structures to hold the boundary nodes
ConstBndNodeRange & bnd_nodes = *_mesh.getBoundaryNodeRange();
for (const auto & bnode : bnd_nodes)
{
BoundaryID boundary_id = bnode->_bnd_id;
dof_id_type node_id = bnode->_node->id();
// If we have a BB only consider saving this node if it's in our inflated BB
if (!my_inflated_box || (my_inflated_box->contains_point(*bnode->_node)))
{
if (boundary_id == _boundary1)
trial_master_nodes.push_back(node_id);
else if (boundary_id == _boundary2)
trial_slave_nodes.push_back(node_id);
}
}
// Convert trial master nodes to a vector of Points. This will be used to
// construct the Kdtree.
std::vector<Point> master_points(trial_master_nodes.size());
for (unsigned int i = 0; i < trial_master_nodes.size(); ++i)
{
const Node & node = _mesh.nodeRef(trial_master_nodes[i]);
master_points[i] = node;
}
// Create object kd_tree of class KDTree using the coordinates of trial
// master nodes.
KDTree kd_tree(master_points, _mesh.getMaxLeafSize());
NodeIdRange trial_slave_node_range(trial_slave_nodes.begin(), trial_slave_nodes.end(), 1);
SlaveNeighborhoodThread snt(
_mesh, trial_master_nodes, node_to_elem_map, _mesh.getPatchSize(), kd_tree);
Threads::parallel_reduce(trial_slave_node_range, snt);
_slave_nodes = snt._slave_nodes;
_neighbor_nodes = snt._neighbor_nodes;
// If 'iteration' patch update strategy is used, a second neighborhood
// search using the ghosting_patch_size, which is larger than the regular
// patch_size used for contact search, is conducted. The ghosted element set
// given by this search is used for ghosting the elements connected to the
// slave and neighboring master nodes.
if (_patch_update_strategy == Moose::Iteration)
{
SlaveNeighborhoodThread snt_ghosting(
_mesh, trial_master_nodes, node_to_elem_map, _mesh.getGhostingPatchSize(), kd_tree);
Threads::parallel_reduce(trial_slave_node_range, snt_ghosting);
for (const auto & dof : snt_ghosting._ghosted_elems)
_subproblem.addGhostedElem(dof);
}
else
{
for (const auto & dof : snt._ghosted_elems)
_subproblem.addGhostedElem(dof);
}
// Cache the slave_node_range so we don't have to build it each time
_slave_node_range = new NodeIdRange(_slave_nodes.begin(), _slave_nodes.end(), 1);
}
_nearest_node_info.clear();
NearestNodeThread nnt(_mesh, _neighbor_nodes);
Threads::parallel_reduce(*_slave_node_range, nnt);
_max_patch_percentage = nnt._max_patch_percentage;
_nearest_node_info = nnt._nearest_node_info;
if (_patch_update_strategy == Moose::Iteration)
{
// Get the set of elements that are currently being ghosted
std::set<dof_id_type> ghost = _subproblem.ghostedElems();
for (const auto & node_id : *_slave_node_range)
{
const Node * nearest_node = _nearest_node_info[node_id]._nearest_node;
// Check if the elements attached to the nearest node are within the ghosted
// set of elements. If not produce an error.
auto node_to_elem_pair = node_to_elem_map.find(nearest_node->id());
if (node_to_elem_pair != node_to_elem_map.end())
{
const std::vector<dof_id_type> & elems_connected_to_node = node_to_elem_pair->second;
for (const auto & dof : elems_connected_to_node)
if (std::find(ghost.begin(), ghost.end(), dof) == ghost.end() &&
_mesh.elemPtr(dof)->processor_id() != _mesh.processor_id())
mooseError("Error in NearestNodeLocator : The nearest neighbor lies outside the "
"ghosted set of elements. Increase the ghosting_patch_size parameter in the "
"mesh block and try again.");
}
}
}
Moose::perf_log.pop("NearestNodeLocator::findNodes()", "Execution");
}
void
NearestNodeLocator::updatePatch(std::vector<dof_id_type> & slave_nodes)
{
Moose::perf_log.push("NearestNodeLocator::updatePatch()", "Execution");
std::vector<dof_id_type> trial_master_nodes;
// Build a bounding box. No reason to consider nodes outside of our inflated BB
std::unique_ptr<BoundingBox> my_inflated_box = nullptr;
const std::vector<Real> & inflation = _mesh.getGhostedBoundaryInflation();
// This means there was a user specified inflation... so we can build a BB
if (inflation.size() > 0)
{
BoundingBox my_box = MeshTools::create_local_bounding_box(_mesh);
Point distance;
for (unsigned int i = 0; i < inflation.size(); ++i)
distance(i) = inflation[i];
my_inflated_box =
libmesh_make_unique<BoundingBox>(my_box.first - distance, my_box.second + distance);
}
// Data structures to hold the boundary nodes
ConstBndNodeRange & bnd_nodes = *_mesh.getBoundaryNodeRange();
for (const auto & bnode : bnd_nodes)
{
BoundaryID boundary_id = bnode->_bnd_id;
dof_id_type node_id = bnode->_node->id();
// If we have a BB only consider saving this node if it's in our inflated BB
if (!my_inflated_box || (my_inflated_box->contains_point(*bnode->_node)))
{
if (boundary_id == _boundary1)
trial_master_nodes.push_back(node_id);
}
}
// Convert trial master nodes to a vector of Points. This will be used to construct the KDTree.
std::vector<Point> master_points(trial_master_nodes.size());
for (unsigned int i = 0; i < trial_master_nodes.size(); ++i)
{
const Node & node = _mesh.nodeRef(trial_master_nodes[i]);
master_points[i] = node;
}
const std::map<dof_id_type, std::vector<dof_id_type>> & node_to_elem_map = _mesh.nodeToElemMap();
// Create object kd_tree of class KDTree using the coordinates of trial
// master nodes.
KDTree kd_tree(master_points, _mesh.getMaxLeafSize());
NodeIdRange slave_node_range(slave_nodes.begin(), slave_nodes.end(), 1);
SlaveNeighborhoodThread snt(
_mesh, trial_master_nodes, node_to_elem_map, _mesh.getPatchSize(), kd_tree);
Threads::parallel_reduce(slave_node_range, snt);
// Calculate new ghosting patch for the slave_node_range
SlaveNeighborhoodThread snt_ghosting(
_mesh, trial_master_nodes, node_to_elem_map, _mesh.getGhostingPatchSize(), kd_tree);
Threads::parallel_reduce(slave_node_range, snt_ghosting);
// Add the new set of elements that need to be ghosted into _new_ghosted_elems
for (const auto & dof : snt_ghosting._ghosted_elems)
_new_ghosted_elems.push_back(dof);
std::vector<dof_id_type> tracked_slave_nodes = snt._slave_nodes;
// Update the neighbor nodes (patch) for these tracked slave nodes
for (const auto & node_id : tracked_slave_nodes)
_neighbor_nodes[node_id] = snt._neighbor_nodes[node_id];
NodeIdRange tracked_slave_node_range(tracked_slave_nodes.begin(), tracked_slave_nodes.end(), 1);
NearestNodeThread nnt(_mesh, snt._neighbor_nodes);
Threads::parallel_reduce(tracked_slave_node_range, nnt);
_max_patch_percentage = nnt._max_patch_percentage;
// Get the set of elements that are currently being ghosted
std::set<dof_id_type> ghost = _subproblem.ghostedElems();
// Update the nearest node information corresponding to these tracked slave nodes
for (const auto & node_id : tracked_slave_node_range)
{
_nearest_node_info[node_id] = nnt._nearest_node_info[node_id];
// Check if the elements attached to the nearest node are within the ghosted
// set of elements. If not produce an error.
const Node * nearest_node = nnt._nearest_node_info[node_id]._nearest_node;
auto node_to_elem_pair = node_to_elem_map.find(nearest_node->id());
if (node_to_elem_pair != node_to_elem_map.end())
{
const std::vector<dof_id_type> & elems_connected_to_node = node_to_elem_pair->second;
for (const auto & dof : elems_connected_to_node)
if (std::find(ghost.begin(), ghost.end(), dof) == ghost.end() &&
_mesh.elemPtr(dof)->processor_id() != _mesh.processor_id())
mooseError("Error in NearestNodeLocator : The nearest neighbor lies outside the ghosted "
"set of elements. Increase the ghosting_patch_size parameter in the mesh "
"block and try again.");
}
}
Moose::perf_log.pop("NearestNodeLocator::updatePatch()", "Execution");
}
