Exemple #1
0
confirm_equal(y, nps.matmult(x, R0_ref), msg='rotate_point_R(inverted) result')
confirm_equal(J_R, J_R_ref, msg='rotate_point_R(inverted) J_R')
confirm_equal(J_x, J_x_ref, msg='rotate_point_R(inverted) J_x')

# inverted in-place
R0_ref_copy = np.array(R0_ref)
x_copy = np.array(x)
y = \
    mrcal.rotate_point_R(R0_ref_copy, x_copy, out = x_copy, inverted = True)
confirm_equal(y,
              nps.matmult(x, R0_ref),
              msg='rotate_point_R result written in-place into x')

################# rotate_point_r

y = mrcal.rotate_point_r(r0_ref, x, out=out3)
confirm_equal(y,
              nps.matmult(x, nps.transpose(R_from_r(r0_ref))),
              msg='rotate_point_r result')

y, J_r, J_x = mrcal.rotate_point_r(r0_ref,
                                   x,
                                   get_gradients=True,
                                   out=(out3, out33, out33a))
J_r_ref = grad(lambda r: nps.matmult(x, nps.transpose(R_from_r(r))), r0_ref)
J_x_ref = grad(lambda x: nps.matmult(x, nps.transpose(R_from_r(r0_ref))), x)
confirm_equal(y,
              nps.matmult(x, nps.transpose(R_from_r(r0_ref))),
              msg='rotate_point_r result')
confirm_equal(J_r, J_r_ref, msg='rotate_point_r J_r')
confirm_equal(J_x, J_x_ref, msg='rotate_point_r J_x')
Exemple #2
0
    el0_deg = 10.0
    models_rectified = \
        mrcal.rectified_system( (model0, model1),
                                az_fov_deg = az_fov_deg,
                                el_fov_deg = el_fov_deg,
                                el0_deg    = el0_deg,
                                pixels_per_deg_az = -1./8.,
                                pixels_per_deg_el = -1./4.,
                                rectification_model = lensmodel)
    el0 = el0_deg*np.pi/180.
    # az0 is the "forward" direction of the two cameras, relative to the
    # baseline vector
    baseline = rt01[3:] / nps.mag(rt01[3:])
    # "forward" for each of the two cameras, in the cam0 coord system
    forward0 = np.array((0,0,1.))
    forward1 = mrcal.rotate_point_r(rt01[:3], np.array((0,0,1.)))
    forward01 = forward0 + forward1
    az0 = np.arcsin( nps.inner(forward01,baseline) / nps.mag(forward01) )

    try:
        mrcal.stereo._validate_models_rectified(models_rectified)
        testutils.confirm(True,
                          msg=f'Generated models pass validation ({lensmodel})')
    except:
        testutils.confirm(False,
                          msg=f'Generated models pass validation ({lensmodel})')

    Rt_cam0_rect = mrcal.compose_Rt( model0.extrinsics_Rt_fromref(),
                                     models_rectified[0].extrinsics_Rt_toref())
    Rt01_rectified = mrcal.compose_Rt( models_rectified[0].extrinsics_Rt_fromref(),
                                       models_rectified[1].extrinsics_Rt_toref())
