コード例 #1
0
ファイル: filter.py プロジェクト: zzzzzjh/lwoi
    def h_imu(self, u):
        """
		Transforms the imu measurement (gyro, acc) in pre-integrated measurement
		:param u: imu measurements, shape [k, 6]
		:return: pre-integrated measurement
		"""
        delta_R_prev = torch.eye(3)
        delta_v_prev = torch.zeros(3)
        delta_p_prev = torch.zeros(3)
        self.J = torch.zeros(u.shape[0], 9, 8)
        for k in range(u.shape[0]):
            self.J[k, :3, :3] = delta_R_prev * self.delta_t
            self.J[
                k,
                3:6, :3] = -delta_R_prev.mm(self.skew(u[k, 3:])) * self.delta_t
            self.J[k, 3:6, 3:6] = delta_R_prev * self.delta_t
            self.J[k, 3:6, :3] = -1 / 2 * delta_R_prev.mm(self.skew(
                u[k, 3:])) * (self.delta_t**2)
            self.J[k, 6:9, 3:6] = 1 / 2 * delta_R_prev * (self.delta_t**2)
            delta_R = delta_R_prev.mm(
                SO3.exp(u[k, :3] * self.delta_t).as_matrix())
            delta_v = delta_v_prev + delta_R.mv(u[k, 3:] * self.delta_t)
            delta_p = delta_p_prev + delta_v * self.delta_t + delta_R.mv(
                u[k, 3:] * self.delta_t) * (self.delta_t**2) / 2
            delta_R_prev = SO3.from_matrix(delta_R, normalize=True).as_matrix()
            delta_v_prev = delta_v
            delta_p_prev = delta_p

        return torch.cat((SO3.from_matrix(delta_R).log(), delta_v, delta_p), 0)
コード例 #2
0
ファイル: filter.py プロジェクト: zzzzzjh/lwoi
    def h_hat(self, u_odo):
        def odo2speed(u):
            v = 1 / 2 * (u[0] + u[1])
            return torch.Tensor([
                v * torch.cos(self.x_prev[5]), v * torch.sin(self.x_prev[5]), 0
            ])

        # initial speed
        v0 = odo2speed(u_odo[0])

        # end speed
        v_end = odo2speed(u_odo[1])

        R0 = SO3.from_rpy(self.x_prev[3:6]).as_matrix()
        Rend = SO3.from_rpy(self.x[3:6]).as_matrix()

        p0 = self.x_prev[:3]
        p_end = self.x[:3]

        delta_R = SO3.from_matrix(R0.t().mm(Rend)).log()
        delta_v = R0.t().mv(v_end - v0 - self.g * self.Delta_t)
        delta_p = R0.t().mv(p_end - p0 - v0 * self.Delta_t - 1 / 2 * self.g *
                            (self.Delta_t**2))

        return torch.cat((delta_R, delta_v, delta_p), 0)
コード例 #3
0
ファイル: trainer.py プロジェクト: tym2103/DeepLIO
    def se3_to_SE3(self, f2f_x, f2f_r):
        batch_size, seq_size, _ = f2f_x.shape

        f2g_q = torch.zeros((batch_size, seq_size, 4), dtype=f2f_x.dtype, device=f2f_x.device)
        f2g_x = torch.zeros((batch_size, seq_size, 3), dtype=f2f_x.dtype, device=f2f_x.device)

        for b in range(batch_size):
            R_prev = torch.zeros((3, 3), dtype=f2f_x.dtype, device=f2f_x.device)
            R_prev[:] = torch.eye(3, dtype=f2f_x.dtype, device=f2f_x.device)
            t_prev = torch.zeros((3), dtype=f2f_x.dtype, device=f2f_x.device)

            for s in range(0, seq_size):
                t_cur = f2f_x[b, s]
                #q_cur = spatial.euler_to_rotation_matrix (f2f_r[b, s])
                w_cur = f2f_r[b, s]
                R_cur = SO3.exp(w_cur).as_matrix() # spatial.quaternion_to_rotation_matrix(q_cur)

                if not torch.isclose(torch.det(R_cur), torch.FloatTensor([1.]).to(self.device)).all():
                    raise ValueError("Det error:\nR\n{}\nq:\n{}".format(R_cur, w_cur))

                t_prev = torch.matmul(R_prev, t_cur) + t_prev
                R_prev = torch.matmul(R_prev, R_cur)

                if not torch.isclose(torch.det(R_prev), torch.FloatTensor([1.]).to(self.device)).all():
                    raise ValueError("Det error:\nR\n{}".format(R_prev))
                f2g_q[b, s] = SO3.from_matrix(R_prev, normalize=True).to_quaternion()
                f2g_x[b, s] = t_prev
        return f2g_x, f2g_q
