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