src/ipa/raspberrypi/controller/rpi/lux.cpp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106

/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi (Trading) Limited
 *
 * lux.cpp - Lux control algorithm
 */
#include <math.h>

#include <linux/bcm2835-isp.h>

#include <libcamera/base/log.h>

#include "../device_status.h"

#include "lux.hpp"

using namespace RPiController;
using namespace libcamera;
using namespace std::literals::chrono_literals;

LOG_DEFINE_CATEGORY(RPiLux)

#define NAME "rpi.lux"

Lux::Lux(Controller *controller)
	: Algorithm(controller)
{
	// Put in some defaults as there will be no meaningful values until
	// Process has run.
	status_.aperture = 1.0;
	status_.lux = 400;
}

char const *Lux::Name() const
{
	return NAME;
}

void Lux::Read(boost::property_tree::ptree const &params)
{
	reference_shutter_speed_ =
		params.get<double>("reference_shutter_speed") * 1.0us;
	reference_gain_ = params.get<double>("reference_gain");
	reference_aperture_ = params.get<double>("reference_aperture", 1.0);
	reference_Y_ = params.get<double>("reference_Y");
	reference_lux_ = params.get<double>("reference_lux");
	current_aperture_ = reference_aperture_;
}

void Lux::SetCurrentAperture(double aperture)
{
	current_aperture_ = aperture;
}

void Lux::Prepare(Metadata *image_metadata)
{
	std::unique_lock<std::mutex> lock(mutex_);
	image_metadata->Set("lux.status", status_);
}

void Lux::Process(StatisticsPtr &stats, Metadata *image_metadata)
{
	DeviceStatus device_status;
	if (image_metadata->Get("device.status", device_status) == 0) {
		double current_gain = device_status.analogue_gain;
		double current_aperture = device_status.aperture;
		if (current_aperture == 0)
			current_aperture = current_aperture_;
		uint64_t sum = 0;
		uint32_t num = 0;
		uint32_t *bin = stats->hist[0].g_hist;
		const int num_bins = sizeof(stats->hist[0].g_hist) /
				     sizeof(stats->hist[0].g_hist[0]);
		for (int i = 0; i < num_bins; i++)
			sum += bin[i] * (uint64_t)i, num += bin[i];
		// add .5 to reflect the mid-points of bins
		double current_Y = sum / (double)num + .5;
		double gain_ratio = reference_gain_ / current_gain;
		double shutter_speed_ratio =
			reference_shutter_speed_ / device_status.shutter_speed;
		double aperture_ratio = reference_aperture_ / current_aperture;
		double Y_ratio = current_Y * (65536 / num_bins) / reference_Y_;
		double estimated_lux = shutter_speed_ratio * gain_ratio *
				       aperture_ratio * aperture_ratio *
				       Y_ratio * reference_lux_;
		LuxStatus status;
		status.lux = estimated_lux;
		status.aperture = current_aperture;
		LOG(RPiLux, Debug) << ": estimated lux " << estimated_lux;
		{
			std::unique_lock<std::mutex> lock(mutex_);
			status_ = status;
		}
		// Overwrite the metadata here as well, so that downstream
		// algorithms get the latest value.
		image_metadata->Set("lux.status", status);
	} else
		LOG(RPiLux, Warning) << ": no device metadata";
}

// Register algorithm with the system.
static Algorithm *Create(Controller *controller)
{
	return (Algorithm *)new Lux(controller);
}
static RegisterAlgorithm reg(NAME, &Create);
/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi (Trading) Limited
 *
 * alsc.cpp - ALSC (auto lens shading correction) control algorithm
 */

#include <math.h>
#include <numeric>

#include <libcamera/base/log.h>
#include <libcamera/base/span.h>

#include "../awb_status.h"
#include "alsc.hpp"

// Raspberry Pi ALSC (Auto Lens Shading Correction) algorithm.

using namespace RPiController;
using namespace libcamera;

LOG_DEFINE_CATEGORY(RPiAlsc)

#define NAME "rpi.alsc"

static const int X = ALSC_CELLS_X;
static const int Y = ALSC_CELLS_Y;
static const int XY = X * Y;
static const double INSUFFICIENT_DATA = -1.0;

Alsc::Alsc(Controller *controller)
	: Algorithm(controller)
{
	async_abort_ = async_start_ = async_started_ = async_finished_ = false;
	async_thread_ = std::thread(std::bind(&Alsc::asyncFunc, this));
}