コード例 #4
0
def test_perturb():
    C = SO3.exp(0.25 * np.pi * torch.ones(3))
    C_copy = copy.deepcopy(C)
    phi = torch.Tensor([0.1, 0.2, 0.3])
    C.perturb(phi)
    assert utils.allclose(C.as_matrix(),
                          (SO3.exp(phi).dot(C_copy)).as_matrix())
コード例 #5
0
def test_left_jacobians_batch():
    phis = torch.Tensor([[0., 0., 0.], [np.pi / 2, np.pi / 3, np.pi / 4]])

    left_jacobian = SO3.left_jacobian(phis)
    inv_left_jacobian = SO3.inv_left_jacobian(phis)
    assert utils.allclose(torch.bmm(left_jacobian, inv_left_jacobian),
                          torch.eye(3).unsqueeze_(dim=0).expand(2, 3, 3))
コード例 #6
0
    def correct(self, x, u_odo, u_fog, compute_G=False, full_cov=False):
        u_odo_fog = torch.cat((u_odo, u_fog), 1).unsqueeze(0)
        u_odo_fog.requires_grad = True
        Xnew = self.normalize(u_odo_fog)

        # take mean to speed up correction
        y_cor_nor, _ = self.gp_f.forward(Xnew, full_cov)

        # # sample corrections and take mean
        # N = 100
        # mean, cov = self.gp_f.forward(Xnew, full_cov=True)
        # y_cor_nor = torch.zeros(6)
        # dist = torch.distributions.MultivariateNormal(loc=mean, cov)
        # for i in range(N):
        # 	y_cor_nor += 1/N * dist.sample()

        y_cor = self.unnormalize(y_cor_nor.t(), var="y_odo_fog").squeeze()
        G_cor = self.correct_cov(u_odo_fog, y_cor, compute_G)
        u_odo_fog.requires_grad = False
        y_cor = y_cor.detach()
        y_cor[[3, 4]] = 0  # pitch and roll corrections are set to 0
        G_cor[[3, 4], :] = 0
        Rot = SO3.from_rpy(x[3:6]).as_matrix()
        # correct state
        dRot_cor = SO3.exp(y_cor[3:]).as_matrix()
        x[:3] = x[:3] + Rot.mv(SE3.exp(y_cor).as_matrix()[:3, 3])
        x[3:6] = SO3.from_matrix(Rot.mm(dRot_cor)).to_rpy()
        return x, G_cor
コード例 #7
0
def test_identity_batch():
    C = SO3.identity(5)
    assert isinstance(C, SO3) \
        and C.mat.dim() == 3 \
        and C.mat.shape == (5, 3, 3)

    C_copy = SO3.identity(5, copy=True)
    assert isinstance(C_copy, SO3) \
        and C_copy.mat.dim() == 3 \
        and C_copy.mat.shape == (5, 3, 3)
コード例 #8
0
def test_chordal_squared_loss_equality():
    print('Equality of quaternion and rotation matrix chordal loss...')
    C1 = SO3.exp(torch.randn(1000, 3, dtype=torch.double)).as_matrix()
    C2 = SO3.exp(torch.randn(1000, 3, dtype=torch.double)).as_matrix()

    q1 = rotmat_to_quat(C1)
    q2 = rotmat_to_quat(C2)

    assert (allclose(rotmat_frob_squared_norm_loss(C1, C2),
                     quat_chordal_squared_loss(q1, q2)))
    print('All passed.')
