[HOW-TO] Run AutoFocus only on a specific range of lenses

ilirosmanaj commented 8 months ago

My end goal is simple, but to do so, I need some understanding of Picamera2 and libcamera as well.

Let me explain my problem in a couple of bullet points:

I want to use camera autofocus, which runs only when I trigger it
I want to use the Fast autofocus speed
I want to use a Macro autofocus range, limited in lens position from 13 to 18. Defaulting to 13
When the autofocus is triggered, I want to use PDAF in a way that it acts as quickly as possible
- Potentially, I would want to allow PDAF to do only one range of scans (as in starting from lens 13 and going up, but then stopping as quickly as it finds some suitable capture)
- I would be cool with PDAF only testing rounded lens positions (e.g., only values 13, 14, 15, 16, 17, or 18). Currently, this spends plenty of time in fractional values (e.g. 13.55555)

Note: what I am trying to capture is my hand, which is in front of the camera (about 10-15 cm from the camera itself).

Here is my camera tunning file (adding only the autofocus part here):

{
            "rpi.af":
            {
                "ranges":
                {
                    "normal":
                    {
                        "min": 1.0,
                        "max": 32.0,
                        "default": 1.0
                    },
                    "macro":
                    {
                        "min": 13.0,
                        "max": 18.0,
                        "default": 13.0
                    }
                },
                "speeds":
                {
                    "normal":
                    {
                        "step_coarse": 2.0,
                        "step_fine": 0.5,
                        "contrast_ratio": 0.75,
                        "pdaf_gain": -0.03,
                        "pdaf_squelch": 0.2,
                        "max_slew": 4.0,
                        "pdaf_frames": 20,
                        "dropout_frames": 6,
                        "step_frames": 4
                    },
                    "fast":
                    {
                        "step_coarse": 1.0,
                        "step_fine": 1.0,
                        "contrast_ratio": 0.75,
                        "pdaf_gain": -0.05,
                         # modified from 0.2, which is the default
                        "pdaf_squelch": 0.4,
                        "max_slew": 5.0,
                         # modified from 16, which is the default
                        "pdaf_frames": 10,
                        "dropout_frames": 6,
                        "step_frames": 2
                    }
                },
                "conf_epsilon": 8,
                "conf_thresh": 12,
                "conf_clip": 512,
                "skip_frames": 5,
                "map": [ 0.0, 420, 35.0, 920 ]
            }
        }

Here is a test script I am using. For testing, please use a lighter in front of the camera so that the hand is visible to the camera more quickly (and this is what I will be using as well - an LED that lightens the hand).

import io

import logging
import cv2
from time import sleep
from picamera2 import Picamera2
from picamera2.encoders import MJPEGEncoder
from picamera2.outputs import FileOutput
from http.server import BaseHTTPRequestHandler
from threading import Condition, get_ident
from time import perf_counter

logging.basicConfig(level = logging.DEBUG, format='%(asctime)s - %(name)s - %(levelname)s: %(message)s')

SHOULD_SCAN = True

def print_af_state(request):
    global SHOULD_SCAN
    md = request.get_metadata()
    af_camera_state = ("Idle", "Scanning", "Success", "Fail")[md['AfState']]
    lens_position = md.get('LensPosition')

    if af_camera_state == "Idle":
        return

    if af_camera_state == "Success" and SHOULD_SCAN:
        logging.debug(f"AF Success on lens position: {lens_position}")
        SHOULD_SCAN = False
        return

    if SHOULD_SCAN:
        logging.debug(f"Autofocus camera state: {af_camera_state}, lens position: {lens_position}")

# used for running live streaming of the video from my device
class StreamingOutput(io.BufferedIOBase):
    def __init__(self):
        self.frame = None
        self.condition = Condition()

    def write(self, buf):
        with self.condition:
            self.frame = buf
            self.condition.notify_all()

active_thread = 0

