void MsgProcessor::append( msg_ptr msg ) { if( QThread::currentThread() != thread() ) { qDebug() << "reinvoking msgprocessor::append in correct thread, ie not" << QThread::currentThread(); QMetaObject::invokeMethod( this, "append", Qt::QueuedConnection, Q_ARG(msg_ptr, msg) ); return; } m_msgs.append( msg ); m_msg_ready.insert( msg.data(), false ); m_totmsgsize += msg->payload().length(); if( m_mode & NOTHING ) { //qDebug() << "MsgProcessor::NOTHING"; handleProcessedMsg( msg ); return; } QFuture<msg_ptr> fut = QtConcurrent::run(&MsgProcessor::process, msg, m_mode, m_threshold); QFutureWatcher<msg_ptr> * watcher = new QFutureWatcher<msg_ptr>; connect( watcher, SIGNAL( finished() ), this, SLOT( processed() ), Qt::QueuedConnection ); watcher->setFuture( fut ); }
bool GuassianBlur::process(cv::Mat image) { cv::Mat out; cv::GaussianBlur(image, out, cv::Size(settingsWidget->xKernel,settingsWidget->yKernel),0,0); emit processed(out); return true; }
void csPdfSearch::progressUpdate() { const int p = qBound(0, _cntDone * 100 / _numToDo, 100); if( p != _lastProgress ) { emit processed(p); } _lastProgress = p; }
shared_ptr<Epetra_CrsMatrix> sparseCholesky(const Epetra_CrsMatrix &mat) { // Note: we assume the matrix mat is symmetric and positive-definite size_t size = mat.NumGlobalCols(); if (mat.NumGlobalRows() != size) throw std::invalid_argument("sparseCholesky(): matrix must be square"); int *rowOffsets = 0; int *colIndices = 0; double *values = 0; mat.ExtractCrsDataPointers(rowOffsets, colIndices, values); Epetra_SerialComm comm; Epetra_LocalMap rowMap(static_cast<int>(size), 0 /* index_base */, comm); Epetra_LocalMap columnMap(static_cast<int>(size), 0 /* index_base */, comm); shared_ptr<Epetra_CrsMatrix> result = boost::make_shared<Epetra_CrsMatrix>( Copy, rowMap, columnMap, mat.GlobalMaxNumEntries()); arma::Mat<double> localMat; arma::Mat<double> localCholesky; std::vector<bool> processed(size, false); for (size_t r = 0; r < size; ++r) { if (processed[r]) continue; int localSize = rowOffsets[r + 1] - rowOffsets[r]; localMat.set_size(localSize, localSize); localMat.fill(0.); localCholesky.set_size(localSize, localSize); for (int s = 0; s < localSize; ++s) { int row = colIndices[rowOffsets[r] + s]; for (int c = 0; c < localSize; ++c) { int col = colIndices[rowOffsets[row] + c]; if (col != colIndices[rowOffsets[r] + c]) throw std::invalid_argument("sparseCholesky(): matrix is not " "block-diagonal"); localMat(s, c) = values[rowOffsets[row] + c]; } } assert(arma::norm(localMat - localMat.t(), "fro") < 1e-12 * arma::norm(localMat, "fro")); localCholesky = arma::chol(localMat); // localCholesky: U for (int s = 0; s < localSize; ++s) { int row = colIndices[rowOffsets[r] + s]; processed[row] = true; #ifndef NDEBUG int errorCode = #endif result->InsertGlobalValues(row, s + 1 /* number of values */, localCholesky.colptr(s), colIndices + rowOffsets[r]); assert(errorCode == 0); } } result->FillComplete(columnMap, rowMap); return result; }
void WorkspaceDevPurgeWorker::run() { QRegExp FilterRegExp(m_BuildDirRegexStr); for (auto& WType : m_Selection) { for (auto& WItem : WType.second) { writeMessage(); writeMessage("====== "+WItem.ID+" ======"); QDir Dir(WItem.Path); Dir.setFilter(QDir::Dirs | QDir::NoDotAndDotDot); QFileInfoList SubDirs = Dir.entryInfoList(); for (auto SubDir : SubDirs) { if (FilterRegExp.exactMatch(SubDir.fileName())) { openfluid::tools::Filesystem::removeDirectory(SubDir.absoluteFilePath().toStdString()); if (openfluid::tools::Filesystem::isDirectory(SubDir.absoluteFilePath().toStdString())) { writeMessage(SubDir.fileName() + ": <font style='color: red;'>"+tr("Error")+"</font>"); emit processed(WType.first,WItem.ID,"purge", false); } else { writeMessage(SubDir.fileName() + ": <font style='color: green;'>"+tr("Deleted")+"</font>"); emit processed(WType.first,WItem.ID,"purge", true); } } } writeMessage(); emit wareCompleted(); } } emit finished(); }
void GTcpFlowMgr::process(GPacket* packet) { long now = packet->ts_.tv_sec; if (checkInterval_ != 0 && now - lastCheckTick_ >= checkInterval_) { deleteOldFlowMaps(packet); lastCheckTick_ = now; } GIpHdr* ipHdr = packet->pdus_.findFirst<GIpHdr>(); if (ipHdr == nullptr) return; GTcpHdr* tcpHdr = packet->pdus_.findFirst<GTcpHdr>(); if (tcpHdr == nullptr) return; GFlow::TcpFlowKey key{ipHdr->sip(), tcpHdr->sport(), ipHdr->dip(), tcpHdr->dport()}; FlowMap::iterator it = flowMap_.find(key); if (it == flowMap_.end()) { GFlow::Value* value = GFlow::Value::allocate(packet->ts_, GFlow::Value::Half, requestItems_.totalMemSize_); it = flowMap_.insert(key, value); key_ = const_cast<GFlow::TcpFlowKey*>(&it.key()); value_ = value; emit _flowCreated(packet); GFlow::TcpFlowKey reverseKey = GFlow::TcpFlowKey(it.key()).reverse(); FlowMap::iterator reverseIt = flowMap_.find(reverseKey); if (reverseIt != flowMap_.end()) { it.value()->state_ = GFlow::Value::Full; reverseIt.value()->state_ = GFlow::Value::Full; } } else { GFlow::Value* value = it.value(); value->ts_ = packet->ts_; } if ((tcpHdr->flags() & (TH_RST | TH_FIN)) != 0) { GFlow::Value* value = it.value(); GFlow::Value::State state = (tcpHdr->flags() & TH_RST) ? GFlow::Value::Rst : GFlow::Value::Fin; value->state_ = state; GFlow::TcpFlowKey reverseKey = GFlow::TcpFlowKey(it.key()).reverse(); FlowMap::iterator reverseIt = flowMap_.find(reverseKey); if (reverseIt != flowMap_.end()) { reverseIt.value()->state_ = state; } } this->key_ = const_cast<GFlow::TcpFlowKey*>(&it.key()); this->value_ = it.value(); emit processed(packet); }
void PlotBuilderWorker::process() { if(_function != NULL) { for(_valueCurrent; (_valueCurrent <= _valueTo) && _manager->isProcessing(); _valueCurrent += _step) { // QThread::msleep(10); // emulate slowness :) double y = _function->calculate(_valueCurrent); emit processed(_valueCurrent, y, getProgress()); } if (_manager->isProcessing()) { emit finished(); } } }
const MonoBuffer& ColorDetector::process(ColorBuffer& image) { if (m_outBuffer.size() != image.size()) { m_outBuffer = MonoBuffer(image.size()); } ColorBuffer::const_iterator itIn = image.begin(); ColorBuffer::const_iterator itInEnd = image.end(); MonoBuffer::iterator itOut = m_outBuffer.begin(); MonoBuffer::iterator itOutEnd = m_outBuffer.end(); for ( ;itIn != itInEnd && itOut != itOutEnd; ++itIn, ++itOut) { if (getDistance(*itIn) > m_threshold) { *itOut = 0xFF; } else { *itOut = 0; } } emit processed(m_outBuffer); return m_outBuffer; }
char *get_status ( int hosts_size ) { json_lib::Status json_val; std::map<std::string, int> total_status; std::map<std::string, std::map<std::string, int> > status; std::string processed ("processed_items"); std::string synced ("synced_items"); total_status[processed] = * ( uint64_t * ) total_status_addr; total_status[synced] = * ( uint64_t * ) ( total_status_addr + sizeof (uint64_t ) ); for ( int i = 0; i < hosts_size; i ++ ) { std::map<std::string, int> single_status; single_status[processed] = * ( uint64_t * ) total_status_addrs[i]; single_status[synced] = * ( uint64_t * ) ( total_status_addrs[i] + sizeof (uint64_t ) ); status[hosts[i]] = single_status; } json_val.total_status = total_status; json_val.status = status; std::string res; if ( ! json_lib::Serialize (json_val, res) ) { return NULL; } size_t status_size = res.size (); char *status_addr = ( char * ) malloc (status_size + 1); if ( ! status_addr ) { return NULL; } memcpy (status_addr, res.c_str (), status_size); status_addr[status_size] = '\0'; return status_addr; }
void UpdateManager::replyFinished(QNetworkReply* reply) { // finished after this m_finished = true; if (reply){ // don't delete here reply->deleteLater(); m_error = (reply->error() != QNetworkReply::NoError); if (!m_error){ // create xml document to read the response QDomDocument document; document.setContent(reply->readAll()); QDomNodeList openstudioelements = document.elementsByTagName("openstudio"); if (openstudioelements.size() > 0) { // Only process the first one for now QDomNodeList nodes = openstudioelements.at(0).childNodes(); // all child nodes will be releases for(int i = 0; i < nodes.size(); ++i){ QDomElement release = nodes.at(i).toElement(); if (!release.isNull()){ if (!checkRelease(release)){ // break if not newer than current break; } } } } }else{ LOG(Error, "QNetworkReply " << reply->error()); } } emit processed(); }
void planning_models::KinematicModel::buildGroups(const std::vector<GroupConfig>& group_configs) { //the only thing tricky is dealing with subgroups std::vector<bool> processed(group_configs.size(), false); while(true) { //going to make passes until we can't do anything else bool added = false; for(unsigned int i = 0; i < group_configs.size(); i++) { if(!processed[i]) { //if we haven't processed, check and see if the dependencies are met yet bool all_subgroups_added = true; for(unsigned int j = 0; j < group_configs[i].subgroups_.size(); j++) { if(joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end()) { all_subgroups_added = false; break; } } if(all_subgroups_added) { //only get one chance to do it right if(addModelGroup(group_configs[i])) { processed[i] = true; added = true; } else { ROS_WARN_STREAM("Failed to add group " << group_configs[i].name_); } } } } if(!added) { for(unsigned int i = 0; i < processed.size(); i++) { if(!processed[i]) { ROS_WARN_STREAM("Could not process group " << group_configs[i].name_ << " due to unmet subgroup dependencies"); } } return; } } }
void test_expression(const char* expression_string, const boost::function<adobe::array_t (const adobe::array_t&)>& process_func) { adobe::array_t expression; std::stringstream stream(expression_string); adobe::expression_parser(stream, adobe::line_position_t("expression_string")).require_expression(expression); std::cout << std::endl << "-=- testing expression -=- " << std::endl << std::endl; std::cout << "Raw token stream:" << std::endl; std::cout << adobe::begin_asl_cel_unsafe << expression << adobe::end_asl_cel_unsafe << std::endl; std::cout << "Round-trip expression:" << std::endl; std::cout << adobe::format_expression(expression) << std::endl; adobe::array_t processed(process_func(expression)); std::cout << "Raw Processed expression:" << std::endl; std::cout << adobe::begin_asl_cel_unsafe << processed << adobe::end_asl_cel_unsafe << std::endl; std::cout << "Processed expression:" << std::endl; std::cout << adobe::format_expression(processed) << std::endl; static adobe::virtual_machine_t vm; vm.evaluate(expression); std::cout << "VM Round-trip expression:" << std::endl; std::cout << adobe::begin_asl_cel_unsafe << vm.back().value_m << adobe::end_asl_cel_unsafe << std::endl; vm.pop_back(); vm.evaluate(processed); std::cout << "VM processed expression:" << std::endl; std::cout << adobe::begin_asl_cel_unsafe << vm.back().value_m << adobe::end_asl_cel_unsafe << std::endl; vm.pop_back(); }
bool get_status (int hosts_size, std::string& resp) { jos_lib::Status json_val; std::map<std::string, int> total_status; std::map<std::string, std::map<std::string, int> > status; std::string processed ("processed_items"); std::string synced ("synced_items"); total_status[processed] = * ( uint64_t * ) total_status_addr; total_status[synced] = * ( uint64_t * ) ( total_status_addr + sizeof (uint64_t ) ); for ( int i = 0; i < hosts_size; i ++ ) { std::map<std::string, int> single_status; single_status[processed] = * ( uint64_t * ) total_status_addrs[i]; single_status[synced] = * ( uint64_t * ) ( total_status_addrs[i] + sizeof (uint64_t ) ); status[hosts[i]] = single_status; } json_val.total_status = total_status; json_val.status = status; return jos_lib::Serialize <jos_lib::Status, true> (json_val, resp); }
void FxChannel::doProcessing() { const fpp_t fpp = Engine::mixer()->framesPerPeriod(); const bool exporting = Engine::getSong()->isExporting(); if( m_muted == false ) { for( FxRoute * senderRoute : m_receives ) { FxChannel * sender = senderRoute->sender(); FloatModel * sendModel = senderRoute->amount(); if( ! sendModel ) qFatal( "Error: no send model found from %d to %d", senderRoute->senderIndex(), m_channelIndex ); if( sender->m_hasInput || sender->m_stillRunning ) { // figure out if we're getting sample-exact input ValueBuffer * sendBuf = sendModel->valueBuffer(); ValueBuffer * volBuf = sender->m_volumeModel.valueBuffer(); // mix it's output with this one's output sampleFrame * ch_buf = sender->m_buffer; // use sample-exact mixing if sample-exact values are available if( ! volBuf && ! sendBuf ) // neither volume nor send has sample-exact data... { const float v = sender->m_volumeModel.value() * sendModel->value(); if( exporting ) { MixHelpers::addSanitizedMultiplied( m_buffer, ch_buf, v, fpp ); } else { MixHelpers::addMultiplied( m_buffer, ch_buf, v, fpp ); } } else if( volBuf && sendBuf ) // both volume and send have sample-exact data { if( exporting ) { MixHelpers::addSanitizedMultipliedByBuffers( m_buffer, ch_buf, volBuf, sendBuf, fpp ); } else { MixHelpers::addMultipliedByBuffers( m_buffer, ch_buf, volBuf, sendBuf, fpp ); } } else if( volBuf ) // volume has sample-exact data but send does not { const float v = sendModel->value(); if( exporting ) { MixHelpers::addSanitizedMultipliedByBuffer( m_buffer, ch_buf, v, volBuf, fpp ); } else { MixHelpers::addMultipliedByBuffer( m_buffer, ch_buf, v, volBuf, fpp ); } } else // vice versa { const float v = sender->m_volumeModel.value(); if( exporting ) { MixHelpers::addSanitizedMultipliedByBuffer( m_buffer, ch_buf, v, sendBuf, fpp ); } else { MixHelpers::addMultipliedByBuffer( m_buffer, ch_buf, v, sendBuf, fpp ); } } m_hasInput = true; } } const float v = m_volumeModel.value(); if( m_hasInput ) { // only start fxchain when we have input... m_fxChain.startRunning(); } m_stillRunning = m_fxChain.processAudioBuffer( m_buffer, fpp, m_hasInput ); float peakLeft = 0.; float peakRight = 0.; Engine::mixer()->getPeakValues( m_buffer, fpp, peakLeft, peakRight ); m_peakLeft = qMax( m_peakLeft, peakLeft * v ); m_peakRight = qMax( m_peakRight, peakRight * v ); } else { m_peakLeft = m_peakRight = 0.0f; } // increment dependency counter of all receivers processed(); }
void EuclideanClusterExtractorCurvature::segment(std::vector<pcl::PointIndices::Ptr> &segments) { //ptrdiff_t (*p_myrandom)(ptrdiff_t) = myrandom; //uncommnet srand if you want indices to be truely if(useSrand_) { srand(time(NULL)); } if(!treeSet_) { std::cerr<<"No KdTree was set...creating"<<std::endl; createTree(); } // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns //and indices[i] if(tree_->getInputCloud()->points.size() != cloud_->size()) { std::cerr<<"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (" << tree_->getInputCloud()->points.size() <<") than the input cloud ("<<cloud_->size()<<")!"<<std::endl; return; } if(!normalsSet_) { std::cerr<<"No Normals were set...calculating"<<std::endl; calculateNormals(); } if(cloud_->size() != normals_->points.size()) { std::cerr<<"[pcl::extractEuclideanClusters] Number of points in the input point cloud (" <<cloud_->size()<<") different than normals ("<<normals_->size()<<")!"<<std::endl; return; } // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed(cloud_->size(), false); //create a random copy of indices std::vector<int> indices_rnd(cloud_->size()); for(unsigned int i = 0; i < indices_rnd.size(); ++i) { indices_rnd[i] = i; } //uncommnet myrandom part if you want indices to be truely if(useSrand_) { std::random_shuffle(indices_rnd.begin(), indices_rnd.end(), myrandom); std::cerr<<"\"REAL\" RANDOM"<<std::endl; } else { std::random_shuffle(indices_rnd.begin(), indices_rnd.end()); } std::cerr << "Processed size: " << processed.size() << std::endl; std::vector<int> index_lookup(indices_rnd.size()); for(unsigned int i = 0; i < indices_rnd.size(); ++i) { index_lookup[indices_rnd[i]] = i; } std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector for(size_t i = 0; i < indices_rnd.size(); ++i) { if(processed[i] || normals_->points[indices_rnd[i]].curvature > maxCurvature_) { /*if(normals.points[indices_rnd[i]].curvature > max_curvature) std::cerr<<"Curvature of point skipped: "<<normals.points[indices_rnd[i]].curvature<<std::endl;*/ continue; } pcl::PointIndices::Ptr seed_queue(new pcl::PointIndices()); int sq_idx = 0; seed_queue->indices.push_back(indices_rnd[i]); processed[i] = true; while(sq_idx < (int)seed_queue->indices.size()) { // Search for sq_idx if(!tree_->radiusSearch(seed_queue->indices[sq_idx], distTolerance_, nn_indices, nn_distances)) { sq_idx++; continue; } for(size_t j = 1; j < nn_indices.size(); ++j) // nn_indices[0] should be sq_idx { // std::cerr<<nn_indices[j]<<std::endl; if(processed[index_lookup[nn_indices[j]]]) // Has this point been processed before ? { continue; } // [-1;1] double dot_p = normals_->points[indices_rnd[i]].normal[0] * normals_->points[nn_indices[j]].normal[0] + normals_->points[indices_rnd[i]].normal[1] * normals_->points[nn_indices[j]].normal[1] + normals_->points[indices_rnd[i]].normal[2] * normals_->points[nn_indices[j]].normal[2]; if(fabs(acos(dot_p)) < eps_angle_) { processed[index_lookup[nn_indices[j]]] = true; seed_queue->indices.push_back(nn_indices[j]); } } sq_idx++; } // If this queue is satisfactory, add to the clusters if(seed_queue->indices.size() >= minPts_ && seed_queue->indices.size() <= maxPts_) { seed_queue->header = cloud_->header; segments.push_back(seed_queue); } } int unprocessed_counter = 0; for(unsigned int i = 0; i < processed.size(); ++i) { if(processed[i] == false) { //std::cerr<<"Indice not processed at " <<i<<" : "<<indices_rnd[i]<<std::endl; unprocessed_counter++; } } std::cerr<<"Number of unprocessed indices: "<<unprocessed_counter<<std::endl; }
void SmoothEuclideanSegmenter<PointT>::segment() { size_t max_pts_per_cluster = std::numeric_limits<int>::max(); clusters_.clear(); CHECK (scene_->points.size() == normals_->points.size ()); if(!octree_ || octree_->getInputCloud() != scene_) {// create an octree for search octree_.reset( new pcl::octree::OctreePointCloudSearch<PointT> (param_.octree_resolution_ ) ); octree_->setInputCloud(scene_); octree_->addPointsFromInputCloud(); } // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (scene_->points.size (), false); std::vector<int> nn_indices; std::vector<float> nn_distances; float eps_angle_threshold_rad = pcl::deg2rad(param_.eps_angle_threshold_deg_); // Process all points in the indices vector for (size_t i = 0; i < scene_->points.size (); ++i) { if (processed[i] || !pcl::isFinite(scene_->points[i])) continue; std::vector<size_t> seed_queue; size_t sq_idx = 0; seed_queue.push_back (i); processed[i] = true; // this is used if planar surface extraction only is enabled Eigen::Vector3f avg_normal = normals_->points[i].getNormalVector3fMap(); Eigen::Vector3f avg_plane_pt = scene_->points[i].getVector3fMap(); while (sq_idx < seed_queue.size ()) { size_t sidx = seed_queue[sq_idx]; const PointT &query_pt = scene_->points[ sidx ]; const pcl::Normal &query_n = normals_->points[ sidx ]; if (normals_->points[ sidx ].curvature > param_.curvature_threshold_) { sq_idx++; continue; } // Search for sq_idx - scale radius with distance of point (due to noise) float radius = param_.cluster_tolerance_; float curvature_threshold = param_.curvature_threshold_; float eps_angle_threshold = eps_angle_threshold_rad; if ( param_.z_adaptive_ ) { radius = param_.cluster_tolerance_ * ( 1 + (std::max(query_pt.z, 1.f) - 1.f)); curvature_threshold = param_.curvature_threshold_ * ( 1 + (std::max(query_pt.z, 1.f) - 1.f)); eps_angle_threshold = eps_angle_threshold_rad * ( 1 + (std::max(query_pt.z, 1.f) - 1.f)); } if (!scene_->isOrganized() && !param_.force_unorganized_) { if(!octree_->radiusSearch (query_pt, radius, nn_indices, nn_distances)) { sq_idx++; continue; } } else // check pixel neighbors { int width = scene_->width; int height = scene_->height; int u = sidx%width; int v = sidx/width; nn_indices.resize(8); nn_distances.resize(8); size_t kept=0; for(int shift_u=-1; shift_u<=1; shift_u++) { int uu = u + shift_u; if ( uu < 0 || uu>=width) continue; for(int shift_v=-1; shift_v<=1; shift_v++) { int vv = v + shift_v; if ( vv < 0 || vv>=height) continue; int nn_idx = vv*width + uu; float dist = ( scene_->points[sidx].getVector3fMap() - scene_->points[nn_idx].getVector3fMap() ).norm(); if (dist < radius) { nn_indices[kept] = nn_idx; nn_distances[kept] = dist; kept++; } } } nn_indices.resize(kept); nn_distances.resize(kept); } for (size_t j = 0; j < nn_indices.size (); j++) { if ( processed[nn_indices[j]] ) // Has this point been processed before ? continue; if (normals_->points[nn_indices[j]].curvature > curvature_threshold) continue; Eigen::Vector3f n1; if(param_.compute_planar_patches_only_) n1 = avg_normal; else n1 = query_n.getNormalVector3fMap(); pcl::Normal nn = normals_->points[ nn_indices[j] ]; const Eigen::Vector3f &n2 = nn.getNormalVector3fMap(); double dot_p = n1.dot(n2); if (fabs (dot_p) > cos(eps_angle_threshold)) { if(param_.compute_planar_patches_only_) { const Eigen::Vector3f &nn_pt = scene_->points[ nn_indices[j] ].getVector3fMap(); float dist = fabs(avg_normal.dot(nn_pt - avg_plane_pt)); if(dist > param_.planar_inlier_dist_) continue; runningAverage( avg_normal, seed_queue.size(), n2 ); avg_normal.normalize(); runningAverage( avg_plane_pt, seed_queue.size(), nn_pt ); } processed[nn_indices[j]] = true; seed_queue.push_back (nn_indices[j]); } } sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= param_.min_points_ && seed_queue.size () <= max_pts_per_cluster) { std::vector<int> r; r.resize ( seed_queue.size () ); for (size_t j = 0; j < seed_queue.size (); ++j) r[j] = seed_queue[j]; std::sort (r.begin (), r.end ()); r.erase (std::unique (r.begin (), r.end ()), r.end ()); clusters_.push_back ( r ); // We could avoid a copy by working directly in the vector } } }
template<typename PointInT, typename PointNT, typename PointOutT> void pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth ( const pcl::PointCloud<pcl::PointNormal> &cloud, const pcl::PointCloud<pcl::PointNormal> &normals, float tolerance, const pcl::search::Search<pcl::PointNormal>::Ptr &tree, std::vector<pcl::PointIndices> &clusters, double eps_angle, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster) { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (cloud.points.size () != normals.points.size ()) { PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (cloud.points.size (), false); std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i) { if (processed[i]) continue; std::vector<unsigned int> seed_queue; int sq_idx = 0; seed_queue.push_back (i); processed[i] = true; while (sq_idx < static_cast<int> (seed_queue.size ())) { // Search for sq_idx if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) { sq_idx++; continue; } for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx { if (processed[nn_indices[j]]) // Has this point been processed before ? continue; //processed[nn_indices[j]] = true; // [-1;1] double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0] + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2]; if (fabs (acos (dot_p)) < eps_angle) { processed[nn_indices[j]] = true; seed_queue.push_back (nn_indices[j]); } } sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) { pcl::PointIndices r; r.indices.resize (seed_queue.size ()); for (size_t j = 0; j < seed_queue.size (); ++j) r.indices[j] = seed_queue[j]; std::sort (r.indices.begin (), r.indices.end ()); r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); r.header = cloud.header; clusters.push_back (r); // We could avoid a copy by working directly in the vector } } }
void pcl::seededHueSegmentation (const PointCloud<PointXYZRGB> &cloud, const boost::shared_ptr<search::Search<PointXYZRGB> > &tree, float tolerance, PointIndices &indices_in, PointIndices &indices_out, float delta_hue) { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (cloud.points.size (), false); std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector for (size_t k = 0; k < indices_in.indices.size (); ++k) { int i = indices_in.indices[k]; if (processed[i]) continue; processed[i] = true; std::vector<int> seed_queue; int sq_idx = 0; seed_queue.push_back (i); PointXYZRGB p; p = cloud.points[i]; PointXYZHSV h; PointXYZRGBtoXYZHSV(p, h); while (sq_idx < static_cast<int> (seed_queue.size ())) { int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max()); if(ret == -1) PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1"); // Search for sq_idx if (!ret) { sq_idx++; continue; } for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx { if (processed[nn_indices[j]]) // Has this point been processed before ? continue; PointXYZRGB p_l; p_l = cloud.points[nn_indices[j]]; PointXYZHSV h_l; PointXYZRGBtoXYZHSV(p_l, h_l); if (fabs(h_l.h - h.h) < delta_hue) { seed_queue.push_back (nn_indices[j]); processed[nn_indices[j]] = true; } } sq_idx++; } // Copy the seed queue into the output indices for (size_t l = 0; l < seed_queue.size (); ++l) indices_out.indices.push_back(seed_queue[l]); } // This is purely esthetical, can be removed for speed purposes std::sort (indices_out.indices.begin (), indices_out.indices.end ()); }
template <typename PointT> void pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud, const boost::shared_ptr<search::Search<PointT> > &tree, float tolerance, std::vector<std::vector<PointIndices> > &labeled_clusters, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster, unsigned int max_label) { if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", (unsigned long)tree->getInputCloud ()->points.size (), (unsigned long)cloud.points.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (cloud.points.size (), false); std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector for (size_t i = 0; i < cloud.points.size (); ++i) { if (processed[i]) continue; std::vector<int> seed_queue; int sq_idx = 0; seed_queue.push_back (i); processed[i] = true; while (sq_idx < (int)seed_queue.size ()) { // Search for sq_idx if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) { sq_idx++; continue; } for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx { if (processed[nn_indices[j]]) // Has this point been processed before ? continue; if (cloud.points[i].label == cloud.points[nn_indices[j]].label) { // Perform a simple Euclidean clustering seed_queue.push_back (nn_indices[j]); processed[nn_indices[j]] = true; } } sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) { pcl::PointIndices r; r.indices.resize (seed_queue.size ()); for (size_t j = 0; j < seed_queue.size (); ++j) r.indices[j] = seed_queue[j]; std::sort (r.indices.begin (), r.indices.end ()); r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); r.header = cloud.header; labeled_clusters[cloud.points[i].label].push_back (r); // We could avoid a copy by working directly in the vector } } }
void DenseReconstruction::activeSegmentation (const pcl::PointCloud<pcl::PointXYZLRegionF> &cloud_input, float search_radius, double eps_angle, int fp_index, pcl::PointIndices &indices_out) { pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::Boundary> boundary; normalsEstimation(cloud_input.makeShared(),normals); boundaryEstimation(cloud_input.makeShared(),normals,boundary); std::cerr<<"Index of seed point is: "<<fp_index<<std::endl; std::cerr<<"Curvature of seed point: "<<normals->points[fp_index].curvature<<std::endl; pcl::PointCloud<pcl::PointXYZRGBA> cloud_in; pcl::copyPointCloud(cloud_input,cloud_in); pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ()); tree->setInputCloud(cloud_in.makeShared()); if (fp_index > cloud_in.points.size () || fp_index <0) { PCL_ERROR ("[pcl::activeSegmentation] Fixation point given is invalid\n"); return; } if (boundary.points.size () != cloud_in.points.size ()) { PCL_ERROR ("[pcl::activeSegmentation] Boundary map given was built for a different dataset (%zu) than the input cloud (%zu)!\n", boundary.points.size () , cloud_in.points.size ()); return; } if (tree->getInputCloud ()->points.size () != cloud_in.points.size ()) { PCL_ERROR ("[pcl::activeSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud_in.points.size ()); return; } if (normals->points.size () != cloud_in.points.size ()) { PCL_ERROR ("[pcl::activeSegmentation] Input Normals are for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", normals->points.size (), cloud_in.points.size ()); return; } std::vector<int> seed_queue; std::vector<bool> processed (cloud_in.size(), false); seed_queue.push_back (fp_index); indices_out.indices.push_back (fp_index); processed[fp_index] = true; std::vector<int> nn_indices; std::vector<float> nn_distances; //process while there are valid seed points for(size_t seed_idx = 0; seed_idx < seed_queue.size();++seed_idx) { int curr_seed; curr_seed = seed_queue[seed_idx]; // Search for seeds if (!tree->radiusSearch (curr_seed, search_radius, nn_indices, nn_distances)) continue; //process all points found in the neighborhood bool stop_growing = false; size_t indices_old_size = indices_out.indices.size(); for (size_t i=1; i < nn_indices.size (); ++i) { if (processed[nn_indices.at (i)]) continue; if (boundary.points[nn_indices.at (i)].boundary_point != 0) { stop_growing=true; indices_out.indices.push_back (nn_indices.at (i)); processed[nn_indices.at (i)] = true; break; } bool is_convex = false; pcl::PointXYZRGBA temp; temp.x = cloud_in.points[fp_index].x - cloud_in.points[nn_indices.at (i)].x; temp.y = cloud_in.points[fp_index].y - cloud_in.points[nn_indices.at (i)].y; temp.z = cloud_in.points[fp_index].z - cloud_in.points[nn_indices.at (i)].z; double dot_p = normals->points[nn_indices.at (i)].normal[0] * temp.x + normals->points[nn_indices.at (i)].normal[1] * temp.y + normals->points[nn_indices.at (i)].normal[2] * temp.z; dot_p = dot_p>1? 1:dot_p; dot_p = dot_p<-1 ? -1:dot_p; if ((acos (dot_p) > eps_angle*M_PI / 180)) is_convex = true; if (is_convex) { indices_out.indices.push_back (nn_indices.at (i)); processed[nn_indices.at (i)] = true; } else break; }//new neighbor if(!stop_growing && (indices_old_size != indices_out.indices.size())) { for (size_t j = indices_old_size-1; j < indices_out.indices.size(); ++j) seed_queue.push_back(indices_out.indices.at (j)); } }//new seed point }
/** @todo: fix the return value, make sure the exit is not needed anymore*/ template <typename PointT> void pcl16::extractEuclideanClusters (const PointCloud<PointT> &cloud, const std::vector<int> &indices, const boost::shared_ptr<search::Search<PointT> > &tree, float tolerance, std::vector<PointIndices> &clusters, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster) { // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns //and indices[i] if (tree->getInputCloud ()->points.size () != cloud.points.size ()) { PCL16_ERROR ("[pcl16::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); return; } if (tree->getIndices ()->size () != indices.size ()) { PCL16_ERROR ("[pcl16::extractEuclideanClusters] Tree built for a different set of indices (%zu) than the input set (%zu)!\n", tree->getIndices ()->size (), indices.size ()); return; } // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (cloud.points.size (), false); std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector for (int i = 0; i < static_cast<int> (indices.size ()); ++i) { if (processed[indices[i]]) continue; std::vector<int> seed_queue; int sq_idx = 0; seed_queue.push_back (indices[i]); processed[indices[i]] = true; while (sq_idx < static_cast<int> (seed_queue.size ())) { // Search for sq_idx int ret = tree->radiusSearch (cloud.points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances); if( ret == -1) { PCL16_ERROR("[pcl16::extractEuclideanClusters] Received error code -1 from radiusSearch\n"); exit(0); } if (!ret) { sq_idx++; continue; } for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx { if (nn_indices[j] == -1 || processed[nn_indices[j]]) // Has this point been processed before ? continue; // Perform a simple Euclidean clustering seed_queue.push_back (nn_indices[j]); processed[nn_indices[j]] = true; } sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) { pcl16::PointIndices r; r.indices.resize (seed_queue.size ()); for (size_t j = 0; j < seed_queue.size (); ++j) // This is the only place where indices come into play r.indices[j] = seed_queue[j]; // These two lines should not be needed: (can anyone confirm?) -FF //r.indices.assign(seed_queue.begin(), seed_queue.end()); std::sort (r.indices.begin (), r.indices.end ()); r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); r.header = cloud.header; clusters.push_back (r); // We could avoid a copy by working directly in the vector } } }
void seededHueSegmentation (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > &host_cloud_, const pcl::gpu::Octree::Ptr &tree, float tolerance, PointIndices &indices_in, PointIndices &indices_out, float delta_hue) { // Create a bool vector of processed point indices, and initialize it to false // cloud is a DeviceArray<PointType> std::vector<bool> processed (host_cloud_->points.size (), false); int max_answers = host_cloud_->points.size(); // Process all points in the indices vector for (size_t k = 0; k < indices_in.indices.size (); ++k) { int i = indices_in.indices[k]; // if we already processed this point continue with the next one if (processed[i]) continue; // now we will process this point processed[i] = true; PointXYZRGB p; p = host_cloud_->points[i]; PointXYZHSV h; PointXYZRGBtoXYZHSV(p, h); // Create the query queue on the device, point based not indices pcl::gpu::Octree::Queries queries_device; // Create the query queue on the host pcl::PointCloud<pcl::PointXYZ>::VectorType queries_host; // Push the starting point in the vector queries_host.push_back (host_cloud_->points[i]); unsigned int found_points = queries_host.size (); unsigned int previous_found_points = 0; pcl::gpu::NeighborIndices result_device; // Host buffer for results std::vector<int> sizes, data; // once the area stop growing, stop also iterating. while (previous_found_points < found_points) { // Move queries to GPU queries_device.upload(queries_host); // Execute search tree->radiusSearch(queries_device, tolerance, max_answers, result_device); // Store the previously found number of points previous_found_points = found_points; // Clear the Host vectors sizes.clear (); data.clear (); // Copy results from GPU to Host result_device.sizes.download (sizes); result_device.data.download (data); for(size_t qp = 0; qp < sizes.size (); qp++) { for(int qp_r = 0; qp_r < sizes[qp]; qp_r++) { if(processed[data[qp_r + qp * max_answers]]) continue; PointXYZRGB p_l; p_l = host_cloud_->points[data[qp_r + qp * max_answers]]; PointXYZHSV h_l; PointXYZRGBtoXYZHSV(p_l, h_l); if (fabs(h_l.h - h.h) < delta_hue) { processed[data[qp_r + qp * max_answers]] = true; queries_host.push_back (host_cloud_->points[data[qp_r + qp * max_answers]]); found_points++; } } } } for(size_t qp = 0; qp < sizes.size (); qp++) { for(int qp_r = 0; qp_r < sizes[qp]; qp_r++) { indices_out.indices.push_back(data[qp_r + qp * max_answers]); } } } // @todo: do we need to sort here and remove double points? }
void extractEuclideanClusters ( PointCloud<PointXYZRGB >::Ptr cloud, pcl::PointCloud<pcl::Normal >::Ptr normals, pcl::search::KdTree<PointXYZRGB >::Ptr tree, float tolerance, std::vector<pcl::PointIndices > &clusters, double eps_angle, unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int >::max) ()) { // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns //and indices[i] float adjTolerance = 0; // Create a bool vector of processed point indices, and initialize it to false std::vector<bool > processed(cloud->points.size(), false); std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector std::cout << "Point size is " << cloud->points.size () << std::endl; for (size_t i = 0; i < cloud->points.size (); ++i) { if(processed[i]) continue; std::vector<int > seed_queue; int sq_idx = 0; seed_queue.push_back(i); processed[i] = true; int cnt = 0; while (sq_idx < (int)seed_queue.size()) { cnt++; // Search for sq_idx // adjTolerance = cloud->points[seed_queue[sq_idx]].distance * tolerance; adjTolerance = tolerance; if (!tree->radiusSearch(seed_queue[sq_idx], adjTolerance, nn_indices, nn_distances)) { sq_idx++; continue; } for(size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx { if (processed[nn_indices[j]]) // Has this point been processed before ? continue; processed[nn_indices[j]] = true; // [-1;1] double dot_p = normals->points[i].normal[0] * normals->points[nn_indices[j]].normal[0] + normals->points[i].normal[1] * normals->points[nn_indices[j]].normal[1] + normals->points[i].normal[2] * normals->points[nn_indices[j]].normal[2]; if ( fabs (acos (dot_p)) < eps_angle ) { processed[nn_indices[j]] = true; seed_queue.push_back (nn_indices[j]); } } sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) { pcl::PointIndices r; r.indices.resize (seed_queue.size ()); for (size_t j = 0; j < seed_queue.size (); ++j) r.indices[j] = seed_queue[j]; sort (r.indices.begin (), r.indices.end ()); r.indices.erase (unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); r.header = cloud->header; //ROS_INFO ("cluster of size %d data point\n ",r.indices.size()); clusters.push_back(r); } } }
template<typename PointT> void pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clusters) { // Prepare output (going to use push_back) clusters.clear (); if (extract_removed_clusters_) { small_clusters_->clear (); large_clusters_->clear (); } // Validity checks if (!initCompute () || input_->points.empty () || indices_->empty () || !condition_function_) return; // Initialize the search class if (!searcher_) { if (input_->isOrganized ()) searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); else searcher_.reset (new pcl::search::KdTree<PointT> ()); } searcher_->setInputCloud (input_, indices_); // Temp variables used by search class std::vector<int> nn_indices; std::vector<float> nn_distances; // Create a bool vector of processed point indices, and initialize it to false // Need to have it contain all possible points because radius search can not return indices into indices std::vector<bool> processed (input_->points.size (), false); // Process all points indexed by indices_ for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator { // Has this point been processed before? if ((*indices_)[iii] == -1 || processed[(*indices_)[iii]]) continue; // Set up a new growing cluster std::vector<int> current_cluster; int cii = 0; // cii = cluster indices iterator // Add the point to the cluster current_cluster.push_back ((*indices_)[iii]); processed[(*indices_)[iii]] = true; // Process the current cluster (it can be growing in size as it is being processed) while (cii < static_cast<int> (current_cluster.size ())) { // Search for neighbors around the current seed point of the current cluster if (searcher_->radiusSearch (input_->points[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1) { cii++; continue; } // Process the neighbors for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator { // Has this point been processed before? if (nn_indices[nii] == -1 || processed[nn_indices[nii]]) continue; // Validate if condition holds if (condition_function_ (input_->points[current_cluster[cii]], input_->points[nn_indices[nii]], nn_distances[nii])) { // Add the point to the cluster current_cluster.push_back (nn_indices[nii]); processed[nn_indices[nii]] = true; } } cii++; } // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range if (extract_removed_clusters_ || (current_cluster.size () >= min_cluster_size_ && current_cluster.size () <= max_cluster_size_)) { pcl::PointIndices pi; pi.header = input_->header; pi.indices.resize (current_cluster.size ()); for (int ii = 0; ii < static_cast<int> (current_cluster.size ()); ++ii) // ii = indices iterator pi.indices[ii] = current_cluster[ii]; if (extract_removed_clusters_ && current_cluster.size () < min_cluster_size_) small_clusters_->push_back (pi); else if (extract_removed_clusters_ && current_cluster.size () > max_cluster_size_) large_clusters_->push_back (pi); else clusters.push_back (pi); } } deinitCompute (); }
std::vector<std::vector<PointId>> extractClusters(PointView& view, uint64_t min_points, uint64_t max_points, double tolerance) { // Index the incoming PointView for subsequent radius searches. KD3Index kdi(view); kdi.build(); // Create variables to track PointIds that have already been added to // clusters and to build the list of cluster indices. std::vector<PointId> processed(view.size(), 0); std::vector<std::vector<PointId>> clusters; for (PointId i = 0; i < view.size(); ++i) { // Points can only belong to a single cluster. if (processed[i]) continue; // Initialize list of indices belonging to current cluster, marking the // seed point as processed. std::vector<PointId> seed_queue; size_t sq_idx = 0; seed_queue.push_back(i); processed[i] = 1; // Check each point in the cluster for additional neighbors within the // given tolerance, remembering that the list can grow if we add points // to the cluster. while (sq_idx < seed_queue.size()) { // Find neighbors of the next cluster point. PointId j = seed_queue[sq_idx]; std::vector<PointId> ids = kdi.radius(j, tolerance); // The case where the only neighbor is the query point. if (ids.size() == 1) { sq_idx++; continue; } // Skip neighbors that already belong to a cluster and add the rest // to this cluster. for (auto const& k : ids) { if (processed[k]) continue; seed_queue.push_back(k); processed[k] = 1; } sq_idx++; } // Keep clusters that are within the min/max number of points. if (seed_queue.size() >= min_points && seed_queue.size() <= max_points) clusters.push_back(seed_queue); } return clusters; }
void DenseReconstruction::extractEuclideanClustersCurvature(std::vector<pcl::PointIndices::Ptr> &clusters){ float tolerance=0.01;//0.01radius of KDTree radius search in region growing in meters double eps_angle=35*M_PI/180;//35, 10 double max_curvature=0.1;//0.1 //max value of the curvature of the point form which you can start region growing unsigned int min_pts_per_cluster = 1; unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) () ; // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (region_grow_point_cloud_->size (), false); //create a random copy of indices std::vector<int> indices_rnd(region_grow_point_cloud_->size ()); for(unsigned int i=0;i<indices_rnd.size();++i) { indices_rnd[i]=i; } //uncommnet myrandom part if you want indices to be truely // if(use_srand == 1) // { // std::random_shuffle(indices_rnd.begin(), indices_rnd.end(), myrandom); // ROS_ERROR("REAL RANDOM"); // } // else std::random_shuffle(indices_rnd.begin(), indices_rnd.end()); std::cerr<<"Processed size: "<<processed.size()<<std::endl; std::vector<int> index_lookup(indices_rnd.size()); for(unsigned int i= 0; i<indices_rnd.size();++i) index_lookup[indices_rnd[i]] = i; std::vector<int> nn_indices; std::vector<float> nn_distances; // Process all points in the indices vector for (size_t i = 0; i < indices_rnd.size (); ++i) { if (processed[i] || cloud_normals_->points[indices_rnd[i]].curvature > max_curvature) { /*if(normals.points[indices_rnd[i]].curvature > max_curvature) std::cerr<<"Curvature of point skipped: "<<normals.points[indices_rnd[i]].curvature<<std::endl;*/ continue; } pcl::PointIndices::Ptr seed_queue(new pcl::PointIndices()); int sq_idx = 0; seed_queue->indices.push_back (indices_rnd[i]); processed[i] = true; while (sq_idx < (int)seed_queue->indices.size ()) { // Search for sq_idx if (!tree_->radiusSearch (seed_queue->indices[sq_idx], tolerance, nn_indices, nn_distances)) { sq_idx++; continue; } for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx { // std::cerr<<nn_indices[j]<<std::endl; if (processed[index_lookup[nn_indices[j]]]) // Has this point been processed before ? continue; // [-1;1] double dot_p = cloud_normals_->points[indices_rnd[i]].normal[0] * cloud_normals_->points[nn_indices[j]].normal[0] + cloud_normals_->points[indices_rnd[i]].normal[1] * cloud_normals_->points[nn_indices[j]].normal[1] + cloud_normals_->points[indices_rnd[i]].normal[2] * cloud_normals_->points[nn_indices[j]].normal[2]; if ( fabs (acos (dot_p)) < eps_angle ) { processed[index_lookup[nn_indices[j]]] = true; seed_queue->indices.push_back (nn_indices[j]); } } sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue->indices.size () >= min_pts_per_cluster && seed_queue->indices.size () <= max_pts_per_cluster) { seed_queue->header = region_grow_point_cloud_->header; clusters.push_back (seed_queue); } } int unprocessed_counter = 0; for(unsigned int i =0; i<processed.size(); ++i) { if(processed[i] == false) { //std::cerr<<"Indice not processed at " <<i<<" : "<<indices_rnd[i]<<std::endl; unprocessed_counter++; } } //std::cerr<<"Number of unprocessed indices: "<<unprocessed_counter<<std::endl; }
void QmlScanner::processingFinished(const QImage& image) { processed(image); m_busy = false; busyChanged(); }
// For each cluster we check if we can identify an independent // bag, which might be useful for clustered planarity testing. // compute independent bag affiliation for all vertices, // store result in m_indyBagNumber, and set m_numIndyBags accordingly. // We could interweave this with computeBags to be more efficient but // this would it make the code more complex and in case we don't need // the IndyBags we would do additional work in vain. void ClusterAnalysis::computeIndyBags() { m_numIndyBags = 0; //used both to count the bags and to store current //indyBag index number for vertex assignment (i.e., starts with 1) const Graph &G = m_C->constGraph(); // Store the root cluster of each indyBag delete m_indyBagRoots; // Intermediate storage during computation, maximum of #vertices possible #ifdef OGDF_DEBUG Array<cluster> bagRoots(0,G.numberOfNodes(),nullptr); #else Array<cluster> bagRoots(G.numberOfNodes()); #endif // Store indyBag affiliation. Every vertex will get a number != -1 (DefaultIndex, // as in the worst case the whole graph is an indyBag (in root cluster). // Once assigned, the number won't change during the processing. m_indyBagNumber.init(G, DefaultIndex); // We run bottom up over all clusters (to find the minimum inclusion). // For each vertex, we use the outer activity and bag index information. // In case we find a bag without outeractive vertices it is a IndyBag. // Already processed vertices are simply marked by an indyBag index entry different to -1. #if 0 NodeArray<bool> processed(G, false); //mark vertices when IndyBag detected #endif // We do not have the sets of vertices for all bags, as only the bag index // has been stored for a cluster. // Detect the current leaf clusters for bottom up traversal. List<cluster> ccleafs; ClusterArray<int> unprocessedChildren(*m_C); //processing below: compute bags for(cluster c : m_C->clusters) { if (c->cCount() == 0) ccleafs.pushBack(c); unprocessedChildren[c] = c->cCount(); } OGDF_ASSERT(!ccleafs.empty()); // Run through all clusters, leaves first. while (!ccleafs.empty()){ // We cannot store the following information over the whole // graph even though the bag index (which is one out of the // set of vertex set ids in union find) would allow this. // The bag index is defined per cluster. // However, when moving up in the cluster tree indy information // is not monotone, we therefore would need to update it accordingly. // (Bag which is not outeractive will not become outeractive, but // may get a part of an outeractive bag with the same id, and an // outeractive bag might become enclosed). HashArray<int, bool> indyBag(true); // true if bag with index i does not have outeractive vertices // We want to store all vertices for each index that may be a // potential indyBag index. We could add these in the entry stored // in our index Skiplist, but then we need a comparison of the // respecting class objects, slowing down the processing. HashArray<int, List<node> > bagNodes; //holds vertices for index i // We need a data structure that holds all indexes used, allows // to search for them and to iterate through them. Could use // insertion sorted dynamically allocated array here, but as // SkipLists are implemented in OGDF, which provide comparable // efficiency, we use them. Skiplist<int*> indexNumbers; //holds all index numbers in c cluster c = ccleafs.popFrontRet(); List<node> nodes; ListConstIterator<node> it; // Process leaves separately. As long as we don't do something // special for non-leaves, we could just use getClusterNodes instead. if (c->cCount() == 0) { it = c->nBegin(); } else { // At this point all child clusters of c have been processed. // This is somewhat slow, as we repeatedly collect the vertices // Todo: Instead, we could clone the clusternodes code // here and process them as we see them. c->getClusterNodes(nodes); #if 0 std::cout <<"Processing cluster with "<<nodes.size()<<"\n"; #endif it = nodes.begin(); } // Run through all vertices in c partitionCluster(it, c, bagNodes, indyBag, indexNumbers, bagRoots); // Now we update the status of the parent cluster and, // in case all its children are processed, add it to // the process queue. if (c != m_C->rootCluster()) { OGDF_ASSERT(unprocessedChildren[c->parent()] > 0); unprocessedChildren[c->parent()]--; if (unprocessedChildren[c->parent()] == 0) ccleafs.pushBack(c->parent()); } } // Copying into smaller array is a bit slower than just reserving the whole #v array, // but we do it anyway. m_indyBagRoots = new cluster[m_numIndyBags]; for (int k = 0; k < m_numIndyBags; k++) { OGDF_ASSERT(bagRoots[k] != nullptr); m_indyBagRoots[k] = bagRoots[k]; } #ifdef OGDF_DEBUG #if 0 List<node> rnodes; m_C->rootCluster()->getClusterNodes(rnodes); for(node v : rnodes) { std::cout << "Root bag index: "<<bagIndex(v, m_C->rootCluster())<<"\n"; std::cout << "Indy bag index: "<<m_indyBagNumber[v]<<"\n"; } #endif Skiplist<int*> ibind; for(node v : G.nodes) { int i = m_indyBagNumber[v]; if (!ibind.isElement(&i)) { ibind.add(new int(i)); } std::cout << "numIndyBags: "<<m_numIndyBags<<" i: "<<i<<"\n"; OGDF_ASSERT(i != DefaultIndex); OGDF_ASSERT(i >= 0); OGDF_ASSERT(i < m_numIndyBags); } int storedBags = 0; SkiplistIterator<int*> its = ibind.begin(); while (its.valid()) { storedBags++; delete *its; ++its; } Logger::slout() << m_numIndyBags<< " independent bags detected, "<<storedBags<<" stored\n"; OGDF_ASSERT(m_numIndyBags==storedBags); #endif }
void TLSConnectionPrivate::OnRead(uv_stream_t *stream, ssize_t nread, uv_buf_t buf) { TLSConnectionPrivate *cp = static_cast<TLSConnectionPrivate *>(stream->data); assert(cp != nullptr); // Only allow OnRead calls to have an effect on the TLSConnection in // the STARVED_SSL_CONNECT and ESTABLISHED states. bool bad = cp->state_ != TLS_CONNECTION_STATE_STARVED_SSL_CONNECT && cp->state_ != TLS_CONNECTION_STATE_ESTABLISHED; if (bad) { return; } if (nread == -1) { uv_err_t last = uv_last_error(cp->loop_); if (last.code == UV_EOF) { cp->ShutdownRemote(); } else { cp->ShutdownError(UVUtils::ErrorFromUVError(last)); } return; } else if (nread == 0) { cp->ShutdownRemote(); return; } ByteArray b(buf.base, nread, buf.len); cp->biostate_->PutNewBuffer(b); if (cp->state_ == TLS_CONNECTION_STATE_STARVED_SSL_CONNECT) { if (!cp->HandleStarvedConnectState()) { return; } } if (cp->state_ == TLS_CONNECTION_STATE_ESTABLISHED) { ByteArray processed(static_cast<int>(nread)); bool backoff = false; while (!backoff) { int nread = SSL_read(cp->ssl_, reinterpret_cast<void *>(processed.Data()), processed.Capacity()); if (nread == -1) { int err = SSL_get_error(cp->ssl_, nread); if (err == SSL_ERROR_WANT_READ || err == SSL_ERROR_WANT_WRITE) { // OpenSSL needs to be fed more data. Back off. backoff = true; break; } cp->ShutdownError(OpenSSLUtils::ErrorFromOpenSSLErrorCode(err)); return; } else if (nread == 0) { cp->ShutdownRemote(); return; } processed.Truncate(nread); if (cp->read_handler_) { cp->read_handler_(processed); } } } return; }
void EuclideanClustering::extractClusters(brics_3d::PointCloud3D *inCloud){ int k = inCloud->getSize(); brics_3d::NearestNeighborANN nearestneighborSearch; brics_3d::Point3D querryPoint3D; vector<int> neighborIndices; nearestneighborSearch.setData(inCloud); nearestneighborSearch.setMaxDistance(this->clusterTolerance); // nearestneighborSearch.setMaxDistance(1000); // Create a bool vector of processed point indices, and initialize it to false std::vector<bool> processed (inCloud->getSize(), false); // Process all points in the indices vector for (size_t i = 0; i < inCloud->getSize(); ++i) { if (processed[i]) continue; std::vector<int> seed_queue; int sq_idx = 0; seed_queue.push_back (i); //std::cout << "[CHEAT][EuclideanClustering3D] "<< i << " : " << seed_queue.size () << std::endl; processed[i] = true; while (sq_idx < (int)seed_queue.size ()) { // Search for sq_idx neighborIndices.clear(); querryPoint3D = (*inCloud->getPointCloud())[sq_idx]; nearestneighborSearch.findNearestNeighbors(&querryPoint3D, &neighborIndices, k); //if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) if(neighborIndices.size()==0) { sq_idx++; continue; } for (size_t j = 0; j < neighborIndices.size(); ++j) // nn_indices[0] should be sq_idx { if (processed[neighborIndices[j]]) // Has this point been processed before ? continue; // Perform a simple Euclidean clustering seed_queue.push_back (neighborIndices[j]); processed[neighborIndices[j]] = true; } sq_idx++; } // std::cout << "[CHEAT][EuclideanClustering3D] {sq_id : seed_q.size}"<< sq_idx << " : "<< seed_queue.size() << std::endl; // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= minClusterSize && seed_queue.size () <= maxClusterSize) { // std::cout << "[CHEAT][EuclideanClustering3D] found one cluster, size="<< seed_queue.size() << std::endl; seed_queue.erase(std::unique(seed_queue.begin(), seed_queue.end()),seed_queue.end()); brics_3d::PointCloud3D *tempPointCloud = new brics_3d::PointCloud3D(); for (size_t j = 0; j < seed_queue.size (); ++j) { tempPointCloud->addPointPtr((*inCloud->getPointCloud())[seed_queue[j]].clone()); } extractedClusters.push_back(tempPointCloud); // delete tempPointCloud; //? } // std::cout << "[CHEAT][EuclideanClustering3D] "<< i << " : " << seed_queue.size () << std::endl; } }