コード例 #9
0
def test_normalize_batch():
    C = SO3.exp(torch.Tensor([[1, 2, 3], [4, 5, 6], [0, 0, 0]]))
    assert (SO3.is_valid_matrix(C.mat) == torch.ByteTensor([1, 1, 1])).all()

    C.mat.add_(0.1)
    assert (SO3.is_valid_matrix(C.mat) == torch.ByteTensor([0, 0, 0])).all()

    C.normalize(inds=[0, 2])
    assert (SO3.is_valid_matrix(C.mat) == torch.ByteTensor([1, 0, 1])).all()

    C.normalize()
    assert SO3.is_valid_matrix(C.mat).all()
コード例 #10
0
def test_from_matrix():
    C_good = SO3.from_matrix(torch.eye(3))
    assert isinstance(C_good, SO3) \
        and C_good.mat.dim() == 2 \
        and C_good.mat.shape == (3, 3) \
        and SO3.is_valid_matrix(C_good.mat).all()

    C_bad = SO3.from_matrix(torch.eye(3).add_(1e-3), normalize=True)
    assert isinstance(C_bad, SO3) \
        and C_bad.mat.dim() == 2 \
        and C_bad.mat.shape == (3, 3) \
        and SO3.is_valid_matrix(C_bad.mat).all()
コード例 #11
0
def test_left_jacobians():
    phi_small = torch.Tensor([0., 0., 0.])
    phi_big = torch.Tensor([np.pi / 2, np.pi / 3, np.pi / 4])

    left_jacobian_small = SO3.left_jacobian(phi_small)
    inv_left_jacobian_small = SO3.inv_left_jacobian(phi_small)
    assert utils.allclose(
        torch.mm(left_jacobian_small, inv_left_jacobian_small), torch.eye(3))

    left_jacobian_big = SO3.left_jacobian(phi_big)
    inv_left_jacobian_big = SO3.inv_left_jacobian(phi_big)
    assert utils.allclose(torch.mm(left_jacobian_big, inv_left_jacobian_big),
                          torch.eye(3))
コード例 #12
0
def test_dot_batch():
    C1 = SO3(torch.Tensor([[0, -1, 0], [1, 0, 0], [0, 0, 1]]).expand(5, 3, 3))
    C3 = SO3(torch.Tensor([[0, -1, 0], [1, 0, 0], [0, 0, 1]]))
    pt1 = torch.Tensor([1, 2, 3])
    pt3 = torch.Tensor([4, 5, 6])
    pt3 = torch.Tensor([7, 8, 9])
    pts = torch.cat(
        [pt1.unsqueeze(dim=0),
         pt3.unsqueeze(dim=0),
         pt3.unsqueeze(dim=0)],
        dim=0)  # 3x3
    ptsbatch = pts.unsqueeze(dim=0).expand(5, 3, 3)

    C1C1 = torch.bmm(C1.mat, C1.mat)
    C1C1_SO3 = C1.dot(C1).mat
    assert C1C1_SO3.shape == C1.mat.shape and utils.allclose(C1C1_SO3, C1C1)

    C1C3 = torch.matmul(C1.mat, C3.mat)
    C1C3_SO3 = C1.dot(C3).mat
    assert C1C3_SO3.shape == C1.mat.shape and utils.allclose(C1C3_SO3, C1C3)

    C1pt1 = torch.matmul(C1.mat, pt1)
    C1pt1_SO3 = C1.dot(pt1)
    assert C1pt1_SO3.shape == (C1.mat.shape[0], pt1.shape[0]) \
        and utils.allclose(C1pt1_SO3, C1pt1)

    C1pt3 = torch.matmul(C1.mat, pt3)
    C1pt3_SO3 = C1.dot(pt3)
    assert C1pt3_SO3.shape == (C1.mat.shape[0], pt3.shape[0]) \
        and utils.allclose(C1pt3_SO3, C1pt3)

    C1pts = torch.matmul(C1.mat, pts.transpose(1, 0)).transpose(2, 1)
    C1pts_SO3 = C1.dot(pts)
    assert C1pts_SO3.shape == (C1.mat.shape[0], pts.shape[0], pts.shape[1]) \
        and utils.allclose(C1pts_SO3, C1pts) \
        and utils.allclose(C1pt1, C1pts[:, 0, :]) \
        and utils.allclose(C1pt3, C1pts[:, 1, :])

    C1ptsbatch = torch.bmm(C1.mat, ptsbatch.transpose(2, 1)).transpose(2, 1)
    C1ptsbatch_SO3 = C1.dot(ptsbatch)
    assert C1ptsbatch_SO3.shape == ptsbatch.shape \
        and utils.allclose(C1ptsbatch_SO3, C1ptsbatch) \
        and utils.allclose(C1pt1, C1ptsbatch[:, 0, :]) \
        and utils.allclose(C1pt3, C1ptsbatch[:, 1, :])

    C3ptsbatch = torch.matmul(C3.mat, ptsbatch.transpose(2, 1)).transpose(2, 1)
    C3ptsbatch_SO3 = C3.dot(ptsbatch)
    assert C3ptsbatch_SO3.shape == ptsbatch.shape \
        and utils.allclose(C3ptsbatch_SO3, C3ptsbatch) \
        and utils.allclose(C3.dot(pt1), C3ptsbatch[:, 0, :]) \
        and utils.allclose(C3.dot(pt3), C3ptsbatch[:, 1, :])
