Exemple #1
0
    def construct_cuboid(self, dim, rvec, tvec):
        d0, d1, d2 = dim[0], dim[1], dim[2]

        #x_corners = [+d0/2, +d0/2, -d0/2, -d0/2, +d0/2, +d0/2, -d0/2, -d0/2]
        #y_corners = [+d1/2, +d1/2, +d1/2, +d1/2, -d1/2, -d1/2, -d1/2, -d1/2]
        #z_corners = [+d2/2, -d2/2, -d2/2, +d2/2, +d2/2, -d2/2, -d2/2, +d2/2]

        x_corners = [
            -d0 / 2, -d0 / 2, +d0 / 2, +d0 / 2, +d0 / 2, +d0 / 2, -d0 / 2,
            -d0 / 2
        ]
        y_corners = [
            -d1 / 2, +d1 / 2, -d1 / 2, +d1 / 2, -d1 / 2, +d1 / 2, -d1 / 2,
            +d1 / 2
        ]
        z_corners = [
            -d2 / 2, -d2 / 2, -d2 / 2, -d2 / 2, +d2 / 2, +d2 / 2, +d2 / 2,
            +d2 / 2
        ]

        X = numpy.array([x_corners, y_corners, z_corners],
                        dtype=numpy.float32).T

        RT = tools_pr_geom.compose_RT_mat(rvec,
                                          tvec,
                                          do_rodriges=True,
                                          do_flip=False,
                                          GL_style=False)
        Xt = tools_pr_geom.apply_matrix_GL(RT, X)[:, :3]

        uu = 0
        return Xt
Exemple #2
0
 def init_mat_model(self, rvec, tvec, do_rodriges=False, do_flip=False):
     self.mat_model = tools_pr_geom.compose_RT_mat(rvec,
                                                   tvec,
                                                   do_rodriges=do_rodriges,
                                                   do_flip=do_flip)
     glUniformMatrix4fv(glGetUniformLocation(self.shader, "model"), 1,
                        GL_FALSE, self.mat_model)
     return
Exemple #3
0
    def __init_mat_model(self,rvec, tvec,do_rodriges=False):
        self.mat_model = tools_pr_geom.compose_RT_mat(rvec, tvec, do_flip=False, do_rodriges=do_rodriges)
        glUniformMatrix4fv(glGetUniformLocation(self.shader, "model"), 1, GL_FALSE, self.mat_model)

        # check
        #rvec_model, tvec_model = tools_pr_geom.decompose_to_rvec_tvec(self.mat_model)
        #print(rvec,tvec)
        #print(rvec_model,tvec_model)
        #print()

        return
Exemple #4
0
    def __init_mat_view_RT(self, rvec,tvec,do_flip=True,do_rodriges=False):
        self.mat_view = tools_pr_geom.compose_RT_mat(rvec,tvec,do_flip,do_rodriges)
        glUniformMatrix4fv(glGetUniformLocation(self.shader, "view"), 1, GL_FALSE, self.mat_view)

        #check
        #rvec_view, tvec_view = tools_pr_geom.decompose_to_rvec_tvec(self.mat_view,do_flip=do_flip)
        #print(rvec,tvec)
        #print(rvec_view,tvec_view)
        #print()

        return
Exemple #5
0
    def get_pretty_model_rotation(self, mat_model):

        if mat_model is None: return numpy.nan, numpy.nan, numpy.nan

        rvec, tvec = tools_pr_geom.decompose_to_rvec_tvec(mat_model)
        a_pitch_deg, a_yaw_deg, a_roll_deg = rvec[[0, 1, 2]] * 180 / numpy.pi

        #check
        rvec_check = numpy.array(
            (a_pitch_deg, a_yaw_deg, a_roll_deg)) * numpy.pi / 180
        mat_model_check3 = tools_pr_geom.compose_RT_mat(rvec_check, (0, 0, 0),
                                                        do_rodriges=False,
                                                        do_flip=False)

        return a_pitch_deg, a_yaw_deg, a_roll_deg
