3D Mapping & Reconstruction: Building Digital Twins

class SimpleSfM:
    """Simplified Structure from Motion for education."""

    def __init__(self):
        self.camera_poses = []
        self.points_3d = []
        self.observations = []

    def estimate_pose(self, points_2d, points_3d_ref):
        """Estimate camera pose from 2D-3D correspondences."""
        # Simplified PnP (Perspective-n-Point)
        # In practice: cv2.solvePnPRansac()

        if len(points_2d) < 6:
            return None

        # Simplified: assume small motion from reference
        R = np.eye(3)
        t = np.mean(points_3d_ref[:,:2] - points_2d, axis=0)
        t = np.append(t, 0)

        pose = np.eye(4)
        pose[:3, :3] = R
        pose[:3, 3] = t
        return pose

    def triangulate(self, pts1, pts2, P1, P2):
        """Triangulate 3D point from two views."""
        # Simplified triangulation
        # In practice: cv2.triangulatePoints()

        A = np.array([
            pts1[0] * P1[2] - P1[0],
            pts1[1] * P1[2] - P1[1],
            pts2[0] * P2[2] - P2[0],
            pts2[1] * P2[2] - P2[1]
        ])

        _, _, Vt = np.linalg.svd(A)
        point_3d = Vt[-1]
        return point_3d[:3] / point_3d[3]

    def bundle_adjustment(self, camera_params, points, observations, iterations=10):
        """Simplified bundle adjustment."""
        # In practice: use scipy.optimize.least_squares

        for _ in range(iterations):
            # Compute reprojection errors
            total_error = 0
            for obs in observations:
                cam_idx, pt_idx, pixel = obs
                # Project 3D point to 2D
                R = camera_params[cam_idx][:3, :3]
                t = camera_params[cam_idx][:3, 3]
                point_3d = points[pt_idx]

                projected = R @ point_3d + t
                if projected[2] > 0:
                    projected_2d = projected[:2] / projected[2]
                    error = np.linalg.norm(projected_2d - pixel)
                    total_error += error

            # Update parameters (gradient descent - simplified)
            # Real implementation uses Gauss-Newton or Levenberg-Marquardt

        return camera_params, points

# Demo
sfm = SimpleSfM()

# Simulate two views of the same scene
points_3d = np.random.randn(100, 3) * 5  # 100 3D points

# Project to two camera views
P1 = np.array([[1, 0, 0, 0],
               [0, 1, 0, 0],
               [0, 0, 1, 0]], dtype=float)

P2 = np.array([[1, 0, 0, -1],  # Second camera shifted
               [0, 1, 0, 0],
               [0, 0, 1, 0]], dtype=float)

pts1 = (P1[:3,:3] @ points_3d.T + P1[:3,3:]).T
pts1 = pts1[:,:2] / pts1[:,2:3]

pts2 = (P2[:3,:3] @ points_3d.T + P2[:3,3:]).T
pts2 = pts2[:,:2] / pts2[:,2:3]

print(f"Triangulating {len(points_3d)} points from 2 views")
print(f"Point cloud range: X=[{points_3d[:,0].min():.1f}, {points_3d[:,0].max():.1f}]")
print(f"                    Y=[{points_3d[:,1].min():.1f}, {points_3d[:,1].max():.1f}]")
print(f"                    Z=[{points_3d[:,2].min():.1f}, {points_3d[:,2].max():.1f}]")

