from fx_vis import *
from fx_load import *
from fx_utilities import *
import matplotlib.pyplot as plt
import numpy as np
import cv2
import logging
import scipy.io
import copy
import matlab.engine
from scipy.spatial.transform import Rotation

import matplotlib

matplotlib.use('TkAgg')
matplotlib.rcParams['font.family'] = 'Times New Roman'
matplotlib.rcParams['font.size'] = 20  # You can adjust this value to make the font size bigger or smaller

eng = matlab.engine.start_matlab()
logging.basicConfig(level=logging.INFO)


def esim_single_dvs_process(filename, begin_time, duration, filter_points, filter_radius, pos_time_interval, K,
                            rad_dist):
    """
    Process to aedat4 file
    :param filename: filename
    :param begin_time:
    :param duration:
    :param filter_points:
    :param filter_radius:
    :param pos_time_interval:
    :param K: camera intrinsic matrix
    :param rad_dist: distortion params
    :return:
    """
    # load events left
    events = read_events_from_txt(filename, skip_events=1)

    # # undistortion
    # events_undist = cv2.undistortPoints(events[:, :2], K, np.array([rad_dist[0, 0], rad_dist[0, 1], 0, 0]),
    #                                     None, None, K)
    # events_undist = np.squeeze(events_undist)
    # events[:, :2] = events_undist

    # select events in time slot
    begin_idx = np.searchsorted(events[:, 2], begin_time)
    end_idx = np.searchsorted(events[:, 2], begin_time + duration)
    logging.info("Index selection: " + str(begin_idx) + " to " + str(end_idx))
    events_pcd = events_array_to_pcd(events[begin_idx:end_idx],
                                     t_factor=t_factor,
                                     t_from_zero=True)
    draw_pcd_open3d(events_pcd, img_shape[0], img_shape[1])

    # remove noise left
    logging.info("Radius oulier removal")
    # cl, ind = events_pcd.remove_radius_outlier(nb_points=filter_points, radius=filter_radius)
    pcd_ball = events_pcd
    logging.info("Num of remained events: " + str(np.asarray(pcd_ball.points).shape[0]))
    draw_pcd_open3d(pcd_ball, img_shape[0], img_shape[1])

    # calculate trajectory
    ball_traj = np.zeros(shape=[traj_num, 3])
    for idx, t in enumerate(np.linspace(0.0, duration * t_factor, traj_num)):
        average_pos = average_position_density(pcd_ball, t, pos_time_interval * t_factor, density_limit=density_limit)
        if average_pos[-1] != -1:
            ball_traj[idx, :2] = average_pos[:2]
            ball_traj[idx, 2] = t

    ball_traj_pcd = o3d.geometry.PointCloud()
    ball_traj_pcd.points = o3d.utility.Vector3dVector(ball_traj)
    draw_pcd_open3d(ball_traj_pcd, img_shape[0], img_shape[1])

    return pcd_ball




