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; }
inline Vector& operator/=(const VectorExpression<T2, A> &expr) { #ifdef ENABLE_BOUNDS_CHECKING assert(isize == expr.Size(0)); #endif for (int i = 0; i < data_m.size(); i++) { data_m[i] /= expr(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; }