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