def main():
    logging.info("Begin to process esim data")
    # Step: load fundamental matrix
    if (not import_params_from_yaml):
        stereo_params = scipy.io.loadmat(stereo_params_dir)
        F_calib = stereo_params["F"]
        E_calib = stereo_params["E"]
        R_calib = stereo_params["R"]
        t_calib = stereo_params["t"]
        t_calib = np.squeeze(t_calib)
        K1 = stereo_params["K1"]
        K2 = stereo_params["K2"]

        rad_dist1 = stereo_params["radial_distortion1"]
        rad_dist2 = stereo_params["radial_distortion2"]
    else:
        K1 = np.array(config["K1"])
        K2 = np.array(config["K2"])
        rad_dist1 = np.array([[0, 0]])
        rad_dist2 = np.array([[0, 0]])

        E_calib = np.array(config["E"])
        R_calib = np.array(config["R"])
        t_calib = np.array(config["t"])
        t_calib = np.squeeze(t_calib)
        F_calib = np.linalg.inv(K1).transpose().dot(E_calib).dot(np.linalg.inv(K2))

    left_pcd_ball = esim_single_dvs_process(left_data_dir,
                                            left_begin_time,
                                            left_duration,
                                            nb_points,
                                            radius,
                                            traj_time_interval,
                                            K1,
                                            rad_dist1)
    right_pcd_ball = esim_single_dvs_process(right_data_dir,
                                             right_begin_time,
                                             right_duration,
                                             nb_points,
                                             radius,
                                             traj_time_interval,
                                             K2,
                                             rad_dist2)

    # Step: search the best time diff
    if find_best_time == 1:
        # calculate time difference
        time_diff_list = []
        avg_dist_list = []
        avg_dist_list_calib = []
        for time_diff in np.linspace(time_search_begin, time_search_end, time_search_num):
            logging.info("*************************")
            logging.info("time offset t_d: %.3f", time_diff)

            time_diff_list.append(time_diff)
            dist_sum = 0
            dist_sum_calib = 0
            dist_fail_num = 0
            left_dist_fail_num = 0
            right_dist_fail_num = 0

            # compute F_ball for each time_diff
            left_array = []
            right_array = []
            for idx, t in enumerate(np.linspace(0.0, right_duration * t_factor, dist_cnt_num)):
                right_pos = average_position_density(right_pcd_ball,
                                                     t,
                                                     traj_time_interval * t_factor,
                                                     density_limit=density_limit)
                right_pt = right_pos[:2]

                left_pos = average_position_density(left_pcd_ball,
                                                    t + time_diff * t_factor,
                                                    traj_time_interval * t_factor,
                                                    density_limit=density_limit)
                left_pt = left_pos[:2]

                if left_pos[-1] != -1 and right_pos[-1] != -1:
                    left_array.append(left_pt)
                    right_array.append(right_pt)

            left_array = np.array(left_array)
            right_array = np.array(right_array)

            # call matlab
            left_matlab = matlab.double(left_array.tolist())
            right_matlab = matlab.double(right_array.tolist())
            fLMedS = eng.estimateFundamentalMatrix(left_matlab, right_matlab, 'NumTrials', 2000)
            fLMedS = np.asarray(fLMedS)
            F_ball = fLMedS

            for idx, t in enumerate(np.linspace(0.0, right_duration * t_factor, dist_cnt_num)):
                right_pos = average_position_density(right_pcd_ball,
                                                     t,
                                                     traj_time_interval * t_factor,
                                                     density_limit=density_limit)
                right_pt = right_pos[:2]

                lines_left = cv2.computeCorrespondEpilines(right_pt.reshape(-1, 1, 2), 2, F_ball)
                lines_left = lines_left.reshape(-1, 3)
                lines_left_calib = cv2.computeCorrespondEpilines(right_pt.reshape(-1, 1, 2), 2, F_calib)
                lines_left_calib = lines_left_calib.reshape(-1, 3)

                left_pos = average_position_density(left_pcd_ball,
                                                    t + time_diff * t_factor,
                                                    traj_time_interval * t_factor,
                                                    density_limit=density_limit)
                left_pt = left_pos[:2]

                if left_pos[2] == -1:
                    left_dist_fail_num += 1
                if right_pos[2] == -1:
                    right_dist_fail_num += 1

                # when event density is not enough
                if left_pos[2] == -1 or right_pos[2] == -1:
                    dist_fail_num += 1
                    continue

                dist_sum += point_line_distance(lines_left[0], left_pt[:2])
                dist_sum_calib += point_line_distance(lines_left_calib[0], left_pt[:2])

            logging.info("dist fail num: " + str(dist_fail_num))
            logging.info("left dist fail num: " + str(left_dist_fail_num))
            logging.info("right dist fail num: " + str(right_dist_fail_num))
            avg_dist_list.append(dist_sum / (dist_cnt_num - dist_fail_num))
            avg_dist_list_calib.append(dist_sum_calib / (dist_cnt_num - dist_fail_num))
            logging.info("d_avg estimated params: %.3f", dist_sum / (dist_cnt_num - dist_fail_num))
            logging.info("d_avg calibrated params: %.3f", dist_sum_calib / (dist_cnt_num - dist_fail_num))

        # fig, ax = plt.subplots()
        # fig.set_size_inches(14, 6)
        # ax.plot(time_diff_list, avg_dist_list)
        # ax.set_title("Average distance d_avg")
        # ax.set_xlabel("time offset t_d (s)")
        # ax.set_ylabel("distance d_avg (pixel)")
        # plt.savefig(output_dir + "fundamental_distance.png", dpi=500)
        # plt.show()
        numpy.savetxt(output_dir + "fundamental_distance.csv", np.vstack([time_diff_list, avg_dist_list]),
                      delimiter=",")

        fig, ax = plt.subplots()
        fig.set_size_inches(14, 6)
        ax.plot(time_diff_list, avg_dist_list, label="d_avg estimated F matrix")
        ax.plot(time_diff_list, avg_dist_list_calib, label="d_avg calibrated F matrix")
        ax.set_title("Average distance")
        ax.set_xlabel("time offset t_d (s)")
        ax.set_ylabel("distance d_avg (pixel)")
        plt.legend()
        plt.savefig(output_dir + "fundamental_distance.png", dpi=500)
        plt.show()
        numpy.savetxt(output_dir + "fundamental_distance_calib_params.csv",
                      np.vstack([time_diff_list, avg_dist_list_calib]), delimiter=",")

        best_time_diff = np.min(time_diff_list[np.argmin(avg_dist_list)])
        logging.info("optimal t_d: %.3f", best_time_diff)
        logging.info("minimum d_avg: %.3f", avg_dist_list[np.argmin(avg_dist_list)])

    else:
        # Step: just for test, use the real time diff
        best_time_diff = real_time_diff

    logging.info("*************************")
    logging.info("optimal t_d: %.3f", best_time_diff)

    # Step: compute fundamental matrix at best time diff
    left_array = []
    right_array = []
    for idx, t in enumerate(np.linspace(0.0, right_duration * t_factor, dist_cnt_num)):
        right_pos = average_position_density(right_pcd_ball,
                                             t,
                                             traj_time_interval * t_factor,
                                             density_limit=density_limit)
        right_pt = right_pos[:2]

        left_pos = average_position_density(left_pcd_ball,
                                            t + best_time_diff * t_factor,
                                            traj_time_interval * t_factor,
                                            density_limit=density_limit)
        left_pt = left_pos[:2]

        if left_pos[-1] != -1 and right_pos[-1] != -1:
            left_array.append(left_pt)
            right_array.append(right_pt)

    left_array = np.array(left_array)
    right_array = np.array(right_array)

    # save matched points
    scipy.io.savemat(output_dir + "matched_points.mat", {"left_array": left_array, "right_array": right_array})

    # call matlab function to compute F matrix from ball trajectory
    left_matlab = matlab.double(left_array.tolist())
    right_matlab = matlab.double(right_array.tolist())

    # LMedS
    fLMedS = eng.estimateFundamentalMatrix(left_matlab, right_matlab, 'NumTrials', 12000)
    fLMedS = np.asarray(fLMedS)

    # # Note: RANSAC
    # fRANSAC = eng.estimateFundamentalMatrix(left_matlab, right_matlab, 'Method', 'RANSAC', 'NumTrials', 100000,
    #                                         'DistanceThreshold', 1e-2)
    # fRANSAC = np.asarray(fRANSAC)

    # # Note: SVD
    # F_svd = solve_fundamental_matrix_svd(left_array, right_array)

    F_ball = fLMedS

    logging.info("F_ball: \n" + str(F_ball / np.linalg.norm(F_ball)))
    # logging.info("F_svd: \n" + str(F_svd / np.linalg.norm(F_svd)))
    logging.info("F_calib: \n" + str(F_calib / np.linalg.norm(F_calib)))

    # # Note: test opencv fundamental matrix
    # F_opencv_lmeds, mask = cv2.findFundamentalMat(left_array, right_array, cv2.LMEDS, 0.99)
    # F_ball = F_opencv_lmeds

    # # Note: test F svd fundamental matrix
    # F_ball = F_svd

    # Step: decompose matrix F to get R and t and visualize them
    # normalized pose
    left_array_normalized = cv2.undistortPoints(left_array, K1, distCoeffs=None, dst=None, R=None, P=None)
    right_array_normalized = cv2.undistortPoints(right_array, K2, distCoeffs=None, dst=None, R=None, P=None)

    # get Essential matrix and decompose it to R and t
    E = K2.transpose().dot(F_ball).dot(K1)

    # logging.info("E computed: " + E)
    # logging.info("E calibrated: " + E_calib)

    _, R, t, mask = cv2.recoverPose(E, right_array_normalized, left_array_normalized, focal=1.0, pp=[0.0, 0.0])

    # _, R, t, mask = cv2.recoverPose(E, left_array_normalized, right_array_normalized, focal=1.0, pp=[0.0, 0.0])
    t = np.squeeze(t)
    t_skew = np.array([[0, -t[2], t[1]],
                       [t[2], 0, -t[0]],
                       [-t[1], t[0], 0]])

    # calculate the angle between R_calib with R
    r_error = Rotation.from_matrix(np.linalg.inv(R).dot(R_calib))
    r_error_angle = np.linalg.norm(r_error.as_rotvec()) / np.pi * 180
    logging.info("The angle between R and R_calib: %.3f degrees", r_error_angle)

    # calculate the angle between t_calib with t
    t_normalized = t / np.linalg.norm(t)
    t_calib_normalized = t_calib / np.linalg.norm(t_calib)
    t_error_angle = np.arccos(np.dot(t_normalized, t_calib_normalized)) / np.pi * 180
    logging.info("Normalized t: " + str(t_normalized))
    logging.info("Normalized t calibration: " + str(t_calib_normalized))
    logging.info("The angle between t and t_calib: %.3f degrees", t_error_angle)

    # visualize R_calib with R, t_calib with t
    # draw_transformations_open3d(R, R_calib, t, t_calib)

    # Step: draw images
    # draw lines for each Fundamental matrix
    for idx, t in enumerate(np.linspace(0.0, right_duration * t_factor, img_num)):
        right_pos = average_position_density(right_pcd_ball,
                                             t,
                                             traj_time_interval * t_factor,
                                             density_limit=density_limit)
        right_pt = right_pos[:2]

        left_pos = average_position_density(left_pcd_ball,
                                            t + best_time_diff * t_factor,
                                            traj_time_interval * t_factor,
                                            density_limit=density_limit)
        left_pt = left_pos[:2]

        if left_pos[2] == -1 or right_pos[2] == -1:
            right_eimg = get_event_image(right_pcd_ball,
                                         t,
                                         traj_time_interval * t_factor,
                                         img_shape)
            left_eimg = get_event_image(left_pcd_ball,
                                        t + best_time_diff * t_factor,
                                        traj_time_interval * t_factor,
                                        img_shape)

            fig, ax = plt.subplots(1, 2)
            fig.set_size_inches(14, 6)
            ax[0].imshow(left_eimg)
            ax[0].plot([], [], label="estimated F matrix ")
            ax[0].plot([], [], "-.", label="ground-truth F matrix")
            ax[0].scatter([], [], c='lime', edgecolor="darkgreen", marker='o',
                          label="left events center")
            ax[0].set_title("Left event image")
            ax[0].legend(loc='upper right')
            ax[1].imshow(right_eimg)
            ax[1].scatter([], [], c='lime', edgecolor="darkgreen", marker='o',
                          label="right events center")
            ax[1].set_title("Right event image")

            ax[1].legend(loc='upper right')
            ax[0].set_ylabel('y (pixel)')
            ax[0].set_xlabel('x (pixel)')
            ax[1].set_xlabel('x (pixel)')
            ax[1].set_ylabel('y (pixel)')

            plt.tight_layout()
            plt.savefig(output_dir + "left_right_" + str(idx) + ".png", dpi=300)
            plt.show()
            continue

        lines_left = cv2.computeCorrespondEpilines(right_pt.reshape(-1, 1, 2), 2, F_ball)
        lines_left_calib = cv2.computeCorrespondEpilines(right_pt.reshape(-1, 1, 2), 2, F_calib)

        lines_left = lines_left.reshape(-1, 3)
        xs, ys = get_line_mpl(lines_left[0], img_shape)

        lines_left_calib = lines_left_calib.reshape(-1, 3)
        xs_calib, ys_calib = get_line_mpl(lines_left_calib[0], img_shape)

        right_eimg = get_event_image(right_pcd_ball,
                                     t,
                                     traj_time_interval * t_factor,
                                     img_shape)
        left_eimg = get_event_image(left_pcd_ball,
                                    t + best_time_diff * t_factor,
                                    traj_time_interval * t_factor,
                                    img_shape)
        fig, ax = plt.subplots(1, 2)
        fig.set_size_inches(14, 6)
        ax[0].imshow(left_eimg)
        ax[0].plot(xs, ys, label="estimated F matrix ")
        ax[0].plot(xs_calib, ys_calib, "-.", label="ground-truth F matrix")
        ax[0].scatter(left_pt[0], left_pt[1], c='lime', edgecolor="darkgreen", marker='o', label="left events center")
        ax[0].set_title("Left event image")
        ax[0].legend(loc='upper right')
        ax[1].imshow(right_eimg)
        ax[1].scatter(right_pt[0], right_pt[1], c='lime', edgecolor="darkgreen", marker='o',
                      label="right events center")
        ax[1].set_title("Right event image")

        ax[1].legend(loc='upper right')
        ax[0].set_ylabel('y (pixel)')
        ax[0].set_xlabel('x (pixel)')
        ax[1].set_xlabel('x (pixel)')
        ax[1].set_ylabel('y (pixel)')

        plt.tight_layout()
        plt.savefig(output_dir + "left_right_" + str(idx) + ".png", dpi=300)
        plt.show()


