The following codes is used for an rgb slam project. Unfortunately, in the current state rgb slam cannot be done correctly. For example, the camera trajectory regularly decreases linearly in the -z direction. This is most likely a logic error.I think it is a logic error because I see a regular decrease even in videos where the camera is moving straight ahead in a completely fixed direction. I would like to solve this problem. You should identify the extra parts that might be causing the problem. Suggest code edits. Use incremental progression and explain the steps one by one. 

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()