Exemple #6
0
def example_project_GL_vs_CV(filename_in, folder_out):

    tools_IO.remove_files(folder_out)

    W, H = 800, 800
    empty = numpy.full((H, W, 3), 32, dtype=numpy.uint8)
    rvec, tvec, aperture = (0.0, 0, 0), [+5.0, 0, +15], 0.50
    rvec = numpy.array(rvec)
    tvec = numpy.array(tvec)
    scale = 1.0

    R = tools_GL3D.render_GL3D(filename_obj=filename_in,
                               W=W,
                               H=H,
                               is_visible=False,
                               projection_type='P')
    RT_GL = tools_pr_geom.compose_RT_mat(rvec, tvec, do_flip=True)
    camera_matrix_3x3 = tools_pr_geom.compose_projection_mat_3x3(
        W, H, aperture, aperture)

    cv2.imwrite(
        folder_out + 'GL.png',
        R.get_image_perspective(rvec,
                                tvec,
                                aperture,
                                aperture,
                                scale=scale * numpy.array((1, 1, 1)),
                                lookback=False,
                                do_debug=True))
    cv2.imwrite(
        folder_out + 'GL_RT.png',
        R.get_image_perspective_M(RT_GL,
                                  aperture,
                                  aperture,
                                  scale=scale * numpy.array((1, 1, 1)),
                                  lookback=False,
                                  do_debug=True))

    object = tools_wavefront.ObjLoader()
    object.load_mesh(filename_in, do_autoscale=True)
    points_3d = numpy.array(object.coord_vert, dtype=numpy.float32)

    mat_trans = scale * numpy.eye(4)
    mat_trans[3, 3] = 1

    mat_flip = numpy.eye(4)
    mat_flip[0, 0] *= -1

    RT_CV = tools_pr_geom.compose_RT_mat(rvec, tvec, do_flip=False)

    cv2.imwrite(
        folder_out + 'CV_MVP_points.png',
        tools_render_CV.draw_points_numpy_MVP(points_3d, empty,
                                              R.mat_projection, R.mat_view,
                                              R.mat_model, R.mat_trns))
    cv2.imwrite(
        folder_out + 'CV_RT.png',
        tools_render_CV.draw_points_numpy_RT(points_3d, empty, RT_CV,
                                             camera_matrix_3x3))
    cv2.imwrite(
        folder_out + 'CV_numpy.png',
        tools_render_CV.draw_cube_numpy(empty, camera_matrix_3x3,
                                        numpy.zeros(4), rvec, tvec))
    cv2.imwrite(
        folder_out + 'CV_MVP_cube.png',
        tools_render_CV.draw_cube_numpy_MVP(empty, R.mat_projection,
                                            mat_flip.dot(R.mat_view),
                                            R.mat_model, R.mat_trns))
    cv2.imwrite(
        folder_out + 'CV_cuboids_M.png',
        tools_draw_numpy.draw_cuboid(empty,
                                     tools_pr_geom.project_points_M(
                                         points_3d, RT_CV, camera_matrix_3x3,
                                         numpy.zeros(5)),
                                     color=(0, 90, 255),
                                     w=2))
    cv2.imwrite(
        folder_out + 'CV_cuboids.png',
        tools_draw_numpy.draw_cuboid(empty,
                                     tools_pr_geom.project_points(
                                         points_3d, rvec, tvec,
                                         camera_matrix_3x3, numpy.zeros(5))[0],
                                     color=(0, 190, 255),
                                     w=2))

    return
