I have built a SLAM system with three different Python scripts: slam.py, pose_estimator.py, and point_cloud_generator.py. The goal of this system is to estimate the camera trajectory from a video and create a 3D map using point clouds, depth estimation models, and ORB-based pose estimation. However, I am facing several issues, and I need detailed instructions, code examples, and suggestions to address these problems. Here's a breakdown of the issues I have encountered and relevant code excerpts:

1. Point Cloud Scale and Metric Accuracy Problem
Issue: The point clouds generated using the frame_interval parameter do not match real-world metrics. For instance, in a video of a corridor with an actual length of 20-30 meters, the map produced shows a corridor almost 100 meters long. This discrepancy suggests that the point clouds are overlapping and not accurately representing the camera's movement and speed.

Potential Causes:

The camera's movement is not being reflected accurately in the point cloud generation process.
Inaccurate scaling of the depth map in the point_cloud_generator.py script.
Incorrect application of transformation matrices, leading to a skewed or stretched map.
Relevant Code: In slam.py, the function calculate_scale is used to determine the relative scaling between frames, but there might be an issue with how the scaling is computed or applied to the pose transformations.

python
Kodu kopyala
def calculate_scale(self, depth_prev, depth_curr):
    # Use percentile-based scaling
    prev_depth = np.percentile(depth_prev_nonzero, 50)
    curr_depth = np.percentile(depth_curr_nonzero, 50)
    scale = prev_depth / curr_depth
    return scale
Questions:

How should the scaling of depth maps be handled to ensure that the camera trajectory matches the real-world movement?
Should there be further adjustments made to the transformation matrices to accurately reflect the scale in real-world units (e.g., meters)?
Is there an optimal way to refine the depth model's output to achieve real-world accuracy?
2. Overlapping Point Clouds and Map Remapping
Issue: The generated point clouds overlap across frames, leading to multiple mappings of the same areas. Ideally, a point in the map should only be generated once, and new points should be added as the camera moves. The current implementation re-maps regions that have already been mapped.

Potential Causes:

The SLAM system is not recognizing previously mapped areas and continues to add redundant points to the point cloud.
Transformation matrices may be misapplied, causing misalignment between the frames and remapping of the same location multiple times.
Relevant Code: The current implementation does perform some down-sampling via voxelization, but there is no explicit filtering of already-mapped regions:


self.global_map += current_pcd
self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05)
Questions:

How can I better ensure that previously mapped points are recognized, and new points are only added when the camera moves to a new area?
Would integrating a feature like loop closure detection (e.g., using GTSAM) help to refine the map and eliminate redundancy?
3. Pose Estimation Errors
Issue: The ORB-based pose estimation method is not working correctly. Specifically, the camera's movement is incorrectly estimated, with straight movements along the Z-axis when they should occur along the X or Y axes. The camera's trajectory is often misaligned with the real-world coordinate system, leading to inaccurate mapping.

Potential Causes:

ORB feature matching is insufficient for accurate pose estimation in this scenario.
The rotation and translation matrices generated during pose estimation might be incorrect or misapplied.
Relevant Code: In slam.py, the pose estimation process uses ORB features and essential matrices, but the calculation may be flawed:
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
Questions:

How can I improve the robustness of pose estimation with ORB? Should I switch to a different feature detection and matching method (e.g., SIFT, SuperPoint)?
Are there better alternatives to using essential matrices and ORB for SLAM in real-time scenarios?
4. High VRAM Usage and Slow Performance
Issue: The SLAM system consumes a lot of VRAM and runs very slowly. It seems to lack optimization and may contain redundant operations, such as the application of transformation matrices multiple times across the three scripts.

Potential Causes:

The depth estimation models (Metric3D, ZoeDepth, etc.) are computationally heavy, leading to slow performance.
There are likely redundant calculations of transformation matrices across different scripts.
The real-time visualization is resource-intensive, slowing down the overall process.

Relevant Code: In point_cloud_generator.py, the depth model processing seems to be a bottleneck:
with torch.no_grad():
    pred_depth, confidence, _ = self.metric3d_model.inference({'input': image_tensor})
Questions:

How can I reduce VRAM usage while maintaining accuracy in depth estimation and SLAM?
Would it be beneficial to offload certain computations, such as using lower-resolution frames or running pose estimation in a separate thread?
5. Redundant Transformations and Unoptimized Mapping
Issue: There are likely redundant applications of transformation matrices in the codebase, particularly because three different scripts are being used (slam.py, pose_estimator.py, and point_cloud_generator.py). Furthermore, the final 3D point cloud is not optimized after the mapping process, leading to inaccuracies.

Potential Causes:

Multiple transformation matrices may be applied unnecessarily, leading to incorrect poses or point cloud positions.
The lack of a final optimization pass (e.g., pose graph optimization) may result in a noisy, inaccurate map.
Relevant Code: Transformation matrices are applied in slam.py, but there may be multiple redundant applications elsewhere:


current_pcd.transform(self.pose)
Questions:

How can I ensure that transformation matrices are only applied once and are not redundantly processed across scripts?
What optimization techniques (e.g., bundle adjustment, pose graph optimization) would help improve the accuracy of the final 3D map?
6. Keyframe Selection and Efficiency
Issue: The current keyframe selection criteria are based on both frame interval and pose changes, but it might not be effectively selecting the optimal keyframes, particularly when the frame_interval is adjusted.

Potential Causes:

The current logic for keyframe selection may not be taking into account the correct thresholds for translation and rotation changes.
The frame_interval may cause too many or too few keyframes to be selected, affecting both the map quality and the system’s performance.
Relevant Code: The is_keyframe function evaluates whether a frame should be a keyframe, but the translation and rotation thresholds might need tuning:


def is_keyframe(self, frame_index, current_pose):
    translation = np.linalg.norm(current_pose[:3, 3] - last_pose[:3, 3])
    rotation_angle = np.arccos((np.trace(current_pose[:3, :3] @ last_pose[:3, :3].T) - 1) / 2)
    return translation > self.min_translation or rotation > self.min_rotation
Questions:

How can I improve the keyframe selection logic to ensure optimal mapping and performance?
Should I adjust the thresholds for translation and rotation, or consider a different approach to keyframe selection?
Final Notes:

Please include any code examples, references to libraries, or suggestions for improving the performance and accuracy of my SLAM system.
If there are more efficient alternatives to any of the existing methods (e.g., better point cloud generation models, pose estimation techniques), please suggest them.
Feel free to raise additional questions or issues you think are relevant based on the code and problems outlined above.


slam.py script is here 

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

        self.frame_interval = frame_interval
        self.keyframes = []
        self.last_keyframe_index = -1
        self.min_keyframe_distance_in_frames = 10  # Minimum number of processed frames between keyframes
        self.min_translation = 2  # Minimum translation to consider a new keyframe (in meters)
        self.min_rotation = 20  # Minimum rotation to consider a new keyframe (in degrees)

    def is_keyframe(self, frame_index, current_pose):
        if not self.keyframes:
            return True  # Always select the first frame as a keyframe

        frames_since_last_keyframe = frame_index - self.last_keyframe_index
        if frames_since_last_keyframe < self.min_keyframe_distance_in_frames:
            return False

        last_keyframe = self.keyframes[-1]
        last_pose = last_keyframe['pose']

        # Calculate translation and rotation between current pose and last keyframe
        translation = np.linalg.norm(current_pose[:3, 3] - last_pose[:3, 3])
        rotation_angle = np.arccos((np.trace(current_pose[:3, :3] @ last_pose[:3, :3].T) - 1) / 2)
        rotation = np.degrees(rotation_angle)

        return translation > self.min_translation or rotation > self.min_rotation

    def add_keyframe(self, frame, depth_map, pose, frame_index):
        self.keyframes.append({
            'frame': frame,
            'depth_map': depth_map,
            'pose': pose,
            'index': frame_index
        })
        self.last_keyframe_index = frame_index
        print(f"Added keyframe at frame {frame_index}")

    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
        processed_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) < 8:
                                print(f"Frame {frame_count}: Insufficient matches found ({len(matches)}). Skipping pose estimation.")
                            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}")
                                    
                                    # Add keyframe if necessary
                                    if self.is_keyframe(frame_count, self.pose):
                                        self.add_keyframe(frame, depth_map, self.pose, processed_frame_count)
                                    
                                    # 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()
                        
                        processed_frame_count +=1

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

point_cloud_generator.py is here

import cv2
import torch
import numpy as np
from PIL import Image
from torchvision import transforms
import open3d as o3d