if __name__ == "__main__":
    # load parameters
    config_file = "/home/wlxing/Codes/ssac_github/config/test/test.yaml"
    config = load_config(config_file)

    t_factor = config["t_factor"]
    left_begin_time = config["left_begin_time"]
    left_duration = config["left_duration"]
    real_time_diff = config["real_time_diff"]
    right_begin_time = left_begin_time + real_time_diff
    right_duration = config["right_duration"]
    traj_num = config["traj_num"]
    traj_time_interval = config["traj_time_interval"]
    img_shape = config["img_shape"]
    nb_points = config["nb_points"]
    radius = config["radius"]
    time_search_num = config["time_search_num"]
    dist_cnt_num = config["dist_cnt_num"]
    density_limit = config["density_limit"]
    left_data_dir = config["left_data_dir"]
    right_data_dir = config["right_data_dir"]
    stereo_params_dir = config["stereo_params_dir"]
    output_dir = config["output_dir"]
    find_best_time = config["find_best_time"]
    time_search_begin = config["time_search_begin"]
    time_search_end = config["time_search_end"]
    img_num = config["img_num"]
    import_params_from_yaml = config["import_params_from_yaml"]
    main()

src/esim_ssac.py
FORREST1901/ssac
import numpy as np
import open3d as o3d


def align_vector_to_another(a=np.array([0, 0, 1]), b=np.array([1, 0, 0])):
    """
    Aligns vector a to vector b with axis angle rotation
    """
    if np.array_equal(a, b):
        return None, None
    axis_ = np.cross(a, b)
    axis_ = axis_ / np.linalg.norm(axis_)
    angle = np.arccos(np.dot(a, b))

    return axis_, angle