class StreamingHandler(BaseHTTPRequestHandler):
    def do_GET(self):
        # New requests kill old requests, only one active loop allowed
        global active_thread
        active_thread = get_ident()

        self.send_response(200)
        self.send_header("Age", 0)
        self.send_header("Cache-Control", "no-cache, private")
        self.send_header("Pragma", "no-cache")
        self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=FRAME")
        self.end_headers()

        try:
            while active_thread == get_ident():
                with STREAMING_OUTPUT.condition:
                    if not STREAMING_OUTPUT.condition.wait(timeout=30.0):
                        raise Exception("Timed out waiting for next frame!")
                frame = STREAMING_OUTPUT.frame

                self.wfile.write(b"--FRAME\r\n")
                self.send_header("Content-Type", "image/jpeg")
                self.send_header("Content-Length", len(frame))
                self.end_headers()
                self.wfile.write(frame)
                self.wfile.write(b"\r\n")

        except Exception as e:
            logging.warning("Removed streaming client %s: %s", self.client_address, str(e))

STREAMING_OUTPUT = StreamingOutput()

MJPEG_ENCODER = MJPEGEncoder(bitrate=14 * 1000000)
FILE_OUTPUT = FileOutput(STREAMING_OUTPUT)

camera = Picamera2(1)

camera.set_logging(level=logging.INFO)
camera.pre_callback = print_af_state
video_config = camera.create_video_configuration(
    main={"size": (2304, 1296), "format": "RGB888"}, 
    lores={"size": (192, 108)},
    encode="lores", 
    buffer_count=6,
    queue=False,
    controls={
        "Brightness": 0,
        "Contrast": 1.4,
        "Saturation": 1,
        "AeConstraintMode":0,
        "AeEnable": True,
        "ExposureTime": 3000,
        "AnalogueGain": 3,
        "AeExposureMode": 0,
        "ExposureValue": 0,
        "AeMeteringMode": 1,
        "AwbEnable": 0,
        "AwbMode": 0,
        "ColourGains": (2.0, 1.5),
        "AfMetering": 0, # 0 auto (uses central part of image), windows 1 (uses region specificed in AfWindows)
        "AfMode": 1, # auto (do AF only when triggered)
        "AfRange": 1, # macro, but with edited tunning file
        "AfSpeed": 1, # fast
        "FrameRate": 80,
    }
)
camera.configure(video_config)

camera.start_recording(MJPEG_ENCODER, FILE_OUTPUT)
logging.debug(f"Camera recording has started")
camera.autofocus_cycle(wait=False)

for i in range(1000):
    if not SHOULD_SCAN:
        break

    with STREAMING_OUTPUT.condition:
        STREAMING_OUTPUT.condition.wait()
    frame = STREAMING_OUTPUT.frame
    logging.debug(f"Processed frame: {i}")

logging.debug(f"Finished")

metadata_start_timestamp = perf_counter()

request = camera.capture_request()
image = request.make_array("main")
metadata = request.get_metadata()
request.release()
normal_camera_captures = [image]

cv2.imwrite(f"final_capture.jpg", image)

A test run:

