Spresenseでレンズレスカメラを作りたい

イメージングと情報工学を組み合わせる，コンピュテーショナルイメージングという研究分野があります．
コンピュテーショナルイメージングの分野では，超解像イメージングや圧縮センシング，レンズレスカメラといった研究が行われています．

ん．．．レンズレスカメラ．．．写真撮るのにレンズいらないの？．．．面白すぎる！

コンピュテーショナルイメージングと検索すると，レンズレスカメラを研究している先生方のホームページが出てきます．こんな画像を私も撮りたい！
自分でもトライしてみたいと思い立ち，Spresenseのコンテストでチャレンジしてみました．

レンズレスカメラを用いると解析前には人が見ても何のこっちゃわからない画像が撮れます．
それを計算で復元することで，人が理解可能な画像になります．

復元前の画像をAIに入力することで，人間が理解できる絵を作ることなく人物判定など可能となるのです．
なんてセキュリティの高いシステムなんでしょう．
ということで，この高セキュリティなシステムの作成にトライすることにしました．

レンズレスカメラの作成方法を公開している方たちがいます．
DiffuserCam

プログラムもBSD 3-Clause Licenseで公開してくれています．
今回はこちらを参考にspresenseで動くものを作成します．
以下に，簡単に説明を書きます．

通常のカメラはレンズで光を屈折させ，センサー上に像を結びます．レンズレスカメラでは，レンズの代わりにマスク（開口やコード化マスク）をセンサー直前に置きます．光はマスクを通ってセンサーに届くため，そのままではボヤけた模様のような画像しか得られません．これが「何のこっちゃわからない」生データです．

レンズレスイメージングでは，PSF（Point Spread Function：点像分布関数）が重要です．「1点の光源」がマスクを通ってセンサーにどう写るかを表した関数で，いわばその系の「指紋」です．マスク形状やセンサーとの距離で決まります．

観測画像（生データ）は，元のシーンとPSFの畳み込みで近似されます．


観測 b ≈ 元画像 x * PSF h    （* は畳み込み）

復元では「b と h から x を求める」問題を解きます．畳み込みはフーリエ空間では掛け算になるので，FFT（高速フーリエ変換）を使って効率的に計算します．

「b ≈ A(x)」となる線形演算子 A を用意し，‖b − A(x)‖² を最小化するように x を更新するのが勾配降下法です．今回の実装では FISTA（Fast Iterative Shrinkage-Thresholding Algorithm） を使い，反復的に画像を更新します．あわせて非負制約（画素値 ≥ 0）をかけることで，自然な画像に近づけます．

Sony Spresense（FCBGAパッケージ）
Spresense対応カメラモジュール
ILI9341コントローラ搭載TFT LCD

すべてモニター品の提供をいただきました．
WiFiモジュールは審査に落ちましたが，きっと「Spresenseには画像処理をするパワーがある！Spresense内で処理をするんだ！」というメッセージであると受けとって，画像送信はせず内部処理をしてみたいと思います．

Step1として，カメラ・LCDの動作確認をしました．
チュートリアルが充実していてとても助かります．以下のチュートリアルを参考に画面表示を確認しました．
Spresense

組んだ画像はこんな感じ．すでにカメラが改造されていますが気にしないでください．

最初に参考にする先駆者のコードを実行してみます．
サンプルの画像たちを使用して計算してみました．

PSF画像サンプル

撮像画像サンプル

復元画像

手が見えます！提供されているコードで計算可能であることを確認しました．

先駆者によると，マスクはイメージャの３ｍｍ程度離れたところあたりに設置するとよく．
粘着テープのような物でも良いとのことでした．

実際につけてみます．

いい感じに設置できたのではないでしょうか？
しかし，撮っても撮ってもサンプル画像のような明瞭なPSFは撮れませんでした．．．
ガーゼ越しに電灯を見ているような絵しか撮れず，復元プログラムを試してもボヤッとした白い塊が映るだけでした．．．．

ということで，Spresense用にプログラムを書き換えていきます


test_lensless_gd_spresense.ino
#include <SDHCI.h>
#include <stdio.h>
#include <string.h>
#include "lensless_gd.h"

#define BAUDRATE (115200)
#define MAX_IMAGE_SIZE (320 * 240)  // 最大画像サイズ（メモリ制約を考慮）

SDClass theSD;

// グローバル変数
image_t *psf_image = NULL;
image_t *data_image = NULL;
image_t *output_image = NULL;

/**
 * SDカードからTIFFファイルを読み込む（簡易版）
 * 注意: 完全なTIFFパーサーではない。特定の形式のみ対応。
 */