コード例 #13
0
def test_perturb_batch():
    C = SO3.exp(torch.Tensor([[1, 2, 3], [4, 5, 6]]))
    C_copy1 = copy.deepcopy(C)
    C_copy2 = copy.deepcopy(C)

    phi = torch.Tensor([0.1, 0.2, 0.3])
    C_copy1.perturb(phi)
    assert utils.allclose(C_copy1.as_matrix(),
                          (SO3.exp(phi).dot(C)).as_matrix())

    phis = torch.Tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]])
    C_copy2.perturb(phis)
    assert utils.allclose(C_copy2.as_matrix(),
                          (SO3.exp(phis).dot(C)).as_matrix())
コード例 #14
0
def test_from_matrix_batch():
    C_good = SO3.from_matrix(torch.eye(3).repeat(5, 1, 1))
    assert isinstance(C_good, SO3) \
        and C_good.mat.dim() == 3 \
        and C_good.mat.shape == (5, 3, 3) \
        and SO3.is_valid_matrix(C_good.mat).all()

    C_bad = copy.deepcopy(C_good.mat)
    C_bad[3].add_(0.1)
    C_bad = SO3.from_matrix(C_bad, normalize=True)
    assert isinstance(C_bad, SO3) \
        and C_bad.mat.dim() == 3 \
        and C_bad.mat.shape == (5, 3, 3) \
        and SO3.is_valid_matrix(C_bad.mat).all()
コード例 #15
0
def test_rot_angles():
    print('Rotation angles...')
    C1 = SO3.exp(torch.randn(100, 3, dtype=torch.double))
    C2 = SO3.exp(torch.randn(100, 3, dtype=torch.double))

    angles_1 = (C1.dot(C2.inv())).log().norm(dim=1) * (180. / np.pi)
    angles_2 = quat_angle_diff(rotmat_to_quat(C1.as_matrix()),
                               rotmat_to_quat(C2.as_matrix()),
                               units='deg',
                               reduce=False)
    angles_3 = rotmat_angle_diff(C1.as_matrix(), C2.as_matrix(), reduce=False)
    assert (allclose(angles_1, angles_2))
    assert (allclose(angles_1, angles_3))
    print('All passed.')
コード例 #16
0
    def h_hat(self, u):
        delta_R_prev = torch.eye(3).repeat(u.shape[0], 1, 1)
        delta_v_prev = torch.zeros(3).repeat(u.shape[0], 1)
        delta_p_prev = torch.zeros(3).repeat(u.shape[0], 1)
        for k in range(u.shape[1]):
            delta_R = delta_R_prev.matmul(
                SO3.exp(u[:, k, :3] * self.delta_t).as_matrix())
            delta_v = delta_v_prev + bmv(delta_R, u[:, k, 3:]) * self.delta_t
            delta_p = delta_p_prev + delta_v * self.delta_t + bmv(
                delta_R, u[:, k, 3:] * self.delta_t) * (self.delta_t**2) / 2
            delta_R_prev = SO3.from_matrix(delta_R, normalize=True).as_matrix()
            delta_v_prev = delta_v
            delta_p_prev = delta_p

        return torch.cat((SO3.from_matrix(delta_R).log(), delta_v, delta_p), 1)
コード例 #17
0
def test_180_quat():
    a = torch.randn(25, 3).to(torch.float64)
    a = a / a.norm(dim=1, keepdim=True)
    angle = (150) * (np.pi / 180.)
    aa = a * angle
    C = SO3.exp(aa).as_matrix()
    print(rotmat_to_quat(C))