2024-02-05 10:28:35,839 - picamera2.picamera2 - INFO: Initialization successful.
2024-02-05 10:28:35,840 - picamera2.picamera2 - INFO: Camera now open.
2024-02-05 10:28:35,841 - picamera2.picamera2 - DEBUG: <libcamera._libcamera.CameraManager object at 0x7f935be1b0>
picamera2.picamera2 INFO: Configuration successful!
2024-02-05 10:28:35,909 - picamera2.picamera2 - INFO: Configuration successful!
picamera2.picamera2 INFO: Camera started
2024-02-05 10:28:36,351 - picamera2.picamera2 - INFO: Camera started
2024-02-05 10:28:36,352 - root - DEBUG: Camera recording has started
2024-02-05 10:28:36,553 - root - DEBUG: Processed frame: 0
2024-02-05 10:28:36,579 - root - DEBUG: Processed frame: 1
2024-02-05 10:28:36,599 - root - DEBUG: Processed frame: 2
2024-02-05 10:28:36,607 - root - DEBUG: Processed frame: 3
2024-02-05 10:28:36,644 - root - DEBUG: Processed frame: 4
2024-02-05 10:28:36,652 - root - DEBUG: Processed frame: 5
2024-02-05 10:28:36,672 - root - DEBUG: Autofocus camera state: Scanning, lens position: 1.0
2024-02-05 10:28:36,686 - root - DEBUG: Processed frame: 6
2024-02-05 10:28:36,689 - root - DEBUG: Autofocus camera state: Scanning, lens position: 6.0
2024-02-05 10:28:36,694 - root - DEBUG: Processed frame: 7
2024-02-05 10:28:36,714 - root - DEBUG: Autofocus camera state: Scanning, lens position: 6.0
2024-02-05 10:28:36,729 - root - DEBUG: Processed frame: 8
2024-02-05 10:28:36,733 - root - DEBUG: Autofocus camera state: Scanning, lens position: 11.0
2024-02-05 10:28:36,736 - root - DEBUG: Processed frame: 9
2024-02-05 10:28:36,755 - root - DEBUG: Autofocus camera state: Scanning, lens position: 11.0
2024-02-05 10:28:36,770 - root - DEBUG: Processed frame: 10
2024-02-05 10:28:36,773 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,777 - root - DEBUG: Processed frame: 11
2024-02-05 10:28:36,797 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,811 - root - DEBUG: Processed frame: 12
2024-02-05 10:28:36,815 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,819 - root - DEBUG: Processed frame: 13
2024-02-05 10:28:36,836 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,853 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,856 - root - DEBUG: Processed frame: 14
2024-02-05 10:28:36,871 - root - DEBUG: Processed frame: 15
2024-02-05 10:28:36,874 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,878 - root - DEBUG: Processed frame: 16
2024-02-05 10:28:36,893 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,911 - root - DEBUG: Processed frame: 17
2024-02-05 10:28:36,915 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,931 - root - DEBUG: Processed frame: 18
2024-02-05 10:28:36,935 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,953 - root - DEBUG: Processed frame: 19
2024-02-05 10:28:36,957 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,971 - root - DEBUG: Processed frame: 20
2024-02-05 10:28:36,975 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:36,979 - root - DEBUG: Processed frame: 21
2024-02-05 10:28:36,998 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:37,016 - root - DEBUG: Processed frame: 22
2024-02-05 10:28:37,022 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.0
2024-02-05 10:28:37,038 - root - DEBUG: Processed frame: 23
2024-02-05 10:28:37,041 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.0
2024-02-05 10:28:37,057 - root - DEBUG: Processed frame: 24
2024-02-05 10:28:37,062 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.0
2024-02-05 10:28:37,066 - root - DEBUG: Processed frame: 25
2024-02-05 10:28:37,085 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.0
2024-02-05 10:28:37,099 - root - DEBUG: Processed frame: 26
2024-02-05 10:28:37,102 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.0
2024-02-05 10:28:37,119 - root - DEBUG: Processed frame: 27
2024-02-05 10:28:37,122 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.0
2024-02-05 10:28:37,136 - root - DEBUG: Processed frame: 28
2024-02-05 10:28:37,139 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.0
2024-02-05 10:28:37,156 - root - DEBUG: Processed frame: 29
2024-02-05 10:28:37,160 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.0
2024-02-05 10:28:37,175 - root - DEBUG: Processed frame: 30
2024-02-05 10:28:37,178 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.0
2024-02-05 10:28:37,195 - root - DEBUG: Processed frame: 31
2024-02-05 10:28:37,198 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.0
2024-02-05 10:28:37,213 - root - DEBUG: Processed frame: 32
2024-02-05 10:28:37,216 - root - DEBUG: Autofocus camera state: Scanning, lens position: 16.0
2024-02-05 10:28:37,234 - root - DEBUG: Processed frame: 33
2024-02-05 10:28:37,240 - root - DEBUG: Autofocus camera state: Scanning, lens position: 16.0
2024-02-05 10:28:37,254 - root - DEBUG: Processed frame: 34
2024-02-05 10:28:37,257 - root - DEBUG: Autofocus camera state: Scanning, lens position: 16.0
2024-02-05 10:28:37,276 - root - DEBUG: Processed frame: 35
2024-02-05 10:28:37,279 - root - DEBUG: Autofocus camera state: Scanning, lens position: 16.0
2024-02-05 10:28:37,293 - root - DEBUG: Processed frame: 36
2024-02-05 10:28:37,296 - root - DEBUG: Autofocus camera state: Scanning, lens position: 16.0
2024-02-05 10:28:37,315 - root - DEBUG: Processed frame: 37
2024-02-05 10:28:37,319 - root - DEBUG: Autofocus camera state: Scanning, lens position: 16.0
2024-02-05 10:28:37,324 - root - DEBUG: Processed frame: 38
2024-02-05 10:28:37,339 - root - DEBUG: Autofocus camera state: Scanning, lens position: 17.0
2024-02-05 10:28:37,356 - root - DEBUG: Processed frame: 39
2024-02-05 10:28:37,361 - root - DEBUG: Autofocus camera state: Scanning, lens position: 17.0
2024-02-05 10:28:37,379 - root - DEBUG: Processed frame: 40
2024-02-05 10:28:37,382 - root - DEBUG: Autofocus camera state: Scanning, lens position: 17.0
2024-02-05 10:28:37,397 - root - DEBUG: Processed frame: 41
2024-02-05 10:28:37,400 - root - DEBUG: Autofocus camera state: Scanning, lens position: 17.0
2024-02-05 10:28:37,418 - root - DEBUG: Processed frame: 42
2024-02-05 10:28:37,422 - root - DEBUG: Autofocus camera state: Scanning, lens position: 17.0
2024-02-05 10:28:37,437 - root - DEBUG: Processed frame: 43
2024-02-05 10:28:37,440 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.609925270080566
2024-02-05 10:28:37,458 - root - DEBUG: Processed frame: 44
2024-02-05 10:28:37,461 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.609925270080566
2024-02-05 10:28:37,479 - root - DEBUG: Processed frame: 45
2024-02-05 10:28:37,482 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.609925270080566
2024-02-05 10:28:37,497 - root - DEBUG: Processed frame: 46
2024-02-05 10:28:37,500 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.609925270080566
2024-02-05 10:28:37,520 - root - DEBUG: Processed frame: 47
2024-02-05 10:28:37,523 - root - DEBUG: Autofocus camera state: Scanning, lens position: 15.609925270080566
2024-02-05 10:28:37,541 - root - DEBUG: Processed frame: 48
2024-02-05 10:28:37,543 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.609925270080566
2024-02-05 10:28:37,557 - root - DEBUG: Processed frame: 49
2024-02-05 10:28:37,559 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.609925270080566
2024-02-05 10:28:37,575 - root - DEBUG: Processed frame: 50
2024-02-05 10:28:37,579 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.609925270080566
2024-02-05 10:28:37,594 - root - DEBUG: Processed frame: 51
2024-02-05 10:28:37,597 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.609925270080566
2024-02-05 10:28:37,614 - root - DEBUG: Processed frame: 52
2024-02-05 10:28:37,620 - root - DEBUG: Autofocus camera state: Scanning, lens position: 14.609925270080566
2024-02-05 10:28:37,633 - root - DEBUG: Processed frame: 53
2024-02-05 10:28:37,637 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.609925270080566
2024-02-05 10:28:37,655 - root - DEBUG: Processed frame: 54
2024-02-05 10:28:37,659 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.609925270080566
2024-02-05 10:28:37,675 - root - DEBUG: Processed frame: 55
2024-02-05 10:28:37,678 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.609925270080566
2024-02-05 10:28:37,698 - root - DEBUG: Processed frame: 56
2024-02-05 10:28:37,703 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.609925270080566
2024-02-05 10:28:37,719 - root - DEBUG: Processed frame: 57
2024-02-05 10:28:37,721 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.609925270080566
2024-02-05 10:28:37,738 - root - DEBUG: Processed frame: 58
2024-02-05 10:28:37,742 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.609925270080566
2024-02-05 10:28:37,760 - root - DEBUG: Processed frame: 59
2024-02-05 10:28:37,763 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:37,778 - root - DEBUG: Processed frame: 60
2024-02-05 10:28:37,781 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:37,796 - root - DEBUG: Processed frame: 61
2024-02-05 10:28:37,800 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:37,815 - root - DEBUG: Processed frame: 62
2024-02-05 10:28:37,818 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:37,836 - root - DEBUG: Processed frame: 63
2024-02-05 10:28:37,839 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.0
2024-02-05 10:28:37,859 - root - DEBUG: Processed frame: 64
2024-02-05 10:28:37,863 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.492711067199707
2024-02-05 10:28:37,880 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.492711067199707
2024-02-05 10:28:37,881 - root - DEBUG: Processed frame: 65
2024-02-05 10:28:37,899 - root - DEBUG: Processed frame: 66
2024-02-05 10:28:37,902 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.492711067199707
2024-02-05 10:28:37,918 - root - DEBUG: Processed frame: 67
2024-02-05 10:28:37,921 - root - DEBUG: Autofocus camera state: Scanning, lens position: 13.492711067199707
2024-02-05 10:28:37,926 - root - DEBUG: Processed frame: 68
2024-02-05 10:28:37,945 - root - DEBUG: AF Success on lens position: 13.492711067199707
2024-02-05 10:28:37,963 - root - DEBUG: Processed frame: 69
2024-02-05 10:28:37,966 - root - DEBUG: Finished