def triangulate_nograd(intrinsics_data0,
                       intrinsics_data1,
                       rt_cam0_ref,
                       rt_cam0_ref_baseline,
                       rt_cam1_ref,
                       rt_ref_frame,
                       rt_ref_frame_baseline,
                       q,
                       lensmodel,
                       stabilize_coords=True):

    q = nps.atleast_dims(q, -3)

    rt01 = mrcal.compose_rt(rt_cam0_ref, mrcal.invert_rt(rt_cam1_ref))

    # all the v have shape (...,3)
    vlocal0 = \
        mrcal.unproject(q[...,0,:],
                        lensmodel, intrinsics_data0)
    vlocal1 = \
        mrcal.unproject(q[...,1,:],
                        lensmodel, intrinsics_data1)

    v0 = vlocal0
    v1 = \
        mrcal.rotate_point_r(rt01[:3], vlocal1)

    # The triangulated point in the perturbed camera-0 coordinate system.
    # Calibration-time perturbations move this coordinate system, so to get
    # a better estimate of the triangulation uncertainty, we try to
    # transform this to the original camera-0 coordinate system; the
    # stabilization path below does that.
    #
    # shape (..., 3)
    p_triangulated0 = \
        mrcal.triangulate_leecivera_mid2(v0, v1, rt01[3:])

    if not stabilize_coords:
        return p_triangulated0

    # Stabilization path. This uses the "true" solution, so I cannot do
    # this in the field. But I CAN do this in the randomized trials in
    # the test. And I can use the gradients to propagate the uncertainty
    # of this computation in the field
    #
    # Data flow:
    #   point_cam_perturbed -> point_ref_perturbed -> point_frames
    #   point_frames -> point_ref_baseline -> point_cam_baseline

    p_cam0_perturbed = p_triangulated0

    p_ref_perturbed = mrcal.transform_point_rt(rt_cam0_ref,
                                               p_cam0_perturbed,
                                               inverted=True)

    # shape (..., Nframes, 3)
    p_frames = \
        mrcal.transform_point_rt(rt_ref_frame,
                                 nps.dummy(p_ref_perturbed,-2),
                                 inverted = True)

    # shape (..., Nframes, 3)
    p_ref_baseline_all = mrcal.transform_point_rt(rt_ref_frame_baseline,
                                                  p_frames)

    # shape (..., 3)
    p_ref_baseline = np.mean(p_ref_baseline_all, axis=-2)

    # shape (..., 3)
    return mrcal.transform_point_rt(rt_cam0_ref_baseline, p_ref_baseline)
