void Munkres::step4() { /* find an uncovered zero and prime it * if now starred zeros in row goto step 5 * else cover row and uncover colum of starred zero * * once no uncovered exist goto step 6. */ bool done = false; int i = 0; int j = 0; int a; while (!done) { if (find_zero(&i, &j)) { if (!is_covered(i, j)) { prime(i, j); a = starred_in_row(i); if (a == -1) // if no starred zeros { done = true; step5(i, j); } else { uncover_col(a); cover_row(i); } } } else { done = true; step6( min_uncovered()); } } }
bool Munkres::find_zero(int* row, int* col) { // find a zero thats uncovered for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { if (cost[i][j] == 0) { if (!is_covered(i, j)) { *row = i; *col = j; return true; } } } } return false; }
double Munkres::min_uncovered() { // find the minumum uncovered value in the cost matrix. double min = std::numeric_limits<double>::infinity(); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { if (!is_covered(i, j)) { if (cost[i][j] < min) { min = cost[i][j]; } } } } return min; }
bool Munkres::find_zero(std::vector<std::vector<double> > mat, int* row, int* col) { // find a zero thats uncovered for (int i =0; i< size; i++) { for (int j = 0; j< size; j++) { if (mat[i][j] == 0) { if (!is_covered(i,j)) { *row = i; *col = j; return true; } } } } return false; }
void grid_segment::get_uncertain_neighbour_cells(OutIterator out_iterator) { std::set<grid_cell::ptr> uncertain_cells; for(auto &bn: boundary_cells_) { for(size_t i=0; i<4; i++) { auto nc = grid_.get_neighbour_cell_4(bn, i); if(nc) { if(/*nc->is_uncertain() ||*/ !nc->is_covered()) { uncertain_cells.insert(nc); } } } } copy(uncertain_cells.begin(), uncertain_cells.end(), out_iterator); }
void grid_segment::grow(std::function<grid_segment::ptr(grid_cell_base::label)> segments_accessor) { vector<grid_cell::ptr> tovisit; tovisit.push_back(*cells_.begin()); set_visited(tovisit.front()); grid_cell_base::label label = tovisit.front()->get_label(); while(!tovisit.empty()) { grid_cell::ptr c = tovisit.back(); tovisit.pop_back(); c->set_label(label); cells_.insert(c); for(size_t i=0; i<4; i++) { auto nc = grid_.get_neighbour_cell_4(c,i); if(nc) { if(nc->has_label()) { if(nc->get_label() != label && !nc->is_ignored()) { add_edge(std::static_pointer_cast<graph_node>(segments_accessor(label)), std::static_pointer_cast<graph_node>(segments_accessor(nc->get_label()))); } continue; } if(nc->is_target() && !nc->is_ignored() && nc->is_covered() && !is_visited(nc)) { set_visited(nc); tovisit.push_back(nc); } } } } }
void grid_cell::draw() { static const double df_iso =0.05; glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glLineWidth(1.0); if(active_survey_param::non_ros::cell_drawing_mode == 0) glColor3f(1-0.8*ground_truth_value_, 1-0.8*ground_truth_value_, 1); else if(active_survey_param::non_ros::cell_drawing_mode == 1) { if(!is_covered()) glColor3f(0.45,0.55,0.4); else glColor3f(1-2*df_iso*ceil(estimated_value_/df_iso), 1-2*df_iso*ceil(estimated_value_/df_iso), 1-2*df_iso*ceil(estimated_value_/df_iso)); } else if(active_survey_param::non_ros::cell_drawing_mode == 3) glColor3f(2*variance_, 2*variance_, 2*variance_); else if(active_survey_param::non_ros::cell_drawing_mode == 4) { bool l = (estimated_value_+ active_survey_param::non_ros::beta*variance_ < active_survey_param::non_ros::target_threshold); bool h = (estimated_value_- active_survey_param::non_ros::beta*variance_ > active_survey_param::non_ros::target_threshold); bool u = !(l||h); glColor3f(l?1:0, h?1:0, u?1:0); // if(is_ignored()) // glColor3f(0,0,0); //glColor4f(estimated_value_+ variance_, estimated_value_+ variance_, // estimated_value_+ variance_,1); } // glColor4f(2*variance_, // ((1-variance_)<0?0:(1-variance_))*estimated_value_, // 2*variance_, ignored_?0.3:1.0); else if(active_survey_param::non_ros::cell_drawing_mode == 5) utility::gl_color(utility::get_altitude_color(label_)); else if(active_survey_param::non_ros::cell_drawing_mode == 2) { double dsc = pow(0.999, sensed_time_); utility::gl_color(sensed_&&is_target()?Vector3f(1-dsc,1-dsc,1-dsc):Vector3f(1,1,1)); //double rt = (sensed_time_>active_survey_param::time_limit)?0:sensed_time_/active_survey_param::time_limit; //utility::gl_color(Vector3f(1-rt, 0 ,rt)); } else { if(is_covered()) glColor3f(0,is_target()?1:0,0); else glColor3f(0.5,0.5,0.5); } glBegin(GL_QUADS); utility::draw_quad(get_rect()); glEnd(); if(ground_truth_value_ > active_survey_param::non_ros::target_threshold) { glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); glColor3f(1,0.95,0.7); glBegin(GL_QUADS); utility::draw_quad(get_rect(), 0.1); glEnd(); } static Vector4f offset{0.1,0.1,-0.1,-0.1}; if(false && is_ignored()) { glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glColor4f(0.0,0.0,1, 0.2); glBegin(GL_QUADS); utility::draw_quad(get_rect()+offset, 0.15); glEnd(); } }