Exemple #7
0
    def centralize_points(self,
                          filename_image,
                          filename_points_in,
                          filename_points_out,
                          mat_camera,
                          rvec,
                          tvec,
                          do_debug=False):

        image = cv2.imread(filename_image)
        H, W = image.shape[:2]

        points_2d_all, points_gps_all, IDs = self.load_points(
            filename_points_in)
        points_xyz = self.shift_scale(points_gps_all, points_gps_all[0])

        M = tools_pr_geom.compose_RT_mat(rvec,
                                         tvec,
                                         do_rodriges=True,
                                         do_flip=False,
                                         GL_style=False)
        M_GL = tools_pr_geom.compose_RT_mat(rvec,
                                            tvec,
                                            do_rodriges=True,
                                            do_flip=True,
                                            GL_style=True)
        P = tools_pr_geom.compose_projection_mat_4x4(
            mat_camera[0, 0], mat_camera[1, 1],
            mat_camera[0, 2] / mat_camera[0, 0],
            mat_camera[1, 2] / mat_camera[1, 1])

        loss, a_yaws = [], numpy.arange(0, 360, 1)
        cuboid_3d_centred = self.construct_cuboid_v0((-1, +30, 0, +1, +31, 1))

        for a_yaw in a_yaws:
            R_CV = tools_pr_geom.compose_RT_mat((0, 0, a_yaw * numpy.pi / 180),
                                                (0, 0, 0),
                                                do_rodriges=True,
                                                do_flip=False,
                                                GL_style=False)
            cuboid_r_CV = tools_pr_geom.apply_matrix_GL(
                R_CV.T, cuboid_3d_centred)
            points_2d = tools_pr_geom.project_points_p3x4(cuboid_r_CV,
                                                          numpy.matmul(P, M),
                                                          check_reverce=True)
            if not numpy.isnan(points_2d[:, 0].mean()):
                loss.append(numpy.abs(points_2d[:, 0].mean() - W / 2))
            else:
                loss.append(numpy.inf)
            #if do_debug:cv2.imwrite(self.folder_out + 'AR2_GL_%03d.png' % (a_yaw),tools_draw_numpy.draw_cuboid(gray,points_2d,color=(0, 0, 255)))

        a_yaw = a_yaws[numpy.argmin(loss)]

        R_CV = tools_pr_geom.compose_RT_mat((0, 0, -a_yaw * numpy.pi / 180),
                                            (0, 0, 0),
                                            do_rodriges=True,
                                            do_flip=True,
                                            GL_style=False)
        R_GL = tools_pr_geom.compose_RT_mat((0, 0, -a_yaw * numpy.pi / 180),
                                            (0, 0, 0),
                                            do_rodriges=True,
                                            do_flip=True,
                                            GL_style=True)
        iR_CV = numpy.linalg.inv(R_CV)
        iR_GL = numpy.linalg.inv(R_GL)

        points_xyz_t_CV = tools_pr_geom.apply_matrix_GL(R_CV.T, points_xyz)
        points_xyz_t_GL = tools_pr_geom.apply_matrix_GL(R_GL, points_xyz)
        points_xyz_t_CV[:, 1] *= -1
        points_xyz_t_GL[:, 1] *= -1

        flip = numpy.identity(4)
        flip[1][1] = -1

        M_new_CV = numpy.matmul(M, iR_CV)
        M_new_CV = numpy.matmul(M_new_CV, flip)
        M_new_GL = numpy.matmul(iR_GL, M_GL)
        M_new_GL = numpy.matmul(flip, M_new_GL)

        #check
        gray = tools_image.desaturate(image)
        fov = mat_camera[0, 2] / mat_camera[0, 0]
        mat_projection = tools_pr_geom.compose_projection_mat_4x4_GL(
            W, H, fov, fov)
        # cv2.imwrite(self.folder_out + 'GL_0.png',tools_render_CV.draw_points_numpy_MVP_GL(points_xyz, gray, mat_projection, M_GL, numpy.eye(4),numpy.eye(4),w=8))
        # cv2.imwrite(self.folder_out + 'GL_1.png',tools_render_CV.draw_points_numpy_MVP_GL(points_xyz_t_GL, gray, mat_projection, M_new_GL, numpy.eye(4),numpy.eye(4),w=8))
        # cv2.imwrite(self.folder_out + 'CV_0.png',tools_draw_numpy.draw_points(gray,tools_pr_geom.project_points_p3x4(points_xyz, numpy.matmul(P, M)),color=(0, 0, 255),w=10))
        # cv2.imwrite(self.folder_out + 'CV_1.png',tools_draw_numpy.draw_points(gray,tools_pr_geom.project_points_p3x4(points_xyz_t_CV, numpy.matmul(P, M_new_CV)),color=(0, 0, 255), w=10))

        self.save_points(filename_points_out, IDs[1:], points_2d_all[1:],
                         points_xyz_t_CV[1:], W, H)

        mat_camera_new = numpy.matmul(P, M_new_CV)

        return mat_camera_new
