Exemplo n.º 1
0
    def test_schur_compliment(self):
        damping = 0.
        bundle = self.bundle
    
        r = bundle.residuals()   # do not adjust the first camera
        J = bundle.Jresiduals()[:,6:]  # do not adjust the first camera
        JTJ = dots(J.T, J)
        JTr = dots(J.T, r)
    
        n = 6*(len(bundle.cameras)-1)
        #print '\n'.join([''.join(row) for row in np.array([' ','x']).take(JTJ>1e-5)])
        #print np.round(JTr,2)
        #print n
    
        optimize.apply_lm_damping_inplace(JTJ, damping)
        Aslow, bslow = schur.get_schur_complement(JTJ, JTr, n)
    
        ba = BundleAdjuster(bundle)
        ba.prepare_schur_complement()
        ba.apply_damping(damping)
        A,b = ba.compute_schur_complement()

        nc = len(ba.optim_camera_ids)
        #print 'nc=%d'%nc
        A = A.transpose((0,2,1,3)).reshape((6*nc, 6*nc))
        b = b.flatten()

        self.assertArrayEqual(A, Aslow)
        self.assertArrayEqual(b, bslow)
Exemplo n.º 2
0
    def test_schur_compliment(self):
        damping = 0.
        bundle = self.bundle

        r = bundle.residuals()  # do not adjust the first camera
        J = bundle.Jresiduals()[:, 6:]  # do not adjust the first camera
        JTJ = dots(J.T, J)
        JTr = dots(J.T, r)

        n = 6 * (len(bundle.cameras) - 1)
        #print '\n'.join([''.join(row) for row in np.array([' ','x']).take(JTJ>1e-5)])
        #print np.round(JTr,2)
        #print n

        optimize.apply_lm_damping_inplace(JTJ, damping)
        Aslow, bslow = schur.get_schur_complement(JTJ, JTr, n)

        ba = BundleAdjuster(bundle)
        ba.prepare_schur_complement()
        ba.apply_damping(damping)
        A, b = ba.compute_schur_complement()

        nc = len(ba.optim_camera_ids)
        #print 'nc=%d'%nc
        A = A.transpose((0, 2, 1, 3)).reshape((6 * nc, 6 * nc))
        b = b.flatten()

        self.assertArrayEqual(A, Aslow)
        self.assertArrayEqual(b, bslow)
Exemplo n.º 3
0
    def test_subset_schur(self):
        bundle = self.bundle
        damping = 2.

        camera_ids = [3, 1]
        track_ids = [0, 1, 2]

        cam_mask = [False, True]
        track_mask = [False, True, False]

        cameras_to_optimize = list(np.array(camera_ids)[np.array(cam_mask)])
        tracks_to_optimize = list(np.array(track_ids)[np.array(track_mask)])

        # Compute normal equations
        ba = BundleAdjuster()
        ba.set_bundle(bundle, camera_ids, track_ids, cam_mask, track_mask)
        ba.prepare_schur_complement()
        ba.apply_damping(damping)
        A, b = ba.compute_schur_complement()

        # Flatten the arrays
        nc = np.sum(cam_mask)
        self.assertShape(A, (nc, nc, 6, 6))
        self.assertShape(b, (nc, 6))
        Aflat = A.transpose((0, 2, 1, 3)).reshape((nc * 6, nc * 6))
        bflat = b.flatten()

        # Compute terms explicitly for reference

        # Note that for equivalence with BundleAdjuster, we must
        # compute residuals with *all* selected cameras and tracks
        # (i.e. ignoring cam_mask and track_mask), then compute the
        # Schur compliment, then eliminate rows and columns from the
        # normal equations *afterwards*
        r = bundle.residuals_partial(camera_ids, track_ids)
        J = bundle.Jresiduals_partial(camera_ids, track_ids)
        JTJ = dots(J.T, J)
        JTr = dots(J.T, r)
        optimize.apply_lm_damping_inplace(JTJ, damping)
        Aslow, bslow = schur.get_schur_complement(JTJ, JTr,
                                                  len(camera_ids) * 6)

        # Apply masking now
        cam_param_mask = np.array(
            [cam_mask[i / 6] for i in range(len(camera_ids) * 6)])
        track_param_mask = np.array(
            [track_mask[i / 3] for i in range(len(track_ids) * 3)])
        Aslow = Aslow[cam_param_mask].T[cam_param_mask].T
        bslow = bslow[cam_param_mask]

        # Check the results
        self.assertArrayEqual(Aflat, Aslow)
        self.assertArrayEqual(bflat, bslow)
Exemplo n.º 4
0
    def test_subset_schur(self):
        bundle = self.bundle
        damping = 2.
    
        camera_ids = [ 3,1 ]
        track_ids =  [ 0,1,2 ]

        cam_mask   = [ False, True ]
        track_mask = [ False, True, False ]

        cameras_to_optimize = list(np.array(camera_ids)[np.array(cam_mask)])
        tracks_to_optimize = list(np.array(track_ids)[np.array(track_mask)])

        # Compute normal equations
        ba = BundleAdjuster()
        ba.set_bundle(bundle, camera_ids, track_ids, cam_mask, track_mask)
        ba.prepare_schur_complement()
        ba.apply_damping(damping)
        A,b = ba.compute_schur_complement()

        # Flatten the arrays
        nc = np.sum(cam_mask)
        self.assertShape(A, (nc,nc,6,6))
        self.assertShape(b, (nc,6))
        Aflat = A.transpose((0,2,1,3)).reshape((nc*6, nc*6))
        bflat = b.flatten()

        # Compute terms explicitly for reference
    
        # Note that for equivalence with BundleAdjuster, we must
        # compute residuals with *all* selected cameras and tracks
        # (i.e. ignoring cam_mask and track_mask), then compute the
        # Schur compliment, then eliminate rows and columns from the
        # normal equations *afterwards*
        r = bundle.residuals_partial(camera_ids, track_ids)
        J = bundle.Jresiduals_partial(camera_ids, track_ids)
        JTJ = dots(J.T, J)
        JTr = dots(J.T, r)
        optimize.apply_lm_damping_inplace(JTJ, damping)
        Aslow, bslow = schur.get_schur_complement(JTJ, JTr, len(camera_ids)*6)

        # Apply masking now
        cam_param_mask = np.array([ cam_mask[i/6] for i in range(len(camera_ids)*6) ])
        track_param_mask = np.array([ track_mask[i/3] for i in range(len(track_ids)*3) ])
        Aslow = Aslow[cam_param_mask].T[cam_param_mask].T
        bslow = bslow[cam_param_mask]

        # Check the results
        self.assertArrayEqual(Aflat, Aslow)
        self.assertArrayEqual(bflat, bslow)