#+title: A tour of mrcal: stereo processing
#+OPTIONS: toc:nil

* Previous
We just [[file:tour-choreography.org][found the best chessboard-dancing technique]]

* Stereo
This is an overview of a more detailed discussion about [[file:stereo.org][dense stereo]].

#+begin_src sh :exports none :eval no-export
# all the images downsampled for view on the page like this
D=~/projects/mrcal-doc-external/2022-11-05--dtla-overpass--samyang--alpha7/stereo
Dout=~/projects/mrcal-doc-external/figures/stereo
mkdir -p $Dout
for img ( $D/[01].jpg ) { \
  convert $img -scale 12% $Dout/${img:r:t}.downsampled.${img:e}
}
#+end_src

We computed intrinsics from chessboards observations earlier, so let's use these
for stereo processing.

I took several images off [[https://www.openstreetmap.org/#map=19/34.05565/-118.25333][a catwalk over Figueroa Street in downtown Los
Angeles]]. This is the view South along Figueroa Street. There're tall buildings
ahead and on either side, which makes an interesting stereo scene.

The two images out of the camera look like this:

[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/0.jpg][file:external/figures/stereo/0.downsampled.jpg]]
[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/1.jpg][file:external/figures/stereo/1.downsampled.jpg]]

All the full-size images are available by clicking on an image.

The cameras are ~ 2m apart. In order to compute stereo images we need an
accurate estimate of the relative geometry of the cameras, which we usually get
as an output of the calibration process. /Here/ I only had one physical camera,
so I did something different:

- I calibrated the one camera monocularly, to get its intrinsics. This is what
  we computed thus far
- I used this camera to take a pair of images from slightly different locations.
  This is the "stereo pair"
- I used a separate tool to estimate the extrinsics from corresponding feature
  detections in the two images. For the purposes of this document the details of
  this don't matter

Generating a stereo pair in this way works well to demo the stereo processing
tools. It does break the [[file:triangulation.org][triangulation uncertainty reporting]], since we lose the
uncertainty information in the extrinsics, however. Preserving this uncertainty,
and propagating it through the recomputed extrinsics to triangulation is on the
[[file:roadmap.org][mrcal roadmap]]. The resulting models we use for stereo processing:

- [[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/0.cameramodel][Left camera]]
- [[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/1.cameramodel][Right camera]]


#+begin_src sh :exports none :eval no-export
See external/2022-11-05--dtla-overpass--samyang--alpha7/notes.org for
documentation about how I made these
#+end_src

I then use the [[file:mrcal-stereo.html][=mrcal-stereo=]] tool to compute rectification maps, rectify the
images, compute disparities and convert them to ranges:

#+begin_src sh
mrcal-stereo                     \
  --az-fov-deg 170               \
  --az0-deg    10                \
  --el-fov-deg 95                \
  --el0-deg    -15               \
  --disparity-range 0 300        \
  --sgbm-p1 600                  \
  --sgbm-p2 2400                 \
  --sgbm-uniqueness-ratio 5      \
  --sgbm-disp12-max-diff  1      \
  --sgbm-speckle-window-size 100 \
  --sgbm-speckle-range 2         \
  --force-grayscale              \
  --clahe                        \
  [01].cameramodel               \
  [01].jpg                       \
#+end_src
#+begin_src sh :exports none :eval no-export
D=~/projects/mrcal-doc-external/2022-11-05--dtla-overpass--samyang--alpha7/stereo;
Dout=~/projects/mrcal-doc-external/figures/stereo

PYTHONPATH=~/projects/mrcal;
export PYTHONPATH;
$PYTHONPATH/mrcal-stereo         \
  --az-fov-deg 170               \
  --az0-deg    10                \
  --el-fov-deg 95                \
  --el0-deg    -15               \
  --disparity-range 0 300        \
  --sgbm-p1 600                  \
  --sgbm-p2 2400                 \
  --sgbm-uniqueness-ratio 5      \
  --sgbm-disp12-max-diff  1      \
  --sgbm-speckle-window-size 100 \
  --sgbm-speckle-range 2         \
  --force-grayscale              \
  --clahe                        \
  --outdir $D                    \
  $D/[01].cameramodel            \
  $D/[01].jpg

