google-research

1
// Copyright 2024 The Google Research Authors.
2
//
3
// Licensed under the Apache License, Version 2.0 (the "License");
4
// you may not use this file except in compliance with the License.
5
// You may obtain a copy of the License at
6
//
7
//     http://www.apache.org/licenses/LICENSE-2.0
8
//
9
// Unless required by applicable law or agreed to in writing, software
10
// distributed under the License is distributed on an "AS IS" BASIS,
11
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
// See the License for the specific language governing permissions and
13
// limitations under the License.
14

15
// Implements the two black boxes, surface definition and surface-box
16
// intersection predicate, (required for the general voter), for the toy example
17
// of 2d line fitting used in our paper:
18
//
19
// Dror Aiger, Simon Lynen, Jan Hosang, Bernhard Zeisl:
20
// Efficient Large Scale Inlier Voting for Geometric Vision Problems. iccv
21
// (2021)
22

23
#include <stdio.h>
24

25
#include <fstream>
26
#include <iostream>
27

28
#include "Eigen/Core"
29
#include "Eigen/Geometry"
30
#include "absl/flags/flag.h"
31
#include "absl/flags/parse.h"
32
#include "absl/random/distributions.h"
33
#include "absl/random/random.h"
34
#include "absl/time/time.h"
35
#include "opencv2/core.hpp"
36
#include "opencv2/core/core.hpp"
37
#include "opencv2/core/types.hpp"
38
#include "opencv2/highgui/highgui.hpp"
39
#include "opencv2/imgcodecs.hpp"
40
#include "opencv2/imgproc.hpp"
41
#include "opencv2/imgproc/imgproc.hpp"
42
#include "general_voter.h"
43

44
ABSL_FLAG(int, num_points, 100000, "Number of points");
45
ABSL_FLAG(bool, use_canon, true, "Use canonization");
46
ABSL_FLAG(bool, use_ransac, false, "Use ransac");
47
ABSL_FLAG(double, inlier_fraction, 0.01, "Inlier fraction");
48