def normalized(a, axis=-1, order=2):
    """Normalizes a numpy array of points"""
    l2 = np.atleast_1d(np.linalg.norm(a, order, axis))
    l2[l2 == 0] = 1
    return a / np.expand_dims(l2, axis), l2


class LineMesh(object):
    def __init__(self, points, lines=None, colors=[0, 1, 0], radius=0.15):
        """Creates a line represented as sequence of cylinder triangular meshes

        Arguments:
            points {ndarray} -- Numpy array of ponts Nx3.

        Keyword Arguments:
            lines {list[list] or None} -- List of point index pairs denoting line segments. If None, implicit lines from ordered pairwise points. (default: {None})
            colors {list} -- list of colors, or single color of the line (default: {[0, 1, 0]})
            radius {float} -- radius of cylinder (default: {0.15})
        """
        self.points = np.array(points)
        self.lines = np.array(
            lines) if lines is not None else self.lines_from_ordered_points(self.points)
        self.colors = np.array(colors)
        self.radius = radius
        self.cylinder_segments = []

        self.create_line_mesh()

    @staticmethod
    def lines_from_ordered_points(points):
        lines = [[i, i + 1] for i in range(0, points.shape[0] - 1, 1)]
        return np.array(lines)

    def create_line_mesh(self):
        first_points = self.points[self.lines[:, 0], :]
        second_points = self.points[self.lines[:, 1], :]
        line_segments = second_points - first_points
        line_segments_unit, line_lengths = normalized(line_segments)

        z_axis = np.array([0, 0, 1])
        # Create triangular mesh cylinder segments of line
        for i in range(line_segments_unit.shape[0]):
            line_segment = line_segments_unit[i, :]
            line_length = line_lengths[i]
            # get axis angle rotation to allign cylinder with line segment
            axis, angle = align_vector_to_another(z_axis, line_segment)
            # Get translation vector
            translation = first_points[i, :] + line_segment * line_length * 0.5
            # create cylinder and apply transformations
            cylinder_segment = o3d.geometry.TriangleMesh.create_cylinder(
                self.radius, line_length)
            cylinder_segment = cylinder_segment.translate(
                translation, relative=False)
            if axis is not None:
                axis_a = axis * angle
                # cylinder_segment = cylinder_segment.rotate(
                #     R=o3d.geometry.get_rotation_matrix_from_axis_angle(axis_a), center=True)
                cylinder_segment = cylinder_segment.rotate(
                    R=o3d.geometry.get_rotation_matrix_from_axis_angle(axis_a),
                    center=cylinder_segment.get_center())
                # cylinder_segment = cylinder_segment.rotate(
                #   axis_a, center=True, type=o3d.geometry.RotationType.AxisAngle)
            # color cylinder
            color = self.colors if self.colors.ndim == 1 else self.colors[i, :]
            cylinder_segment.paint_uniform_color(color)

            self.cylinder_segments.append(cylinder_segment)

    def add_line(self, vis):
        """Adds this line to the visualizer"""
        for cylinder in self.cylinder_segments:
            vis.add_geometry(cylinder)

    def remove_line(self, vis):
        """Removes this line from the visualizer"""
        for cylinder in self.cylinder_segments:
            vis.remove_geometry(cylinder)


def main():
    print("Demonstrating LineMesh vs LineSet")
    # Create Line Set
    points = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0], [0, 0, 1], [1, 0, 1],
              [0, 1, 1], [1, 1, 1]]
    lines = [[0, 1], [0, 2], [1, 3], [2, 3], [4, 5], [4, 6], [5, 7], [6, 7],
             [0, 4], [1, 5], [2, 6], [3, 7]]
    colors = [[1, 0, 0] for i in range(len(lines))]

    line_set = o3d.geometry.LineSet()
    line_set.points = o3d.utility.Vector3dVector(points)
    line_set.lines = o3d.utility.Vector2iVector(lines)
    line_set.colors = o3d.utility.Vector3dVector(colors)

    # Create Line Mesh 1
    points = np.array(points) + [0, 0, 2]
    line_mesh1 = LineMesh(points, lines, colors, radius=0.02)
    line_mesh1_geoms = line_mesh1.cylinder_segments

    # Create Line Mesh 1
    points = np.array(points) + [0, 2, 0]
    line_mesh2 = LineMesh(points, radius=0.03)
    line_mesh2_geoms = line_mesh2.cylinder_segments

    o3d.visualization.draw_geometries(
        [line_set, *line_mesh1_geoms, *line_mesh2_geoms])


if __name__ == "__main__":
    main()

src/fx_lineset.py
FORREST1901/ssac
import numpy
from dv import AedatFile
import numpy as np
import open3d as o3d
import logging


def events_array_to_pcd(events_array, t_factor=1000, t_from_zero=True):
    """
    Function convert events numpy array to open3d pcd
    :param events_array: numpy array [ x, y, t, p ]
    :return: pcd
    """
    timestamp = events_array[:, 2].copy()
    polarity = events_array[:, 3].copy()
    space = np.zeros_like(timestamp)
    allones = np.ones_like(timestamp)

    if t_from_zero:
        events_array[:, 2] -= events_array[0, 2]
    events_array[:, 2] *= t_factor
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(events_array[:, :3])
    # pcd.colors = o3d.utility.Vector3dVector(np.array([timestamp, polarity, space]).transpose())
    pcd.colors = o3d.utility.Vector3dVector(np.array([polarity, space, allones - polarity]).transpose())
    return pcd


def read_events_from_txt(txt_path, dtype=float, comments="#", delimiter=None, skip_events=0, max_events=None):
    """
    Function to read events from text file
    :param txt_path: text file path
    :param dtype: data type
    :param comments: the character comments begin with
    :param delimiter: the delimiter character
    :param skip_events: the number of events need to be skipped at the beginning of the file
    :param max_events: the maximum number of events
    :return: a numpy array [x, y, t, p]
    """
    events = np.loadtxt(txt_path, dtype=dtype, comments=comments, delimiter=delimiter, skiprows=skip_events,
                        max_rows=max_events)
    return events[:, [1, 2, 0, 3]]


def read_poses_from_txt(txt_path, dtype=float, comments="#", delimiter=None, skip_poses=0, max_poses=None):
    """
    Read poses from text file
    Args:
        txt_path: text path
        dtype: data type
        comments: comment character
        delimiter: delimit character
        skip_poses: skip poses
        max_poses: max poses

    Returns: a numpy array [timestamp px py pz qx qy qz qw]

    """
    poses = np.loadtxt(txt_path, dtype=dtype, comments=comments, delimiter=delimiter, skiprows=skip_poses,
                       max_rows=max_poses)
    return poses


