there is three code that i am using and provide you them. just now, i have a problem. for stationary video that forward motion on a corridor, my camera trajectory is linearly increasing or decreasing on the z axis. but i must see this type of change on the x axis. i think the problem is about the coordinate system mismatch between open3d and opencv. open3d use z as forward and y as up. i want to use x as forward and z as up. examine the all of the codes properly there is more than 1 rotation and transformation matrix definitions. how can i change the behaviour of camera trajectory to what i want. 

import cv2
import numpy as np

class PoseEstimator:
    def __init__(self, focal_length, principal_point):
        self.orb = cv2.ORB_create(5000)
        self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        self.K = np.array([
            [focal_length, 0, principal_point[0]],
            [0, focal_length, principal_point[1]],
            [0, 0, 1]
        ])

    def detect_and_compute(self, img):
        keypoints, descriptors = self.orb.detectAndCompute(img, None)
        return keypoints, descriptors

    def match_features(self, desc1, desc2):
        if desc1 is None or desc2 is None:
            print("One of the descriptors is None.")
            return []

        # Use KNN matcher with k=2
        matcher = cv2.BFMatcher(cv2.NORM_HAMMING)
        knn_matches = matcher.knnMatch(desc1, desc2, k=2)
        
        # Apply ratio test
        good_matches = []
        for m, n in knn_matches:
            if m.distance < 0.75 * n.distance:
                good_matches.append(m)
                
        return good_matches

    def estimate_pose(self, kp1, kp2, matches, scale=1.0):
        if len(matches) < 8:
            print("Not enough matches to compute pose.")
            return None, None

        try:
            pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
            pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])
            
            E, mask = cv2.findEssentialMat(pts1, pts2, self.K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
            if E is None:
                print("Essential matrix computation failed.")
                return None, None
            
            _, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, self.K)

            # Apply scale to the translation vector
            t = t * scale
            print(f"Applied scale factor: {scale}")
            print(f"Estimated rotation R:\n{R}")
            print(f"Estimated translation t:\n{t.T}")
            return R, t
        except Exception as e:
            print(f"Exception during pose estimation: {e}")
            return None, None

import os
import sys
import cv2
import numpy as np
import open3d as o3d
import argparse
import threading
import time
import matplotlib.pyplot as plt

# Add the current directory to the Python path
current_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.append(current_dir)

# Add the 'zoe' and 'Metric3D' directories to the Python path
zoe_dir = os.path.join(current_dir, 'zoe')
metric3d_dir = os.path.join(current_dir, 'metric3dd')
sys.path.append(zoe_dir)
sys.path.append(metric3d_dir)

# Import point cloud generators
from point_cloud_generator import PointCloudGenerator as MidasPointCloudGenerator
from zoe.point_cloud_generator import PointCloudGenerator as ZoePointCloudGenerator
from metric3dd.point_cloud_generator import PointCloudGenerator as Metric3DPointCloudGenerator
from pose_estimator import PoseEstimator

def register_point_clouds(source_pcd, target_pcd):
    icp_threshold = 0.02
    trans_init = np.eye(4)
    reg_p2p = o3d.pipelines.registration.registration_icp(
        source_pcd, target_pcd, icp_threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPoint())
    return reg_p2p.transformation