Alsc::~Alsc()
{
	{
		std::lock_guard<std::mutex> lock(mutex_);
		async_abort_ = true;
	}
	async_signal_.notify_one();
	async_thread_.join();
}

char const *Alsc::Name() const
{
	return NAME;
}

static void generate_lut(double *lut, boost::property_tree::ptree const &params)
{
	double cstrength = params.get<double>("corner_strength", 2.0);
	if (cstrength <= 1.0)
		throw std::runtime_error("Alsc: corner_strength must be > 1.0");
	double asymmetry = params.get<double>("asymmetry", 1.0);
	if (asymmetry < 0)
		throw std::runtime_error("Alsc: asymmetry must be >= 0");
	double f1 = cstrength - 1, f2 = 1 + sqrt(cstrength);
	double R2 = X * Y / 4 * (1 + asymmetry * asymmetry);
	int num = 0;
	for (int y = 0; y < Y; y++) {
		for (int x = 0; x < X; x++) {
			double dy = y - Y / 2 + 0.5,
			       dx = (x - X / 2 + 0.5) * asymmetry;
			double r2 = (dx * dx + dy * dy) / R2;
			lut[num++] =
				(f1 * r2 + f2) * (f1 * r2 + f2) /
				(f2 * f2); // this reproduces the cos^4 rule
		}
	}
}

static void read_lut(double *lut, boost::property_tree::ptree const &params)
{
	int num = 0;
	const int max_num = XY;
	for (auto &p : params) {
		if (num == max_num)
			throw std::runtime_error(
				"Alsc: too many entries in LSC table");
		lut[num++] = p.second.get_value<double>();
	}
	if (num < max_num)
		throw std::runtime_error("Alsc: too few entries in LSC table");
}

static void read_calibrations(std::vector<AlscCalibration> &calibrations,
			      boost::property_tree::ptree const &params,
			      std::string const &name)
{
	if (params.get_child_optional(name)) {
		double last_ct = 0;
		for (auto &p : params.get_child(name)) {
			double ct = p.second.get<double>("ct");
			if (ct <= last_ct)
				throw std::runtime_error(
					"Alsc: entries in " + name +
					" must be in increasing ct order");
			AlscCalibration calibration;
			calibration.ct = last_ct = ct;
			boost::property_tree::ptree const &table =
				p.second.get_child("table");
			int num = 0;
			for (auto it = table.begin(); it != table.end(); it++) {
				if (num == XY)
					throw std::runtime_error(
						"Alsc: too many values for ct " +
						std::to_string(ct) + " in " +
						name);
				calibration.table[num++] =
					it->second.get_value<double>();
			}
			if (num != XY)
				throw std::runtime_error(
					"Alsc: too few values for ct " +
					std::to_string(ct) + " in " + name);
			calibrations.push_back(calibration);
			LOG(RPiAlsc, Debug)
				<< "Read " << name << " calibration for ct " << ct;
		}
	}
}

void Alsc::Read(boost::property_tree::ptree const &params)
{
	config_.frame_period = params.get<uint16_t>("frame_period", 12);
	config_.startup_frames = params.get<uint16_t>("startup_frames", 10);
	config_.speed = params.get<double>("speed", 0.05);
	double sigma = params.get<double>("sigma", 0.01);
	config_.sigma_Cr = params.get<double>("sigma_Cr", sigma);
	config_.sigma_Cb = params.get<double>("sigma_Cb", sigma);
	config_.min_count = params.get<double>("min_count", 10.0);
	config_.min_G = params.get<uint16_t>("min_G", 50);
	config_.omega = params.get<double>("omega", 1.3);
	config_.n_iter = params.get<uint32_t>("n_iter", X + Y);
	config_.luminance_strength =
		params.get<double>("luminance_strength", 1.0);
	for (int i = 0; i < XY; i++)
		config_.luminance_lut[i] = 1.0;
	if (params.get_child_optional("corner_strength"))
		generate_lut(config_.luminance_lut, params);
	else if (params.get_child_optional("luminance_lut"))
		read_lut(config_.luminance_lut,
			 params.get_child("luminance_lut"));
	else
		LOG(RPiAlsc, Warning)
			<< "no luminance table - assume unity everywhere";
	read_calibrations(config_.calibrations_Cr, params, "calibrations_Cr");
	read_calibrations(config_.calibrations_Cb, params, "calibrations_Cb");
	config_.default_ct = params.get<double>("default_ct", 4500.0);
	config_.threshold = params.get<double>("threshold", 1e-3);
	config_.lambda_bound = params.get<double>("lambda_bound", 0.05);
}