image_t* load_tiff_from_sd(const char *filename) {
    Serial.print("Loading: ");
    Serial.println(filename);
    
    File file = theSD.open(filename, FILE_READ);
    if (!file) {
        Serial.print("Error: Cannot open file ");
        Serial.println(filename);
        return NULL;
    }
    
    // ファイルサイズを確認
    size_t file_size = file.size();
    Serial.print("File size: ");
    Serial.println(file_size);
    
    // メモリ制約を考慮して、テスト用には小さな固定サイズで読み込む
    // 実際のTIFFの解像度に関わらず、ここでは 128x128 だけを使用する
    int width = 128;
    int height = 128;
    
    image_t *img = allocate_image(width, height);
    if (!img) {
        Serial.println("Error: Failed to allocate image");
        file.close();
        return NULL;
    }
    
    // TIFFファイルの構造は厳密には解析せず、
    // ファイルの最後の width*height バイトだけをグレースケール画像として使用する
    size_t data_size = width * height;
    if (data_size > file_size) {
        data_size = file_size;
        width = (int)sqrt(data_size);
        height = width;
        // 画像サイズを再調整
        free_image(img);
        img = allocate_image(width, height);
        if (!img) {
            file.close();
            return NULL;
        }
    }
    
    // ファイルの後半から読み込む（TIFFヘッダーをスキップ）
    size_t skip_bytes = file_size - data_size;
    if (skip_bytes > 0) {
        file.seek(skip_bytes);
    }
    
    uint8_t *buffer = (uint8_t*)malloc(data_size);
    if (!buffer) {
        Serial.println("Error: Failed to allocate buffer");
        free_image(img);
        file.close();
        return NULL;
    }
    
    size_t bytes_read = file.read(buffer, data_size);
    file.close();
    
    if (bytes_read == data_size) {
        // 8bitグレースケールとして読み込み
        for (size_t i = 0; i < data_size; i++) {
            img->data[i] = (float)buffer[i];
        }
        Serial.print("Loaded image: ");
        Serial.print(width);
        Serial.print("x");
        Serial.println(height);
    } else {
        Serial.print("Warning: Read only ");
        Serial.print(bytes_read);
        Serial.print(" bytes (expected ");
        Serial.print(data_size);
        Serial.println(")");
        // 読み込めた分だけ使用
        for (size_t i = 0; i < bytes_read && i < (size_t)(width * height); i++) {
            img->data[i] = (float)buffer[i];
        }
        // 残りはゼロで埋める
        for (size_t i = bytes_read; i < (size_t)(width * height); i++) {
            img->data[i] = 0.0f;
        }
    }
    
    free(buffer);
    return img;
}

/**
 * 画像データを生データ形式でSDカードに保存（デバッグ用）
 */
int save_raw_image(const char *filename, const image_t *img) {
    if (!img || !img->data) return -1;
    
    File file = theSD.open(filename, FILE_WRITE);
    if (!file) {
        Serial.print("Error: Cannot create file ");
        Serial.println(filename);
        return -1;
    }
    
    // ヘッダー（幅、高さ）
    file.write((uint8_t*)&img->width, sizeof(int));
    file.write((uint8_t*)&img->height, sizeof(int));
    
    // データ（float形式）
    size_t data_size = img->width * img->height * sizeof(float);
    file.write((uint8_t*)img->data, data_size);
    
    file.close();
    Serial.print("Saved: ");
    Serial.println(filename);
    return 0;
}

/**
 * 生データ形式の画像を読み込む
 */
image_t* load_raw_image(const char *filename) {
    Serial.print("Loading raw image: ");
    Serial.println(filename);
    
    File file = theSD.open(filename, FILE_READ);
    if (!file) {
        Serial.print("Error: Cannot open file ");
        Serial.println(filename);
        return NULL;
    }
    
    int width, height;
    if (file.read((uint8_t*)&width, sizeof(int)) != sizeof(int) ||
        file.read((uint8_t*)&height, sizeof(int)) != sizeof(int)) {
        Serial.println("Error: Failed to read header");
        file.close();
        return NULL;
    }
    
    image_t *img = allocate_image(width, height);
    if (!img) {
        Serial.println("Error: Failed to allocate image");
        file.close();
        return NULL;
    }
    
    size_t data_size = width * height * sizeof(float);
    if (file.read((uint8_t*)img->data, data_size) != data_size) {
        Serial.println("Error: Failed to read image data");
        free_image(img);
        file.close();
        return NULL;
    }
    
    file.close();
    Serial.print("Loaded raw image: ");
    Serial.print(width);
    Serial.print("x");
    Serial.println(height);
    return img;
}

/**
 * 画像の統計情報を表示
 */
void print_image_stats(const char *name, const image_t *img) {
    if (!img || !img->data) return;
    
    float min_val = img->data[0];
    float max_val = img->data[0];
    float sum = 0.0f;
    int size = img->width * img->height;
    
    for (int i = 0; i < size; i++) {
        if (img->data[i] < min_val) min_val = img->data[i];
        if (img->data[i] > max_val) max_val = img->data[i];
        sum += img->data[i];
    }
    
    Serial.print(name);
    Serial.print(": ");
    Serial.print(img->width);
    Serial.print("x");
    Serial.print(img->height);
    Serial.print(", min=");
    Serial.print(min_val);
    Serial.print(", max=");
    Serial.print(max_val);
    Serial.print(", mean=");
    Serial.println(sum / size);
}

/**
 * セットアップ
 */
