C++ concurrent_vector::size方法代码示例

	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);

	}

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;
}

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);
    }
}

        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;
        }

        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
        }

        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;
        }

	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;
	}

 inline Vector& operator/=(const T& val) {
     for (int i = 0; i < data_m.size(); i++) {
         data_m[i] /= val;
     }
     return *this;
 }

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;
		}

	}
}

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;
}