class SLAMSystem:
    def __init__(self, video_path, calibration_file, real_time_vis=False, model='zoe', save_point_clouds=False, frame_interval=1):
        self.calibration_file = calibration_file
        self.camera_matrix, self.dist_coeffs = self.load_camera_parameters()
        self.frame_interval = frame_interval
        
        if model == 'midas':
            self.generator = MidasPointCloudGenerator(calibration_path=calibration_file)
            self.pcd_folder = 'pcd_midas'
        elif model == 'metric3d':
            self.generator = Metric3DPointCloudGenerator(calibration_path=calibration_file)
            self.pcd_folder = 'pcd_metric'
        else:  # default to zoe
            self.generator = ZoePointCloudGenerator(calibration_path=calibration_file)
            self.pcd_folder = 'pcd_zoe'
        
        focal_length = (self.camera_matrix[0, 0] + self.camera_matrix[1, 1]) / 2
        principal_point = (self.camera_matrix[0, 2], self.camera_matrix[1, 2])
        self.estimator = PoseEstimator(focal_length, principal_point)
        
        self.cap = cv2.VideoCapture(video_path)
        self.global_map = o3d.geometry.PointCloud()
        self.prev_frame = None
        self.pose = np.eye(4)
        self.real_time_vis = real_time_vis
        self.save_point_clouds = save_point_clouds
        self.vis = None
        self.is_running = True
        self.camera_positions = []
        self.vis_updated = threading.Event()
        if self.real_time_vis:
            self.vis = o3d.visualization.Visualizer()
            self.vis.create_window()
            self.vis.add_geometry(self.global_map)
            self.vis_thread = threading.Thread(target=self.visualization_loop)
            self.vis_thread.start()

    def load_camera_parameters(self):
        try:
            data = np.load(self.calibration_file)
            camera_matrix = data['Camera_matrix']
            dist_coeffs = data['distCoeff'].ravel()
            print(f"Camera matrix:\n{camera_matrix}")
            print(f"Distortion coefficients: {dist_coeffs}")
            return camera_matrix, dist_coeffs
        except Exception as e:
            print(f"Error loading camera parameters: {str(e)}")
            print("Using default camera parameters")
            return np.array([[700, 0, 320], [0, 700, 240], [0, 0, 1]]), np.zeros(5)

    def calculate_scale(self, depth_prev, depth_curr):
        depth_prev_nonzero = depth_prev[depth_prev > 0]
        depth_curr_nonzero = depth_curr[depth_curr > 0]

        if len(depth_prev_nonzero) == 0 or len(depth_curr_nonzero) == 0:
            print("Insufficient depth information for scale calculation. Using default scale.")
            return 1.0

        # Use percentile-based scaling
        prev_depth = np.percentile(depth_prev_nonzero, 50)
        curr_depth = np.percentile(depth_curr_nonzero, 50)

        if curr_depth == 0:
            print("Current depth is zero, cannot calculate scale.")
            return 1.0

        scale = prev_depth / curr_depth

        # Limit scale factor to prevent extreme values
        max_scale = 1.1
        min_scale = 0.9
        scale = np.clip(scale, min_scale, max_scale)

        print(f"Calculated scale factor: {scale}")
        return scale

    def update_visualization(self):
        if self.real_time_vis:
            self.vis.update_geometry(self.global_map)
            self.vis.poll_events()
            self.vis.update_renderer()
            self.vis_updated.set()

    def visualization_loop(self):
        while self.is_running:
            self.vis_updated.wait()
            self.update_visualization()
            self.vis_updated.clear()

    def run(self):
        frame_count = 0
        depth_prev = None

        if self.save_point_clouds:
            os.makedirs(self.pcd_folder, exist_ok=True)

        try:
            while True:
                ret, frame = self.cap.read()
                if not ret:
                    print("End of video stream.")
                    break

                if frame_count % self.frame_interval == 0:
                    print(f"Processing frame {frame_count}")

                    point_cloud_np, depth_map = self.generator.process_frame_with_depth(frame)

                    if self.save_point_clouds:
                        pcd_filename = os.path.join(self.pcd_folder, f"frame_{frame_count:04d}.txt")
                        self.generator.save_point_cloud_txt(point_cloud_np, pcd_filename)
                        print(f"Saved point cloud to {pcd_filename}")
                    else:
                        current_pcd = o3d.geometry.PointCloud()
                        current_pcd.points = o3d.utility.Vector3dVector(point_cloud_np[:, :3])
                        current_pcd.colors = o3d.utility.Vector3dVector(point_cloud_np[:, 3:] / 255.0)

                        scale = None

                        if self.prev_frame is not None and depth_prev is not None:
                            gray_prev = cv2.cvtColor(self.prev_frame, cv2.COLOR_BGR2GRAY)
                            gray_current = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                            kp1, desc1 = self.estimator.detect_and_compute(gray_prev)
                            kp2, desc2 = self.estimator.detect_and_compute(gray_current)

                            matches = self.estimator.match_features(desc1, desc2)
                            if len(matches) == 0:
                                print(f"Frame {frame_count}: No valid matches found.")
                            else:
                                scale = self.calculate_scale(depth_prev, depth_map)
                                R, t = self.estimator.estimate_pose(kp1, kp2, matches, scale=scale)
                                if R is not None and t is not None:
                                    T = np.eye(4)
                                    T[:3, :3] = R
                                    T[:3, 3] = t.squeeze()
                                    self.pose = self.pose @ T
                                    print(f"Updated pose:\n{self.pose}")
                                    # Store the camera position
                                    camera_position = self.pose[:3, 3]
                                    self.camera_positions.append(camera_position)
                                else:
                                    print(f"Frame {frame_count}: Pose estimation failed.")

                        current_pcd.transform(self.pose)
                        
                        self.global_map += current_pcd
                        self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05)

                        if self.real_time_vis:
                            self.vis_updated.set()

                        self.prev_frame = frame.copy()
                        depth_prev = depth_map.copy()

                frame_count += 1

        except Exception as e:
            print(f"An error occurred during SLAM processing: {str(e)}")

        finally:
            self.cap.release()

            if not self.save_point_clouds:
                o3d.io.write_point_cloud("global_map.pcd", self.global_map)
                print("Saved global map to 'global_map.pcd'.")

                if self.real_time_vis:
                    self.is_running = False
                    self.vis_thread.join()
                    self.vis.destroy_window()
                else:
                    o3d.visualization.draw_geometries([self.global_map])

            if len(self.camera_positions) > 0:
                camera_positions = np.array(self.camera_positions)
                fig = plt.figure()
                ax = fig.add_subplot(111, projection='3d')
                ax.plot(camera_positions[:, 0], camera_positions[:, 1], camera_positions[:, 2], 'r*-')
                ax.set_xlabel('X')
                ax.set_ylabel('Y')
                ax.set_zlabel('Z')
                ax.set_title('Camera Trajectory')
                plt.show()

                # Save camera trajectory to file
                np.savetxt("camera_trajectory.txt", camera_positions, delimiter=",", header="X,Y,Z", comments="")
                print("Saved camera trajectory to 'camera_trajectory.txt'.")

def main():
    parser = argparse.ArgumentParser(description="Run SLAM system with optional real-time visualization or save point clouds")
    parser.add_argument("--video_path", type=str, default="videos/kutupkisa.mp4", help="Path to input video")
    parser.add_argument("--calibration_file", type=str, required=True, help="Path to camera calibration NPZ file")
    parser.add_argument("--real_time_vis", action="store_true", help="Enable real-time visualization")
    parser.add_argument("--model", type=str, choices=['midas', 'zoe', 'metric3d'], default='zoe', help="Choose the point cloud generation model")
    parser.add_argument("--save_point_clouds", action="store_true", help="Save point clouds as txt files without running SLAM")
    parser.add_argument("--frame_interval", type=int, default=1, help="Process every Nth frame (default: 1)")
    args = parser.parse_args()

    slam = SLAMSystem(args.video_path, args.calibration_file, args.real_time_vis, args.model, args.save_point_clouds, args.frame_interval)
    slam.run()

if __name__ == "__main__":
    main()