diff --git a/src/vw/Stereo/CorrelationView.tcc b/src/vw/Stereo/CorrelationView.tcc index e609d212c..94d22ecf4 100644 --- a/src/vw/Stereo/CorrelationView.tcc +++ b/src/vw/Stereo/CorrelationView.tcc @@ -309,7 +309,8 @@ prerasterize(BBox2i const& bbox) const { // Don't use SGM for larger regions! //bool use_sgm_on_level = (m_use_sgm && (zones.size() == 1)); // TODO: May need to redo this check. - bool use_sgm_on_level = (m_use_sgm && (!on_last_level)); + //bool use_sgm_on_level = (m_use_sgm && (!on_last_level)); + bool use_sgm_on_level = (m_use_sgm); //if (use_sgm_on_level) { // std::cout << "Search parameter workload = " << zones[0].search_volume() << std::endl; // use_sgm_on_level = (zones[0].search_volume() < MAX_SGM_WORKLOAD); @@ -461,29 +462,31 @@ prerasterize(BBox2i const& bbox) const { const float rm_min_matches_percent = 0.5; const float rm_threshold = 3.0; - // TODO: What if SGM used on last level? - if ( !on_last_level ) { - disparity = disparity_mask(disparity_cleanup_using_thresh - (disparity, - rm_half_kernel, rm_half_kernel, - rm_threshold, - rm_min_matches_percent), - left_mask_pyramid[level], - right_mask_pyramid[level]); - } else { - // We don't do a single hot pixel check on the final level as it leaves a border. - disparity = disparity_mask(rm_outliers_using_thresh - (disparity, - rm_half_kernel, rm_half_kernel, - rm_threshold, - rm_min_matches_percent), - left_mask_pyramid[level], - right_mask_pyramid[level]); - } - + // At least for debugging, skip the filtering step with SGM outputs. + if (!use_sgm_on_level) { + if ( !on_last_level ) { + disparity = disparity_mask(disparity_cleanup_using_thresh + (disparity, + rm_half_kernel, rm_half_kernel, + rm_threshold, + rm_min_matches_percent), + left_mask_pyramid[level], + right_mask_pyramid[level]); + } else { + // We don't do a single hot pixel check on the final level as it leaves a border. + disparity = disparity_mask(rm_outliers_using_thresh + (disparity, + rm_half_kernel, rm_half_kernel, + rm_threshold, + rm_min_matches_percent), + left_mask_pyramid[level], + right_mask_pyramid[level]); + } - // The kernel based filtering tends to leave isolated blobs behind. - disparity_blob_filter(disparity, level, m_blob_filter_area); + // The kernel based filtering tends to leave isolated blobs behind. + disparity_blob_filter(disparity, level, m_blob_filter_area); + + } // 3.2b) Refine search estimates but never let them go beyond // the search region defined by the user diff --git a/src/vw/Stereo/SGM.cc b/src/vw/Stereo/SGM.cc index ccb1d8399..35ec34bea 100644 --- a/src/vw/Stereo/SGM.cc +++ b/src/vw/Stereo/SGM.cc @@ -137,7 +137,7 @@ void SemiGlobalMatcher::populate_disp_bound_image(DisparityImage const* prev_dis std::cout << "Mean pixel search area = " << area << std::endl; std::cout << "Percent trusted prior disparities = " << percent_trusted << std::endl; } - +/* SemiGlobalMatcher::AccumCostType SemiGlobalMatcher::get_disparity_dist(DisparityType d1, DisparityType d2) { //// This is the traditional way for 1D disparities @@ -154,7 +154,56 @@ SemiGlobalMatcher::AccumCostType SemiGlobalMatcher::get_disparity_dist(Disparity //return delX*delX + delY*delY; return abs(delX) + abs(delY); // How does this method compare? +}*/ + +void SemiGlobalMatcher::populate_adjacent_disp_lookup_table() { + + std::cout << "Generating lookup table...\n"; + + const int TABLE_WIDTH = 8; + m_adjacent_disp_lookup.resize(m_num_disp*TABLE_WIDTH); + + // Loop through the disparities + int d = 0; + for (int dy=m_min_disp_y; dy<=m_max_disp_y; ++dy) { + // Figure out above and below disparities with bounds checking + int y_less = dy - 1; + int y_more = dy + 1; + if (y_less < m_min_disp_y) y_less = dy; + if (y_more > m_max_disp_y) y_more = dy; + + int y_less_o = y_less - m_min_disp_y; // Offset from min y disparity + int y_o = dy - m_min_disp_y; + int y_more_o = y_more - m_min_disp_y; + + for (int dx=m_min_disp_x; dx<=m_max_disp_x; ++dx) { + + // Figure out left and right disparities with bounds checking + int x_less = dx - 1; + int x_more = dx + 1; + if (x_less < m_min_disp_x) x_less = dx; + if (x_more > m_max_disp_x) x_more = dx; + + int x_less_o = x_less - m_min_disp_x; // Offset from min x disparity + int x_o = dx - m_min_disp_x; + int x_more_o = x_more - m_min_disp_x; + + // Record the disparity indices of each of the adjacent pixels + int table_pos = d*TABLE_WIDTH; + m_adjacent_disp_lookup[table_pos+0] = y_less_o*m_num_disp_x + x_less_o; + m_adjacent_disp_lookup[table_pos+1] = y_less_o*m_num_disp_x + x_o; + m_adjacent_disp_lookup[table_pos+2] = y_less_o*m_num_disp_x + x_more_o; + m_adjacent_disp_lookup[table_pos+3] = y_o *m_num_disp_x + x_less_o; + m_adjacent_disp_lookup[table_pos+4] = y_o *m_num_disp_x + x_more_o; + m_adjacent_disp_lookup[table_pos+5] = y_more_o*m_num_disp_x + x_less_o; + m_adjacent_disp_lookup[table_pos+6] = y_more_o*m_num_disp_x + x_o; + m_adjacent_disp_lookup[table_pos+7] = y_more_o*m_num_disp_x + x_more_o; + + ++d; + } + } + std::cout << "Finished generating lookup table.\n"; } void SemiGlobalMatcher::evaluate_path( int col, int row, int col_p, int row_p, @@ -162,7 +211,17 @@ void SemiGlobalMatcher::evaluate_path( int col, int row, int col_p, int row_p, CostType * const local, // The disparity costs of the current pixel AccumCostType* output, int path_intensity_gradient, bool debug ) { - + + // TODO: Consider intensity diff + AccumCostType P_A = 20; // Dist = 1 + AccumCostType P_B = 20; // Dist = 2 + AccumCostType P_C = 200; // Dist > 2 + + if (path_intensity_gradient > 0) + P_C /= path_intensity_gradient; + if (P_C < P_B) + P_C = P_B; + // Init the output costs to a large value so that we don't // use disparity combinations that we don't compute. std::vector curr_cost(m_num_disp); @@ -171,6 +230,20 @@ void SemiGlobalMatcher::evaluate_path( int col, int row, int col_p, int row_p, Vector4i pixel_disp_bounds = m_disp_bound_image(col, row); Vector4i pixel_disp_bounds_p = m_disp_bound_image(col_p, row_p); + + + // TODO: Get this from the previous iteration! + DisparityType min_prev_disparity_index = 0; + CostType min_prev_disparity_cost = prior[0]; + for (DisparityType i=1; i::max(); - // Loop through disparities in prior pixel - for (int dy_p=pixel_disp_bounds_p[1]; dy_p<=pixel_disp_bounds_p[3]; ++dy_p) { + // Start with the cost for the same disparity + AccumCostType lowest_combined_cost = prior[d]; + + // Compare to the eight adjacent disparities using the lookup table + const int LOOKUP_TABLE_WIDTH = 8; + int lookup_index = d*LOOKUP_TABLE_WIDTH; + AccumCostType lowest_adjacent_cost = prior[m_adjacent_disp_lookup[lookup_index+0]]; + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+1]]); + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+2]]); + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+3]]); + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+4]]); + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+5]]); + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+6]]); + lowest_adjacent_cost = std::min(lowest_adjacent_cost, prior[m_adjacent_disp_lookup[lookup_index+7]]); + // Now add the adjacent penalty cost + lowest_combined_cost = std::min(lowest_combined_cost, lowest_adjacent_cost+P_A); + + // Compare to the lowest prev disparity cost regardless of location + lowest_combined_cost = std::min(lowest_combined_cost, min_prev_disparity_cost+P_C); - AccumCostType y_disp_diff = abs(dy_p - dy); // Precompute part of distance - - // Get initial linear storage index for this dy row - int d_p = (dy_p-m_min_disp_y)*m_num_disp_x + - (pixel_disp_bounds_p[0] - m_min_disp_x); - - for (int dx_p=pixel_disp_bounds_p[0]; dx_p<=pixel_disp_bounds_p[2]; ++dx_p) { - - // This is the costliest portion of the entire algorithm since it is executed so many times! - - // The cost to reach the disparity at the previous pixel - AccumCostType prior_cost = prior[d_p]; - // Physical disparity distance from that disparity to this one - AccumCostType disparity_diff = abs(dx_p - dx) + y_disp_diff; // TODO: Experiment with this! - - // TODO: Consider intensity diff - const AccumCostType P_A = 20; // TODO: This needs to be related to the local cost sizes! - AccumCostType penalty = disparity_diff * P_A; - - lowest_combined_cost = std::min(lowest_combined_cost, prior_cost+penalty); - - //if ((debug)){ - // printf("d: %d, d_p %d, curr_cost = %d, lcc = %d, prior_cost = %d, penalty = %d\n", - // d, d_p, curr_cost[d], lowest_combined_cost, prior_cost, penalty); - //} - - ++d_p; - } - } // End loop through other pixel disparities // The output cost = local cost + lowest combined cost curr_cost[d] = local[d] + lowest_combined_cost; @@ -231,7 +289,7 @@ void SemiGlobalMatcher::evaluate_path( int col, int row, int col_p, int row_p, //print_disparity_vector(prior); //std::cout << "min_prior = " << min_prior << " output = " ; for (int i=0; i const& left_image m_buffer_step_size = m_num_output_cols * m_num_disp; + populate_adjacent_disp_lookup_table(); + // By default the search bounds are the same for each image, // but set them from the zones if the user passed them in. populate_constant_disp_bound_image(); @@ -347,42 +407,35 @@ SemiGlobalMatcher::semi_global_matching_func( ImageView const& left_image m_cost_buffer.reset(new CostType[num_cost_elements]); for (size_t i=0; i::max(); + for ( int dy = m_min_disp_y; dy <= m_max_disp_y; dy++ ) { // For each disparity + for ( int dx = m_min_disp_x; dx <= m_max_disp_x; dx++ ) { + bool debug = ((i == 73) && (j == 159)); size_t cost_index = get_cost_index(i-m_min_col, j-m_min_col, d); - m_cost_buffer[cost_index] = get_cost(left_image, right_image, i, j, i+dx,j+dy, debug); + CostType cost = get_cost(left_image, right_image, i, j, i+dx,j+dy, debug); + m_cost_buffer[cost_index] = cost; - //if (debug) { - // printf("Done with loc %d, %d -->%d\n", i, j, costs(i,j)[d]); - //} - - } // End x loop - }// End y loop - //printf("Done with disparity %d, %d\n", dx, dy); - ++d; // Disparity values are stored in a vector for convenience. - } - } // End disparity loops + ++d; // Disparity values are stored in a vector for convenience. + } + } // End disparity loops + + } // End x loop + }// End y loop //std::cout << "Loc 219, 173 costs --> " << costs(219,173) << std::endl; } @@ -406,7 +459,8 @@ SemiGlobalMatcher::semi_global_matching_func( ImageView const& left_image // ==== Accumulate the global costs across paths ==== - boost::shared_array dir_accumulated_costs; // TODO: Eliminate this! + // TODO: Possible to remove the temporary buffer? + boost::shared_array dir_accumulated_costs; dir_accumulated_costs.reset(new AccumCostType[num_cost_elements]); m_accum_buffer.reset( new AccumCostType[num_cost_elements]); for (size_t i=0; i m_disp_bound_image; + /// Lookup table of the adjacent disparities for each disparity + /// - Handles outer boundaries by repitition. + std::vector m_adjacent_disp_lookup; private: // Functions + /// Populate the lookup table m_adjacent_disp_lookup + void populate_adjacent_disp_lookup_table(); + /// Fill in m_disp_bound_image using image-wide contstants void populate_constant_disp_bound_image(); @@ -125,13 +131,15 @@ class SemiGlobalMatcher { create_disparity_view( boost::shared_array const accumulated_costs ); /// Converts from a linear disparity index to the dx, dy values it represents. + /// - This function is too slow to use inside the inner loop! void disp_to_xy(DisparityType disp, DisparityType &dx, DisparityType &dy) { dy = (disp / m_num_disp_x) + m_min_disp_y; // 2D implementation dx = (disp % m_num_disp_x) + m_min_disp_x; } - /// Compute the physical distance between the disparity values of two adjacent pixels. - AccumCostType get_disparity_dist(DisparityType d1, DisparityType d2); + // This function is unused to allow more optimization at the inner loop. + ///// Compute the physical distance between the disparity values of two adjacent pixels. + //AccumCostType get_disparity_dist(DisparityType d1, DisparityType d2); @@ -206,13 +214,13 @@ class SemiGlobalMatcher { template void iterate_direction( ImageView const& left_image, boost::shared_array & accumulated_costs ) { - +/* // Init the output costs to the max value. // - This means that disparity/location pairs we don't update will never be used. size_t num_cost_elements = m_num_output_cols*m_num_output_rows*m_num_disp; for (size_t i=0; i::max(); - + */ // Walk along the edges in a clockwise fashion if ( DIRX > 0 ) { @@ -221,7 +229,6 @@ class SemiGlobalMatcher { for ( int32 j = 0; j < m_num_output_rows; j++ ) { add_cost_vector(0, j, accumulated_costs); } - //std::cout << "L costs: " << costs(0,185) << std::endl; // Loop across to the opposite edge for ( int32 i = 1; i < m_num_output_cols; i++ ) { // Loop through the pixels in this column, limiting the range according @@ -234,13 +241,7 @@ class SemiGlobalMatcher { get_accum_vector(accumulated_costs, i-DIRX,j-DIRY), // Previous pixel get_cost_vector(i,j), get_accum_vector(accumulated_costs, i,j), - pixel_diff, false ); // Current pixel - - //if (j == 185) { - // std::cout << "i: "<