C++ tbb::concurrent_vector类代码示例

 void operator()( const tbb::concurrent_vector<int>::range_type& range ) const {
     for( iterator i=range.begin(); i!=range.end(); ++i ) {
         if( *i!=0 )
             std::printf("ERROR for v[%ld]\n", long(i-base));
         *i = int(i-base);
     }
 }

static void CheckVector( const tbb::concurrent_vector<Foo>& cv, size_t expected_size, size_t old_size ) {
    ASSERT( cv.size()==expected_size, NULL );
    ASSERT( cv.empty()==(expected_size==0), NULL );
    for( int j=0; j<int(expected_size); ++j ) {
        if( cv[j].bar()!=~j )
            std::printf("ERROR on line %d for old_size=%ld expected_size=%ld j=%d\n",__LINE__,long(old_size),long(expected_size),j);
    }
}

void MR::do_sort(tbb::concurrent_vector<Pair> &vec) {
  tbb::parallel_sort(vec.begin(), vec.end(), compare);
}

int MR::Reduce(int key, const tbb::concurrent_vector<Pair> &pairs, int index, 
           string &values) {
  while (index < pairs.size() && pairs[index].key == key) 
    values += pairs[index++].val + " ";
  return index;
}

void regionDataPublisher(zmqpp::socket &publisher, PATH_FIND_NODE &pathFindNode, tbb::concurrent_queue<PathEntity*> &solvedPathQueue, tbb::concurrent_vector<Entity*> entities) {
	using namespace std;

	std::chrono::steady_clock clock;

	// Initialize path.
	for (auto entity : entities) {
		pathFindNode.try_put(std::tuple<size_t, glm::vec2>(entity->id, entity->position));
	}

	while (1) {
		auto start = clock.now();

		// Grab a bunch fo path
		{
			//size_t size = entities.size();
			for (size_t i = 0; i < 200; ++i) {
				PathEntity* pathEntity;
				if (solvedPathQueue.try_pop(pathEntity)) {

					entities[pathEntity->id]->pathNodes = pathEntity->pathNodes;
					entities[pathEntity->id]->currentNode = 0;
				}
			}
		}

		// Traverse nodes
		{
			for (auto entity : entities) {
				if (entity->pathNodes != 0) {
					if (entity->currentNode < entity->pathNodes->size()) {
						size_t currentIndex = (size_t)(*entity->pathNodes)[entity->currentNode++];
						//wss::Utils::indexToXY(currentIndex, MAP_W, entity->position);
						wss::Utils::indexToXY(currentIndex, MAP_W, entity->position);
					}
					else {
						entity->pathNodes = 0;
						pathFindNode.try_put(std::tuple<size_t, glm::vec2>(entity->id, entity->position));
					}
				}
			}
		}

		{
			rapidjson::Document document;
			document.SetObject();
			serializeEntities(document, 0, entities.size(), entities);

			rapidjson::StringBuffer sb;
			rapidjson::Writer<rapidjson::StringBuffer> writer(sb);

			document.Accept(writer);

			publisher.send(sb.GetString());
		}
		std::chrono::duration<double> elapsed = clock.now() - start;
		if (elapsed.count() < 1.0/5.0) {
			std::this_thread::sleep_for(std::chrono::milliseconds(1000/5 - (size_t)(elapsed.count() * 1000.0)));
		//cout << "elpased region publisher" << endl;
		}

	}
}

void MR::Map(const string &ligand, tbb::concurrent_vector<Pair> &pairs) {
  Pair p(Help::score(ligand.c_str(), protein.c_str()), ligand);
  pairs.push_back(p);
}

