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 Q2Q1PFactory<Scalar, LocalOrdinal, GlobalOrdinal, Node>::BuildP(Level& fineLevel, Level& coarseLevel) const { FactoryMonitor m(*this, "Build", coarseLevel); RCP<Matrix> A = Get< RCP<Matrix> >(fineLevel, "A"); RCP<const Map> rowMap = A->getRowMap(); Xpetra::global_size_t N = rowMap->getGlobalNumElements(); int V; size_t n = as<size_t>(sqrt(N)); if (N == n*n) { // pressure mode V = 1; GetOStream(Runtime1) << "Pressure mode" << std::endl; } else { n = as<size_t>(sqrt(N/2)); if (N == 2*n*n) { // velocity mode V = 2; GetOStream(Runtime1) << "Velocity mode" << std::endl; } else { throw Exceptions::RuntimeError("Matrix size (" + toString(N) + ") is incompatible with both velocity and pressure"); } } const int C = 4; Xpetra::global_size_t nc = (n-1)/C + 1; TEUCHOS_TEST_FOR_EXCEPTION(C*(nc-1)+1 != n, Exceptions::InvalidArgument, "Incorrect dim size: " << n); ArrayView<const GO> elementList = rowMap->getNodeElementList(); GO indexBase = rowMap->getIndexBase(); // Calculate offsets GO offset = (V == 2 ? 0 : 2*(2*n -1)*(2*n -1)); GO coarseOffset = (V == 2 ? 0 : 2*(2*nc-1)*(2*nc-1)); GetOStream(Runtime1) << "offset = " << offset << ", coarseOffset = " << coarseOffset << std::endl; Array<GO> coarseList; for (LO k = 0; k < elementList.size(); k += V) { GO GID = elementList[k] - offset - indexBase; GO i = (GID / V) % n, ii = i/C; GO j = (GID / V) / n, jj = j/C; if (i % C == 0 && j % C == 0) for (int q = 0; q < V; q++) coarseList.push_back(V*(jj*nc + ii) + q + coarseOffset); } typedef Teuchos::ScalarTraits<SC> STS; SC one = STS::one(); Xpetra::global_size_t INVALID = Teuchos::OrdinalTraits<Xpetra::global_size_t>::invalid(); std::vector<size_t> stridingInfo(1,1); const int stridedBlockId = -1; RCP<Map> coarseMap = StridedMapFactory ::Build(rowMap->lib(), INVALID, coarseList, indexBase, stridingInfo, rowMap->getComm(), stridedBlockId, coarseOffset); RCP<MultiVector> coarseNullspace = MultiVectorFactory::Build(coarseMap, 1); coarseNullspace->putScalar(one); int nnzEstimate = 4; RCP<Matrix> P = MatrixFactory::Build(rowMap, coarseMap, nnzEstimate); Array<GO> inds(nnzEstimate), inds1(nnzEstimate); Array<SC> vals(nnzEstimate, one); int sz; for (LO k = 0; k < elementList.size(); k += V) { GO GID = elementList[k] - offset - indexBase; GO i = (GID/V) % n, ii = i/C; GO j = (GID/V) / n, jj = j/C; if (i % C == 0 && j % C == 0) { sz = 1; inds[0] = jj *nc + ii ; } else if (i % C == 0 && j % C != 0) { sz = 2; inds[0] = jj *nc + ii ; inds[1] = (jj+1)*nc + ii ; } else if (i % C != 0 && j % C == 0) { sz = 2; inds[0] = jj *nc + ii ; inds[1] = jj *nc + ii+1; } else { sz = 4; inds[0] = jj *nc + ii ; inds[1] = jj *nc + ii+1; inds[2] = (jj+1)*nc + ii ; inds[3] = (jj+1)*nc + ii+1; } for (int q = 0; q < V; q++) { for (int p = 0; p < sz; p++) inds1[p] = V*inds[p]+q + coarseOffset; P->insertGlobalValues(elementList[k]+q, inds1.view(0,sz), vals.view(0,sz)); } } P->fillComplete(coarseMap, A->getDomainMap()); // Level Set Set(coarseLevel, "Nullspace", coarseNullspace); Set(coarseLevel, "P", P); Set(fineLevel, "CoarseMap", coarseMap); if (IsPrint(Statistics1)) { RCP<ParameterList> params = rcp(new ParameterList()); params->set("printLoadBalancingInfo", true); params->set("printCommInfo", true); GetOStream(Statistics1) << PerfUtils::PrintMatrixInfo(*P, "P", params); } }