void setup() {
    Serial.begin(BAUDRATE);
    while (!Serial) {
        ; // シリアルポートの接続を待つ
    }
    
    Serial.println("========================================");
    Serial.println("Lensless Camera GD Test (Spresense)");
    Serial.println("========================================");
    Serial.println();
    
    // SDカード初期化
    Serial.println("Initializing SD card...");
    int sd_retry_count = 0;
    while (!theSD.begin()) {
        Serial.print("SD card initialization failed (attempt ");
        Serial.print(sd_retry_count + 1);
        Serial.println(")");
        delay(1000);
        sd_retry_count++;
        
        if (sd_retry_count >= 10) {
            Serial.println("ERROR: SD card initialization failed after 10 attempts.");
            Serial.println("Please check your SD card and restart.");
            while(1) delay(1000);
        }
    }
    Serial.println("SD card initialized successfully!");
    Serial.println();
    
    // 画像ファイルの読み込み
    // まずTIFF形式で試行、失敗したら生データ形式を試行
    psf_image = load_tiff_from_sd("psf_sample.tif");
    if (!psf_image) {
        Serial.println("Trying to load as raw format...");
        psf_image = load_raw_image("psf_sample.raw");
    }
    
    data_image = load_tiff_from_sd("rawdata_hand_sample.tif");
    if (!data_image) {
        Serial.println("Trying to load as raw format...");
        data_image = load_raw_image("rawdata_hand_sample.raw");
    }
    
    if (!psf_image || !data_image) {
        Serial.println("ERROR: Failed to load images");
        Serial.println("Please ensure the following files are on SD card:");
        Serial.println("  - psf_sample.tif (or psf_sample.raw)");
        Serial.println("  - rawdata_hand_sample.tif (or rawdata_hand_sample.raw)");
        return;
    }
    
    // 画像統計情報を表示
    print_image_stats("PSF", psf_image);
    print_image_stats("Data", data_image);
    Serial.println();
    
    // 前処理: 背景除去
    Serial.println("Preprocessing images...");
    float bg = calculate_background(psf_image, 5, 5, 15, 15);
    Serial.print("Background value: ");
    Serial.println(bg);
    
    subtract_background(psf_image, bg);
    subtract_background(data_image, bg);
    
    // リサイズ（メモリ制約を考慮）
    Serial.println("Resizing images...");
    image_t *psf_resized = resize_image(psf_image, 0.25f);
    image_t *data_resized = resize_image(data_image, 0.25f);
    
    if (!psf_resized || !data_resized) {
        Serial.println("ERROR: Failed to resize images");
        return;
    }
    
    Serial.print("Resized PSF: ");
    Serial.print(psf_resized->width);
    Serial.print("x");
    Serial.println(psf_resized->height);
    Serial.print("Resized data: ");
    Serial.print(data_resized->width);
    Serial.print("x");
    Serial.println(data_resized->height);
    Serial.println();
    
    // 正規化
    Serial.println("Normalizing images...");
    normalize_image(psf_resized);
    normalize_image(data_resized);
    
    // 出力画像の準備
    output_image = allocate_image(data_resized->width, data_resized->height);
    if (!output_image) {
        Serial.println("ERROR: Failed to allocate output image");
        return;
    }
    
    // 勾配降下法による再構成
    Serial.println("========================================");
    Serial.println("Starting gradient descent...");
    Serial.println("========================================");
    
    int max_iterations = 50;
    int display_interval = 10;
    
    Serial.print("Max iterations: ");
    Serial.println(max_iterations);
    Serial.print("Display interval: ");
    Serial.println(display_interval);
    Serial.println();
    
    unsigned long start_time = millis();
    
    int result = grad_descent(psf_resized, data_resized, output_image, 
                             max_iterations, display_interval);
    
    unsigned long end_time = millis();
    unsigned long elapsed = end_time - start_time;
    
    Serial.println();
    Serial.println("========================================");
    if (result == 0) {
        Serial.println("Gradient descent completed successfully!");
        Serial.print("Elapsed time: ");
        Serial.print(elapsed);
        Serial.println(" ms");
        
        // 結果の統計情報
        print_image_stats("Output", output_image);
        
        // 結果をSDカードに保存（オプション）
        Serial.println();
        Serial.println("Saving results to SD card...");
        save_raw_image("output.raw", output_image);
        save_raw_image("psf_resized.raw", psf_resized);
        save_raw_image("data_resized.raw", data_resized);
        
        Serial.println();
        Serial.println("Results saved:");
        Serial.println("  - output.raw");
        Serial.println("  - psf_resized.raw");
        Serial.println("  - data_resized.raw");
    } else {
        Serial.println("ERROR: Gradient descent failed");
    }
    Serial.println("========================================");
    
    // メモリ解放
    free_image(psf_resized);
    free_image(data_resized);
}

/**
 * メインループ
 */
void loop() {
    // 一度だけ実行するため、何もしない
    delay(1000);
}


lensless_gd.h
#ifndef LENSLESS_GD_H
#define LENSLESS_GD_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>
#include <stdbool.h>

// 複素数型（実部、虚部）
typedef struct {
    float real;
    float imag;
} complex_t;

// 画像データ構造
typedef struct {
    float *data;      // 画像データ（1次元配列）
    int width;        // 幅
    int height;       // 高さ
} image_t;

// FFT設定構造
typedef struct {
    int width;        // FFT幅（2のべき乗）
    int height;       // FFT高さ（2のべき乗）
    int start_i;      // クロップ開始位置（行）
    int end_i;        // クロップ終了位置（行）
    int start_j;      // クロップ開始位置（列）
    int end_j;        // クロップ終了位置（列）
} fft_config_t;

// 勾配降下法パラメータ
typedef struct {
    complex_t *H;           // FFT(PSF)
    complex_t *Hadj;        // 共役転置
    float *b;               // 観測データ
    fft_config_t *config;   // FFT設定
    float alpha;            // 学習率
    int max_iterations;     // 最大反復回数
    int display_interval;   // 表示間隔
} gd_params_t;

// 関数プロトタイプ

/**
 * 2のべき乗に切り上げ
 * @param n 入力値
 * @return 2のべき乗値
 */
int next_pow2(int n);

/**
 * 画像のリサイズ（ダウンサンプリング）
 * @param img 入力画像
 * @param factor リサイズ係数（例: 0.25 = 1/4サイズ）
 * @return リサイズ後の画像（メモリは呼び出し側で解放）
 */
image_t* resize_image(const image_t *img, float factor);

/**
 * 画像の正規化（L2ノルムで正規化）
 * @param img 画像（in-place）
 */
void normalize_image(image_t *img);

/**
 * 背景除去
 * @param img 画像（in-place）
 * @param bg_value 背景値
 */
void subtract_background(image_t *img, float bg_value);

/**
 * 背景値の計算（PSFの特定領域の平均）
 * @param psf PSF画像
 * @param x0, y0, x1, y1 領域の座標
 * @return 背景値
 */
float calculate_background(const image_t *psf, int x0, int y0, int x1, int y1);