コード例 #18
0
    def set_project_mat_from_KRT(self, K, R, T):
        """set projection matrix from KRT matrices
        Args:
            K: (B, 3, 3)
            R: (B, 3, 3) as matrices, or (B, 4) as quaternions
            T: (B, 3)
        """
        if self.camera_mode not in ['projection']:
            raise ValueError('Only projection mode requires project mat (P)')

        bs = K.shape[0]
        if R.ndimension() == 3:
            Rot = R
        elif R.ndimension() == 2:
            if R.shape[1] == 4:  # quaternion
                try:
                    from liegroups.torch import SO3
                except:
                    raise ImportError(
                        f"failed to 'from liegroups.torch import SO3'")
                import torch.nn.functional as F
                R = F.normalize(R, eps=1e-6)
                Rot = SO3.from_quaternion(R).mat
                if Rot.ndimension() == 2:
                    Rot = Rot[
                        None, :, :]  # liegroups tends to squeeze the results
            else:
                raise RuntimeError(f"invalid R.shape = {R.shape}")
        else:
            raise RuntimeError(f"invalid R.ndimension() = {R.ndimension()}")

        P = torch.bmm(K, torch.cat((Rot, T.view(-1, 3, 1)), dim=2))
        self.transformer.P = P
コード例 #19
0
def gen_sim_data_beachball(N_rotations,
                           N_matches_per_rotation,
                           sigma,
                           factors,
                           dtype=torch.double):
    ##Simulation
    #Create a random rotation
    C = SO3_torch.exp(torch.randn(N_rotations, 3, dtype=dtype)).as_matrix()
    #Create two sets of vectors (normalized to unit l2 norm)
    x_1 = torch.randn(3, N_rotations * N_matches_per_rotation, dtype=dtype)
    x_1 = x_1 / x_1.norm(dim=0, keepdim=True)

    region_masks = [
        (x_1[0] < 0.) & (x_1[1] < 0.), (x_1[0] >= 0.) & (x_1[1] < 0.),
        (x_1[0] < 0.) & (x_1[1] >= 0.), (x_1[0] >= 0.) & (x_1[1] >= 0.)
    ]

    noise = torch.zeros_like(x_1)
    for r_i, region in enumerate(region_masks):
        noise[:, region] = factors[r_i] * sigma * torch.randn_like(
            noise[:, region])

    x_1 = x_1.view(3, N_rotations, N_matches_per_rotation).transpose(0, 1)
    noise = noise.view(3, N_rotations, N_matches_per_rotation).transpose(0, 1)

    #Rotate and add noise
    x_2 = C.bmm(x_1) + noise
    return C, x_1, x_2
コード例 #20
0
def gen_sim_data_fast(N_rotations,
                      N_matches_per_rotation,
                      sigma,
                      max_rotation_angle=None,
                      dtype=torch.double):
    ##Simulation
    #Create a random rotation
    axis = torch.randn(N_rotations, 3, dtype=dtype)
    axis = axis / axis.norm(dim=1, keepdim=True)
    if max_rotation_angle:
        max_angle = max_rotation_angle * np.pi / 180.
    else:
        max_angle = np.pi

    angle = max_angle * torch.rand(N_rotations, 1)

    C = SO3_torch.exp(angle * axis).as_matrix()
    if N_rotations == 1:
        C = C.unsqueeze(dim=0)
    #Create two sets of vectors (normalized to unit l2 norm)
    x_1 = torch.randn(N_rotations, 3, N_matches_per_rotation, dtype=dtype)
    x_1 = x_1 / x_1.norm(dim=1, keepdim=True)
    #Rotate and add noise
    noise = sigma * torch.randn_like(x_1)
    x_2 = C.bmm(x_1) + noise

    return C, x_1, x_2
コード例 #21
0
def test_rotz_batch():
    C_got = SO3.rotz(torch.Tensor([np.pi / 2, np.pi]))
    C_expected = torch.cat([
        torch.Tensor([[0, -1, 0], [1, 0, 0], [0, 0, 1]]).unsqueeze_(dim=0),
        torch.Tensor([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]).unsqueeze_(dim=0)
    ],
                           dim=0)
    assert utils.allclose(C_got.mat, C_expected)