def load_aedatfile(file_path, get_frames=False, get_events=True):
    """
    Function to parse adeatfile
    :param get_frames: get frames or not
    :param get_events: get events or not
    :param file_path: file path
    :return: img_shape, frames, events_array
    """
    with AedatFile(file_path) as f:
        logging.info("Data location: " + file_path)
        # list all the names of streams in the file
        logging.info("Data content: " + str(f.names))

        # access dimensions of the event stream
        img_shape = f["events"].size[1], f["events"].size[0]

        # loop through the "frames" stream
        frames = []
        if get_frames:
            for frame in f["frames"]:
                frames.append([frame.image, frame.timestamp])

        logging.info("Event camera image shape: " + str(img_shape))

        if get_events:
            # events will be a named numpy array
            events = np.hstack([packet for packet in f["events"].numpy()])

            # Access information of all events by type
            timestamps, x, y, polarities = events["timestamp"], events["x"], events["y"], events["polarity"]
            events_array = np.array([x, y, timestamps / 1e6, polarities], dtype=float).transpose()
            # data time
            logging.info("Data begin timestamp: " + str(timestamps[0] / 1e6))
            logging.info("Data end timestamp: " + str(timestamps[-1] / 1e6))
            logging.info("Data duration: " + str((timestamps[-1] - timestamps[0]) / 1e6))
        else:
            events_array = np.zeros([1, 1])

    return img_shape, frames, events_array


if __name__ == "__main__":
    load_aedatfile("./data/dvSave-2021_11_05_20_28_33.aedat4")

src/fx_load.py
FORREST1901/ssac
import numpy as np
import logging
import matplotlib.pyplot as plt
# from cv2 import cv2
import cv2
import yaml


def is_sorted(a):
    """
    Function to judge numpy array a is sorted or not
    :param a: one dimension numpy array
    :return: True or False
    """
    return np.all(a[:-1] <= a[1:])


# def get_line_mpl(line_param, img_shape):
#     """
#     Function to get points pair to draw a line
#     :param line_param: numpy array [a, b, c]
#     :param img_shape: image shape numpy array [image width, image height]
#     :return: return [x0, x1], [y0, y1]
#     """
#     a = line_param[0]
#     b = line_param[1]
#     c = line_param[2]
#
#     x0 = 0
#     y0 = -c / b
#     x1 = img_shape[0] - 1
#     y1 = -(c + a * x1) / b
#     return np.array([x0, x1]), np.array([y0, y1])


def get_line_mpl(line_param, img_shape):
    """
    Function to get points pair to draw a line
    :param line_param: numpy array [a, b, c]
    :param img_shape: image shape numpy array [image width, image height]
    :return: return x_points, y_points
    """
    a, b, c = line_param
    img_w, img_h = img_shape

    # 生成1000个点
    x_points = np.linspace(0, img_w - 1, 10000)
    y_points = -(a * x_points + c) / b

    # 筛选在图像范围内的点
    valid_indices = (y_points >= 0) & (y_points <= img_h - 1)
    x_points = x_points[valid_indices]
    y_points = y_points[valid_indices]

    return x_points, y_points

def point_line_distance(line_param, point):
    """
    Function to calculate the distance between a point and a lien
    :param line_param: numpy array [a, b, c]
    :param point: numpy array [x, y]
    :return: the distance
    """
    a = line_param[0]
    b = line_param[1]
    c = line_param[2]
    x = point[0]
    y = point[1]
    d = np.abs(a * x + b * y + c) / np.sqrt(a * a + b * b)
    return d


def average_position(pcd, timestamp, time_interval):
    """
    Get average position of a slice of a point cloud
    :param pcd: point cloud
    :param timestamp: time you want
    :param time_interval: time interval before and after the time you want
    :return: position numpy array [x, y, t]
    """
    events = np.asarray(pcd.points)
    if not is_sorted(events[:, 2]):
        logging.error("Events are not sorted with timestamp!")
    begin_idx = np.searchsorted(events[:, 2], timestamp - time_interval)
    end_idx = np.searchsorted(events[:, 2], timestamp + time_interval)
    events_selected = events[begin_idx:end_idx]
    # logging.info("Num of events selected to average position: " + str(end_idx - begin_idx))
    return np.mean(events_selected, axis=0)


def average_position_density(pcd, timestamp, time_interval, density_limit=1e3, remove_margin_events=True,
                             margin_value=0):
    """
    Get average position of a slice of a point cloud
    :param margin_value:
    :param remove_margin_events:
    :param pcd: point cloud
    :param timestamp: time you want
    :param time_interval: time interval before and after the time you want
    :param density_limit: point limitation of the time interval
    :return: if density is beyond density_limit, position numpy array [x, y, t]
             if not, return numpy array [0, 0, -1]
    """
    events = np.asarray(pcd.points)
    if not is_sorted(events[:, 2]):
        logging.error("Events are not sorted with timestamp!")
    begin_idx = np.searchsorted(events[:, 2], timestamp - time_interval)
    end_idx = np.searchsorted(events[:, 2], timestamp + time_interval)
    events_selected = events[begin_idx:end_idx]
    # logging.info("Num of events selected to average position: " + str(end_idx - begin_idx))
    # if margin events need to be removed
    if remove_margin_events:
        margin_mask = (events_selected[:, 0] < margin_value) | (events_selected[:, 0] > 346 - margin_value) | \
                      (events_selected[:, 1] < margin_value) | (events_selected[:, 1] > 260 - margin_value)
        margin_num = np.sum(margin_mask)

        if events_selected.shape[0] != 0 and margin_num / events_selected.shape[0] > 0.05:
            # logging.info("margin events")
            return np.array([0, 0, -1])

    if events_selected.shape[0] > density_limit and begin_idx != 0 and end_idx != events.shape[0]:
        return np.mean(events_selected, axis=0)
    else:
        return np.array([0, 0, -1])


def get_cross_matrix(lp_mat, rp_mat):
    """
    Get a matrix in solving Fundamental matrix linear equation
    :param lp_mat: left points np.array([u, v, 1])
    :param rp_mat: right points np.array([u, v, 1])
    :return: cross matrix
    """
    result = np.zeros(shape=[lp_mat.shape[0], 9])
    for idx in range(result.shape[0]):
        u1 = lp_mat[idx, 0]
        v1 = lp_mat[idx, 1]
        u2 = rp_mat[idx, 0]
        v2 = rp_mat[idx, 1]
        result[idx] = np.array([u2 * u1, u2 * v1, u2, v2 * u1, v2 * v1, v2, u1, v1, 1])
    return result