/**
 * 行列の初期化（FFT用のパディングと設定）
 * @param h PSF画像
 * @param H_out FFT(PSF)の出力先
 * @param Hadj_out 共役転置の出力先
 * @param v_out 初期ベクトルの出力先
 * @param config_out FFT設定の出力先
 * @return 成功時0、失敗時-1
 */
int init_matrices(const image_t *h, complex_t **H_out, complex_t **Hadj_out, 
                  float **v_out, fft_config_t **config_out);

/**
 * 2次元FFT（正規化付き）
 * @param input 入力データ（実数）
 * @param output 出力データ（複素数）
 * @param width 幅
 * @param height 高さ
 * @param forward true=FFT, false=IFFT
 */
void fft2d(const float *input, complex_t *output, int width, int height, bool forward);

/**
 * IFFTシフト（周波数領域のシフト）
 * @param data データ（in-place）
 * @param width 幅
 * @param height 高さ
 */
void ifftshift(complex_t *data, int width, int height);

/**
 * パディング（画像をFFTサイズに拡張）
 * @param input 入力画像
 * @param output 出力配列（FFTサイズ）
 * @param config FFT設定
 */
void pad_image(const float *input, float *output, const fft_config_t *config);

/**
 * クロップ（FFTサイズから元のサイズに切り出し）
 * @param input 入力配列（FFTサイズ）
 * @param output 出力画像
 * @param config FFT設定
 */
void crop_image(const float *input, float *output, const fft_config_t *config);

/**
 * 複素数配列のクロップ
 * @param input 入力配列（FFTサイズ、複素数）
 * @param output 出力配列（元のサイズ、実数）
 * @param config FFT設定
 */
void crop_complex(const complex_t *input, float *output, const fft_config_t *config);

/**
 * calcA: A(vk) = crop(ifftshift(ifft2(H * fft2(vk))))
 * @param H FFT(PSF)
 * @param vk 現在の推定画像
 * @param output 出力
 * @param config FFT設定
 */
void calc_a(const complex_t *H, const float *vk, float *output, const fft_config_t *config);

/**
 * calcAHerm: A^H(diff) = ifftshift(ifft2(Hadj * fft2(pad(diff))))
 * @param Hadj 共役転置
 * @param diff 差分
 * @param output 出力（複素数）
 * @param config FFT設定
 */
void calc_a_herm(const complex_t *Hadj, const float *diff, complex_t *output, const fft_config_t *config);

/**
 * 勾配の計算
 * @param Hadj 共役転置
 * @param H FFT(PSF)
 * @param vk 現在の推定画像
 * @param b 観測データ
 * @param gradient_out 勾配の出力先
 * @param config FFT設定
 */
void calculate_gradient(const complex_t *Hadj, const complex_t *H, const float *vk, 
                       const float *b, float *gradient_out, const fft_config_t *config);

/**
 * 非負制約（プロジェクション）
 * @param img 画像データ（in-place）
 * @param size データサイズ
 */
void non_negative_projection(float *img, int size);

/**
 * FISTA更新
 * @param vk 現在の推定画像
 * @param tk FISTAパラメータ
 * @param xk FISTA補助変数
 * @param params 勾配降下パラメータ
 * @param new_vk 新しいvk
 * @param new_tk 新しいtk
 * @param new_xk 新しいxk
 */
void fista_update(const float *vk, float tk, const float *xk, const gd_params_t *params,
                  float *new_vk, float *new_tk, float *new_xk);

/**
 * 勾配降下法による画像再構成
 * @param h PSF画像
 * @param b 観測データ
 * @param output 出力画像
 * @param max_iterations 最大反復回数
 * @param display_interval 表示間隔（0=表示なし）
 * @return 成功時0、失敗時-1
 */
int grad_descent(const image_t *h, const image_t *b, image_t *output, 
                 int max_iterations, int display_interval);

/**
 * 画像データのメモリ解放
 * @param img 画像
 */
void free_image(image_t *img);

/**
 * 画像データのメモリ確保
 * @param width 幅
 * @param height 高さ
 * @return 画像構造体（失敗時NULL）
 */
image_t* allocate_image(int width, int height);

/**
 * 画像データのコピー
 * @param src ソース画像
 * @return コピーされた画像（失敗時NULL）
 */
image_t* copy_image(const image_t *src);

#ifdef __cplusplus
}
#endif

#endif // LENSLESS_GD_H


lensless_gd.c
#include "lensless_gd.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdio.h>

// 簡易FFT実装（KissFFTスタイル）
// 注: 本番環境ではKissFFTライブラリを使用することを推奨

// 1次元FFT（Cooley-Tukeyアルゴリズムの簡易版）
static void fft1d(complex_t *x, int n, bool inverse) {
    if (n <= 1) return;
    
    complex_t *even = (complex_t*)malloc(n/2 * sizeof(complex_t));
    complex_t *odd = (complex_t*)malloc(n/2 * sizeof(complex_t));
    
    for (int i = 0; i < n/2; i++) {
        even[i] = x[i*2];
        odd[i] = x[i*2+1];
    }
    
    fft1d(even, n/2, inverse);
    fft1d(odd, n/2, inverse);
    
    float angle = (inverse ? 1.0f : -1.0f) * 2.0f * M_PI / n;
    complex_t w = {1.0f, 0.0f};
    complex_t wn = {cosf(angle), sinf(angle)};
    
    for (int i = 0; i < n/2; i++) {
        complex_t t = {w.real * odd[i].real - w.imag * odd[i].imag,
                       w.real * odd[i].imag + w.imag * odd[i].real};
        x[i].real = even[i].real + t.real;
        x[i].imag = even[i].imag + t.imag;
        x[i+n/2].real = even[i].real - t.real;
        x[i+n/2].imag = even[i].imag - t.imag;
        
        complex_t w_next = {w.real * wn.real - w.imag * wn.imag,
                           w.real * wn.imag + w.imag * wn.real};
        w = w_next;
    }
    
    if (inverse) {
        float scale = 1.0f / sqrtf(n);
        for (int i = 0; i < n; i++) {
            x[i].real *= scale;
            x[i].imag *= scale;
        }
    } else {
        float scale = 1.0f / sqrtf(n);
        for (int i = 0; i < n; i++) {
            x[i].real *= scale;
            x[i].imag *= scale;
        }
    }
    
    free(even);
    free(odd);
}

