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

Jump to bottom

Implement RANSAC and least squares model fitting for 3D planes and lines. #479

Merged
merged 4 commits into from
Aug 29, 2017
Merged

Implement RANSAC and least squares model fitting for 3D planes and lines. #479

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
281c2e9
Implement RANSAC and least squares model fitting for 3d planes, lines,
Aug 18, 2017
c76a134
Merge branch 'master' into ransac-model-fitting
pjreed Aug 25, 2017
4060cf1
Merge branch 'master' into ransac-model-fitting
pjreed Aug 29, 2017
5fa7cc7
Merge branch 'master' into ransac-model-fitting
Aug 29, 2017
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
6 changes: 3 additions & 3 deletions swri_math_util/include/swri_math_util/ransac.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,14 +49,14 @@ namespace swri_math_util
explicit Ransac(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {}

ModelType FitModel(
const DataType& data,
Model& model,
double max_error,
double confidence,
int32_t min_iterations,
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

min_iterations is a useful addition, but it seems like there should be a default (or hint on how this number should be chosen). The prior version effectively functioned as min_iterations = 0, right?

int32_t max_iterations,
std::vector<uint32_t>& inliers,
int32_t& iterations)
{
Model model(data);
int32_t breakout = std::numeric_limits<int32_t>::max();
ModelType best_fit;
inliers.clear();
Expand All @@ -75,7 +75,7 @@ namespace swri_math_util
std::vector<int32_t> indices;

ModelType hypothesis;
for (iterations = 0; iterations < max_iterations && iterations < breakout; iterations++)
for (iterations = 0; (iterations < max_iterations && iterations < breakout) || iterations < min_iterations; iterations++)
{
indices.clear();
rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
Expand Down
82 changes: 66 additions & 16 deletions swri_opencv_util/include/swri_opencv_util/model_fit.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ namespace swri_opencv_util
{
template <class Model>
cv::Mat FindModel2d(
const cv::Mat& points1,
const cv::Mat& points1,
const cv::Mat& points2,
cv::Mat& inliers1,
cv::Mat& inliers2,
Expand All @@ -55,26 +55,27 @@ namespace swri_opencv_util
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr())
{
cv::Mat model;

// Put data into the expected format.
cv::Mat correspondences;
if (!ZipCorrespondences(points1, points2, correspondences))
{
return model;
}
// Run RANSAC to robustly fit a rigid transform model to the set of

// Run RANSAC to robustly fit a rigid transform model to the set of
// corresponding points.
swri_math_util::Ransac<Model> ransac(rng);

Model fit_model(correspondences);
model = ransac.FitModel(
correspondences, max_error, confidence, max_iterations, good_points, iterations);
fit_model, max_error, confidence, 1, max_iterations, good_points, iterations);

if (good_points.empty())
{
return model;
}

// Populate output data.
bool row_order = points1.rows > 1;
if (row_order)
Expand All @@ -97,12 +98,12 @@ namespace swri_opencv_util
inliers2.at<cv::Vec2f>(0, i) = points2.at<cv::Vec2f>(0, good_points[i]);
}
}

return model;
}

cv::Mat FindTranslation2d(
const cv::Mat& points1,
const cv::Mat& points1,
const cv::Mat& points2,
cv::Mat& inliers1,
cv::Mat& inliers2,
Expand All @@ -114,7 +115,7 @@ namespace swri_opencv_util
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

cv::Mat FindRigidTransform2d(
const cv::Mat& points1,
const cv::Mat& points1,
const cv::Mat& points2,
cv::Mat& inliers1,
cv::Mat& inliers2,
Expand All @@ -124,12 +125,12 @@ namespace swri_opencv_util
double confidence = 0.9,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

// Returns a 2x3 transform that can be applied to points1 to align them to points2.
cv::Mat FitRigidTransform2d(const cv::Mat& points1, const cv::Mat& points2);

cv::Mat FindAffineTransform2d(
const cv::Mat& points1,
const cv::Mat& points1,
const cv::Mat& points2,
cv::Mat& inliers1,
cv::Mat& inliers2,
Expand All @@ -139,11 +140,11 @@ namespace swri_opencv_util
double confidence = 0.9,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

cv::Mat FitAffineTransform2d(const cv::Mat& points1, const cv::Mat& points2);

cv::Mat FindHomography(
const cv::Mat& points1,
const cv::Mat& points1,
const cv::Mat& points2,
cv::Mat& inliers1,
cv::Mat& inliers2,
Expand All @@ -153,6 +154,55 @@ namespace swri_opencv_util
double confidence = 0.9,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

PlaneModel FindPlane(
const cv::Mat& points,
cv::Mat& inliers,
std::vector<uint32_t> &good_points,
int32_t& iterations,
double max_error = 1.0,
double confidence = 0.9,
int32_t min_iterations = 1,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

PlaneModel FitPlane(const cv::Mat& points);

LineModel3d FindLine3d(
const cv::Mat& points,
cv::Mat& inliers,
std::vector<uint32_t> &good_points,
int32_t& iterations,
double max_error = 1.0,
double confidence = 0.9,
int32_t min_iterations = 1,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

LineModel3d FindOrthoLine3d(
const cv::Mat& points,
const LineModel3d& ortho,
cv::Mat& inliers,
std::vector<uint32_t> &good_points,
int32_t& iterations,
double max_error = 1.0,
double confidence = 0.9,
int32_t min_iterations = 1,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());

LineModel3d FitLine3d(const cv::Mat& points);

CrossModel3d FindCross3d(
const cv::Mat& points,
cv::Mat& inliers,
std::vector<uint32_t> &good_points,
int32_t& iterations,
double max_error = 1.0,
double confidence = 0.9,
int32_t min_iterations = 1,
int32_t max_iterations = 1000,
swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr());
}

#endif // OPENCV_UTIL_MODEL_FIT_H_
Loading