def solve_fundamental_matrix_svd(left_pts_array, right_pts_array):
    """
    Use SVD to compute fundamental matrix
    :param left_pts_array: left points np.array([u, v])
    :param right_pts_array: left points np.array([u, v])
    :return:
    """
    cross_mat = get_cross_matrix(left_pts_array, right_pts_array)
    # singular value decomposition of cross mat
    U, S, V = np.linalg.svd(cross_mat)
    # F is the singular vector of minimum singular value
    F_svd = V[-1].reshape(3, 3)
    # and F should follow det(F)=0, so SVD again
    U, S, V = np.linalg.svd(F_svd)
    S[2] = 0
    F_svd = np.dot(U, np.dot(np.diag(S), V))
    # normalization
    F_svd = F_svd / np.linalg.norm(F_svd)
    return F_svd


def show_distortion(K, rad_dist):
    # test undistortion
    full_one_events = np.zeros((260 * 346 * 10, 4), dtype=float)
    # full_one_events = np.zeros_like(events[:260 * 346])
    for row in range(260):
        for col in range(346):
            full_one_events[row * 346 + col] = np.array([col, row, 0, 1])

    img = np.zeros([260, 346])
    for e in full_one_events:
        y = int(e[1])
        x = int(e[0])
        if 0 <= x <= 345 and 0 <= y <= 259:
            img[y, x] = 1
    img[0, 0] = 0
    plt.imshow(img)
    plt.show()

    full_one_events_undist = cv2.undistortPoints(full_one_events[:, :2], K,
                                                 np.array([rad_dist[0, 0], rad_dist[0, 1], 0, 0]),
                                                 None, None, K)
    full_one_events_undist = np.squeeze(full_one_events_undist)
    full_one_events[:, :2] = full_one_events_undist

    img = np.zeros([260, 346])
    for e in full_one_events:
        y = round(e[1])
        x = round(e[0])
        if 0 <= x <= 345 and 0 <= y <= 259:
            img[y, x] = 1
    plt.imshow(img)
    plt.show()


def load_config(config_file):
    """
    load yaml config file
    :param config_file:
    :return:
    """
    with open(config_file) as f:
        config = yaml.load(f, Loader=yaml.FullLoader)
        logging.info("config file: " + str(config))
    return config

src/fx_utilities.py
FORREST1901/ssac
import numpy as np
import matplotlib.pyplot as plt
import open3d as o3d
from fx_load import *
from fx_utilities import *
import copy
from fx_lineset import *


def draw_events_xyt_matplotlib(events):
    """
    Function to draw events with matplotlib
    :param events: numpy array, [x, y, t, p]
    :return:
    """
    fig = plt.figure()
    ax = fig.add_subplot(projection="3d")

    for event in events:
        # For each set of style and range settings, plot n random points in the box
        # defined by x in [23, 32], y in [0, 100], z in [zlow, zhigh].

        ax.scatter(event[0], event[1], event[2], s=1.0)
        ax.set_xlabel("x axis")
        ax.set_ylabel("y axis")
        ax.set_zlabel("t axis")


def get_axis_xyt_open3d(x_size, y_size, t_size, origin=None):
    """
    Function to get a bounding box
    :param x_size: x length
    :param y_size: y length
    :param t_size: t length
    :return: Open3D line set
    """
    if origin is None:
        origin = [0, 0, 0]
    x_origin, y_origin, t_origin = origin
    points = [
        [0, 0, 0],
        [x_size, 0, 0],
        [0, y_size, 0],
        [x_size, y_size, 0],
        [0, 0, t_size],
        [x_size, 0, t_size],
        [0, y_size, t_size],
        [x_size, y_size, t_size],
    ]

    origins = [
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin],
        [x_origin, y_origin, t_origin]
    ]
    points = np.array(points) + np.array(origins)
    lines = [
        [0, 1],
        [0, 2],
        [1, 3],
        [2, 3],
        [4, 5],
        [4, 6],
        [5, 7],
        [6, 7],
        [0, 4],
        [1, 5],
        [2, 6],
        [3, 7],
    ]
    colors = [[0, 0, 0] for i in range(len(lines))]
    line_set = o3d.geometry.LineSet(
        points=o3d.utility.Vector3dVector(points),
        lines=o3d.utility.Vector2iVector(lines),
    )
    line_set.colors = o3d.utility.Vector3dVector(colors)

    # return line_set

    line_mesh1 = LineMesh(points, lines, colors, radius=1.5)
    line_mesh1_geoms = line_mesh1.cylinder_segments

    return line_mesh1_geoms


def draw_two_pcd_open3d(pcd1, pcd2, x_size=240, y_size=180,
                        pcd1_origin=None,
                        pcd2_origin=None,
                        other_open3d_geometry=None):
    """
    draw pcd point cloud
    :param pcd2_origin:
    :param pcd1: point cloud
    :param x_size: x size
    :param y_size: y size
    :param other_open3d_geometry: list of other open3d geometry
    :return: none
    """
    if pcd2_origin is None:
        pcd2_origin = [0, 0, 0]
    if pcd1_origin is None:
        pcd1_origin = [0, 0, 0]
    x = np.asarray(pcd1.points)
    t1_size = np.max(np.asarray(pcd1.points)[:, 2])
    t2_size = np.max(np.asarray(pcd2.points)[:, 2])
    axis1 = get_axis_xyt_open3d(x_size, y_size, t1_size, pcd1_origin)
    axis2 = get_axis_xyt_open3d(x_size, y_size, t2_size, pcd2_origin)
    coord = o3d.geometry.TriangleMesh.create_coordinate_frame(
        size=80, origin=[0, 0, 0])
    pcd1 = copy.deepcopy(pcd1).translate(pcd1_origin)
    pcd2 = copy.deepcopy(pcd2).translate(pcd2_origin)
    # objects_to_draw = [pcd1, pcd2, *axis1, *axis2, coord]
    objects_to_draw = [pcd1, pcd2, *axis1, *axis2]

    # pcd_copy = copy.deepcopy(pcd).translate((1.3, 0, 0))
    # objects_to_draw.append(pcd_copy)

    if other_open3d_geometry:
        objects_to_draw.extend(other_open3d_geometry)
    vis = o3d.visualization.Visualizer()
    vis.create_window(width=3840, height=2032)
    # draw objects
    for ob in objects_to_draw:
        vis.add_geometry(ob)

    # set view control
    ctr = vis.get_view_control()
    ctr.set_front([0, 0, -100])
    ctr.set_up([0, -100, 0])
    ctr.set_lookat([x_size / 2, y_size / 2, 100])
    ctr.set_zoom(0.1)

    # load view point
    # param = o3d.io.read_pinhole_camera_parameters("./config/viewpoint.json")
    param = o3d.io.read_pinhole_camera_parameters("./config/draw_viewpoint.json")
    ctr.convert_from_pinhole_camera_parameters(param)

    # load render config
    # vis.get_render_option().load_from_json("./config/graph_renderoption.json")
    vis.get_render_option().load_from_json("./config/graph_renderoption.json")
    # # vis.get_render_option().line_width = 10.0
    vis.run()

    # # save view point
    # param = vis.get_view_control().convert_to_pinhole_camera_parameters()
    # o3d.io.write_pinhole_camera_parameters("./config/draw_viewpoint.json", param)
    #
    # # save render config
    # vis.get_render_option().save_to_json("./config/graph_renderoption.json")

    vis.destroy_window()