int next_pow2(int n) {
    if (n <= 0) return 1;
    int pow = 1;
    while (pow < n) pow <<= 1;
    return pow;
}

image_t* allocate_image(int width, int height) {
    image_t *img = (image_t*)malloc(sizeof(image_t));
    if (!img) return NULL;
    
    img->data = (float*)calloc(width * height, sizeof(float));
    if (!img->data) {
        free(img);
        return NULL;
    }
    
    img->width = width;
    img->height = height;
    return img;
}

void free_image(image_t *img) {
    if (img) {
        if (img->data) free(img->data);
        free(img);
    }
}

image_t* copy_image(const image_t *src) {
    if (!src) return NULL;
    image_t *dst = allocate_image(src->width, src->height);
    if (!dst) return NULL;
    memcpy(dst->data, src->data, src->width * src->height * sizeof(float));
    return dst;
}

image_t* resize_image(const image_t *img, float factor) {
    if (!img || factor <= 0.0f || factor >= 1.0f) return NULL;
    
    int num_steps = (int)(-log2f(factor) + 0.5f);
    image_t *current = copy_image(img);
    if (!current) return NULL;
    
    for (int step = 0; step < num_steps; step++) {
        int new_width = current->width / 2;
        int new_height = current->height / 2;
        
        if (new_width < 1 || new_height < 1) break;
        
        image_t *resized = allocate_image(new_width, new_height);
        if (!resized) {
            free_image(current);
            return NULL;
        }
        
        for (int y = 0; y < new_height; y++) {
            for (int x = 0; x < new_width; x++) {
                int src_x = x * 2;
                int src_y = y * 2;
                float sum = current->data[src_y * current->width + src_x] +
                           current->data[src_y * current->width + src_x + 1] +
                           current->data[(src_y + 1) * current->width + src_x] +
                           current->data[(src_y + 1) * current->width + src_x + 1];
                resized->data[y * new_width + x] = sum * 0.25f;
            }
        }
        
        free_image(current);
        current = resized;
    }
    
    return current;
}

void normalize_image(image_t *img) {
    if (!img || !img->data) return;
    
    float norm = 0.0f;
    int size = img->width * img->height;
    
    for (int i = 0; i < size; i++) {
        norm += img->data[i] * img->data[i];
    }
    
    norm = sqrtf(norm);
    if (norm > 1e-10f) {
        for (int i = 0; i < size; i++) {
            img->data[i] /= norm;
        }
    }
}

void subtract_background(image_t *img, float bg_value) {
    if (!img || !img->data) return;
    
    int size = img->width * img->height;
    for (int i = 0; i < size; i++) {
        img->data[i] -= bg_value;
    }
}

float calculate_background(const image_t *psf, int x0, int y0, int x1, int y1) {
    if (!psf || !psf->data) return 0.0f;
    
    // 境界チェック
    if (x0 < 0) x0 = 0;
    if (y0 < 0) y0 = 0;
    if (x1 > psf->width) x1 = psf->width;
    if (y1 > psf->height) y1 = psf->height;
    
    float sum = 0.0f;
    int count = 0;
    
    for (int y = y0; y < y1; y++) {
        for (int x = x0; x < x1; x++) {
            sum += psf->data[y * psf->width + x];
            count++;
        }
    }
    
    return (count > 0) ? (sum / count) : 0.0f;
}

void ifftshift(complex_t *data, int width, int height) {
    if (!data) return;
    
    int half_w = width / 2;
    int half_h = height / 2;
    
    // 行方向のシフト
    for (int y = 0; y < half_h; y++) {
        for (int x = 0; x < width; x++) {
            int idx1 = y * width + x;
            int idx2 = (y + half_h) * width + x;
            complex_t temp = data[idx1];
            data[idx1] = data[idx2];
            data[idx2] = temp;
        }
    }
    
    // 列方向のシフト
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < half_w; x++) {
            int idx1 = y * width + x;
            int idx2 = y * width + (x + half_w);
            complex_t temp = data[idx1];
            data[idx1] = data[idx2];
            data[idx2] = temp;
        }
    }
}

void fft2d(const float *input, complex_t *output, int width, int height, bool forward) {
    if (!input || !output) return;
    
    // 実数から複素数へ変換
    complex_t *temp = (complex_t*)malloc(width * height * sizeof(complex_t));
    if (!temp) return;
    
    for (int i = 0; i < width * height; i++) {
        temp[i].real = input[i];
        temp[i].imag = 0.0f;
    }
    
    // 行方向FFT
    for (int y = 0; y < height; y++) {
        fft1d(&temp[y * width], width, !forward);
    }
    
    // 転置
    complex_t *transposed = (complex_t*)malloc(width * height * sizeof(complex_t));
    if (!transposed) {
        free(temp);
        return;
    }
    
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            transposed[x * height + y] = temp[y * width + x];
        }
    }
    
    // 列方向FFT
    for (int x = 0; x < width; x++) {
        fft1d(&transposed[x * height], height, !forward);
    }
    
    // 転置して戻す
    for (int x = 0; x < width; x++) {
        for (int y = 0; y < height; y++) {
            output[y * width + x] = transposed[x * height + y];
        }
    }
    
    free(temp);
    free(transposed);
}