double TimeFindPrimes( int nthread ) {
    Primes.clear();
    Primes.reserve(1000000);// TODO: or compact()?
    tbb::task_scheduler_init init(nthread);
    tbb::tick_count t0 = tbb::tick_count::now();
    tbb::parallel_for( tbb::blocked_range<Number>(0,1000000,500), FindPrimes() );
    tbb::tick_count t1 = tbb::tick_count::now();
    return (t1-t0).seconds();
}

	void RGBDCallback(const sensor_msgs::Image::ConstPtr& yuv2_msg,
			const sensor_msgs::Image::ConstPtr& depth_msg,
			const sensor_msgs::CameraInfo::ConstPtr& info_msg) {

		cv_bridge::CvImageConstPtr yuv2 = cv_bridge::toCvShare(yuv2_msg);
		cv_bridge::CvImageConstPtr depth = cv_bridge::toCvShare(depth_msg);

		boost::mutex::scoped_lock lock(closest_keyframe_update_mutex);

		if (keyframes.size() != 0) {

			closest_keyframe_idx = get_closest_keyframe();
			//ROS_INFO("Closest keyframe %d", closest_keyframe_idx);

			keyframe::Ptr closest_keyframe = keyframes[closest_keyframe_idx];

			Sophus::SE3f tt = closest_keyframe->get_pos();
			ROS_DEBUG("Closest keyframe %d", closest_keyframe_idx);

			if ((tt.translation() - camera_position.translation()).norm() > 0.3
					|| tt.unit_quaternion().angularDistance(
							camera_position.unit_quaternion()) > M_PI / 18) {
				keyframe::Ptr k(
						new keyframe(yuv2->image, depth->image, camera_position,
								intrinsics));

				if (closest_keyframe->estimate_position(*k)) {
					camera_position = k->get_pos();

					keyframe_pub.publish(k->to_msg(yuv2, keyframes.size()));
					keyframes.push_back(k);
					ROS_DEBUG("Adding new keyframe");
				}

			} else {
				frame f(yuv2->image, depth->image, camera_position, intrinsics);
				if (closest_keyframe->estimate_position(f)) {
					camera_position = f.get_pos();
				}
			}

		} else {

			intrinsics << info_msg->K[0], info_msg->K[2], info_msg->K[5];

			keyframe::Ptr k(
					new keyframe(yuv2->image, depth->image, camera_position,
							intrinsics));
			keyframe_pub.publish(k->to_msg(yuv2, keyframes.size()));
			keyframes.push_back(k);
		}

		publish_tf(yuv2_msg->header.frame_id, yuv2_msg->header.stamp);

	}

    void operator()( const tbb::blocked_range<Number>& r ) const {
        for( Number i=r.begin(); i!=r.end(); ++i ) { 
            if( i%2 && is_prime(i) ) {
#if TBB_DEPRECATED
                Primes[Primes.grow_by(1)] = i;
#else
                Primes.push_back( i );
#endif
            }
        }
    }

        Vector& operator=(const Vector &other) {
            this->isize = other.isize;
            data_m.resize(other.data_m.size());
#if defined(ATL_CONCURRENCY_ENABLED)
            for (int i = 0; i < data_m.size(); i++) {
                data_m[i] = other.data_m[i];
            }
#else
            data_m.insert(data_m.begin(), other.data_m.begin(), other.data_m.end());
#endif
            return *this;
        }

        Vector(const Vector &orig)
        : isize(orig.isize) {
            this->isize = orig.isize;
            data_m.resize(orig.data_m.size());
#if defined(ATL_CONCURRENCY_ENABLED)
            for (int i = 0; i < data_m.size(); i++) {
                data_m[i] = orig.data_m[i];
            }
#else
            data_m.insert(data_m.begin(), orig.data_m.begin(), orig.data_m.end());
#endif
        }

const vector<Pair> &MR::run(int ml, int nl, int nt, const string& p) {
  max_ligand = ml;  nligands = nl;  nthreads = nt;  protein = p;


  Generate_tasks(tasks);
  // assert -- tasks is non-empty


#pragma omp parallel for num_threads(nthreads)
  for (int t = 0;  t < tasks.size();  t++) {
    Map(tasks[t], pairs);
  }
  
  do_sort(pairs);


  int next = 0;  // index of first unprocessed pair in pairs[]
  while (next < pairs.size()) {
    string values;
    values = "";
    int key = pairs[next].key;
    next = Reduce(key, pairs, next, values);
    Pair p(key, values);
    results.push_back(p);
  }


  return results;
}

	bool clear_keyframes(std_srvs::Empty::Request &req,
			std_srvs::Empty::Response &res) {

		boost::mutex::scoped_lock lock(closest_keyframe_update_mutex);

		keyframes.clear();

		return true;
	}

        inline Vector& operator+=(const Vector &other) {
#ifdef ENABLE_BOUNDS_CHECKING
            assert(isize == other.isize);
#endif   
            for (int i = 0; i < data_m.size(); i++) {
                data_m[i] += other.data_m[i];
            }
            return *this;
        }

	int get_closest_keyframe() {

		int res = -1;
		float dist = 10000.0;

		for (size_t i = 0; i < keyframes.size(); i++) {
			Sophus::SE3f t = keyframes[i]->get_pos();

			if ((camera_position.translation() - t.translation()).norm()
					< 0.5) {

				float current_dist = t.unit_quaternion().angularDistance(
						camera_position.unit_quaternion());

				if (current_dist < dist) {
					res = i;
					dist = current_dist;
				}
			}

		}

		return res;
	}