def model_matrix(q, order): r'''Returns the model matrix S for particular domain points Here the "order" is the number of parameters in the fit. Thus order==2 means "linear" and order==3 means "quadratic"" ''' q = nps.atleast_dims(q,-1) return nps.transpose(nps.cat(*[q ** i for i in range(order)]))
def triangulate_grad( intrinsics_data, rt_cam_ref, rt_ref_frame, rt_ref_frame_true, q, lensmodel, stabilize_coords = True): return _triangulate( intrinsics_data, rt_cam_ref, rt_ref_frame, rt_ref_frame_true, nps.atleast_dims(q,-3), lensmodel, stabilize_coords = stabilize_coords, get_gradients = True)
[ 0.03782446, -0.016981 , 0.03949906, -0.03256744, 0.02496247, 0.02924358]]) extrinsics_rt_fromref_noisy = ref_extrinsics_rt_fromref * ( 1.0 + extrinsics_rt_fromref_noise) return extrinsics_rt_fromref_noisy, points_noisy, observations ############### Do everything in the ref coord system, with a few fixed-position ############### points to set the coords extrinsics_rt_fromref, points, observations = make_noisy_inputs() # De-noise the fixed points. We know where they are exactly. And correctly Npoints_fixed = 3 points[-Npoints_fixed:, ...] = ref_p[-Npoints_fixed:, ...] stats = mrcal.optimize(nps.atleast_dims(intrinsics_data, -2), extrinsics_rt_fromref, None, points, None, None, observations, indices_point_camintrinsics_camextrinsics, lensmodel, imagersizes=nps.atleast_dims(imagersize, -2), Npoints_fixed=Npoints_fixed, point_min_range=1.0, point_max_range=1000.0, do_optimize_intrinsics_core=False, do_optimize_intrinsics_distortions=False, do_optimize_extrinsics=True,
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)
def quat_from_R(R, out=None): r"""Convert a rotation defined as a rotation matrix to a unit quaternion SYNOPSIS print(R.shape) ===> (3,3) quat = mrcal.quat_from_R(R) print(quat.shape) ===> (4,) c = quat[0] s = nps.mag(quat[1:]) rotation_magnitude = 2. * np.arctan2(s,c) rotation_axis = quat[1:] / s This is mostly for compatibility with some old stuff. mrcal doesn't use quaternions anywhere. Test this thoroughly before using. This function supports broadcasting fully. ARGUMENTS - R: array of shape (3,3,). The rotation matrix that defines the rotation. - out: optional argument specifying the destination. By default, new numpy array(s) are created and returned. To write the results into existing (and possibly non-contiguous) arrays, specify them with the 'out' kwarg. If 'out' is given, we return the 'out' that was passed in. This is the standard behavior provided by numpysane_pywrap. RETURNED VALUE We return an array of unit quaternions. Each broadcasted slice has shape (4,). The values in the array are (u,i,j,k) LICENSE AND COPYRIGHT The implementation comes directly from the scipy project, the from_dcm() function in https://github.com/scipy/scipy/blob/master/scipy/spatial/transform/rotation.py Commit: 1169d27ad47a29abafa8a3d2cb5b67ff0df80a8f License: Copyright (c) 2001-2002 Enthought, Inc. 2003-2019, SciPy Developers. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. """ # extra broadcasted shape extra_dims = R.shape[:-2] # R.shape = (..., 3,3) with some non-empty ... R = nps.atleast_dims(R, -3) # R.shape = (N,3,3) R = nps.clump(R, n=R.ndim - 2) num_rotations = R.shape[0] decision_matrix = np.empty((num_rotations, 4)) decision_matrix[:, :3] = R.diagonal(axis1=1, axis2=2) decision_matrix[:, -1] = decision_matrix[:, :3].sum(axis=1) choices = decision_matrix.argmax(axis=1) s = (num_rotations, 4) if out is not None: quat = out.reshape(s) def base(x): r'''Base function, that returns the self array if it's not a view''' return x if x.base is None else x.base if base(quat) is not base(out): raise Exception( "quat_from_R() in-place output isn't yet complete. Please fix") else: quat = np.empty(s) ind = np.nonzero(choices != 3)[0] i = choices[ind] j = (i + 1) % 3 k = (j + 1) % 3 quat[ind, i + 1] = 1 - decision_matrix[ind, -1] + 2 * R[ind, i, i] quat[ind, j + 1] = R[ind, j, i] + R[ind, i, j] quat[ind, k + 1] = R[ind, k, i] + R[ind, i, k] quat[ind, 0] = R[ind, k, j] - R[ind, j, k] ind = np.nonzero(choices == 3)[0] quat[ind, 1] = R[ind, 2, 1] - R[ind, 1, 2] quat[ind, 2] = R[ind, 0, 2] - R[ind, 2, 0] quat[ind, 3] = R[ind, 1, 0] - R[ind, 0, 1] quat[ind, 0] = 1 + decision_matrix[ind, -1] quat /= np.linalg.norm(quat, axis=1)[:, None] return quat.reshape(extra_dims + (4, ))
itest = 0 for kwargs in all_test_kwargs: observations_copy = observations.copy() if 'outlier_indices' in kwargs: # mark the requested outliers, and delete the old way of specifying # these for i in kwargs['outlier_indices']: nps.clump(observations_copy, n=3)[i, 2] = -1. del kwargs['outlier_indices'] optimization_inputs = \ dict( intrinsics = intrinsics_data, extrinsics_rt_fromref = nps.atleast_dims(extrinsics_rt_fromref, -2), frames_rt_toref = frames_rt_toref, points = points, observations_board = observations_copy, indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics, observations_point = observations_point, indices_point_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics, lensmodel = lensmodel, calobject_warp = np.array((1e-3, 2e-3)), imagersizes = imagersizes, calibration_object_spacing = 0.1, point_min_range = 1.0, point_max_range = 1000.0, verbose = False, **kwargs )
def pinhole_model_for_reprojection(model_from, fit=None, scale_focal=None, scale_image=None): r'''Generate a pinhole model suitable for reprojecting an image SYNOPSIS model_orig = mrcal.cameramodel("xxx.cameramodel") image_orig = cv2.imread("image.jpg") model_pinhole = mrcal.pinhole_model_for_reprojection(model_orig, fit = "corners") mapxy = mrcal.image_transformation_map(model_orig, model_pinhole, intrinsics_only = True) image_undistorted = mrcal.transform_image(image_orig, mapxy) Many algorithms work with images assumed to have been captured with a pinhole camera, even though real-world lenses never fit a pinhole model. mrcal provides several functions to remap images captured with non-pinhole lenses into images of the same scene as if they were observed by a pinhole lens. When doing this, we're free to choose all of the parameters of this pinhole lens model. THIS function produces the pinhole camera model based on some guidance in the arguments, and this model can then be used to "undistort" images. ARGUMENTS - model_from: the mrcal.cameramodel object used to build the pinhole model. We use the intrinsics as the baseline, and we copy the extrinsics to the resulting pinhole model. - fit: optional specification for focal-length scaling. By default we use the focal length values from the input model. This is either a numpy array of shape (...,2) containing pixel coordinates that the resulting pinhole model must represent, or one of ("corners","centers-horizontal","centers-vertical"). See the docstring for scale_focal__best_pinhole_fit() for details. Exclusive with 'scale_focal' - scale_focal: optional specification for focal-length scaling. By default we use the focal length values from the input model. If given, we scale the input focal lengths by the given value. Exclusive with 'fit' - scale_image: optional specification for the scaling of the image size. By default the output model represents an image of the same resolution as the input model. If we want something else, the scaling can be given here. RETURNED VALUE A mrcal.cameramodel object with lensmodel = LENSMODEL_PINHOLE corresponding to the input model. ''' if scale_focal is None: if fit is not None: if isinstance(fit, np.ndarray): if fit.shape[-1] != 2: raise Exception( "'fit' is an array, so it must have shape (...,2)") fit = nps.atleast_dims(fit, -2) fit = nps.clump(fit, n=len(fit.shape) - 1) # fit now has shape (N,2) elif re.match("^(corners|centers-horizontal|centers-vertical)$", fit): # this is valid. nothing to do pass else: raise Exception( "'fit' must be an array of shape (...,2) or one of ('corners','centers-horizontal','centers-vertical')", file=sys.stderr) sys.exit(1) scale_focal = mrcal.scale_focal__best_pinhole_fit(model_from, fit) else: if fit is not None: raise Exception( "At most one of 'scale_focal' and 'fit' may be non-None") # I have some scale_focal now. I apply it lensmodel, intrinsics_data = model_from.intrinsics() imagersize = model_from.imagersize() if not mrcal.lensmodel_metadata_and_config(lensmodel)['has_core']: raise Exception( "This currently works only with models that have an fxfycxcy core") cx, cy = intrinsics_data[2:4] intrinsics_data[:2] *= scale_focal if scale_image is not None: # Now I apply the imagersize scale. The center of the imager should # unproject to the same point: # # (q0 - cxy0)/fxy0 = v = (q1 - cxy1)/fxy1 # ((WH-1)/2 - cxy) / fxy = (((ki*WH)-1)/2 - kc*cxy) / (kf*fxy) # # The focal lengths scale directly: kf = ki # ((WH-1)/2 - cxy) / fxy = (((ki*WH)-1)/2 - kc*cxy) / (ki*fxy) # (WH-1)/2 - cxy = (((ki*WH)-1)/2 - kc*cxy) / ki # (WH-1)/2 - cxy = (WH-1/ki)/2 - kc/ki*cxy # -1/2 - cxy = (-1/ki)/2 - kc/ki*cxy # 1/2 + cxy = 1/(2ki) + kc/ki*cxy # -> kc = (1/2 + cxy - 1/(2ki)) * ki / cxy # = (ki + 2*cxy*ki - 1) / (2 cxy) # # Sanity check: cxy >> 1: ki+2*cxy*ki = ki*(1+2cxy) ~ 2*cxy*ki # 2*cxy*ki - 1 ~ 2*cxy*ki # -> kc ~ 2*cxy*ki /( 2 cxy ) = ki. Yes. # Looks like I scale cx and cy separately. imagersize[0] = round(imagersize[0] * scale_image) imagersize[1] = round(imagersize[1] * scale_image) kfxy = scale_image kcx = (kfxy + 2. * cx * kfxy - 1.) / (2. * cx) kcy = (kfxy + 2. * cy * kfxy - 1.) / (2. * cy) intrinsics_data[:2] *= kfxy intrinsics_data[2] *= kcx intrinsics_data[3] *= kcy return \ mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',intrinsics_data[:4]), extrinsics_rt_fromref = model_from.extrinsics_rt_fromref(), imagersize = imagersize )