void pad_image(const float *input, float *output, const fft_config_t *config) {
    if (!input || !output || !config) return;
    
    int fft_size = config->width * config->height;
    memset(output, 0, fft_size * sizeof(float));
    
    int orig_width = config->end_i - config->start_i;
    int orig_height = config->end_j - config->start_j;
    
    for (int y = 0; y < orig_height; y++) {
        for (int x = 0; x < orig_width; x++) {
            int src_idx = y * orig_width + x;
            int dst_y = config->start_j + y;
            int dst_x = config->start_i + x;
            int dst_idx = dst_y * config->width + dst_x;
            if (dst_idx < fft_size) {
                output[dst_idx] = input[src_idx];
            }
        }
    }
}

void crop_image(const float *input, float *output, const fft_config_t *config) {
    if (!input || !output || !config) return;
    
    int orig_width = config->end_i - config->start_i;
    int orig_height = config->end_j - config->start_j;
    
    for (int y = 0; y < orig_height; y++) {
        for (int x = 0; x < orig_width; x++) {
            int src_y = config->start_j + y;
            int src_x = config->start_i + x;
            int src_idx = src_y * config->width + src_x;
            int dst_idx = y * orig_width + x;
            output[dst_idx] = input[src_idx];
        }
    }
}

void crop_complex(const complex_t *input, float *output, const fft_config_t *config) {
    if (!input || !output || !config) return;
    
    int orig_width = config->end_i - config->start_i;
    int orig_height = config->end_j - config->start_j;
    
    for (int y = 0; y < orig_height; y++) {
        for (int x = 0; x < orig_width; x++) {
            int src_y = config->start_j + y;
            int src_x = config->start_i + x;
            int src_idx = src_y * config->width + src_x;
            int dst_idx = y * orig_width + x;
            output[dst_idx] = input[src_idx].real;
        }
    }
}

int init_matrices(const image_t *h, complex_t **H_out, complex_t **Hadj_out, 
                  float **v_out, fft_config_t **config_out) {
    if (!h || !h->data) return -1;
    
    // 初期値の計算
    float min_val = h->data[0];
    float max_val = h->data[0];
    int size = h->width * h->height;
    
    for (int i = 1; i < size; i++) {
        if (h->data[i] < min_val) min_val = h->data[i];
        if (h->data[i] > max_val) max_val = h->data[i];
    }
    
    float pixel_start = (max_val + min_val) * 0.5f;
    
    // パディングサイズの計算
    int padded_width = next_pow2(2 * h->width - 1);
    int padded_height = next_pow2(2 * h->height - 1);
    
    fft_config_t *config = (fft_config_t*)malloc(sizeof(fft_config_t));
    if (!config) return -1;
    
    config->width = padded_width;
    config->height = padded_height;
    config->start_i = (padded_width - h->width) / 2;
    config->end_i = config->start_i + h->width;
    config->start_j = (padded_height / 2) - (h->height / 2);
    config->end_j = config->start_j + h->height;
    
    // PSFのパディング
    float *hpad = (float*)calloc(padded_width * padded_height, sizeof(float));
    if (!hpad) {
        free(config);
        return -1;
    }
    
    pad_image(h->data, hpad, config);
    
    // FFT計算
    complex_t *H = (complex_t*)malloc(padded_width * padded_height * sizeof(complex_t));
    if (!H) {
        free(hpad);
        free(config);
        return -1;
    }
    
    fft2d(hpad, H, padded_width, padded_height, true);
    
    // 共役転置
    complex_t *Hadj = (complex_t*)malloc(padded_width * padded_height * sizeof(complex_t));
    if (!Hadj) {
        free(H);
        free(hpad);
        free(config);
        return -1;
    }
    
    for (int i = 0; i < padded_width * padded_height; i++) {
        Hadj[i].real = H[i].real;
        Hadj[i].imag = -H[i].imag;
    }
    
    // 初期ベクトル
    float *v = (float*)calloc(padded_width * padded_height, sizeof(float));
    if (!v) {
        free(Hadj);
        free(H);
        free(hpad);
        free(config);
        return -1;
    }
    
    for (int y = config->start_j; y < config->end_j; y++) {
        for (int x = config->start_i; x < config->end_i; x++) {
            v[y * padded_width + x] = pixel_start;
        }
    }
    
    *H_out = H;
    *Hadj_out = Hadj;
    *v_out = v;
    *config_out = config;
    
    free(hpad);
    return 0;
}

void calc_a(const complex_t *H, const float *vk, float *output, const fft_config_t *config) {
    if (!H || !vk || !output || !config) return;
    
    int fft_size = config->width * config->height;
    
    // vkのFFT
    complex_t *Vk = (complex_t*)malloc(fft_size * sizeof(complex_t));
    if (!Vk) return;
    
    fft2d(vk, Vk, config->width, config->height, true);
    
    // H * Vk
    complex_t *HVk = (complex_t*)malloc(fft_size * sizeof(complex_t));
    if (!HVk) {
        free(Vk);
        return;
    }
    
    for (int i = 0; i < fft_size; i++) {
        HVk[i].real = H[i].real * Vk[i].real - H[i].imag * Vk[i].imag;
        HVk[i].imag = H[i].real * Vk[i].imag + H[i].imag * Vk[i].real;
    }
    
    // IFFT
    float *ifft_result = (float*)malloc(fft_size * sizeof(float));
    if (!ifft_result) {
        free(HVk);
        free(Vk);
        return;
    }
    
    complex_t *ifft_complex = (complex_t*)malloc(fft_size * sizeof(complex_t));
    if (!ifft_complex) {
        free(ifft_result);
        free(HVk);
        free(Vk);
        return;
    }
    
    fft2d((float*)HVk, ifft_complex, config->width, config->height, false);
    
    // ifftshift
    ifftshift(ifft_complex, config->width, config->height);
    
    // 実部を取り出してクロップ
    crop_complex(ifft_complex, output, config);
    
    free(ifft_complex);
    free(ifft_result);
    free(HVk);
    free(Vk);
}

