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