コード例 #22
0
    def backward(self, grad_output):
        phi, R = self.saved_tensors
        grad = grad_output.new_empty((3, 3, 3))
        e_0 = grad_output.new_tensor([1, 0, 0]).view(3, 1)
        e_1 = grad_output.new_tensor([0, 1, 0]).view(3, 1)
        e_2 = grad_output.new_tensor([0, 0, 1]).view(3, 1)
        I = grad_output.new_empty((3, 3))
        torch.nn.init.eye_(I)

        if phi.norm() < 1e-8:
            grad[0, :, :] = SO3.wedge(e_0)
            grad[1, :, :] = SO3.wedge(e_1)
            grad[2, :, :] = SO3.wedge(e_2)
        else:

            fact = 1. / (phi.norm()**2)
            phi_wedge = SO3.wedge(phi)
            ImR = (I - R)

            grad[0, :, :] = fact * (phi[0] * phi_wedge +
                                    SO3.wedge(phi_wedge.mm(ImR.mm(e_0)))).mm(R)
            grad[1, :, :] = fact * (phi[1] * phi_wedge +
                                    SO3.wedge(phi_wedge.mm(ImR.mm(e_1)))).mm(R)
            grad[2, :, :] = fact * (phi[2] * phi_wedge +
                                    SO3.wedge(phi_wedge.mm(ImR.mm(e_2)))).mm(R)

        out = (grad_output * grad).sum((1, 2)).view(3, 1)

        return out
コード例 #23
0
def test_dot():
    C = SO3(torch.Tensor([[0, -1, 0], [1, 0, 0], [0, 0, 1]]))
    pt = torch.Tensor([1, 2, 3])

    CC = C.mat.mm(C.mat)
    assert utils.allclose(C.dot(C).mat, CC)

    Cpt = C.mat.matmul(pt)
    assert utils.allclose(C.dot(pt), Cpt)
コード例 #24
0
    def process_ground_turth(self, gts):
        T_global = []
        v_global = []

        for gt in gts:
            t = gt[0:3]
            R = gt[3:12].reshape(3, 3)
            T = torch.eye(4)
            T[:3, 3] = t
            T[:3, :3] = R
            T_global.append(T)
            v = gt[12:]
            v_global.append(v)

        state_f2f = []
        for combi in self.combinations:
            T_i = T_global[combi[0]]
            T_i_inv = inv_SE3(T_i)
            T_ip1 = T_global[combi[1]]
            T_i_ip1 = torch.matmul(T_i_inv, T_ip1)
            dx = T_i_ip1[:3, 3].contiguous()
            dq = SO3.from_matrix(T_i_ip1[:3, :3], normalize=False).log(
            )  # rotation_matrix_exp_to_log(T_i_ip1[:3, :3].unsqueeze(0).contiguous()).squeeze()

            if torch.isnan(dq).any() or torch.isinf(dq).any():
                raise ValueError("gt-f2f:\n{}".format(dq))

            #dq = quaternion_exp_to_log(dq).squeeze()
            state_f2f.append(torch.cat([dx, dq]))

        T_0 = T_global[0]
        T_0_inv = inv_SE3(T_0)
        state_f2g = []
        for combi in self.combinations:
            T_ip1 = T_global[combi[1]]
            T_i_ip1 = torch.matmul(T_0_inv, T_ip1)
            dx = T_i_ip1[:3, 3].contiguous()
            dq = SO3.from_matrix(T_i_ip1[:3, :3]).to_quaternion()
            state_f2g.append(torch.cat([dx, dq]))

        gt_f2f = torch.stack(state_f2f).to(self.device, non_blocking=True)
        gt_f2g = torch.stack(state_f2g).to(self.device, non_blocking=True)

        return gt_f2f, gt_f2g
コード例 #25
0
    def forward(self, phi):
        angle = phi.norm()
        I = phi.new_empty((3, 3))
        torch.nn.init.eye_(I)

        if angle < 1e-8:
            R = I + SO3.wedge(phi)
            self.save_for_backward(phi, R)
            return R

        axis = phi / angle
        s = torch.sin(angle)
        c = torch.cos(angle)

        outer_prod_axis = axis.view(3, 1).mm(axis.view(1, 3))
        R = c * I + (1. - c) * outer_prod_axis + s * SO3.wedge(axis)

        self.save_for_backward(phi, R)
        return R