Exemple #4
0
def _triangulate(# shape (Ncameras, Nintrinsics)
                 intrinsics_data,
                 # shape (Ncameras, 6)
                 rt_cam_ref,
                 # shape (Nframes,6),
                 rt_ref_frame, rt_ref_frame_true,
                 # shape (..., Ncameras, 2)
                 q,

                 lensmodel,
                 stabilize_coords,
                 get_gradients):

    if not ( intrinsics_data.ndim == 2 and intrinsics_data.shape[0] == 2 and \
             rt_cam_ref.shape == (2,6) and \
             rt_ref_frame.ndim == 2 and rt_ref_frame.shape[-1] == 6 and \
             q.shape[-2:] == (2,2 ) ):
        raise Exception("Arguments must have a consistent Ncameras == 2")

    # I now compute the same triangulation, but just at the un-perturbed baseline,
    # and keeping track of all the gradients
    rt0r = rt_cam_ref[0]
    rt1r = rt_cam_ref[1]

    if not get_gradients:

        rtr1          = mrcal.invert_rt(rt1r)
        rt01_baseline = mrcal.compose_rt(rt0r, rtr1)

        # all the v have shape (...,3)
        vlocal0 = \
            mrcal.unproject(q[...,0,:],
                            lensmodel, intrinsics_data[0])
        vlocal1 = \
            mrcal.unproject(q[...,1,:],
                            lensmodel, intrinsics_data[1])

        v0 = vlocal0
        v1 = \
            mrcal.rotate_point_r(rt01_baseline[:3], vlocal1)

        # p_triangulated has shape (..., 3)
        p_triangulated = \
            mrcal.triangulate_leecivera_mid2(v0, v1, rt01_baseline[3:])

        if stabilize_coords:

            # shape (..., Nframes, 3)
            p_frames_new = \
                mrcal.transform_point_rt(mrcal.invert_rt(rt_ref_frame),
                                         nps.dummy(p_triangulated,-2))

            # shape (..., Nframes, 3)
            p_refs = mrcal.transform_point_rt(rt_ref_frame_true, p_frames_new)

            # shape (..., 3)
            p_triangulated = np.mean(p_refs, axis=-2)

        return p_triangulated
    else:
        rtr1,drtr1_drt1r = mrcal.invert_rt(rt1r,
                                           get_gradients=True)
        rt01_baseline,drt01_drt0r, drt01_drtr1 = mrcal.compose_rt(rt0r, rtr1, get_gradients=True)

        # all the v have shape (...,3)
        vlocal0, dvlocal0_dq0, dvlocal0_dintrinsics0 = \
            mrcal.unproject(q[...,0,:],
                            lensmodel, intrinsics_data[0],
                            get_gradients = True)
        vlocal1, dvlocal1_dq1, dvlocal1_dintrinsics1 = \
            mrcal.unproject(q[...,1,:],
                            lensmodel, intrinsics_data[1],
                            get_gradients = True)

        v0 = vlocal0
        v1, dv1_dr01, dv1_dvlocal1 = \
            mrcal.rotate_point_r(rt01_baseline[:3], vlocal1,
                                 get_gradients=True)

        # p_triangulated has shape (..., 3)
        p_triangulated, dp_triangulated_dv0, dp_triangulated_dv1, dp_triangulated_dt01 = \
            mrcal.triangulate_leecivera_mid2(v0, v1, rt01_baseline[3:],
                                             get_gradients = True)

        shape_leading = dp_triangulated_dv0.shape[:-2]

        dp_triangulated_dq = np.zeros(shape_leading + (3,) + q.shape[-2:], dtype=float)
        nps.matmult( dp_triangulated_dv0,
                     dvlocal0_dq0,
                     out = dp_triangulated_dq[..., 0, :])
        nps.matmult( dp_triangulated_dv1,
                     dv1_dvlocal1,
                     dvlocal1_dq1,
                     out = dp_triangulated_dq[..., 1, :])

        Nframes = len(rt_ref_frame)

        if stabilize_coords:

            # shape (Nframes,6)
            rt_frame_ref, drtfr_drtrf = \
                mrcal.invert_rt(rt_ref_frame, get_gradients=True)

            # shape (Nframes,6)
            rt_true_shifted, _, drt_drtfr = \
                mrcal.compose_rt(rt_ref_frame_true, rt_frame_ref,
                                 get_gradients=True)

            # shape (..., Nframes, 3)
            p_refs,dprefs_drt,dprefs_dptriangulated = \
                mrcal.transform_point_rt(rt_true_shifted,
                                         nps.dummy(p_triangulated,-2),
                                         get_gradients = True)

            # shape (..., 3)
            p_triangulated = np.mean(p_refs, axis=-2)

            # I have dpold/dx. dpnew/dx = dpnew/dpold dpold/dx

            # shape (...,3,3)
            dpnew_dpold = np.mean(dprefs_dptriangulated, axis=-3)
            dp_triangulated_dv0  = nps.matmult(dpnew_dpold, dp_triangulated_dv0)
            dp_triangulated_dv1  = nps.matmult(dpnew_dpold, dp_triangulated_dv1)
            dp_triangulated_dt01 = nps.matmult(dpnew_dpold, dp_triangulated_dt01)
            dp_triangulated_dq   = nps.xchg(nps.matmult( dpnew_dpold,
                                                         nps.xchg(dp_triangulated_dq,
                                                                  -2,-3)),
                                            -2,-3)

            # shape (..., Nframes,3,6)
            dp_triangulated_drtrf = \
                nps.matmult(dprefs_drt, drt_drtfr, drtfr_drtrf) / Nframes
        else:
            dp_triangulated_drtrf = np.zeros(shape_leading + (Nframes,3,6), dtype=float)

        return \
            p_triangulated, \
            drtr1_drt1r, \
            drt01_drt0r, drt01_drtr1, \
            dvlocal0_dintrinsics0, dvlocal1_dintrinsics1, \
            dv1_dr01, dv1_dvlocal1, \
            dp_triangulated_dv0, dp_triangulated_dv1, dp_triangulated_dt01, \
            dp_triangulated_drtrf, \
            dp_triangulated_dq