Example #1
0
	    void initialize() {
		max.assign(0);
		min.assign(1);
		if (!vector_ . empty()) {
		    max = vector_.front();
		    min = max;
		}
		typedef std::vector<Int4>::const_iterator iterator;
		for (iterator it = vector_.begin(); it < vector_.end(); ++ it) {
		    // std:: cout << *it << std::endl;
		    for (int i = 0; i < 4; ++i) {
			max[i] = std::max(max[i], (*it)[i]);
			min[i] = std::min(min[i], (*it)[i]);
		    }
		}
		base_type::assign(vector_);
	    }
    GeometricVerifier(string base_link_frame_id, string sensor_frame_id, int window_frame_size, double transx, double transy, double transz) :
        base_link_frame_id_(base_link_frame_id),sensor_frame_id_(sensor_frame_id), window_frame_size_(window_frame_size), transx_(transx), transy_(transy), transz_(transz)
    {
        m_proposed_=0;
        m_accepted_=0;
        image_client_ = n_.serviceClient<image_cache::GetImage>("image_service");       // attach to image_cache
        info_client_ = n_.serviceClient<image_cache::GetInfo>("info_service");       // attach to image_cache
        neighbour_client_ = n_.serviceClient<image_cache::GetNeighbours>("neighbour_service");       // attach to image_cache
        match_sub_ = n_.subscribe("/appearance_matches",10,&GeometricVerifier::match_callback,this);    // subscribe to fab-map matches
        add_loop_closure_pub_ = n_.advertise<slam_backend::AddLoopClosure>("add_loop_closure",1);
        debug_pub_ = n_.advertise<geometric_verification::Debug>("verification_debug",1);
        
        ROS_INFO("Base link frame id is %s, sensor frame id is %s, window size is %d subsampled frames",base_link_frame_id_.c_str(), sensor_frame_id_.c_str(), window_frame_size_);
        boost::thread service_thread(boost::bind(&GeometricVerifier::serviceThread,this));
        pose_covariance_.assign(0.0);

    }