class OctreeNode:
    """Octree node for 3D spatial partitioning."""

    def __init__(self, center, size, max_points=8, max_depth=8):
        self.center = np.array(center)
        self.size = size
        self.max_points = max_points
        self.max_depth = max_depth
        self.points = []
        self.children = None
        self.is_leaf = True

    def subdivide(self):
        """Split node into 8 children."""
        half = self.size / 2
        self.children = []
        self.is_leaf = False

        for dx in [-1, 1]:
            for dy in [-1, 1]:
                for dz in [-1, 1]:
                    child_center = self.center + np.array([dx, dy, dz]) * half / 2
                    child = OctreeNode(child_center, half,
                                      self.max_points, self.max_depth - 1)
                    self.children.append(child)

    def insert(self, point):
        """Insert a point into the octree."""
        if self.is_leaf:
            self.points.append(point)
            if len(self.points) > self.max_points and self.max_depth > 0:
                self.subdivide()
                for p in self.points:
                    self._insert_into_children(p)
                self.points = []
        else:
            self._insert_into_children(point)

    def _insert_into_children(self, point):
        """Insert point into appropriate child node."""
        idx = 0
        if point[0] > self.center[0]: idx += 1
        if point[1] > self.center[1]: idx += 2
        if point[2] > self.center[2]: idx += 4
        self.children[idx].insert(point)

    def query_range(self, center, radius):
        """Find all points within a spherical range."""
        results = []

        # Check if this node intersects the query sphere
        dist = np.linalg.norm(self.center - center)
        if dist > self.size * 1.73 + radius:  # sqrt(3) * size
            return results

        if self.is_leaf:
            for point in self.points:
                if np.linalg.norm(point - center) <= radius:
                    results.append(point)
        else:
            for child in self.children:
                results.extend(child.query_range(center, radius))

        return results

    def count_points(self):
        """Count total points in this subtree."""
        if self.is_leaf:
            return len(self.points)
        return sum(child.count_points() for child in self.children)

# Demo
octree = OctreeNode(center=[0, 0, 0], size=100, max_points=8, max_depth=8)

# Insert random points
np.random.seed(42)
points = np.random.randn(1000, 3) * 30
for p in points:
    octree.insert(p)

# Query nearby points
query_center = np.array([0, 0, 0])
query_radius = 10.0
nearby = octree.query_range(query_center, query_radius)

print(f"Total points inserted: 1000")
print(f"Points in octree: {octree.count_points()}")
print(f"Points within {query_radius}m of origin: {len(nearby)}")

class DroneSurvey3D:
    """Convert drone survey images to 3D model parameters."""

    def __init__(self, altitude=50, overlap=0.75, gsd=0.05):
        self.altitude = altitude
        self.overlap = overlap
        self.gsd = gsd  # Ground sampling distance (m/pixel)

    def plan_flight(self, area_width, area_length):
        """Plan flight lines for photogrammetry survey."""
        # Calculate ground coverage per image
        sensor_width_m = self.gsd * 4000  # Assume 4000px wide sensor
        ground_width = sensor_width_m
        ground_height = sensor_width_m * 3/4  # 4:3 aspect ratio

        # Calculate flight lines
        spacing = ground_width * (1 - self.overlap)
        n_lines = int(np.ceil(area_width / spacing)) + 1

        # Calculate images per line
        images_per_line = int(np.ceil(area_length / (ground_height * (1 - self.overlap)))) + 1

        total_images = n_lines * images_per_line
        flight_distance = n_lines * area_length + (n_lines - 1) * spacing

        return {
            "flight_lines": n_lines,
            "images_per_line": images_per_line,
            "total_images": total_images,
            "ground_coverage": f"{ground_width:.1f}m × {ground_height:.1f}m",
            "spacing": f"{spacing:.1f}m",
            "flight_distance_km": f"{flight_distance/1000:.2f}km",
            "estimated_time_min": f"{flight_distance / 5 / 60:.1f}"  # 5 m/s speed
        }

    def estimate_processing(self, n_images, resolution=(4000, 3000)):
        """Estimate processing requirements."""
        pixels_per_image = resolution[0] * resolution[1]
        total_pixels = pixels_per_image * n_images

        # Rough estimates based on pixel count
        ram_gb = total_pixels / 1e9 * 4  # ~4 bytes per pixel in memory
        disk_gb = n_images * 10  # ~10MB per image (JPEG)
        processing_hours = n_images * 0.05  # ~3 min per image

        return {
            "total_pixels": f"{total_pixels/1e6:.1f}MP total",
            "estimated_ram": f"{min(ram_gb, 64):.0f}GB",
            "storage_needed": f"{disk_gb:.1f}GB",
            "processing_time": f"{processing_hours:.1f} hours",
            "recommended Software": "Pix4D, Agisoft, OpenDroneMap"
        }

# Demo
survey = DroneSurvey3D(altitude=50, overlap=0.75, gsd=0.05)

# Plan survey for a 200m × 300m area
flight_plan = survey.plan_flight(area_width=200, area_length=300)
print("=== Flight Plan ===")
for key, value in flight_plan.items():
    print(f"  {key}: {value}")

processing = survey.estimate_processing(n_images=flight_plan["total_images"])
print("\n=== Processing Requirements ===")
for key, value in processing.items():
    print(f"  {key}: {value}")