Exemple #1
0
	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
            }
        }
    }
Exemple #3
0
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);
}