void add_neighbours(MapModel* map_model, int x, int y, XY* neighbours, int* count, int limit, long visits)
{
if (*count >= limit)
return;
if (map_model->points[x][y].visits < visits)
return;
for (int i = 0; i < (*count); i++) {
if (neighbours[i].x == x && neighbours[i].y == y) {
return;
}
}
neighbours[*count].x = x;
neighbours[*count].y = y;
*count += 1;
for (int dx = -1; dx <= 1; dx++) {
for (int dy = -1; dy <= 1; dy++) {
if (dx == 0 && dy == 0) continue;
if ((x + dx) < 0 || (y + dy) < 0) continue;
if ((x + dx) >= map_model->x_size || (y + dy) >= map_model->y_size ) continue;
add_neighbours(map_model, x + dx, y + dy, neighbours, count, limit, visits);
}
}
}
/************************************************************
* Find a seed in an unvisited, trusted region
* with the largest mag signal
************************************************************/
static void find_seed(int *seed_loc,
double *mag,
int *visited,
int *trusted,
int *stack,
int *nstack,
int *nunwrapped,
int nx, int ny)
{
double max_mag = 0.0;
int max_i = 0;
int i;
int imsize = nx * ny;
iloop {
if (trusted[i] == 1 && visited[i] < 2) {
if (mag[i] > max_mag) {
max_mag = mag[i];
max_i = i;
}
}
}
*seed_loc = max_i;
/* Mark the seed point as unwrapped (2) */
visited[*seed_loc] = 2;
(*nunwrapped)++;
/* Initialize the neighbour stack counter */
*nstack = 0;
/* Add compass neighbours to the stack */
add_neighbours(*seed_loc,
stack,
visited,
trusted,
nstack,
nx, ny);
}
void remove_noise(MapModel* map_model, long visit_threshold, int length_threshold)
{
XY* neighbours = malloc(sizeof(XY)*length_threshold);
int neighbours_count = 0;
for (int x = 0; x < map_model->x_size; x++) {
for (int y = 0; y < map_model->y_size; y++) {
map_model->points[x][y].processed = 0;
}
}
for (int x = 0; x < map_model->x_size; x++) {
for (int y = 0; y < map_model->y_size; y++) {
if (map_model->points[x][y].visits < visit_threshold) {
map_model->points[x][y].visits = 0;
continue;
}
if (map_model->points[x][y].processed != 0)
continue;
neighbours_count = 0;
add_neighbours(map_model, x, y, neighbours, &neighbours_count, length_threshold, visit_threshold);
if (neighbours_count < length_threshold) {
for (int i = 0; i < neighbours_count; i++) {
map_model->points[neighbours[i].x][neighbours[i].y].visits = 0;
}
}
else {
for (int i = 0; i < neighbours_count; i++) {
map_model->points[neighbours[i].x][neighbours[i].y].processed = 1;
}
}
}
}
}
bool reduction_entropy::
merge(shared_entropy_surfel target_entropy_surfel,
shared_entropy_surfel_vector const& complete_entropy_surfel_array,
size_t& num_remaining_valid_surfel, size_t num_desired_surfel) const{
size_t num_invalidated_surfels = 0;
shared_entropy_surfel_vector neighbours_to_merge;
//**replace own invalid neighbours by valid neighbours of invalid neighbours**
std::map<size_t, std::set<size_t> > added_neighbours_during_merge;
auto min_distance_ordering = [&target_entropy_surfel](shared_entropy_surfel const& left_entropy_surfel,
shared_entropy_surfel const& right_entropy_surfel) {
double left_en_surfel_distance_measure =
(target_entropy_surfel->contained_surfel->radius() +
left_entropy_surfel->contained_surfel->radius()) -
scm::math::length(target_entropy_surfel->contained_surfel->pos() -
left_entropy_surfel->contained_surfel->pos());
double right_en_surfel_distance_measure =
(target_entropy_surfel->contained_surfel->radius() +
right_entropy_surfel->contained_surfel->radius()) -
scm::math::length(target_entropy_surfel->contained_surfel->pos() -
right_entropy_surfel->contained_surfel->pos());
if(
left_en_surfel_distance_measure
<
right_en_surfel_distance_measure
) {
return true;
} else {
return false;
}
};
//sort neighbours by increasing entropy to current neighbour
std::sort(target_entropy_surfel->neighbours.begin(),
target_entropy_surfel->neighbours.end(),
min_distance_ordering);
shared_entropy_surfel_vector invalidated_neighbours;
for (auto const actual_neighbour_ptr : target_entropy_surfel->neighbours){
if (actual_neighbour_ptr->validity) {
actual_neighbour_ptr->validity = false;
invalidated_neighbours.push_back(actual_neighbour_ptr);
++num_invalidated_surfels;
if( --num_remaining_valid_surfel == num_desired_surfel ) {
break;
}
}
}
for (auto const actual_neighbour_ptr : invalidated_neighbours) {
//iterate the neighbours of the invalid neighbour
for(auto const second_neighbour_ptr : actual_neighbour_ptr->neighbours){
// we only have to consider valid neighbours, all the others are also our own neighbours and already invalid
if(second_neighbour_ptr->validity) {
size_t n_id = second_neighbour_ptr->node_id;
size_t s_id = second_neighbour_ptr->surfel_id;
//avoid getting the surfel itself as neighbour
if( n_id != target_entropy_surfel->node_id ||
s_id != target_entropy_surfel->surfel_id) {
//ignore 2nd neighbours which we found already at another neighbour
if( (added_neighbours_during_merge[n_id]).find(s_id) == added_neighbours_during_merge[n_id].end() ) {
(added_neighbours_during_merge[n_id]).insert(s_id);
neighbours_to_merge.push_back(second_neighbour_ptr);
}
}
}
}
}
add_neighbours(target_entropy_surfel, neighbours_to_merge);
//recompute values for merged surfel
//target_entropy_surfel->level += 1;
update_entropy_surfel_level(target_entropy_surfel, invalidated_neighbours);
update_surfel_attributes(target_entropy_surfel->contained_surfel, invalidated_neighbours);
// now that we , we also have to look for neighbours that we suddenly overlap due to the higher radius
shared_entropy_surfel_vector additional_surfels_to_test_for_overlap;
for( auto const surfel_ptr : complete_entropy_surfel_array) {
if(surfel_ptr->validity) {
if( surfel_ptr->node_id != target_entropy_surfel->node_id ||
surfel_ptr->surfel_id != target_entropy_surfel->surfel_id) {
if( added_neighbours_during_merge[surfel_ptr->node_id].find(surfel_ptr->surfel_id) ==
added_neighbours_during_merge[surfel_ptr->node_id].end()) {
// we did not consider this surfel, we can check for an overlap.
additional_surfels_to_test_for_overlap.push_back(surfel_ptr);
}
}
}
}
auto const additional_overlapping_neighbours
= get_locally_overlapping_neighbours(target_entropy_surfel,
additional_surfels_to_test_for_overlap);
//if(additional_overlapping_neighbours.size() != 0)
// std::cout << "Found something!\n";
add_neighbours(target_entropy_surfel, additional_overlapping_neighbours);
update_entropy(target_entropy_surfel, target_entropy_surfel->neighbours);
if(num_invalidated_surfels == 0)
return false;
if(!target_entropy_surfel->neighbours.empty()) {
return true;
} else {
return false;
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int this_loc, i;
int nx_psi, ny_psi;
int nx_mag, ny_mag;
int nx, ny;
double *psi_uw;
double *psi_w;
double *mag;
double *magth;
double *mask;
double psi_p;
int *visited, *trusted;
int *stack;
int imsize;
int seed_loc;
int nstack;
int nmask;
int nunwrapped;
int m;
double dp;
/* Check for correct number of arguments */
mxAssert(nrhs == 3,
"MEX_Unwrap2D: [psi_uw, mask] = MEX_unwrap2d(psi_w, mag, magthresh)");
mxAssert(nlhs == 2,
"MEX_Unwrap2D: [psi_uw, mask] = MEX_unwrap2d(psi_w, mag, magthresh)");
/* Get matrix dimensions */
nx_psi = mxGetM(PSI_W_MAT);
ny_psi = mxGetN(PSI_W_MAT);
nx_mag = mxGetM(MAG_MAT);
ny_mag = mxGetN(MAG_MAT);
mxAssert((nx_psi == nx_mag) && (ny_psi == ny_mag),
"MEX_Unwrap2D: psi and mag images are different sizes");
nx = nx_psi;
ny = ny_psi;
imsize = nx * ny;
/* Create a real matrix for the return arguments */
PSI_UW_MAT = mxCreateDoubleMatrix(nx, ny, mxREAL);
MASK_MAT = mxCreateDoubleMatrix(nx, ny, mxREAL);
/* Make space for internal matrices and stacks */
trusted = (int *)mxCalloc(imsize, sizeof(int));
visited = (int *)mxCalloc(imsize, sizeof(int));
stack = (int *)mxCalloc(imsize, sizeof(int));
/* Get the data pointers */
psi_w = mxGetPr(PSI_W_MAT);
psi_uw = mxGetPr(PSI_UW_MAT);
mag = mxGetPr(MAG_MAT);
magth = mxGetPr(MAGTH_MAT);
mask = mxGetPr(MASK_MAT);
/************************************************************
* Initialize unwrapped phase matrix, visited map
************************************************************/
nmask = 0;
iloop {
psi_uw[i] = psi_w[i];
trusted[i] = (mag[i] >= *magth);
visited[i] = 0; /* Unvisited*/
mask[i] = trusted[i];
if (mask[i] == 1) nmask++;
}
/************************************************************
* Find the initial seed location
************************************************************/
find_seed(&seed_loc,
mag,
visited,
trusted,
stack,
&nstack,
&nunwrapped,
nx, ny);
/************************************************************
* MAIN REGION GROWING LOOP
************************************************************/
nunwrapped = 0;
while (nunwrapped < nmask) {
while (nstack > 0 && nstack < imsize) {
/* Get location at top of stack */
this_loc = stack[0];
/* Remove this location from the stack */
downstack(&nstack, stack);
psi_p = predict_phase(this_loc,
psi_uw,
visited,
trusted,
nx, ny);
/* Estimate the ambiguity factor for the wrapped phase */
dp = psi_p - psi_w[this_loc];
m = (int)(fabs(dp) / TWO_PI + 0.5);
m = (dp < 0.0) ? -m : m;
/* Unwrap the phase using the ambiguity estimate */
psi_uw[this_loc] = psi_w[this_loc] + m * TWO_PI;
/* Mark this location as unwrapped */
visited[this_loc] = 2;
nunwrapped++;
/* Add this locations neighbours to the end of the stack */
add_neighbours(this_loc,
stack,
visited,
trusted,
&nstack,
nx, ny);
} /* While stack is not empty */
find_seed(&seed_loc,
mag,
visited,
trusted,
stack,
&nstack,
&nunwrapped,
nx, ny);
/************************************************************
* Estimate the phase ambiguity at this seed based on
* previously unwrapped regions
************************************************************/
m = seed_ambiguity(seed_loc,
psi_uw,
visited,
nx, ny);
/* Correct the ambiguity before continuing with the new region growth */
psi_uw[seed_loc] = psi_w[seed_loc] + m * TWO_PI;
} /* While unwrapped pixels still exist in trusted regions */
if (nstack >= imsize)
mexPrintf("Stack grew too large - aborting\n");
/* Clean up */
mxFree(visited);
mxFree(trusted);
mxFree(stack);
}
Vertex_info * getNext()
{
if (finalized_vertices.size () > 0)
{
dias::vertex_id v_id = finalized_vertices.front ();
finalized_vertices.pop ();
dias::vertex_db::iterator v_ptr = vertices.find (v_id);
assert (v_ptr != vertices.end ());
Vertex_info * result = new Vertex_info;
result->vid = v_id;
result->vi = v_ptr->second;
vertices.erase (v_ptr);
return result;
}
if (!f.eof ())
{
while (finalized_vertices.size () == 0 && !f.eof ())
{
std::string line = "";
while (line == "")
{
if (f.eof ())
break;
std::getline (f, line);
}
if (f.eof ())
break;
boost::char_delimiters_separator<char> sep (false, "", 0);
boost::tokenizer<> tokens (line, sep);
boost::tokenizer<>::iterator p = tokens.begin ();
std::string type = *p++;
if (type == "#")
continue;
if (type == "v")
read_vertex (p, tokens.end ());
else if (type == "c")
{
dias::tetrahedra t
= read_tetrahedra (p, tokens.end ());
add_neighbours (t);
}
else
assert (false);
}
if (finalized_vertices.size () > 0)
return getNext ();
}
assert (vertices.size () > 0);
assert (finalized_vertices.size () == 0);
dias::vertex_db::const_iterator p = vertices.begin ();
finalized_vertices.push (p->first);
return getNext ();
}