So this took 2s already, which is way too much for what I want. I would need it at a maximum of 200ms speed.

To wrap up, I need to run the AF on lenses [13, 14, 15, 16, 17, 19] as quickly as possible in a trustable way.

I do expect this to be a combination of picamera and libcamera as well, so happy to perform tunning as much as needed. I failed to find some good explanation of the PDAF algorithm and their parameters so that I would know how to change.

ilirosmanaj commented 8 months ago

@davidplowman a very interesting problem I have been checking recently. Hope you can help!

davidplowman commented 8 months ago

Here are some suggestions:

If your search is always starting at at LP = 13, maybe set the lens position to 13 in your configuration (add 'LensPosition': 13 to the controls for creating the configuration).
You may actually have more luck running at 30fps than 80fps. If you run faster than 30 then algorithms like AF will start skipping frames.
It looks to me from the log like it's currently taking about 1.5s. Can you reduce the range any more? I suspect that doing an AF search over a close range like this is always going to be somewhat slow because the image is so sensitive to lens movement. But if it's still taking too long, @njhollinghurst may have some suggestions on other tuning changes.

njhollinghurst commented 8 months ago

Yes I would amplify that it may work better at 30fps.
It appears that the sensor is not getting much PDAF phase information. Perhaps the hand does not have any strong vertical edges? The long wait at 13.0 is a timeout waiting for this, before it defaults to CDAF. You can shorten the timeout by reducing dropout_frames. However it's likely that a moving hand might not produce a good peak for CDAF.
If you reduce conf_thresh and conf_epsilon, the algorithm will accept PDAF with lower confidence. It might be less stable.
You can get more debug by setting this environment variable: LIBCAMERA_LOG_LEVELS=RPiAf:0 The last three columns print contrast, phase and confidence (for phase) measurements.

ilirosmanaj commented 8 months ago

Thanks for the response, @davidplowman @njhollinghurst.

Have set up the frame rate to 30pfs
I am not sure it's respecting the lens position; even when I set it from the config, it always starts at 1. Does it need some time to pick up? Or the lens position isn't respected if you set the AF mode to auto and manual.
I've tried lowering the confidence threshold and epsilon but it is not stable, and the produced images are also not very well-focused

I am unsure what the next reasonable step would be in this case. Is there a way to completely disable CDAF?

raspberrypi / picamera2

[HOW-TO] Run AutoFocus only on a specific range of lenses #944