def draw_pcd_open3d(pcd, x_size=240, y_size=180, other_open3d_geometry=None):
    """
    draw pcd point cloud
    :param pcd: point cloud
    :param x_size: x size
    :param y_size: y size
    :param other_open3d_geometry: list of other open3d geometry
    :return: none
    """
    x = np.asarray(pcd.points)
    t_size = np.max(np.asarray(pcd.points)[:, 2])
    axis = get_axis_xyt_open3d(x_size, y_size, t_size)
    coord = o3d.geometry.TriangleMesh.create_coordinate_frame(
        size=80, origin=[0, 0, 0])
    objects_to_draw = [pcd, *axis, coord]

    if other_open3d_geometry:
        objects_to_draw.extend(other_open3d_geometry)
    vis = o3d.visualization.Visualizer()
    vis.create_window(width=3840, height=2032)
    # draw objects
    for ob in objects_to_draw:
        vis.add_geometry(ob)

    # set view control
    ctr = vis.get_view_control()
    ctr.set_front([0, 0, -100])
    ctr.set_up([0, -100, 0])
    ctr.set_lookat([x_size / 2, y_size / 2, 100])
    ctr.set_zoom(0.1)

    # load view point
    # param = o3d.io.read_pinhole_camera_parameters("./config/viewpoint.json")
    param = o3d.io.read_pinhole_camera_parameters("./config/draw_viewpoint.json")
    ctr.convert_from_pinhole_camera_parameters(param)

    # load render config
    # vis.get_render_option().load_from_json("./config/graph_renderoption.json")
    vis.get_render_option().load_from_json("./config/graph_renderoption.json")
    # # vis.get_render_option().line_width = 10.0
    vis.run()

    # # save view point
    # param = vis.get_view_control().convert_to_pinhole_camera_parameters()
    # o3d.io.write_pinhole_camera_parameters("./config/draw_viewpoint.json", param)
    #
    # # save render config
    # vis.get_render_option().save_to_json("./config/graph_renderoption.json")

    vis.destroy_window()


def draw_events_xyt_open3d(events, x_size=240, y_size=180, t_factor=1000, t_from_zero=False):
    """
    Function to draw event camera"s events using Open3D
    :param events: numpy array, [x, y, t, p]
    :param x_size: pixel number of x axis of image plane
    :param y_size: pixel number of y axis of image plane
    :param t_factor: factor when drawing t
    :param t_from_zero: t begin from zero
    :return:
    """
    idx_pos = (events[:, 3] == 1)
    idx_neg = (events[:, 3] == 0)

    # Note: numpy fancy index, return a copy, not a view
    events_pos = events[idx_pos]
    events_neg = events[idx_neg]

    if t_from_zero:
        events_pos[:, 2] -= events_pos[0, 2]
        events_neg[:, 2] -= events_neg[0, 2]

    events_pos[:, 2] *= t_factor
    events_neg[:, 2] *= t_factor

    pcd_pos = o3d.geometry.PointCloud()
    pcd_pos.points = o3d.utility.Vector3dVector(events_pos[:, :3])
    pcd_pos.paint_uniform_color([1, 0, 0])

    pcd_neg = o3d.geometry.PointCloud()
    pcd_neg.points = o3d.utility.Vector3dVector(events_neg[:, :3])
    pcd_neg.paint_uniform_color([0, 0, 1])

    # get axis and coordinate
    axis = get_axis_xyt_open3d(x_size, y_size, events_pos[-1, 2])

    coord = o3d.geometry.TriangleMesh.create_coordinate_frame(
        size=80, origin=[0, 0, 0])

    # objects_to_draw = [pcd_pos, pcd_neg, *axis, coord]
    objects_to_draw = [pcd_pos, pcd_neg, *axis, ]
    # objects_to_draw = [pcd_pos, pcd_neg, coord]
    vis = o3d.visualization.Visualizer()
    vis.create_window(width=3840, height=2032)
    # draw objects
    for ob in objects_to_draw:
        vis.add_geometry(ob)

    # set view control
    ctr = vis.get_view_control()
    ctr.set_front([0, 0, -100])
    ctr.set_up([0, -100, 0])
    ctr.set_lookat([x_size / 2, y_size / 2, 100])
    ctr.set_zoom(0.1)

    # # load view point
    param = o3d.io.read_pinhole_camera_parameters("./config/viewpoint.json")
    ctr.convert_from_pinhole_camera_parameters(param)

    # load render config
    vis.get_render_option().load_from_json("./config/renderoption.json")

    vis.run()

    # save view point
    # param = vis.get_view_control().convert_to_pinhole_camera_parameters()
    # o3d.io.write_pinhole_camera_parameters("./config/viewpoint.json", param)

    # save render config
    # vis.get_render_option().save_to_json("./config/renderoption.json")
    vis.destroy_window()


def cv_get_iwe(argmax, xs, ys, ts, ps, img_size, begin_index=0, cnt_num=1200):
    """
    Warp events to a image (a 2D numpy array).
    Args:
        argmax: iwe warp parameter [v_x, v_y]
        xs: xs
        ys: ys
        ts: ts
        ps: ps
        img_size: image size [width, height]
        begin_index: iwe warp begin index
        cnt_num: warp events number

    Returns: a 2d numpy array the image of warped events

    """
    xs_copy, ys_copy = xs[begin_index:begin_index + cnt_num].copy(), ys[begin_index:begin_index + cnt_num].copy()
    img = np.zeros([img_size[1], img_size[0]])
    for i in range(begin_index, begin_index + cnt_num):
        xs_copy[i] = xs_copy[i] - argmax[0] * (ts[i] - ts[0])
        ys_copy[i] = ys_copy[i] - argmax[1] * (ts[i] - ts[0])
        if 0 < xs_copy[i] < img_size[0] and 0 < ys_copy[i] < img_size[1]:
            img[int(ys_copy[i])][int(xs_copy[i])] = 1.0
    return img