49
namespace general_voter_2d_line_fitting {
50

51
using Vector1f = Eigen::Matrix<float, 1, 1>;
52

53
// Surface function for a 2d line.
54
Vector1f LineSurface(const Vector1f& x, const Vector1f& essential_parameters) {
55
  return x * essential_parameters;
56
}
57

58
// Intersection function for a 2d line.
59
bool DoesLineIntersectOct(const Vector1f& essential_parameters,
60
                          const Vector1f& free_parameters,
61
                          const large_scale_voting::HyperOct<2>& oct) {
62
  const float y1 = oct.min.x() * essential_parameters(0) + free_parameters(0);
63
  const float y2 = oct.max.x() * essential_parameters(0) + free_parameters(0);
64
  const float y_min = std::min(y1, y2);
65
  const float y_max = std::max(y1, y2);
66
  const bool y_range_outside = y_max < oct.min.y() || y_min > oct.max.y();
67
  return !y_range_outside;
68
}
69

70
Eigen::Vector2d LineParameters(const Eigen::Vector2d& p0,
71
                               const Eigen::Vector2d& p1) {
72
  const float a = -(-p0.y() + p1.y()) / (p0.x() - p1.x());
73
  const float b = -(p1.x() * p0.y() - p0.x() * p1.y()) / (p0.x() - p1.x());
74
  return Eigen::Vector2d(a, b);
75
}
76

77
int CountNearest(Eigen::Vector2d& line,
78
                 const std::vector<Eigen::Vector2d>& points, float distance) {
79
  int ret = 0;
80
  for (const auto& p : points) {
81
    const float vertical_distance = std::fabs(line(0) * p(0) + line(1) - p(1));
82
    if (vertical_distance < distance) ret++;
83
  }
84
  return ret;
85
}
86

87
// A quick ransac implementation.
88
std::pair<int, Eigen::Vector2d> Ransac(
89
    const std::vector<Eigen::Vector2d>& points, float inliers_fraction,
90
    float distance) {
91
  std::pair<int, Eigen::Vector2d> ret(0, {});
92
  std::default_random_engine reng(1962);
93
  std::uniform_int_distribution<int> uniform_line(0, points.size() - 1);
94
  const int k =
95
      std::log(0.001) / std::log(1.0 - (inliers_fraction * inliers_fraction));
96
  for (int i = 0; i < k; ++i) {
97
    const auto id1 = uniform_line(reng);
98
    const auto id2 = uniform_line(reng);
99
    if (id1 != id2) {
100
      Eigen::Vector2d line = LineParameters(points[id1], points[id2]);
101
      const auto count = CountNearest(line, points, distance);
102
      if (count > ret.first) {
103
        ret.first = count;
104
        ret.second = line;
105
      }
106
    }
107
  }
108
  return ret;
109
}
110

111
int Run() {
112
  std::vector<Eigen::Vector2d> points(absl::GetFlag(FLAGS_num_points));
113
  // Create input data by sampling from a given true line (with noise) and add
114
  // outliers as random points. We use here only lines with slope in [-1,1] to
115
  // satisfy the gradient requirement of the surfaces (for canonization and
116
  // duality, see the paper). This however is no restriction as we can divide
117
  // the entire space of parameters into two and run the second half similarly
118
  // after an appropriate rotation, making sure we eval all posible minimum oct
119
  // results from both (the complexity does not change).
120
  std::default_random_engine reng(1962);
121
  std::uniform_real_distribution<float> uniform_line(-1.0, 1.0);
122
  std::uniform_real_distribution<float> uniform_point(0, 1.0);
123
  std::normal_distribution<float> gaussian_noise{0, 0.002};
124
  const double line_a = 1;
125
  const double line_b = 0.2;
126
  const float kInlierFraction = absl::GetFlag(FLAGS_inlier_fraction);
127
  std::cout << "true line: " << line_a << " " << line_b << std::endl;
128
  for (auto& p : points) {
129
    if (uniform_point(reng) < kInlierFraction) {
130
      const double x = uniform_point(reng);
131
      p = Eigen::Vector2d(x, x * line_a + line_b + gaussian_noise(reng));
132
    } else {
133
      p = Eigen::Vector2d(uniform_point(reng), uniform_point(reng));
134
    }
135
  }
136

137
  constexpr double kEpsilon = 0.002;
138

139
  // Use ransac.
140
  if (absl::GetFlag(FLAGS_use_ransac)) {
141
    absl::Time t1 = absl::Now();
142
    std::pair<int, Eigen::Vector2d> ransac =
143
        Ransac(points, kInlierFraction, kEpsilon);
144
    std::cout << "time: " << absl::ToDoubleSeconds(absl::Now() - t1) << " "
145
              << "line: " << ransac.first << " " << ransac.second.transpose()
146
              << " "
147
              << "errors: " << std::fabs(ransac.second(0) - line_a) << " "
148
              << std::fabs(ransac.second(1) - line_b) << std::endl;
149
    return 0;
150
  }
151

152
  // Our surface is a 1d surface embedded in the 2-space.
153
  using Surface2f = large_scale_voting::Surface<2, 1, 1, float>;
154

155
  constexpr double kSpaceSize = 2.0;
156

157
  absl::Time t1 = absl::Now();
158
  std::vector<Surface2f> surfaces;
159
  // Create a surface for each point, using the dual of the point as a 2d line.
160
  for (const auto& p : points) {
161
    const float a = p.x();
162
    const float b = -p.y();
163
    surfaces.push_back(Surface2f(
164
        /*essential_parameters=*/Surface2f::EssentialParameterVector(a),
165
        /*free_parameters=*/Surface2f::FreeVector(b),
166
        /*surface definition*/ LineSurface,
167
        /*surface-box intersection predicate*/ DoesLineIntersectOct));
168
  }
169

170
  // The parameter space.
171
  const large_scale_voting::HyperOct<2> space{
172
      .min = Eigen::Vector2f(-kSpaceSize / 2.0, -kSpaceSize / 2.0),
173
      .max = Eigen::Vector2f(kSpaceSize / 2.0, kSpaceSize / 2.0)};
174
  const Eigen::Vector2f min_oct_side_length(kEpsilon, kEpsilon);
175

176
  // Canonization parameters found empirically.
177
  typename Surface2f::EssentialParameterVector essential_vector(0.005);
178
  typename Surface2f::FreeVector free_vector(0.001);
179

180
  using VoterT = large_scale_voting::GeneralVoter<2, Surface2f>;
181
  using ScoreFn = VoterT::ScoreHypothesisFn;
182
  using Maximum = VoterT::Maximum;
183

184
  // We don't have priors so we just use the score order (of search).
185
  const ScoreFn score_by_score = [](const large_scale_voting::HyperOct<2>& oct,
186
                                    float score) {
187
    return static_cast<float>(score);
188
  };
189

190
  // No verification function required.
191
  const auto verification_function =
192
      [&](const large_scale_voting::HyperOct<2>& oct,
193
          const std::vector<int>& inliers) { return inliers; };
194

195
  // Define the general voter.
196
  VoterT voter(min_oct_side_length, 1,
197
               /*max_num_results=*/1, essential_vector, free_vector,
198
               /*minimum number of intersections*/ 3,
199
               /*maximum number of intersections*/ 1e9,
200
               /*ext_factor*/ 0.5, /*maximum time seconds*/ 1e9, score_by_score,
201
               verification_function, absl::GetFlag(FLAGS_use_canon));
202

203
  // Run the voter and obtain results.
204
  const std::vector<Maximum> results = *voter.Vote(surfaces, space);
205
  const Eigen::Vector2d result_line(results[0].oct.mid()(0),
206
                                    -results[0].oct.mid()(1));
207
  std::cout << "time: " << absl::ToDoubleSeconds(absl::Now() - t1) << " "
208
            << "line: " << results[0].num_intersections << " "
209
            << result_line.transpose() << " "
210
            << "errors: " << std::fabs(result_line(0) - line_a) << " "
211
            << std::fabs(result_line(1) - line_b) << std::endl;
212

213
  // Draw debug image.
214
  cv::Mat image = cv::Mat::ones(cv::Size(500, 500), CV_8UC3);
215
  for (const auto& p : points) {
216
    const cv::Point2d p2_draw(p(0), p(1));
217
    cv::circle(image, p2_draw * 500, 1, cv::Scalar(255, 255, 255));
218
  }
219
  cv::imwrite("/tmp/points.png", image);
220
  for (const auto& i : results[0].surface_indices) {
221
    const Eigen::Vector2d p2 = points[i];
222
    const cv::Point2d p2_draw(p2(0), p2(1));
223
    cv::circle(image, p2_draw * 500, 3, cv::Scalar(0, 255, 0), 2);
224
  }
225
  cv::imwrite("/tmp/points_line.png", image);
226
  return 0;
227
}
228
}  // namespace general_voter_2d_line_fitting
229

230
int main(int argc, char* argv[]) {
231
  absl::ParseCommandLine(argc, argv);
232
  return general_voter_2d_line_fitting::Run();
233
}
234