Das Scharfstellen ist kein Problem, das macht jede Autofocus-Kamera auch. Hierfür werden im Sichtfeldzentrum zwei oder mehrere dicht beieinander liegende Puntke genommen, und die Schärfe solange justiert, bis maximale Helligkeitsunterschiede dieser Punkte festgestellt werden.

Ich plane eine ähnliche Verfahrensweise, jedoch ersteinmal mit einer Fotodiode auf dem einen Sensorkopf und einer starken Lichtquelle auf dem anderen Sensorkopf. Die vom Sensor aufgefangenen Lichtreflexionen in Verbindung mit den Winkeln der Köpfe sollen hierbei zur Entfernungsbestimmung dienen.
Erheblich simpler als dein Vorhaben, aber ein Anfang