CameraITS: add test_lens_position.py script

Change-Id: I1b556ab580709592f4ab1df22116660cb66172ad
diff --git a/apps/CameraITS/tests/scene3/test_lens_position.py b/apps/CameraITS/tests/scene3/test_lens_position.py
new file mode 100644
index 0000000..2c5151c
--- /dev/null
+++ b/apps/CameraITS/tests/scene3/test_lens_position.py
@@ -0,0 +1,183 @@
+# Copyright 2016 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the 'License');
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an 'AS IS' BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import its.caps
+import its.device
+import its.image
+import its.objects
+import numpy as np
+
+NUM_TRYS = 2
+NUM_STEPS = 6
+SHARPNESS_TOL = 10  # percentage
+POSITION_TOL = 10  # percentage
+FRAME_TIME_TOL = 10  # ms
+VGA_WIDTH = 640
+VGA_HEIGHT = 480
+NAME = os.path.basename(__file__).split('.')[0]
+
+
+def test_lens_position(cam, props, fmt, sensitivity, exp, af_fd):
+    """Return fd, sharpness, lens state of the output images.
+
+    Args:
+        cam: An open device session.
+        props: Properties of cam
+        fmt: dict; capture format
+        sensitivity: Sensitivity for the 3A request as defined in
+            android.sensor.sensitivity
+        exp: Exposure time for the 3A request as defined in
+            android.sensor.exposureTime
+        af_fd: Focus distance for the 3A request as defined in
+            android.lens.focusDistance
+
+    Returns:
+        Dictionary of results for different focal distance captures
+        with static lens positions and moving lens positions
+        d_static, d_moving
+    """
+
+    xnorm, ynorm, wnorm, hnorm = its.image.find_af_chart(cam, props,
+                                                         sensitivity,
+                                                         exp, af_fd)
+    data_static = {}
+    data_moving = {}
+    white_level = int(props['android.sensor.info.whiteLevel'])
+    min_fd = props['android.lens.info.minimumFocusDistance']
+    hyperfocal = props['android.lens.info.hyperfocalDistance']
+    fds_f = np.arange(hyperfocal, min_fd, (min_fd-hyperfocal)/(NUM_STEPS-1))
+    fds_f = np.append(fds_f, min_fd)
+    fds_f = fds_f.tolist()
+    fds_b = list(reversed(fds_f))
+    fds_fb = list(fds_f)
+    fds_fb.extend(fds_b)  # forward and back
+    # take static data set
+    for i, fd in enumerate(fds_fb):
+        req = its.objects.manual_capture_request(sensitivity, exp)
+        req['android.lens.focusDistance'] = fd
+        cap = its.image.stationary_lens_cap(cam, req, fmt)
+        data = {'fd': fds_fb[i]}
+        data['loc'] = cap['metadata']['android.lens.focusDistance']
+        print ' focus distance (diopters): %.3f' % data['fd']
+        print ' current lens location (diopters): %.3f' % data['loc']
+        y, _, _ = its.image.convert_capture_to_planes(cap, props)
+        chart = its.image.normalize_img(its.image.get_image_patch(y,
+                                                                  xnorm, ynorm,
+                                                                  wnorm, hnorm))
+        its.image.write_image(chart, '%s_stat_i=%d_chart.jpg' % (NAME, i))
+        data['sharpness'] = white_level*its.image.compute_image_sharpness(chart)
+        print 'Chart sharpness: %.1f\n' % data['sharpness']
+        data_static[i] = data
+    # take moving data set
+    reqs = []
+    for i, fd in enumerate(fds_f):
+        reqs.append(its.objects.manual_capture_request(sensitivity, exp))
+        reqs[i]['android.lens.focusDistance'] = fd
+    cap = cam.do_capture(reqs, fmt)
+    for i, _ in enumerate(reqs):
+        data = {'fd': fds_f[i]}
+        data['loc'] = cap[i]['metadata']['android.lens.focusDistance']
+        data['lens_moving'] = (cap[i]['metadata']['android.lens.state']
+                               == 1)
+        timestamp = cap[i]['metadata']['android.sensor.timestamp'] * 1E-6
+        if i == 0:
+            timestamp_init = timestamp
+        timestamp -= timestamp_init
+        data['timestamp'] = timestamp
+        print ' focus distance (diopters): %.3f' % data['fd']
+        print ' current lens location (diopters): %.3f' % data['loc']
+        y, _, _ = its.image.convert_capture_to_planes(cap[i], props)
+        chart = its.image.normalize_img(its.image.get_image_patch(y,
+                                                                  xnorm, ynorm,
+                                                                  wnorm, hnorm))
+        its.image.write_image(chart, '%s_move_i=%d_chart.jpg' % (NAME, i))
+        data['sharpness'] = white_level*its.image.compute_image_sharpness(chart)
+        print 'Chart sharpness: %.1f\n' % data['sharpness']
+        data_moving[i] = data
+    return data_static, data_moving
+
+
+def main():
+    """Test if focus position is properly reported for moving lenses."""
+
+    print '\nStarting test_lens_position.py'
+    with its.device.ItsSession() as cam:
+        props = cam.get_camera_properties()
+        its.caps.skip_unless(not its.caps.fixed_focus(props))
+        its.caps.skip_unless(its.caps.lens_calibrated(props))
+        fmt = {'format': 'yuv', 'width': VGA_WIDTH, 'height': VGA_HEIGHT}
+
+        # Get proper sensitivity, exposure time, and focus distance with 3A.
+        s, e, _, _, fd = cam.do_3a(get_results=True)
+
+        # Get sharpness for each focal distance
+        d_stat, d_move = test_lens_position(cam, props, fmt, s, e, fd)
+        print 'Lens stationary'
+        for k in sorted(d_stat):
+            print ('i: %d\tfd: %.3f\tlens location (diopters): %.3f \t'
+                   'sharpness: %.1f' % (k, d_stat[k]['fd'],
+                                        d_stat[k]['loc'],
+                                        d_stat[k]['sharpness']))
+        print 'Lens moving'
+        for k in sorted(d_move):
+            print ('i: %d\tfd: %.3f\tlens location (diopters): %.3f \t'
+                   'sharpness: %.1f  \tlens_moving: %r \t'
+                   'timestamp: %.1fms' % (k, d_move[k]['fd'],
+                                          d_move[k]['loc'],
+                                          d_move[k]['sharpness'],
+                                          d_move[k]['lens_moving'],
+                                          d_move[k]['timestamp']))
+
+        # assert static reported location/sharpness is close
+        print 'Asserting static lens locations/sharpness are similar'
+        for i in range(len(d_stat)/2):
+            j = 2 * NUM_STEPS - 1 - i
+            print (' lens position: %.3f'
+                   % d_stat[i]['fd'])
+            assert np.isclose(d_stat[i]['loc'], d_stat[i]['fd'],
+                              rtol=POSITION_TOL/100.0)
+            assert np.isclose(d_stat[i]['loc'], d_stat[j]['loc'],
+                              rtol=POSITION_TOL/100.0)
+            assert np.isclose(d_stat[i]['sharpness'], d_stat[j]['sharpness'],
+                              rtol=SHARPNESS_TOL/100.0)
+        # assert moving frames approximately consecutive with even distribution
+        print 'Asserting moving frames are consecutive'
+        times = [v['timestamp'] for v in d_move.itervalues()]
+        diffs = np.gradient(times)
+        assert np.isclose(np.amin(diffs), np.amax(diffs), atol=FRAME_TIME_TOL)
+        # assert reported location/sharpness is correct in moving frames
+        print 'Asserting moving lens locations/sharpness are similar'
+        for i in range(len(d_move)):
+            print ' lens position: %.3f' % d_stat[i]['fd']
+            assert np.isclose(d_stat[i]['loc'], d_move[i]['loc'],
+                              rtol=POSITION_TOL)
+            if d_move[i]['lens_moving'] and i > 0:
+                if d_stat[i]['sharpness'] > d_stat[i-1]['sharpness']:
+                    assert (d_stat[i]['sharpness']*(1.0+SHARPNESS_TOL) >
+                            d_move[i]['sharpness'] >
+                            d_stat[i-1]['sharpness']*(1.0-SHARPNESS_TOL))
+                else:
+                    assert (d_stat[i-1]['sharpness']*(1.0+SHARPNESS_TOL) >
+                            d_move[i]['sharpness'] >
+                            d_stat[i]['sharpness']*(1.0-SHARPNESS_TOL))
+            elif not d_move[i]['lens_moving']:
+                assert np.isclose(d_stat[i]['sharpness'],
+                                  d_move[i]['sharpness'], rtol=SHARPNESS_TOL)
+            else:
+                raise its.error.Error('Lens is moving at frame 0!')
+
+if __name__ == '__main__':
+    main()
+