void calc_a_herm(const complex_t *Hadj, const float *diff, complex_t *output, const fft_config_t *config) {
    if (!Hadj || !diff || !output || !config) return;
    
    int fft_size = config->width * config->height;
    
    // パディング
    float *xpad = (float*)calloc(fft_size, sizeof(float));
    if (!xpad) return;
    
    pad_image(diff, xpad, config);
    
    // FFT
    complex_t *X = (complex_t*)malloc(fft_size * sizeof(complex_t));
    if (!X) {
        free(xpad);
        return;
    }
    
    fft2d(xpad, X, config->width, config->height, true);
    
    // Hadj * X
    for (int i = 0; i < fft_size; i++) {
        output[i].real = Hadj[i].real * X[i].real - Hadj[i].imag * X[i].imag;
        output[i].imag = Hadj[i].real * X[i].imag + Hadj[i].imag * X[i].real;
    }
    
    // IFFT
    float *temp_real = (float*)malloc(fft_size * sizeof(float));
    if (!temp_real) {
        free(X);
        free(xpad);
        return;
    }
    
    complex_t *ifft_result = (complex_t*)malloc(fft_size * sizeof(complex_t));
    if (!ifft_result) {
        free(temp_real);
        free(X);
        free(xpad);
        return;
    }
    
    // 実部のみを使用してIFFT（簡易版）
    for (int i = 0; i < fft_size; i++) {
        temp_real[i] = output[i].real;
    }
    
    fft2d(temp_real, ifft_result, config->width, config->height, false);
    ifftshift(ifft_result, config->width, config->height);
    
    // 結果をoutputにコピー
    memcpy(output, ifft_result, fft_size * sizeof(complex_t));
    
    free(ifft_result);
    free(temp_real);
    free(X);
    free(xpad);
}

void calculate_gradient(const complex_t *Hadj, const complex_t *H, const float *vk, 
                       const float *b, float *gradient_out, const fft_config_t *config) {
    if (!Hadj || !H || !vk || !b || !gradient_out || !config) return;
    
    int orig_size = (config->end_i - config->start_i) * (config->end_j - config->start_j);
    
    // Av = calcA(H, vk, crop)
    float *Av = (float*)malloc(orig_size * sizeof(float));
    if (!Av) return;
    
    calc_a(H, vk, Av, config);
    
    // diff = Av - b
    float *diff = (float*)malloc(orig_size * sizeof(float));
    if (!diff) {
        free(Av);
        return;
    }
    
    for (int i = 0; i < orig_size; i++) {
        diff[i] = Av[i] - b[i];
    }
    
    // gradient = real(calcAHerm(Hadj, diff, pad))
    int fft_size = config->width * config->height;
    complex_t *grad_complex = (complex_t*)malloc(fft_size * sizeof(complex_t));
    if (!grad_complex) {
        free(diff);
        free(Av);
        return;
    }
    
    calc_a_herm(Hadj, diff, grad_complex, config);
    
    // 実部を取り出してクロップ
    crop_complex(grad_complex, gradient_out, config);
    
    free(grad_complex);
    free(diff);
    free(Av);
}

void non_negative_projection(float *img, int size) {
    if (!img) return;
    for (int i = 0; i < size; i++) {
        if (img[i] < 0.0f) img[i] = 0.0f;
    }
}

void fista_update(const float *vk, float tk, const float *xk, const gd_params_t *params,
                  float *new_vk, float *new_tk, float *new_xk) {
    if (!vk || !xk || !params || !new_vk || !new_tk || !new_xk) return;
    
    int orig_size = (params->config->end_i - params->config->start_i) * 
                    (params->config->end_j - params->config->start_j);
    int fft_size = params->config->width * params->config->height;
    
    // 勾配の計算
    float *gradient = (float*)malloc(orig_size * sizeof(float));
    if (!gradient) return;
    
    calculate_gradient(params->Hadj, params->H, vk, params->b, gradient, params->config);
    
    // vk -= alpha * gradient
    float *vk_new = (float*)malloc(fft_size * sizeof(float));
    if (!vk_new) {
        free(gradient);
        return;
    }
    
    memcpy(vk_new, vk, fft_size * sizeof(float));
    
    // 勾配をFFTサイズに拡張して適用
    float *grad_padded = (float*)calloc(fft_size, sizeof(float));
    if (!grad_padded) {
        free(vk_new);
        free(gradient);
        return;
    }
    
    pad_image(gradient, grad_padded, params->config);
    
    for (int i = 0; i < fft_size; i++) {
        vk_new[i] -= params->alpha * grad_padded[i];
    }
    
    // xk = proj(vk_new)
    float *xk_new = (float*)malloc(orig_size * sizeof(float));
    if (!xk_new) {
        free(grad_padded);
        free(vk_new);
        free(gradient);
        return;
    }
    
    crop_image(vk_new, xk_new, params->config);
    non_negative_projection(xk_new, orig_size);
    
    // t_k1 = (1 + sqrt(1 + 4*tk^2)) / 2
    float t_k1 = (1.0f + sqrtf(1.0f + 4.0f * tk * tk)) * 0.5f;
    
    // vk = xk + (tk-1)/t_k1 * (xk - x_k1)
    float *x_k1 = (float*)malloc(orig_size * sizeof(float));
    if (!x_k1) {
        free(xk_new);
        free(grad_padded);
        free(vk_new);
        free(gradient);
        return;
    }
    
    // xkは既にクロップされたサイズなので、そのままコピー
    memcpy(x_k1, xk, orig_size * sizeof(float));
    
    float *vk_final = (float*)calloc(fft_size, sizeof(float));
    if (!vk_final) {
        free(x_k1);
        free(xk_new);
        free(grad_padded);
        free(vk_new);
        free(gradient);
        return;
    }
    
    // xkをFFTサイズに拡張
    float *xk_padded = (float*)calloc(fft_size, sizeof(float));
    if (!xk_padded) {
        free(vk_final);
        free(x_k1);
        free(xk_new);
        free(grad_padded);
        free(vk_new);
        free(gradient);
        return;
    }
    
    pad_image(xk_new, xk_padded, params->config);
    
    // x_k1をFFTサイズに拡張
    float *x_k1_padded = (float*)calloc(fft_size, sizeof(float));
    if (!x_k1_padded) {
        free(xk_padded);
        free(vk_final);
        free(x_k1);
        free(xk_new);
        free(grad_padded);
        free(vk_new);
        free(gradient);
        return;
    }
    
    pad_image(x_k1, x_k1_padded, params->config);
    
    float beta = (tk - 1.0f) / t_k1;
    for (int i = 0; i < fft_size; i++) {
        vk_final[i] = xk_padded[i] + beta * (xk_padded[i] - x_k1_padded[i]);
    }
    
    memcpy(new_vk, vk_final, fft_size * sizeof(float));
    *new_tk = t_k1;
    memcpy(new_xk, xk_new, orig_size * sizeof(float));
    
    free(x_k1_padded);
    free(xk_padded);
    free(vk_final);
    free(x_k1);
    free(xk_new);
    free(grad_padded);
    free(vk_new);
    free(gradient);
}