def get_event_image(pcd, timestamp, time_interval, img_size):
    """
    get event image from events point cloud
    :param pcd: events point cloud
    :param timestamp: time you choose
    :param time_interval: time interval before and after the timestamp you choose
    :param img_size: image size
    :return: event image
    """
    events = np.asarray(pcd.points)
    colors = np.asarray(pcd.colors)
    if not is_sorted(events[:, 2]):
        logging.error("Events are not sorted with timestamp!")
    begin_idx = np.searchsorted(events[:, 2], timestamp - time_interval)
    end_idx = np.searchsorted(events[:, 2], timestamp + time_interval)
    events_selected = events[begin_idx:end_idx]
    colors_selected = colors[begin_idx:end_idx]
    # blank image
    # img = np.zeros([img_size[1], img_size[0], 3])
    img = np.zeros((img_size[1], img_size[0], 4), dtype=np.uint8)  # RGBA
    delta_value = 40

    # img[:, :] = [1, 1, 1]
    for idx, event in enumerate(events_selected):
        if 0 <= round(event[0]) < img_size[0] and 0 <= round(event[1]) < img_size[1]:
            if (img[round(event[1]), round(event[0]), 0] == 1):
                img[round(event[1]), round(event[0]), 0] = 0
                img[round(event[1]), round(event[0]), 1] = 0
                img[round(event[1]), round(event[0]), 2] = 0
            if (colors_selected[idx, 0] > 0):
                img[round(event[1]), round(event[0]), 0] = 255
                img[round(event[1]), round(event[0]), 3] += delta_value
                if img[round(event[1]), round(event[0]), 3] >= 255:
                    img[round(event[1]), round(event[0]), 3] = 255
            else:
                img[round(event[1]), round(event[0]), 2] = 255
                img[round(event[1]), round(event[0]), 3] += delta_value
                if img[round(event[1]), round(event[0]), 3] >= 255:
                    img[round(event[1]), round(event[0]), 3] = 255

    return img


def cv_get_iwe(argmax, events, img_size, begin_index=0, cnt_num=1200):
    """
    Warp events to a image (a 2D numpy array).
    Args:
        argmax: iwe warp parameter [v_x, v_y]
        events: a numpy array [x, y, t, p]
        img_size: image size [width, height]
        begin_index: iwe warp begin index
        cnt_num: warp events number

    Returns: a 2d numpy array the image of warped events

    """
    events_copy = events[begin_index:begin_index + cnt_num].copy()
    img = np.zeros([img_size[1], img_size[0]])
    for i in range(cnt_num):
        events_copy[i, 0] = events_copy[i, 0] - argmax[0] * events_copy[i, 2]
        events_copy[i, 1] = events_copy[i, 1] - argmax[1] * events_copy[i, 2]
        if 0 < events_copy[i, 0] < img_size[0] and 0 < events_copy[i, 1] < img_size[1]:
            img[int(events_copy[i, 1])][int(events_copy[i, 0])] = 1.0
    return img


def draw_transformations_open3d(R1, R2, t1, t2, factor=10.0):
    t1 = t1 / np.linalg.norm(t1)
    t2 = t2 / np.linalg.norm(t2)

    trans = np.identity(4)
    trans[:3, :3] = R1
    trans[:3, 3] = t1 * factor
    trans_calib = np.identity(4)
    trans_calib[:3, :3] = R2
    trans_calib[:3, 3] = t2 * factor
    mesh_left = o3d.geometry.TriangleMesh.create_coordinate_frame()
    mesh_right = copy.deepcopy(mesh_left)
    mesh_right.transform(np.linalg.inv(trans))
    mesh_right_calib = copy.deepcopy(mesh_left)
    mesh_right_calib.transform(np.linalg.inv(trans_calib))
    # o3d.visualization.draw_geometries([mesh_left, mesh_right, mesh_right_calib])

    objects_to_draw = [mesh_left, mesh_right, mesh_right_calib]
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    # draw objects
    for ob in objects_to_draw:
        vis.add_geometry(ob)
    # set view control
    ctr = vis.get_view_control()
    # load view point
    param = o3d.io.read_pinhole_camera_parameters("./config/trans_viewpoint.json")
    ctr.convert_from_pinhole_camera_parameters(param)

    # load render config
    vis.get_render_option().load_from_json("./config/trans_renderoption.json")

    vis.run()

    # # save view point
    # param = vis.get_view_control().convert_to_pinhole_camera_parameters()
    # o3d.io.write_pinhole_camera_parameters("./config/trans_viewpoint.json", param)
    #
    # # save render config
    # vis.get_render_option().save_to_json("./config/trans_renderoption.json")

    vis.destroy_window()

# if __name__ == "__main__":
#     events = read_events_from_txt("/home/wlxing/Data/eth_davis/davis_240c/slider_depth/events.txt", max_events=100000)
#     print(events[-1, :])
#     draw_events_xyt_open3d(events, t_factor=1000)
#     print(events[-1, :])

src/fx_vis.py
FORREST1901/ssac
import cv2
import os
from PIL import Image

# Set the path and parameters
path = '/home/wlxing/Codes/ssac/output/test'
output_mp4_path = '/home/wlxing/Codes/ssac/output/output.mp4'
fps = 60  # Frames per second

# Get all .png files and sort them by their filenames
files = [f for f in os.listdir(path) if f.endswith('.png')]
files.sort(key=lambda x: int(x.split('_')[-1].split('.png')[0]))

# Initialize variables
frames = []
last_image = None

# Get the dimensions of the first image
first_image = Image.open(os.path.join(path, files[0]))
width, height = first_image.size

# Initialize the video writer
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = cv2.VideoWriter(output_mp4_path, fourcc, fps, (width, height))

# Loop from 1, according to natural numbers
for i in range(1, max([int(f.split('_')[-1].split('.png')[0]) for f in files]) + 1):
    filename = f"left_right_{i}.png"
    filepath = os.path.join(path, filename)

    # If the image corresponding to the frame does not exist, use the previous image
    if not os.path.exists(filepath):
        if last_image is not None:
            out.write(last_image)
        continue

    # Read the image and save
    img = cv2.imread(filepath)
    out.write(img)
    last_image = img

# Release the video writer
out.release()

print(f"Generated MP4 saved at {output_mp4_path}")

src/image_gif.py
FORREST1901/ssac