Flying through a tunnel, between buildings, inside a warehouse, or under jammers — a standard situation where GPS locks onto 2 satellites or zero. Without external positioning, the drone experiences drift and impacts within 10 seconds. This is especially critical for expensive commercial aircraft. We solve such tasks on nearly every second project, from bridge inspection to logistics in 10,000 m² warehouses. Our AI-powered autonomous drone navigation combines visual-inertial odometry, collision avoidance, and SLAM. We build turnkey self-navigating drone systems — from sensor selection to fault-tolerant software. We'll evaluate your task within 3 business days.
Why Visual-Inertial Odometry Is Essential for GPS-Denied Flight
VIO fuses camera data with IMU readings. The camera provides visual landmarks, the IMU delivers angular velocities and accelerations. An algorithm (e.g., VINS-Mono or ORB-SLAM3) optimizes the joint error using six-degree-of-freedom (6-DOF) pose estimation via bundle adjustment. In practice, this yields 5–15 cm horizontal accuracy under good lighting. The code below is a simplified illustration of the loop: detect ORB features, match them, recover motion via perspective-n-point (PnP) and essential matrix, and integrate with IMU using an extended Kalman filter.
import numpy as np
import cv2
from scipy.spatial.transform import Rotation
class VisualInertialOdometry:
"""
VINS-Mono / ORB-SLAM3 logic — simplified.
In production we use ready libraries with ROS2 integration.
"""
def __init__(self, camera_matrix: np.ndarray,
imu_noise: dict):
self.K = camera_matrix
self.orb = cv2.ORB_create(nfeatures=500)
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
self.prev_frame = None
self.prev_kp = None
self.prev_desc = None
# State: position + orientation
self.position = np.zeros(3)
self.rotation = np.eye(3)
self.imu_noise = imu_noise
def update(self, frame: np.ndarray,
imu_data: dict) -> dict:
"""Update pose estimate from frame + IMU"""
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
kp, desc = self.orb.detectAndCompute(gray, None)
if self.prev_frame is not None and desc is not None:
matches = self.matcher.match(self.prev_desc, desc)
matches = sorted(matches, key=lambda x: x.distance)[:100]
if len(matches) > 20:
pts1 = np.float32([self.prev_kp[m.queryIdx].pt
for m in matches])
pts2 = np.float32([kp[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 not None:
_, R, t, _ = cv2.recoverPose(E, pts1, pts2, self.K)
# Integrate motion
self.position += self.rotation @ t.flatten()
self.rotation = R @ self.rotation
self.prev_frame = gray
self.prev_kp = kp
self.prev_desc = desc if desc is not None else self.prev_desc
return {
'position': self.position.copy(),
'rotation': self.rotation.copy(),
'tracked_features': len(kp) if kp else 0
}
How to Avoid Obstacles Without LiDAR?
For indoor flights, a stereo camera (Intel RealSense D435) provides a dense depth map at 30 fps and consumes only 2 W. LiDAR (e.g., Velodyne Puck) is more accurate but costs $4000+ and weighs 400 g — not every drone can carry it. Stereo cameras are 10x cheaper and 5x lighter than LiDAR, making them ideal for lightweight drones. Our stack uses stereo as the base sensor and optionally adds a ToF camera for outdoor use. The algorithm below divides the field of view into 5 sectors and computes the minimum distance in each — enough for collision avoidance at speeds up to 2 m/s.
| Sensor | Depth Resolution | Range | Power | Weight | Cost |
|---|---|---|---|---|---|
| Intel RealSense D435 | 1280x720 px | up to 10 m | 2 W | 72 g | $300 |
| Velodyne Puck (LiDAR) | 360° x 30° | up to 100 m | 8 W | 400 g | $4000 |
| ToF camera (L515) | 1024x768 px | up to 9 m | 3 W | 100 g | $350 |
class ObstacleAvoidance:
def __init__(self, depth_camera, safety_distance: float = 1.5):
self.depth_cam = depth_camera
self.safety_dist = safety_distance # meters
self.fov_h = 87 # degrees (RealSense D435)
self.sectors = 5 # divide FOV into 5 sectors
def compute_clear_directions(self,
depth_frame: np.ndarray) -> dict:
"""Find clear flight directions"""
h, w = depth_frame.shape
sector_width = w // self.sectors
clearance = {}
for i in range(self.sectors):
sector = depth_frame[:, i*sector_width:(i+1)*sector_width]
# Ignore zero values (no data)
valid = sector[sector > 0]
if len(valid) == 0:
clearance[i] = float('inf')
continue
# P10 — nearest obstacle bound in sector
min_dist = float(np.percentile(valid, 10)) / 1000.0 # mm -> m
clearance[i] = min_dist
# Direction with maximum clearance
best_sector = max(clearance, key=clearance.get)
angle = (best_sector - self.sectors // 2) * (self.fov_h / self.sectors)
return {
'clearance_by_sector': clearance,
'best_direction_angle': angle,
'is_path_clear': clearance[self.sectors//2] > self.safety_dist
}
Path Planning: 3D Occupancy Grid vs. RRT*
Occupancy grid is a deterministic method: each cell (0.2 m³) is marked as free or occupied. A* guarantees the shortest path in discrete space. For dynamic obstacles, RRT* is better but yields suboptimal path length. Occupancy grid with A* is 2x faster than RRT* for static maps, while RRT* handles dynamic obstacles 30% better. We combine: build the map with a voxel grid, and for real-time replanning use RRT* with a limit of 100 iterations. The code below is classic A* on a sparse grid.
import heapq
class OccupancyGridPlanner:
def __init__(self, resolution: float = 0.2):
self.resolution = resolution # meters per cell
self.grid = {} # 3D sparse grid: (ix, iy, iz) -> occupancy
def update_from_depth(self, depth_frame: np.ndarray,
camera_pose: np.ndarray):
"""Update obstacle map"""
# Convert depth to point cloud
points = self._depth_to_pointcloud(depth_frame)
# Transform to world coordinates
points_world = (camera_pose[:3, :3] @ points.T).T + camera_pose[:3, 3]
for pt in points_world:
ix, iy, iz = (int(pt[0] / self.resolution),
int(pt[1] / self.resolution),
int(pt[2] / self.resolution))
self.grid[(ix, iy, iz)] = 1 # occupied
def astar_3d(self, start: tuple, goal: tuple) -> list:
"""A* in 3D occupancy grid"""
def heuristic(a, b):
return np.sqrt(sum((a[i]-b[i])**2 for i in range(3)))
heap = [(0, start)]
came_from = {start: None}
cost = {start: 0}
while heap:
_, current = heapq.heappop(heap)
if current == goal:
break
for dx, dy, dz in [(1,0,0),(-1,0,0),(0,1,0),
(0,-1,0),(0,0,1),(0,0,-1)]:
neighbor = (current[0]+dx, current[1]+dy, current[2]+dz)
if self.grid.get(neighbor, 0) == 1:
continue # obstacle
new_cost = cost[current] + 1
if neighbor not in cost or new_cost < cost[neighbor]:
cost[neighbor] = new_cost
priority = new_cost + heuristic(neighbor, goal)
heapq.heappush(heap, (priority, neighbor))
came_from[neighbor] = current
# Reconstruct path
path = []
node = goal
while node is not None:
path.append(node)
node = came_from.get(node)
return list(reversed(path))
Case Study: Autonomous Warehouse Inspection (8,000 m²)
Our client, a logistics operator, needed monthly inspection of shelving on 4 floors. Racks up to 12 m high, narrow aisles of 2 m, and complete GPS absence. We developed a drone based on DJI F450 with a custom flight controller equipped with Intel RealSense D435i (depth + IMU). The stack: ROS2 Humble + PX4 Autopilot + a fork of ORB-SLAM3 with an EKF filter for barometer fusion.
Results: localization accuracy ±8 cm horizontal, ±5 cm vertical. Survey speed 0.5 m/s, one row of racks (50 m) passes in 110 seconds. The system has been running faultlessly for over a year — 50+ missions. This solution saves the client $7,000 per month in manual inspection costs.
Implementation Process
More on sensor calibration
Camera and IMU calibration is done using Kalibr or a custom script. Several static poses with different orientations are required. The result is the camera matrix and distortion coefficients, as well as the rotation matrix between camera and IMU. Typical accuracy after calibration: reprojection error < 0.5 pixels.- Scenario analysis: measure lighting, geometry, materials (metal/concrete — different reflectivity). Sensor selection.
- VIO prototyping: calibrate camera+IMU, tune ORB parameters (number of features, threshold).
- Collision avoidance: set safety_distance, integrate depth into flight controller via MAVSDK.
- Planner: choose between grid map and RRT* based on task dynamics.
- Tests: fly in simulation (Gazebo + PX4 SITL), then real flights with a backup remote.
Timeline and Estimated Cost
| Project Type | Duration | Comment |
|---|---|---|
| Basic VIO navigation | 6–10 weeks | Includes calibration, tests, flight controller |
| Full autonomous navigation (VIO + obstacle avoidance + planning) | 3–5 months | + Voxel grid, RRT*, ROS2 integration |
| Certification for commercial flights (optional) | +3–6 months | Depends on regulator (EASA, FAA) |
Cost is calculated individually — depends on scenario complexity and number of test iterations. Typical project cost ranges from $30,000 to $100,000. Projects typically pay back within 6 months due to reduced manual inspection costs. Get a consultation — we'll evaluate your task within 3 days.
What You Get
- Source code of the VIO module with comments (Python/C++).
- Docker image with ROS2 workspace for reproducibility.
- Calibration files for camera and IMU.
- Integration with flight controller (PX4/ArduPilot).
- Setup and operation documentation.
- 2 months of post-deployment support.
Why Choose Our Development?
We've been working on AI drone navigation for over 5 years: 20+ implemented projects for warehouses, mines, bridges, and oil rigs. We use only open-source stacks (ORB-SLAM3, ROS2, PX4) — no vendor lock-in. All solutions are tested in simulation on 100+ scenarios before the first flight. Our solutions typically reduce operational costs by $5,000–15,000 per month. Contact us — we'll tell you which sensors and algorithms fit your specific task.