static double get_ct(Metadata *metadata, double default_ct);
static void get_cal_table(double ct,
			  std::vector<AlscCalibration> const &calibrations,
			  double cal_table[XY]);
static void resample_cal_table(double const cal_table_in[XY],
			       CameraMode const &camera_mode,
			       double cal_table_out[XY]);
static void compensate_lambdas_for_cal(double const cal_table[XY],
				       double const old_lambdas[XY],
				       double new_lambdas[XY]);
static void add_luminance_to_tables(double results[3][Y][X],
				    double const lambda_r[XY], double lambda_g,
				    double const lambda_b[XY],
				    double const luminance_lut[XY],
				    double luminance_strength);

void Alsc::Initialise()
{
	frame_count2_ = frame_count_ = frame_phase_ = 0;
	first_time_ = true;
	ct_ = config_.default_ct;
	// The lambdas are initialised in the SwitchMode.
}

void Alsc::waitForAysncThread()
{
	if (async_started_) {
		async_started_ = false;
		std::unique_lock<std::mutex> lock(mutex_);
		sync_signal_.wait(lock, [&] {
			return async_finished_;
		});
		async_finished_ = false;
	}
}

static bool compare_modes(CameraMode const &cm0, CameraMode const &cm1)
{
	// Return true if the modes crop from the sensor significantly differently,
	// or if the user transform has changed.
	if (cm0.transform != cm1.transform)
		return true;
	int left_diff = abs(cm0.crop_x - cm1.crop_x);
	int top_diff = abs(cm0.crop_y - cm1.crop_y);
	int right_diff = fabs(cm0.crop_x + cm0.scale_x * cm0.width -
			      cm1.crop_x - cm1.scale_x * cm1.width);
	int bottom_diff = fabs(cm0.crop_y + cm0.scale_y * cm0.height -
			       cm1.crop_y - cm1.scale_y * cm1.height);
	// These thresholds are a rather arbitrary amount chosen to trigger
	// when carrying on with the previously calculated tables might be
	// worse than regenerating them (but without the adaptive algorithm).
	int threshold_x = cm0.sensor_width >> 4;
	int threshold_y = cm0.sensor_height >> 4;
	return left_diff > threshold_x || right_diff > threshold_x ||
	       top_diff > threshold_y || bottom_diff > threshold_y;
}

void Alsc::SwitchMode(CameraMode const &camera_mode,
		      [[maybe_unused]] Metadata *metadata)
{
	// We're going to start over with the tables if there's any "significant"
	// change.
	bool reset_tables = first_time_ || compare_modes(camera_mode_, camera_mode);

	// Believe the colour temperature from the AWB, if there is one.
	ct_ = get_ct(metadata, ct_);

	// Ensure the other thread isn't running while we do this.
	waitForAysncThread();

	camera_mode_ = camera_mode;

	// We must resample the luminance table like we do the others, but it's
	// fixed so we can simply do it up front here.
	resample_cal_table(config_.luminance_lut, camera_mode_, luminance_table_);

	if (reset_tables) {
		// Upon every "table reset", arrange for something sensible to be
		// generated. Construct the tables for the previous recorded colour
		// temperature. In order to start over from scratch we initialise
		// the lambdas, but the rest of this code then echoes the code in
		// doAlsc, without the adaptive algorithm.
		for (int i = 0; i < XY; i++)
			lambda_r_[i] = lambda_b_[i] = 1.0;
		double cal_table_r[XY], cal_table_b[XY], cal_table_tmp[XY];
		get_cal_table(ct_, config_.calibrations_Cr, cal_table_tmp);
		resample_cal_table(cal_table_tmp, camera_mode_, cal_table_r);
		get_cal_table(ct_, config_.calibrations_Cb, cal_table_tmp);
		resample_cal_table(cal_table_tmp, camera_mode_, cal_table_b);
		compensate_lambdas_for_cal(cal_table_r, lambda_r_,
					   async_lambda_r_);
		compensate_lambdas_for_cal(cal_table_b, lambda_b_,
					   async_lambda_b_);
		add_luminance_to_tables(sync_results_, async_lambda_r_, 1.0,
					async_lambda_b_, luminance_table_,
					config_.luminance_strength);
		memcpy(prev_sync_results_, sync_results_,
		       sizeof(prev_sync_results_));
		frame_phase_ = config_.frame_period; // run the algo again asap
		first_time_ = false;
	}
}