for img ( $D/*-{rectified,disparity,range}.png ) { \
  convert $img -scale 12% $Dout/${img:r:t}.downsampled.${img:e}
}
#+end_src

The =--sgbm-...= options configure the [[https://docs.opencv.org/4.5.3/d2/d85/classcv_1_1StereoSGBM.html][OpenCV SGBM stereo matcher]]. Not
specifying them uses the OpenCV defaults, which usually produces poor results.

The rectified images look like this:

[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/0-rectified.png][file:external/figures/stereo/0-rectified.downsampled.png]]
[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/1-rectified.png][file:external/figures/stereo/1-rectified.downsampled.png]]

And the disparity and range images look like this:

[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/0-disparity.png][file:external/figures/stereo/0-disparity.downsampled.png]]
[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/0-range.png][file:external/figures/stereo/0-range.downsampled.png]]

From a glance, this is clearly working well.

If you've used other stereo libraries previously, these rectified images may
look odd: these are fisheye images, so [[file:stereo.org::#stereo-rectification-models][the usual pinhole rectification would
pull the corners out towards infinity]]. We don't see that here because
[[file:mrcal-stereo.html][=mrcal-stereo=]] uses the [[file:lensmodels.org::#lensmodel-latlon][transverse equirectangular projection]] for rectification
by default. This scheme samples the azimuth and elevation angles evenly, which
minimizes the visual distortion inside each image row. Rectified models for this
scene are generated by [[file:mrcal-stereo.html][=mrcal-stereo=]] [[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/rectified0.cameramodel][here]] and [[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/rectified1.cameramodel][here]]. See the [[file:stereo.org][stereo-processing
documentation]] for details.

* Geometry
The rectified models define a system with axes

- $x$: the baseline; from the origin of the left camera to the origin of the
  right camera
- $y$: down
- $z$: forward

[[file:mrcal-stereo.html][=mrcal-stereo=]] selects the $y$ and $z$ axes to line up as much as possible with
the geometry of the input models. Since the geometry of the actual cameras may
not match the idealized rectified geometry (the "forward" view direction may not
be orthogonal to the baseline), the usual expectations that $c_x \approx
\frac{\mathrm{imagerwidth}}{2}$ and $c_y \approx
\frac{\mathrm{imagerheight}}{2}$ are not necessarily true in the rectified
system. Thus it's valuable to visualize the models /and/ the rectified field of
view, for verification. [[file:mrcal-stereo.html][=mrcal-stereo=]] can do that:

#+begin_src sh
mrcal-stereo        \
  --az-fov-deg 170  \
  --az0-deg    10   \
  --el-fov-deg 95   \
  --el0-deg    -15  \
  --set 'view 70,5' \
  --viz geometry    \
  splined-[01].cameramodel
#+end_src
#+begin_src sh :exports none :eval no-export
PYTHONPATH=~/projects/mrcal;
export PYTHONPATH;
$PYTHONPATH/mrcal-stereo                                            \
  --az-fov-deg 170                                                  \
  --az0-deg    10                                                   \
  --el-fov-deg 95                                                   \
  --el0-deg    -15                                                  \
  --set 'view 70,5'                                                 \
  --viz geometry                                                    \
  --hardcopy $Dout/stereo-rectified-system.svg                      \
  --terminal 'svg size 800,600 noenhanced solid dynamic font ",14"' \
  $D/[01].cameramodel
#+end_src

[[file:external/figures/stereo/stereo-rectified-system.svg]]

Here, the geometry /is/ mostly nominal and the rectified view (indicated by the
purple lines) /does/ mostly lie along the $z$ axis. This is a 3D plot that can
be rotated by the user when =mrcal-stereo --viz geometry= is executed, making
the geometry clearer.

* ranged pixels ground-truth                                       :noexport:
**** Buildings
top of Paul Hastings building. 530m away horizontally, 200m vertically: 566m away
https://en.wikipedia.org/wiki/City_National_Plaza

top of 7th/metro building at 7th/figueroa: 860m horizontally, 108m vertically: 870m
Figueroa Tower
https://www.emporis.com/buildings/116486/figueroa-tower-los-angeles-ca-usa

Top of library tower at 5th/figueroa. 529m horizontally, 300m vertically: 608m

Near the top of the wilshire grand: 830m horizontall 270m vertically: 873
http://www.skyscrapercenter.com/building/wilshire-grand-center/9686

Near the top of the N Wells Fargo plaza building. 337m horizontally, 220m vertically: 402m
https://en.wikipedia.org/wiki/Wells_Fargo_Center_(Los_Angeles)

Los Angeles Center studios ~ 50m tall, on a hill. 520m horizontally: 522m

333 S Beaudry building. 291m horizontally 111m vertically: 311m
https://www.emporis.com/buildings/116570/beaudry-center-los-angeles-ca-usa

**** tests

Command to test all the ranges

#+begin_src sh :exports none :eval no-export
PYTHONPATH=~/projects/mrcal;
export PYTHONPATH
what=opencv8; (
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 2874 1231 --range-estimate 566 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 2968 1767 --range-estimate 870 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 1885 864  --range-estimate 594 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 3090 1384 --range-estimate 862 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg  541  413 --range-estimate 402 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 4489 1631 --range-estimate 522 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 5483  930 --range-estimate 311 --search-radius 10
$PYTHONPATH/mrcal-triangulate $D/$what-[01].cameramodel $D/[01].jpg 5351  964 --range-estimate 311 --search-radius 10
) | egrep 'q1|Range'
#+end_src

=tst.py= to just look at a set of ranged features, and to compute the extrinsics
with a simple procrustes fit. Bypasses deltapose entirely. Works ok, but not
amazingly well

#+begin_src python :exports none :eval no-export
#!/usr/bin/python3

import sys
import numpy as np
import numpysane as nps

sys.path[:0] = '/home/dima/projects/mrcal',
sys.path[:0] = '/home/dima/deltapose-lite',
sys.path[:0] = '/home/dima/img_any',
import mrcal

model_intrinsics = mrcal.cameramodel(2022-11-05--dtla-overpass--samyang--alpha7/2-f22-infinity/splined.cameramodel')
t01              = np.array((7.*12*2.54/100, 0, 0))  # 7ft separation on the x

xy_xy_range = \
    np.array((

        (2874, 1231, 2831.68164062, 1233.9498291,  566.0),
        (2968, 1767, 2916.48388672, 1771.91601562, 870.0),
        (1885, 864,  1851.86499023, 843.52398682,  594.0),
        (3090, 1384, 3046.8894043,  1391.49401855, 862.0),
        (541,  413,  513.77832031,  355.37588501,  402.0),
        (4489, 1631, 4435.24023438, 1665.17492676, 522.0),
        (5483, 930,  5435.96582031, 987.39813232,  311.0),
        (5351, 964,  5304.21630859, 1018.49682617, 311.0),

        # Ranged pavement points. These don't appear to help
        (3592.350428, 3199.133514, 3198.330034, 3227.890159, 14.6),
        (3483.817362, 3094.172913, 3117.605605, 3115.684005, 15.76),
 ))

xy_xy = None
#xy_xy = np.array(( (3483.817362, 3094.172913,	3117.605605, 3115.684005),))





q0 = xy_xy_range[:,0:2]
q1 = xy_xy_range[:,2:4]
r  = xy_xy_range[:,(4,)]

# Points observed by camera0, represented in camera1 frame
p0 = mrcal.unproject(q0, *model_intrinsics.intrinsics(), normalize=True)*r - t01

# The unit observation vectors from the two cameras, observed in camera1. These
# must match via a rotation
v0 = p0 / nps.dummy(nps.mag(p0), -1)
v1 = mrcal.unproject(q1, *model_intrinsics.intrinsics(), normalize=True)

R01  = mrcal.align_procrustes_vectors_R01(v0,v1)
Rt01 = nps.glue(R01, t01, axis=-2)


if xy_xy is not None:
    import deltapose_lite
    rt10 = mrcal.rt_from_Rt(mrcal.invert_Rt(Rt01))
    p = \
        deltapose_lite.compute_3d_intersection_lindstrom(rt10,
                                                         model_intrinsics.intrinsics(),
                                                         model_intrinsics.intrinsics(),
                                                         xy_xy[:,0:2],
                                                         xy_xy[:,2:4],)
    print(nps.mag(p))
    sys.exit()


model0 = mrcal.cameramodel(model_intrinsics)
model0.extrinsics_Rt_toref(mrcal.identity_Rt())
model0.write('/tmp/0.cameramodel')

model1 = mrcal.cameramodel(model_intrinsics)
model1.extrinsics_Rt_toref( Rt01 )
model1.write('/tmp/1.cameramodel')
#+end_src

* Ranging
:PROPERTIES:
:CUSTOM_ID: tour-stereo-ranging
:END:

The [[file:mrcal-stereo.html][=mrcal-stereo=]] tool contains a visualizer that allows the user to quickly
examine the stereo scene, evaluating the epipolar line alignment, disparities,
ranges, etc. It can be invoked by runnung =mrcal-stereo --viz stereo ...=. After
panning/zooming, pressing =r= to display ranges (not disparities), and clicking
on the [[https://en.wikipedia.org/wiki/Wilshire_Grand_Center][Wilshire Grand building]] we get this:

[[file:external/2022-11-05--dtla-overpass--samyang--alpha7/stereo/mrcal-stereo-viz.png]]

The computed range at that pixel is 965.52m. My estimated ground truth range is
873m. According to the linearized sensitivity reported by the tool, this
corresponds to an error of
$\frac{966\mathrm{m}-873\mathrm{m}}{241.3\frac{\mathrm{m}}{\mathrm{pixel}}} =
0.39 \mathrm{pixels}$. Usually, stereo matching errors are in the 1/2 - 1/3
pixel range, so this is in-line with expectations.

Here we used dense stereo processing to compute a range map over the whole
image. This is slow, and a lot of the time you can get away with computing
ranges at a sparse set of points instead. So let's talk about [[file:tour-triangulation.org][triangulation
routines]].

* Next
We're ready to talk about [[file:tour-triangulation.org][triangulation routines]]