Exemple #8
0
    def evaluate_matrices_GL(self,
                             image,
                             rvec,
                             tvec,
                             a_fov,
                             points_3d=None,
                             virt_obj=None,
                             do_debug=False):

        if numpy.any(numpy.isnan(rvec)) or numpy.any(numpy.isnan(tvec)):
            return numpy.full((4, 4), numpy.nan), numpy.full(
                (4, 4), numpy.nan), numpy.full((4, 4), numpy.nan)
        H, W = image.shape[:2]

        mR = tools_pr_geom.compose_RT_mat(rvec, (0, 0, 0),
                                          do_rodriges=True,
                                          do_flip=True,
                                          GL_style=True)
        mRT = tools_pr_geom.compose_RT_mat(rvec,
                                           tvec,
                                           do_rodriges=True,
                                           do_flip=True,
                                           GL_style=True)

        imR = numpy.linalg.inv(mR)
        mat_projection = tools_pr_geom.compose_projection_mat_4x4_GL(
            W, H, a_fov, a_fov)
        T = numpy.matmul(mRT, imR)

        if do_debug:
            import tools_GL3D
            cuboid_3d = self.construct_cuboid_v0(virt_obj)
            gray = tools_image.desaturate(cv2.resize(image, (W, H)))

            filename_obj = self.folder_out + 'temp.obj'
            self.save_obj_file(filename_obj, virt_obj)
            R = tools_GL3D.render_GL3D(filename_obj=filename_obj,
                                       W=W,
                                       H=H,
                                       do_normalize_model_file=False,
                                       is_visible=False,
                                       projection_type='P',
                                       textured=False)
            tools_IO.remove_file(filename_obj)

            cv2.imwrite(
                self.folder_out + 'AR0_GL_m1.png',
                R.get_image_perspective(rvec,
                                        tvec,
                                        a_fov,
                                        a_fov,
                                        mat_view_to_1=False,
                                        do_debug=True))
            cv2.imwrite(
                self.folder_out + 'AR0_GL_v1.png',
                R.get_image_perspective(rvec,
                                        tvec,
                                        a_fov,
                                        a_fov,
                                        mat_view_to_1=True,
                                        do_debug=True))
            cv2.imwrite(
                self.folder_out + 'AR0_CV_cube.png',
                tools_render_CV.draw_cube_numpy_MVP_GL(gray,
                                                       mat_projection,
                                                       numpy.eye(4),
                                                       mRT,
                                                       numpy.eye(4),
                                                       points_3d=cuboid_3d))
            cv2.imwrite(
                self.folder_out + 'AR0_CV_pnts.png',
                tools_render_CV.draw_points_numpy_MVP_GL(points_3d,
                                                         gray,
                                                         mat_projection,
                                                         numpy.eye(4),
                                                         mRT,
                                                         numpy.eye(4),
                                                         w=8))

            #cv2.imwrite(self.folder_out + 'AR1_CV_pnts.png',tools_render_CV.draw_points_numpy_MVP_GL(points_3dt, gray, mat_projection, numpy.eye(4), mR,numpy.eye(4), w=8))
            cv2.imwrite(
                self.folder_out + 'AR1_CV.png',
                tools_render_CV.draw_cube_numpy_MVP_GL(gray,
                                                       mat_projection,
                                                       mR,
                                                       numpy.eye(4),
                                                       T,
                                                       points_3d=cuboid_3d))
            cv2.imwrite(
                self.folder_out + 'AR1_GL.png',
                R.get_image(mat_view=numpy.eye(4),
                            mat_model=mR,
                            mat_trans=T,
                            do_debug=True))

        return numpy.eye(4), mR, T
Exemple #9
0
    def AR_points(self,
                  image,
                  filename_points,
                  camera_matrix_init,
                  dist,
                  do_shift_scale,
                  list_of_R=None,
                  virt_obj=None):

        gray = tools_image.desaturate(image)
        points_2d, points_gps, IDs = self.load_points(filename_points)
        if do_shift_scale:
            points_xyz = self.shift_scale(points_gps, points_gps[0])
        else:
            points_xyz = points_gps.copy()
        IDs, points_xyz, points_2d = IDs[1:], points_xyz[1:], points_2d[1:]
        image_AR = gray.copy()

        camera_matrix = numpy.array(camera_matrix_init, dtype=numpy.float32)
        rvecs, tvecs, points_2d_check = tools_pr_geom.fit_pnp(
            points_xyz, points_2d, camera_matrix, dist)
        rvecs, tvecs = numpy.array(rvecs).flatten(), numpy.array(
            tvecs).flatten()

        if list_of_R is not None:
            RR = -numpy.sort(-numpy.array(list_of_R).flatten())
            colors_ln = tools_draw_numpy.get_colors(
                len(list_of_R), colormap=self.colormap_circles)[::-1]
            for rad, color_ln in zip(RR, colors_ln):
                image_AR = tools_render_CV.draw_compass(
                    image_AR,
                    camera_matrix,
                    numpy.zeros(5),
                    rvecs,
                    tvecs,
                    rad,
                    color=color_ln.tolist())

        if virt_obj is not None:
            for p in points_xyz:
                cuboid_3d = p + self.construct_cuboid_v0(virt_obj)
                M = tools_pr_geom.compose_RT_mat(rvecs,
                                                 tvecs,
                                                 do_rodriges=True,
                                                 do_flip=False,
                                                 GL_style=False)
                P = tools_pr_geom.compose_projection_mat_4x4(
                    camera_matrix[0, 0], camera_matrix[1, 1],
                    camera_matrix[0, 2] / camera_matrix[0, 0],
                    camera_matrix[1, 2] / camera_matrix[1, 1])
                image_AR = tools_draw_numpy.draw_cuboid(
                    image_AR,
                    tools_pr_geom.project_points_p3x4(cuboid_3d,
                                                      numpy.matmul(P, M)))

        labels = [
            'ID %02d: %2.1f,%2.1f' % (pid, p[0], p[1])
            for pid, p in zip(IDs, points_xyz)
        ]
        image_AR = tools_draw_numpy.draw_ellipses(image_AR,
                                                  [((p[0], p[1]), (25, 25), 0)
                                                   for p in points_2d],
                                                  color=(0, 0, 190),
                                                  w=4,
                                                  labels=labels)
        image_AR = tools_draw_numpy.draw_points(image_AR,
                                                points_2d_check,
                                                color=(0, 128, 255),
                                                w=8)

        return image_AR