void Alsc::fetchAsyncResults()
{
	LOG(RPiAlsc, Debug) << "Fetch ALSC results";
	async_finished_ = false;
	async_started_ = false;
	memcpy(sync_results_, async_results_, sizeof(sync_results_));
}

double get_ct(Metadata *metadata, double default_ct)
{
	AwbStatus awb_status;
	awb_status.temperature_K = default_ct; // in case nothing found
	if (metadata->Get("awb.status", awb_status) != 0)
		LOG(RPiAlsc, Debug) << "no AWB results found, using "
				    << awb_status.temperature_K;
	else
		LOG(RPiAlsc, Debug) << "AWB results found, using "
				    << awb_status.temperature_K;
	return awb_status.temperature_K;
}

static void copy_stats(bcm2835_isp_stats_region regions[XY], StatisticsPtr &stats,
		       AlscStatus const &status)
{
	bcm2835_isp_stats_region *input_regions = stats->awb_stats;
	double *r_table = (double *)status.r;
	double *g_table = (double *)status.g;
	double *b_table = (double *)status.b;
	for (int i = 0; i < XY; i++) {
		regions[i].r_sum = input_regions[i].r_sum / r_table[i];
		regions[i].g_sum = input_regions[i].g_sum / g_table[i];
		regions[i].b_sum = input_regions[i].b_sum / b_table[i];
		regions[i].counted = input_regions[i].counted;
		// (don't care about the uncounted value)
	}
}

void Alsc::restartAsync(StatisticsPtr &stats, Metadata *image_metadata)
{
	LOG(RPiAlsc, Debug) << "Starting ALSC calculation";
	// Get the current colour temperature. It's all we need from the
	// metadata. Default to the last CT value (which could be the default).
	ct_ = get_ct(image_metadata, ct_);
	// We have to copy the statistics here, dividing out our best guess of
	// the LSC table that the pipeline applied to them.
	AlscStatus alsc_status;
	if (image_metadata->Get("alsc.status", alsc_status) != 0) {
		LOG(RPiAlsc, Warning)
			<< "No ALSC status found for applied gains!";
		for (int y = 0; y < Y; y++)
			for (int x = 0; x < X; x++) {
				alsc_status.r[y][x] = 1.0;
				alsc_status.g[y][x] = 1.0;
				alsc_status.b[y][x] = 1.0;
			}
	}
	copy_stats(statistics_, stats, alsc_status);
	frame_phase_ = 0;
	async_started_ = true;
	{
		std::lock_guard<std::mutex> lock(mutex_);
		async_start_ = true;
	}
	async_signal_.notify_one();
}

void Alsc::Prepare(Metadata *image_metadata)
{
	// Count frames since we started, and since we last poked the async
	// thread.
	if (frame_count_ < (int)config_.startup_frames)
		frame_count_++;
	double speed = frame_count_ < (int)config_.startup_frames
			       ? 1.0
			       : config_.speed;
	LOG(RPiAlsc, Debug)
		<< "frame_count " << frame_count_ << " speed " << speed;
	{
		std::unique_lock<std::mutex> lock(mutex_);
		if (async_started_ && async_finished_)
			fetchAsyncResults();
	}
	// Apply IIR filter to results and program into the pipeline.
	double *ptr = (double *)sync_results_,
	       *pptr = (double *)prev_sync_results_;
	for (unsigned int i = 0;
	     i < sizeof(sync_results_) / sizeof(double); i++)
		pptr[i] = speed * ptr[i] + (1.0 - speed) * pptr[i];
	// Put output values into status metadata.
	AlscStatus status;
	memcpy(status.r, prev_sync_results_[0], sizeof(status.r));
	memcpy(status.g, prev_sync_results_[1], sizeof(status.g));
	memcpy(status.b, prev_sync_results_[2], sizeof(status.b));
	image_metadata->Set("alsc.status", status);
}

void Alsc::Process(StatisticsPtr &stats, Metadata *image_metadata)
{
	// Count frames since we started, and since we last poked the async