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