int grad_descent(const image_t *h, const image_t *b, image_t *output, 
                 int max_iterations, int display_interval) {
    if (!h || !b || !output) return -1;
    
    // 行列の初期化
    complex_t *H = NULL;
    complex_t *Hadj = NULL;
    float *v = NULL;
    fft_config_t *config = NULL;
    
    if (init_matrices(h, &H, &Hadj, &v, &config) != 0) {
        return -1;
    }
    
    // alphaの計算: 2 / max(Hadj * H)
    int fft_size = config->width * config->height;
    float max_val = 0.0f;
    
    for (int i = 0; i < fft_size; i++) {
        float val = Hadj[i].real * H[i].real - Hadj[i].imag * H[i].imag;
        if (val > max_val) max_val = val;
    }
    
    float alpha = (max_val > 1e-10f) ? (2.0f / max_val) : 0.01f;
    
    // パラメータ設定
    gd_params_t params;
    params.H = H;
    params.Hadj = Hadj;
    params.b = b->data;
    params.config = config;
    params.alpha = alpha;
    params.max_iterations = max_iterations;
    params.display_interval = display_interval;
    
    // FISTA初期化
    float tk = 1.0f;
    int orig_size = (config->end_i - config->start_i) * (config->end_j - config->start_j);
    float *xk = (float*)malloc(orig_size * sizeof(float));
    if (!xk) {
        free(config);
        free(v);
        free(Hadj);
        free(H);
        return -1;
    }
    
    crop_image(v, xk, config);
    
    float *vk = v;
    float *vk_new = (float*)malloc(fft_size * sizeof(float));
    float *xk_new = (float*)malloc(orig_size * sizeof(float));
    
    if (!vk_new || !xk_new) {
        free(xk_new);
        free(vk_new);
        free(xk);
        free(config);
        free(v);
        free(Hadj);
        free(H);
        return -1;
    }
    
    // 反復処理
    for (int iter = 0; iter < max_iterations; iter++) {
        fista_update(vk, tk, xk, &params, vk_new, &tk, xk_new);
        
        // 更新
        float *temp_vk = vk;
        vk = vk_new;
        vk_new = temp_vk;
        
        float *temp_xk = xk;
        xk = xk_new;
        xk_new = temp_xk;
        
        // 表示（オプション）
        if (display_interval > 0 && (iter % display_interval == 0)) {
            printf("Iteration %d\n", iter);
        }
    }
    
    // 最終結果をクロップして出力
    crop_image(vk, output->data, config);
    non_negative_projection(output->data, orig_size);
    
    // メモリ解放
    free(xk_new);
    free(vk_new);
    free(xk);
    free(config);
    free(v);
    free(Hadj);
    free(H);
    
    return 0;
}

Arduino IDE：test_lensless_gd_spresense.ino を開き，同梱の lensless_gd.h / lensless_gd.c を同じフォルダに置く．ボードはSpresense，メモリは大きめに設定．
実行：SDからPSFと観測データを読ませ，grad_descent() で再構成．結果は output.raw，psf_resized.raw，data_resized.raw としてSDに保存．

こちらも動かない！！！
サンプルデータをそのまま読み込むと，エラーが発生しました．
AIに要因を聞いてみるとメモリが足りてないとか．．．

リサイズを試しても画像を崩してしまったり，苦戦していると日時はすでに1月30日．．．タイムアップになってしましました．

「先駆者がいるもののアレンジだし．．．」と余裕を感じていたのが最大の反省点です．
そもそも開発環境作りで躓く．．．
ドライバインストールの部分で，認識しない！！！と奮闘していました．

年末に認識しない！助けて！とヘルプを運営さんに送ったのは私です．
結局はUSBケーブルを変えたら認識した落ちでした．すみません．返事もなかったけど．

VSCodeで環境を作ろうとしてハマる．結局Arduino IDEでも十分なものが作れるとわかって方向転換したり．
そこからは時間がない！作りきれない！という状況になってしましました．
作りたいものの主題とはずれたところで時間を使ってしまって，作りきれないという状況になってしまいました．
すみませんでしたー！

今回は以上で終了ですが，いつか作り上げたいな．