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