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