class PointCloudGenerator:
    def __init__(self, calibration_path='zoe/CalibrationMatrix_college_cpt.npz', transformation=None, voxel_size=0.02):
        # Metric3D setup
        self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
        self.metric3d_model = torch.hub.load('yvanyin/metric3d', 'metric3d_vit_large', pretrain=True)
        self.metric3d_model.to(self.device)
        self.metric3d_model.eval()

        # Load camera parameters from NPZ file
        if calibration_path:
            self.load_camera_parameters(calibration_path)
        else:
            self.set_default_camera_parameters()

       
        # Rotation matrix (keeping this as it was in the original code)
        self.R = np.eye(3)
        if transformation is None:
            self.transformation = np.eye(4)
        else:
            self.transformation = transformation

        self.voxel_size = voxel_size

    def set_default_camera_parameters(self):
        self.FX = 350.0  # Default focal length
        self.FY = 350.0
        self.CX = 160.0  # Default principal point
        self.CY = 120.0
        self.dist_coeffs = np.zeros(5)
        print("Using default camera parameters")

    def load_camera_parameters(self, calibration_path):
        try:
            calibration_data = np.load(calibration_path)
            if 'Camera_matrix' in calibration_data:
                camera_matrix = calibration_data['Camera_matrix']
                self.FX = camera_matrix[0, 0]
                self.FY = camera_matrix[1, 1]
                self.CX = camera_matrix[0, 2]
                self.CY = camera_matrix[1, 2]
                print(f"Camera parameters loaded: FX: {self.FX}, FY: {self.FY}, CX: {self.CX}, CY: {self.CY}")
            else:
                raise KeyError("Camera matrix not found in calibration file")

            if 'distCoeff' in calibration_data:
                self.dist_coeffs = calibration_data['distCoeff'].ravel()
                print(f"Distortion coefficients loaded: {self.dist_coeffs}")
            else:
                print("Distortion coefficients not found, assuming no distortion")
                self.dist_coeffs = np.zeros(5)

        except Exception as e:
            print(f"Error loading camera parameters: {e}")
            self.set_default_camera_parameters()

    

    def process_frame_with_depth(self, frame):
        # Undistort the frame
        h, w = frame.shape[:2]
        new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(
            np.array([[self.FX, 0, self.CX], [0, self.FY, self.CY], [0, 0, 1]]), 
            self.dist_coeffs, (w, h), 1, (w, h)
        )
        undistorted_frame = cv2.undistort(frame, 
                                          np.array([[self.FX, 0, self.CX], [0, self.FY, self.CY], [0, 0, 1]]), 
                                          self.dist_coeffs, 
                                          None, 
                                          new_camera_matrix)

        # RGB to Depth
        color_image = cv2.cvtColor(undistorted_frame, cv2.COLOR_BGR2RGB)
        image_tensor = transforms.ToTensor()(color_image).unsqueeze(0).to(self.device)

        with torch.no_grad():
            pred_depth, confidence, _ = self.metric3d_model.inference({'input': image_tensor})
            depth_map = pred_depth.squeeze().cpu().numpy()

        # Ensure depth_map matches the original frame size
        depth_map = cv2.resize(depth_map, (w, h), interpolation=cv2.INTER_LINEAR)

        # Scale depth map to real-world units (e.g., meters)
        depth_scale = 1.0  # Adjust this value based on your depth model's output scale
        scaled_depth_map = depth_map * depth_scale

        # Create 3D points
        rows, cols = scaled_depth_map.shape
        c, r = np.meshgrid(np.arange(cols), np.arange(rows))
        points = np.zeros((rows * cols, 3), dtype=np.float32)
        points[:, 0] = (c.flatten() - self.CX) * scaled_depth_map.flatten() / self.FX
        points[:, 1] = (r.flatten() - self.CY) * scaled_depth_map.flatten() / self.FY
        points[:, 2] = scaled_depth_map.flatten()

        # Improved filtering
        min_depth = 2.0  # Adjust based on your scene
        max_depth = 20.0  # Adjust based on your scene
        min_brightness = 10  # Adjust to filter out very dark points

        mask = (scaled_depth_map.flatten() > min_depth) & \
               (scaled_depth_map.flatten() < max_depth) & \
               (np.mean(color_image.reshape(-1, 3), axis=1) > min_brightness)

        valid_points = points[mask]
        valid_colors = color_image.reshape(-1, 3)[mask]

        # Apply transformations
        transformed_points = np.dot(valid_points, self.R.T)
        homogeneous_points = np.hstack((valid_points, np.ones((valid_points.shape[0], 1))))
        transformed_points = np.dot(homogeneous_points, self.transformation.T)[:, :3]
        # Downsample
        if self.voxel_size > 0:
            pcd = o3d.geometry.PointCloud()
            pcd.points = o3d.utility.Vector3dVector(transformed_points)
            pcd.colors = o3d.utility.Vector3dVector(valid_colors.astype(np.float64) / 255.0)
            downsampled_pcd = pcd.voxel_down_sample(voxel_size=self.voxel_size)
            transformed_points = np.asarray(downsampled_pcd.points)
            valid_colors = np.asarray(downsampled_pcd.colors) * 255

        # Combine points and colors for final point cloud
        point_cloud = np.hstack((transformed_points, valid_colors.astype(np.uint8)))

        return point_cloud, depth_map


    def save_point_cloud_txt(self, point_cloud, filename):
        np.savetxt(filename, point_cloud, fmt='%.6f %.6f %.6f %d %d %d')

def save_point_cloud_pcd(point_cloud, filename):
    with open(filename, 'w') as f:
        f.write("# .PCD v0.7 - Point Cloud Data file format\n")
        f.write("VERSION 0.7\n")
        f.write("FIELDS x y z rgb\n")
        f.write("SIZE 4 4 4 4\n")
        f.write("TYPE F F F U\n")
        f.write("COUNT 1 1 1 1\n")
        f.write(f"WIDTH {len(point_cloud)}\n")
        f.write("HEIGHT 1\n")
        f.write("VIEWPOINT 0 0 0 1 0 0 0 0\n")
        f.write(f"POINTS {len(point_cloud)}\n")
        f.write("DATA ascii\n")
        for point in point_cloud:
            x, y, z, r, g, b = point
            rgb = int(r) << 16 | int(g) << 8 | int(b)
            f.write(f"{x} {y} {z} {rgb}\n")
            
            
 
pose_estimator.py is here

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