コード例 #26
0
def test_rotmat_quat_large_conversions():
    print('Large (angle=pi) rotation matrix to quaternion conversions...')
    axis = torch.randn(100, 3, dtype=torch.double)
    axis = axis / axis.norm(dim=1, keepdim=True)
    angle = np.pi

    C1 = SO3.exp(angle * axis).as_matrix()
    C2_new = quat_to_rotmat(rotmat_to_quat(C1))
    assert (allclose(C1, C2_new))
    print('All passed.')
コード例 #27
0
 def integrate_odo_fog(self, u_odo, u_fog, delta_t):
     if self.nclt:
         v = 1 / 2 * (u_odo[:, 0] + u_odo[:, 1])
     else:
         v, _ = self.encoder2speed(u_odo, delta_t)
     xi = u_odo.new_zeros(u_odo.shape[0], 6)
     xi[:, 0] = v * delta_t
     xi[:, 5] = u_fog.squeeze()
     Rot = SO3.from_rpy(xi[:, 3:]).as_matrix()
     p = xi[:, :3]
     return Rot, p
コード例 #28
0
ファイル: filter.py プロジェクト: zzzzzjh/lwoi
 def propagate(self, u_odo, u_fog, delta_t, compare):
     self.x_prev = self.x
     for i in range(u_odo.shape[0]):
         self.integrate_odo_fog(u_odo[i], u_fog[i], delta_t)
     if self.gp_odo_fog:
         self.x, G_cor = self.gp_odo_fog.correct(
             self.x, u_odo, u_fog, compute_G=(compare == 'filter'))
     else:
         G_cor = torch.zeros(u_odo.shape[0], 15, 9)
     if compare == 'filter':
         self.propagate_cov(u_odo[i], u_fog[i], delta_t, G_cor)
     else:
         self.x[3:6] = SO3.from_rpy(self.x[3:6]).to_rpy()
コード例 #29
0
ファイル: filter.py プロジェクト: zzzzzjh/lwoi
    def compute_mate(self, t, x, chi, dataset_name):
        chi_est = torch.zeros(x.shape[0], 4, 4)
        chi_est[:, :3, :3] = SO3.from_rpy(x[:, 3:6]).as_matrix()
        chi_est[:, :3, 3] = x[:, :3]
        chi_est[:, 3, 3] = 1

        chi_est = SE3.from_matrix(chi_est)
        chi = SE3.from_matrix(chi)
        error = (chi.inv().dot(chi_est)).log()

        mate_translation = error[:, :3].abs().mean()
        mate_rotation = error[:, 3:].abs().mean()
        return mate_translation, mate_rotation
コード例 #30
0
ファイル: filter.py プロジェクト: zzzzzjh/lwoi
    def compute_jac_update(self, u_odo):
        J = self.J
        H = self.H

        Rot_prev = SO3.from_rpy(self.x_prev[3:6]).as_matrix()
        Rot_new = SO3.from_rpy(self.x[3:6]).as_matrix()

        # speed is not in state
        J[0, 3:6, 6:] = -Rot_prev.t()[:3, :2]
        J[-1, 3:6, 6:] = -J[0, 3:6, 6:]
        J[0, 6:9, 6:] = J[0, 6:9, 6:] * self.Delta_t

        v = torch.Tensor([1 / 2 * (u_odo[0][0] + u_odo[0][1]), 0, 0])
        H[:3, 3:6] = -Rot_prev.t().mm(Rot_new)
        H[:3, 9:12] = -Rot_prev.t()
        H[3:6, 9:12] = -Rot_prev.t().mm(
            self.skew(self.x[:3] - self.x_prev[:3] - v * self.Delta_t -
                      1 / 2 * self.g * self.Delta_t**2))
        H[6:9, :3] = Rot_prev.t()
        H[6:9, 12:15] = -Rot_prev.t()
        H[6:9,
          9:12] = self.skew(self.x[:3] - self.x_prev[:3] - v * self.Delta_t -
                            1 / 2 * self.g * self.Delta_t**2)
        return H, J