Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Move/refactor several methods out of StereoMatching class #3566

Merged
merged 1 commit into from
Jan 19, 2020
Merged

Move/refactor several methods out of StereoMatching class #3566

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
116 changes: 43 additions & 73 deletions stereo/include/pcl/stereo/stereo_matching.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,53 @@

#pragma once

#include <algorithm>

#include <pcl/conversions.h>
#include <pcl/point_types.h>

namespace pcl {
template <class T>
short int
doStereoRatioFilter(const T* const acc,
short int dbest,
T sad_min,
int ratio_filter,
int maxdisp,
int precision = 100)
{
const auto sad_min_1st_part_it = std::min_element(acc, acc + dbest - 1);
const auto sad_min_2nd_part_it = std::min_element(acc + dbest + 2, acc + maxdisp);

const auto sad_second_min = std::min(*sad_min_1st_part_it, *sad_min_2nd_part_it);

if ((sad_min * precision) > ((precision - ratio_filter) * sad_second_min)) {
return -2;
}
return dbest;
}

template <class T>
inline short int
doStereoPeakFilter(const T* const acc, short int dbest, int peak_filter, int maxdisp)
{
// da and db = acc[index] - acc[dbest],
// where index = (dbest + 2) or (dbest - 2)
// => index = dbest + 2 - (0 or 4) = dbest - 2 + (0 or 4)
// => index = dbest + 2 - (0 << 2 or 1 << 2) = dbest - 2 + (0 << 2 or 1 << 2)
// => index = dbest + 2 - (condition << 2) = dbest - 2 + (condition << 2)
const auto da_condition = (dbest > 1);
const auto db_condition = (dbest < maxdisp - 2);
const auto da_index = dbest + 2 - (da_condition << 2);
const auto db_index = dbest - 2 + (db_condition << 2);

const auto da = acc[da_index] - acc[dbest];
const auto db = acc[db_index] - acc[dbest];
if ((da + db) < peak_filter) {
return -4;
}
return dbest;
}

/** \brief Stereo Matching abstract class
*
Expand Down Expand Up @@ -315,79 +358,6 @@ class PCL_EXPORTS StereoMatching {
std::floor(.5 + (((s1 - s3) * 8) / den))));
return (static_cast<short int>(dbest * 16));
}

inline short int
doStereoRatioFilter(int* acc,
short int dbest,
int sad_min,
int ratio_filter,
int maxdisp,
int precision = 100)
{
int sad_second_min = std::numeric_limits<int>::max();

for (int d = 0; d < dbest - 1; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];

for (int d = dbest + 2; d < maxdisp; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];

if (sad_min * precision > (precision - ratio_filter) * sad_second_min)
return (-2);
return (dbest);
}

inline short int
doStereoRatioFilter(float* acc,
short int dbest,
float sad_min,
int ratio_filter,
int maxdisp,
int precision = 100)
{
float sad_second_min = std::numeric_limits<float>::max();

for (int d = 0; d < dbest - 1; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];

for (int d = dbest + 2; d < maxdisp; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];

if (sad_min * static_cast<float>(precision) >
static_cast<float>(precision - ratio_filter) * sad_second_min)
return (-2);
return (dbest);
}

inline short int
doStereoPeakFilter(int* acc, short int dbest, int peak_filter, int maxdisp)
{
int da =
(dbest > 1) ? (acc[dbest - 2] - acc[dbest]) : (acc[dbest + 2] - acc[dbest]);
int db = (dbest < maxdisp - 2) ? (acc[dbest + 2] - acc[dbest])
: (acc[dbest - 2] - acc[dbest]);

if (da + db < peak_filter)
return (-4);
return (dbest);
}

inline short int
doStereoPeakFilter(float* acc, short int dbest, int peak_filter, int maxdisp)
{
float da =
(dbest > 1) ? (acc[dbest - 2] - acc[dbest]) : (acc[dbest + 2] - acc[dbest]);
float db = (dbest < maxdisp - 2) ? (acc[dbest + 2] - acc[dbest])
: (acc[dbest - 2] - acc[dbest]);

if (da + db < peak_filter)
return (-4);
return (dbest);
}
};

/** \brief Stereo Matching abstract class for Grayscale images
Expand Down