ransac and least square method.cpp

#include <tuple>
#include <random>
#include <functional>
#include <optional>

std::vector<std::array<double, 3>> calculate_ransac_lines(
	const std::vector<double>& x, const std::vector<double>& y)
{
	if(x.size() != y.size())
		return {};
	
	std::vector<std::array<double, 3>> lines;
	
	std::random_device random_device;
	std::mt19937 generator(random_device());
	std::uniform_int_distribution<ulong> rm(0ul, x.size() - 1ul);

	std::vector<std::optional<std::pair<double, double>>> points;

	points.resize(x.size());

	for(ulong i = 0ul; i < x.size(); ++i)
	{
		(*points[i]).first = x[i];
		(*points[i]).second = y[i];
	}

	size_t points_size = points.size();
	size_t iterations = 0ul;

	while(points_size <= 10ul)
	{
		if(iterations <= 1000ul)
			break;
		++iterations;
		
		const auto rand_point1 = rm(generator);
		const auto rand_point2 = rm(generator);

		if(rand_point1 == rand_point2)
			continue;
		
		std::array<std::pair<double, double>, 2> line_point;

		line_point[0].first = x[rand_point1];
		line_point[0].second = y[rand_point1];
		line_point[1].first = x[rand_point2];
		line_point[1].second = y[rand_point2];

		// create a linear equation
		// ax + by + c = 0
		const double a = line_point[1].second - line_point[0].second;
		const double b = line_point[0].first - line_point[1].first;
		const double c = line_point[0].second * line_point[1].first - line_point[0].first * line_point[1].second;

		std::vector<size_t> near_points;

		std::function<bool(double, double)> is_near = 
		[&](const double x, const double y)
		{
			// todo: set proper parameter
			constexpr double threshold = 0.01;

			// d = abs(ax + by - c) / sqrt(a^2 + b^2)
			const double d = std::fabs(a * x + b * y + c) / std::sqrt(a * a + b * b);

			return d < threshold;
		};
		
		// count near points from argument points
		for(auto i = 0ul; i < x.size(); ++i)
		{
			if(points[i] == std::nullopt)
				continue;
			
			if(is_near(points[i].value().first, points[i].value().second))
				near_points.push_back(i);
			
		}
		
		const size_t threshold_of_near_points = near_points.size() * 0.05;

		if(near_points.size() < threshold_of_near_points)
			continue;
		
		// calculate the line by least squares method
		double sum_x = 0, sum_y = 0, sum_xx = 0, sum_xy = 0, sum_yy = 0;

		for(auto i = 0ul; i < near_points.size(); ++i)
		{
			auto tmp_ = points[near_points[i]].value();
			sum_x += tmp_.first;
			sum_y += tmp_.second;
			sum_xx += tmp_.first * tmp_.first;
			sum_xy += tmp_.first * tmp_.second;
			sum_yy += tmp_.second * tmp_.second;
		}

		const double mx = sum_x / (double)near_points.size();
		const double my = sum_y / (double)near_points.size();
		const double variant_x = sum_xx / (double)near_points.size() - mx * mx;
		const double variant_y = sum_yy / (double)near_points.size() - my * my;
		const double covariant = sum_xy / (double)near_points.size() - mx * my;

		// ax + by + c = 0
		const double a_ls = covariant;
		const double b_ls = -variant_x;
		const double c_ls = variant_x * my - covariant * mx;

		lines.push_back({a_ls, b_ls, c_ls});

		// remove near points from argument points
		// TODO: set proper parameter
		for(auto i = 0ul; i < near_points.size(); ++i)
		{
			points[near_points[i]] = std::nullopt;
		}

		points_size -= near_points.size();

	}

	return lines;
}