### End-to-End Data Flow Example

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/Datasets.md

A complete example demonstrating the data flow from initial setup to preparing data for network input. This includes rendering, batch creation, and transformation steps.

```python
import numpy as np
import torch
from learning.datasets.h5_dataset import ScoreMultiPairH5Dataset
from learning.datasets.pose_dataset import BatchPoseData
from Utils import nvdiffrast_render, make_mesh_tensors

# Setup
cfg = {'input_resize': [224, 224], 'use_normal': False, 'crop_ratio': 1.2}
dataset = ScoreMultiPairH5Dataset(cfg=cfg, h5_file='', mode='test')
mesh_tensors = make_mesh_tensors(mesh)

# Input
rgb = np.random.randint(0, 256, (480, 640, 3), dtype=np.uint8)
depth = np.random.rand(480, 640) * 2
K = np.array([[640, 0, 320], [0, 640, 240], [0, 0, 1]], dtype=np.float32)
poses = np.tile(np.eye(4)[None], (10, 1, 1))  # 10 hypotheses

# Step 1: Render
rgb_rendered, depth_rendered, _ = nvdiffrast_render(
    K=K, H=480, W=640,
    ob_in_cams=torch.as_tensor(poses, device='cuda', dtype=torch.float),
    mesh_tensors=mesh_tensors
)

# Step 2: Make batch
batch = BatchPoseData(
    rgbAs=rgb_rendered,
    rgbBs=torch.as_tensor(rgb, device='cuda', dtype=torch.float)[None].expand(10, -1, -1, -1),
    depthAs=depth_rendered[..., None],
    depthBs=torch.as_tensor(depth, device='cuda', dtype=torch.float)[None, None].expand(10, -1, -1, -1),
    poseA=torch.as_tensor(poses, device='cuda', dtype=torch.float),
    Ks=torch.as_tensor(K, device='cuda', dtype=torch.float)[None].expand(10, -1, -1),
    mesh_diameters=torch.ones(10, device='cuda', dtype=torch.float) * 0.15
)

# Step 3: Transform
batch = dataset.transform_batch(batch, H_ori=480, W_ori=640)

# Step 4: Now ready for network
# model(batch.rgbAs, batch.rgbBs, L=len(batch))
```

--------------------------------

### Environment Setup with Conda

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Set up your environment using Conda by creating a new environment, installing PyTorch with CUDA support, and then installing PyTorch3D and NVDiffRast from source. Finally, install remaining dependencies and build C++ extensions.

```bash
# 1. Create conda environment
conda env create -f environment.yml
conda activate foundationpose

# 2. Install PyTorch with CUDA support
# Replace cu124 with your CUDA version (cu118, cu121, cu124, etc.)
python -m pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu124

# 3. Install PyTorch3D and NVDiffRast from source
export CUDA_HOME=/usr/local/cuda   # or /usr/local/cuda-12.4, etc.
export PATH="$CUDA_HOME/bin:$PATH"

python -m pip install --no-build-isolation "git+https://github.com/facebookresearch/pytorch3d.git"
python -m pip install --no-build-isolation "git+https://github.com/NVlabs/nvdiffrast.git"

# 4. Install remaining dependencies and build C++ extensions
python -m pip install -r requirements.txt
bash build_all_conda.sh
```

--------------------------------

### Install Project Requirements

Source: https://github.com/nvlabs/foundationpose/blob/main/requirements.txt

Install the remaining project dependencies by referencing the requirements.txt file.

```bash
pip install -r requirements.txt
```

--------------------------------

### FoundationPose Initialization Example

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/FoundationPose.md

Example demonstrating how to load a mesh, initialize ScorePredictor and PoseRefinePredictor, and then create an instance of the FoundationPose estimator with these components.

```python
import trimesh
from estimater import FoundationPose, ScorePredictor, PoseRefinePredictor
import numpy as np

# Load mesh
mesh = trimesh.load('model.obj')

# Initialize predictors
scorer = ScorePredictor()
refiner = PoseRefinePredictor()

# Create estimator
est = FoundationPose(
    model_pts=mesh.vertices,
    model_normals=mesh.vertex_normals,
    mesh=mesh,
    scorer=scorer,
    refiner=refiner,
    debug=1,
    debug_dir='./debug'
)
```

--------------------------------

### Check Core Dependencies Installation

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Verify that essential libraries like PyTorch, Trimesh, Nvdiffrast, Open3D, and Kornia are installed correctly by running a simple Python script.

```bash
python -c "
import torch
import trimesh
import nvdiffrast.torch as dr
import open3d
import kornia
print('✓ Core dependencies OK')
"
```

--------------------------------

### Install PyTorch3D and NVDiffRast from Source

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Installs PyTorch3D and NVDiffRast by compiling them from their GitHub repositories. Requires the CUDA toolkit to be installed and CUDA_HOME environment variable to be set correctly. The --no-build-isolation flag is crucial for the build process to access the already installed PyTorch.

```bash
export CUDA_HOME=/usr/local/cuda   # or e.g. /usr/local/cuda-12.8
export PATH="$CUDA_HOME/bin:$PATH"
python -m pip install --no-build-isolation "git+https://github.com/facebookresearch/pytorch3d.git"
python -m pip install --no-build-isolation "git+https://github.com/NVlabs/nvdiffrast.git"
```

--------------------------------

### CMake Project Setup

Source: https://github.com/nvlabs/foundationpose/blob/main/mycpp/CMakeLists.txt

Initializes CMake version and project name.

```cmake
cmake_minimum_required(VERSION 3.15)
project(mycpp)
```

--------------------------------

### Install Remaining Dependencies and Build Extension

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Installs Python dependencies from 'requirements.txt' and builds the 'mycpp' extension using a provided bash script. The 'mycuda' step is optional and related to specific model-free/NeRF paths.

```bash
python -m pip install -r requirements.txt
bash build_all_conda.sh
```

--------------------------------

### FoundationPose Register Method Example

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/FoundationPose.md

Example showing how to load RGB, depth, and mask data, define camera intrinsics (K), and then call the `register` method to estimate the object's 6D pose. The output pose is a 4x4 transformation matrix.

```python
import cv2
import numpy as np

# Load input data
rgb = cv2.imread('image.png')[..., ::-1]  # BGR to RGB
depth = cv2.imread('depth.png', cv2.IMREAD_ANYDEPTH) / 1000.0  # mm to m
mask = (rgb.sum(axis=2) > 0).astype(np.uint8)
K = np.array([[640, 0, 320], [0, 640, 240], [0, 0, 1]], dtype=np.float32)

# Estimate pose
pose = est.register(K=K, rgb=rgb, depth=depth, ob_mask=mask, iteration=5)
print(f"Estimated pose shape: {pose.shape}")  # (4, 4)
```

--------------------------------

### Docker Setup and Execution

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Set up your environment using Docker by pulling the official image, running the container with GPU support, and building extensions. Subsequent executions can directly use the container without rebuilding.

```bash
# 1. Pull official image
docker pull wenbowen123/foundationpose
docker tag wenbowen123/foundationpose foundationpose

# 2. Run container with GPU
bash docker/run_container.sh

# 3. Inside container, build extensions (first time only)
bash build_all.sh

# 4. Later executions (no rebuild needed)
docker exec -it foundationpose bash
```

--------------------------------

### Usage Example: Loading BOP Format Data

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Shows how to load data from a dataset in the BOP format using the get_bop_reader function. This example iterates through the first 10 frames and prints the shape of the RGB image.

```python
# Load BOP format
bop_reader = get_bop_reader('/path/to/YCB_Video/test/0000')
for i in range(min(10, len(bop_reader))):
    rgb = bop_reader.get_color(i)
    print(f"BOP Frame {i}: {rgb.shape}")
```

--------------------------------

### Install GPU Extensions

Source: https://github.com/nvlabs/foundationpose/blob/main/requirements.txt

Install GPU extensions for PyTorch3D and nvdiffrast. Ensure nvcc is available and CUDA_HOME is set to your CUDA toolkit path.

```bash
pip install --no-build-isolation git+https://github.com/facebookresearch/pytorch3d.git
```

```bash
pip install --no-build-isolation git+https://github.com/NVlabs/nvdiffrast.git
```

--------------------------------

### Install PyTorch with CUDA Support

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Installs PyTorch, Torchvision, and Torchaudio using pip, specifying a CUDA build index URL that matches the user's machine. Ensure the index URL corresponds to the correct CUDA version (e.g., cu124).

```bash
python -m pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu124
```

--------------------------------

### Install FoundationPose weights

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/errors.md

This bash command downloads the necessary pre-trained model weights for ScorePredictor or PoseRefinePredictor, resolving FileNotFoundError.

```bash
bash scripts/download_models.sh
```

--------------------------------

### Pose Refinement Example

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/Predictors.md

Example demonstrating how to use the PoseRefinePredictor to refine pose estimates. It includes converting depth to a 3D coordinate map and calling the predict method.

```python
from learning.training.predict_pose_refine import PoseRefinePredictor
from Utils import depth2xyzmap

refiner = PoseRefinePredictor()

# Convert depth to 3D coordinates
xyz_map = depth2xyzmap(depth, K)

# Refine poses
refined_poses, vis = refiner.predict(
    rgb=rgb,
    depth=depth,
    K=K,
    ob_in_cams=poses,
    xyz_map=xyz_map,
    mesh=mesh,
    mesh_diameter=0.15,
    iteration=5,
    get_vis=True
)

print(f"Refined poses shape: {refined_poses.shape}")
```

--------------------------------

### Create BatchPoseData Example

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/types.md

Demonstrates how to instantiate and populate a BatchPoseData object using PyTorch tensors. Useful for preparing data for model training or inference.

```python
import torch
from learning.datasets.pose_dataset import BatchPoseData

batch_size = 16
height, width = 224, 224

batch = BatchPoseData(
    rgbAs=torch.rand(batch_size, 3, height, width),
    rgbBs=torch.rand(batch_size, 3, height, width),
    xyz_mapAs=torch.randn(batch_size, 3, height, width),
    xyz_mapBs=torch.randn(batch_size, 3, height, width),
    poseA=torch.eye(4)[None].expand(batch_size, -1, -1),
    Ks=torch.eye(3)[None].expand(batch_size, -1, -1),
    mesh_diameters=torch.ones(batch_size) * 0.15
)

print(f"Batch size: {len(batch)}")
print(f"RGB shape: {batch.rgbAs.shape}")
```

--------------------------------

### Resolve Permission Denied Building Extensions

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Fix 'Permission denied' errors during extension building by running the pip install command with the `--user` flag, which installs packages in the user's home directory.

```bash
# Run with user flag
python -m pip install --user -r requirements.txt
```

--------------------------------

### Install PyTorch with CUDA

Source: https://github.com/nvlabs/foundationpose/blob/main/requirements.txt

Install PyTorch with a CUDA build that matches your NVIDIA driver. cu124 is compatible with most recent NVIDIA GPUs.

```bash
pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu124
```

--------------------------------

### Docker Image Pull and Tag

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Pulls the FoundationPose Docker image and tags it for use. This is the recommended environment setup.

```bash
cd docker/
docker pull wenbowen123/foundationpose && docker tag wenbowen123/foundationpose foundationpose  # Or to build from scratch: docker build --network host -t foundationpose .
```

--------------------------------

### Install Dependencies with Conda

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Use this command to install project dependencies using conda, reading package names from the requirements.txt file. This method helps avoid permission issues.

```bash
conda install -c conda-forge $(cat requirements.txt | tr '\n' ' ')
```

--------------------------------

### Usage Example: Loading Unstructured RGB-D Video

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Demonstrates how to load and iterate through an unstructured RGB-D video using YcbineoatReader. Shows how to access color, depth, mask, and ground truth pose for each frame.

```python
import numpy as np
from datareader import YcbineoatReader, get_bop_reader

# Load unstructured RGB-D video
reader = YcbineoatReader('demo_data/mustard0', shorter_side=480)

for i in range(len(reader)):
    rgb = reader.get_color(i)
    depth = reader.get_depth(i)
    mask = reader.get_mask(i)
    K = reader.K
    
    print(f"Frame {i}: RGB {rgb.shape}, Depth {depth.shape}, Mask {mask.shape}")
    
    if i == 0:
        gt_pose = reader.get_gt_pose(i)
        if gt_pose is not None:
            print(f"GT Pose:\n{gt_pose}")
```

--------------------------------

### FoundationPose Constructor

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/FoundationPose.md

Initializes the FoundationPose estimator with model data and optional predictors. It handles the setup for pose estimation and tracking.

```APIDOC
## FoundationPose Constructor

### Description
Initializes the FoundationPose estimator with model data and optional predictors. It handles the setup for pose estimation and tracking.

### Parameters

#### Required Parameters
- **model_pts** (np.ndarray) - 3D model points (N, 3) or mesh vertices. Must be used with `mesh` parameter.
- **model_normals** (np.ndarray) - Surface normals for model points (N, 3). Should match point count.

#### Optional Parameters
- **symmetry_tfs** (np.ndarray) - Symmetry transformations (K, 4, 4). Set to None for no symmetries; creates identity transform. Used for pose deduplication. (Default: None)
- **mesh** (trimesh.Trimesh) - Trimesh object representing the 3D model. Required for rendering and visualization. (Default: None)
- **scorer** (ScorePredictor) - Pre-trained pose scoring network. Automatically instantiated if None. (Default: None)
- **refiner** (PoseRefinePredictor) - Pre-trained pose refinement network. Automatically instantiated if None. (Default: None)
- **glctx** (nvdiffrast context) - GPU rasterization context. Automatically created if None. (Default: None)
- **debug** (int) - Debug verbosity level (0=silent, 1=minimal, 2=detailed, 3=full). (Default: 0)
- **debug_dir** (str) - Directory for debug outputs and visualizations. (Default: '/home/bowen/debug/novel_pose_debug/')

### Initialization Example
```python
import trimesh
from estimater import FoundationPose, ScorePredictor, PoseRefinePredictor
import numpy as np

# Load mesh
mesh = trimesh.load('model.obj')

# Initialize predictors
scorer = ScorePredictor()
refiner = PoseRefinePredictor()

# Create estimator
est = FoundationPose(
    model_pts=mesh.vertices,
    model_normals=mesh.vertex_normals,
    mesh=mesh,
    scorer=scorer,
    refiner=refiner,
    debug=1,
    debug_dir='./debug'
)
```
```

--------------------------------

### Get BOP Video Directories

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Retrieves a sorted list of all scene directories for a specified BOP dataset. This function requires the 'BOP_DIR' environment variable to be set.

```python
def get_bop_video_dirs(dataset)

```

--------------------------------

### Check CUDA Availability and Device Information

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Verify if CUDA is available, get the name of the default CUDA device, and retrieve its compute capability. This is crucial for GPU-accelerated operations.

```bash
python -c "
import torch
print(f'CUDA available: {torch.cuda.is_available()}')
print(f'CUDA device: {torch.cuda.get_device_name(0)}')
print(f'CUDA capability: {torch.cuda.get_device_capability(0)}')
"
```

--------------------------------

### BatchPoseData select_by_indices Method Example

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/Datasets.md

Demonstrates how to select a subset of poses from a BatchPoseData object using indices. This is useful for iterative filtering, such as in a binary tournament.

```python
# Start with 512 poses
batch = make_crop_data_batch(...)  # BatchPoseData with B=512

# Binary tournament: compare pairs, keep winners
while len(global_ids) > 1:
    # Score current batch
    ids, scores = model(batch)
    
    # Keep winners
    batch = batch.select_by_indices(ids)
    global_ids = global_ids[ids]
```

--------------------------------

### Basic FoundationPose Usage

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/README.md

Demonstrates how to load a mesh, create a FoundationPose estimator, and perform pose registration and tracking on image data. Requires trimesh, cv2, and numpy.

```python
import trimesh
import cv2
import numpy as np
from estimater import FoundationPose, ScorePredictor, PoseRefinePredictor

# Load mesh
mesh = trimesh.load('model.obj')

# Create estimator
scorer = ScorePredictor()
refiner = PoseRefinePredictor()
est = FoundationPose(
    model_pts=mesh.vertices,
    model_normals=mesh.vertex_normals,
    mesh=mesh,
    scorer=scorer,
    refiner=refiner,
    debug=1
)

# Load image
rgb = cv2.imread('image.png')[..., ::-1]  # BGR → RGB
depth = cv2.imread('depth.png', cv2.IMREAD_ANYDEPTH) / 1000.0  # mm → m
mask = (rgb.sum(axis=2) > 0).astype(np.uint8)
K = np.array([[640, 0, 320], [0, 640, 240], [0, 0, 1]], dtype=np.float32)

# Register pose on first frame
pose = est.register(K=K, rgb=rgb, depth=depth, ob_mask=mask, iteration=5)
print(f"Estimated pose:\n{pose}")

# Track on subsequent frames
rgb2 = cv2.imread('image2.png')[..., ::-1]
depth2 = cv2.imread('depth2.png', cv2.IMREAD_ANYDEPTH) / 1000.0
pose2 = est.track_one(rgb=rgb2, depth=depth2, K=K, iteration=2)
```

--------------------------------

### Run FoundationPose Demo

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Execute the demo script to run pose estimation and tracking on provided demo data. Specify the mesh file, test scene directory, and refinement iterations. Debugging output can be directed to a specified directory.

```bash
cd foundationpose

# Run pose estimation and tracking on demo data
python run_demo.py \
    --mesh_file demo_data/mustard0/mesh/textured_simple.obj \
    --test_scene_dir demo_data/mustard0 \
    --est_refine_iter 5 \
    --track_refine_iter 2 \
    --debug 1 \
    --debug_dir ./debug

# Output saved to ./debug/
```

--------------------------------

### Run Model-Based Demo on LINEMOD Dataset

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Executes the model-based version of the demo on the LINEMOD dataset. Requires the LINEMOD dataset to be downloaded and the path specified using the --linemod_dir argument. Results are saved in the 'debug' folder.

```python
python run_linemod.py --linemod_dir /mnt/9a72c439-d0a7-45e8-8d20-d7a235d02763/DATASET/LINEMOD --use_reconstructed_mesh 0
```

--------------------------------

### BundleSDF Bibtex Entry

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Bibtex entry for the BundleSDF paper. Cite this when using the model-free setup.

```bibtex
@InProceedings{bundlesdfwen2023,
author        = {Bowen Wen and Jonathan Tremblay and Valts Blukis and Stephen Tyree and Thomas M"{u}ller and Alex Evans and Dieter Fox and Jan Kautz and Stan Birchfield},
title         = {{BundleSDF}: {N}eural 6-{DoF} Tracking and {3D} Reconstruction of Unknown Objects},
booktitle     = {CVPR},
year          = {2023},
}
```

--------------------------------

### Initialize FoundationPose Estimator

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Initialize the FoundationPose estimator by loading a 3D model using trimesh, creating predictor networks for scoring and refinement, and then instantiating the FoundationPose class with the model data and predictors.

```python
import trimesh
import numpy as np
from estimater import FoundationPose, ScorePredictor, PoseRefinePredictor

# 1. Load 3D model
mesh = trimesh.load('model.obj')

# 2. Create predictor networks
scorer = ScorePredictor(amp=True)
refiner = PoseRefinePredictor()

# 3. Initialize estimator
est = FoundationPose(
    model_pts=mesh.vertices,
    model_normals=mesh.vertex_normals,
    mesh=mesh,
    scorer=scorer,
    refiner=refiner,
    debug=0,
    debug_dir='./debug'
)

print("✓ FoundationPose initialized successfully")
```

--------------------------------

### FoundationPose Bibtex Entry

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Bibtex entry for the FoundationPose paper. Cite this when using the model-based setup.

```bibtex
@InProceedings{foundationposewen2024,
author        = {Bowen Wen, Wei Yang, Jan Kautz, Stan Birchfield},
title         = {{FoundationPose}: Unified 6D Pose Estimation and Tracking of Novel Objects},
booktitle     = {CVPR},
year          = {2024},
}
```

--------------------------------

### YcbineoatReader Get Frame Count

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Returns the total number of frames available in the video sequence.

```python
def __len__(self) -> int
```

--------------------------------

### Download and Extract Required Models

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Download the necessary pre-trained models for FoundationPose using `gdown` and extract them into the weights directory. Verify the directory structure after extraction.

```bash
# 1. Create weights directory
mkdir -p foundationpose/weights

# 2. Download from Google Drive (manual or using gdown)
pip install gdown

# Refiner
gdown 1psfXXXXXXXXXXXXXX -O foundationpose/weights/refiner.zip
unzip foundationpose/weights/refiner.zip -d foundationpose/weights/

# Scorer
gdown 1qsfXXXXXXXXXXXXXX -O foundationpose/weights/scorer.zip
unzip foundationpose/weights/scorer.zip -d foundationpose/weights/

# 3. Verify structure
tree foundationpose/weights/ -L 2
# Should show:
# weights/
#  2023-10-28-18-33-37/
#      model_best.pth
#      config.yml
#  2024-01-11-20-02-45/
#      model_best.pth
#      config.yml
```

--------------------------------

### Include Directories

Source: https://github.com/nvlabs/foundationpose/blob/main/mycpp/CMakeLists.txt

Adds project include directories and Boost include directories.

```cmake
include_directories(
  include
  ${BLAS_INCLUDE_DIR}
)
```

--------------------------------

### Run Model-Based Demo on YCB-Video Dataset

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Executes the model-based version of the demo on the YCB-Video dataset. Requires the YCB-Video dataset to be downloaded and the path specified using the --ycbv_dir argument. Results are saved in the 'debug' folder.

```python
python run_ycb_video.py --ycbv_dir /mnt/9a72c439-d0a7-45e8-8d20-d7a235d02763/DATASET/YCB_Video --use_reconstructed_mesh 0
```

--------------------------------

### YcbineoatReader Get Video Name

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Method to retrieve the name of the video, which corresponds to the last component of the directory path.

```python
def get_video_name(self) -> str
```

--------------------------------

### Symlink Pre-Downloaded Models

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

If your models are stored in a different location, create a symbolic link from the expected `foundationpose/weights` directory to your model directory.

```bash
ln -s /path/to/weights foundationpose/weights
```

--------------------------------

### Set Environment Variables

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/configuration.md

Set environment variables for dataset directories and CUDA installation. These are required for specific reader functions and extension compilation.

```bash
export BOP_DIR=/path/to/bop
export YCB_VIDEO_DIR=/path/to/YCB_Video
export CUDA_HOME=/usr/local/cuda
```

--------------------------------

### Loading BOP Benchmark Data

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/README.md

Demonstrates loading data from the BOP benchmark using the get_bop_reader function. Requires the path to the benchmark dataset.

```python
from datareader import YcbineoatReader, get_bop_reader

# Load BOP benchmark
reader = get_bop_reader('/path/to/YCB_Video/test/0000')
for i in range(len(reader)):
    rgb = reader.get_color(i)
    depth = reader.get_depth(i)
```

--------------------------------

### Run Model-Based Demo

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Executes the main demo script for model-based pose estimation. The script automatically handles pose estimation on the first frame and switches to tracking mode for subsequent frames. Visualizations are saved to 'debug_dir'.

```python
python run_demo.py
```

--------------------------------

### Create and Activate Conda Environment

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Creates a new Conda environment from an 'environment.yml' file and activates it. This is the initial step for setting up the project locally.

```bash
conda env create -f environment.yml
conda activate foundationpose
```

--------------------------------

### YcbineoatReader Initialization

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/configuration.md

Initializes the YcbineoatReader with video directory and optional downscaling or shorter side resizing. Specifies the maximum valid depth.

```python
reader = YcbineoatReader(
    video_dir: str,
    downscale: float = 1,
    shorter_side: int | None = None,
    zfar: float = np.inf
)
```

--------------------------------

### get_bop_reader

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Factory function to get an appropriate reader for a BOP benchmark dataset based on the provided directory path. It inspects the path for keywords to determine the correct reader subclass.

```APIDOC
## get_bop_reader

### Description
Get appropriate reader for BOP benchmark dataset based on directory path.

### Signature
```python
def get_bop_reader(video_dir, zfar=np.inf)
```

### Parameters
#### Path Parameters
- **video_dir** (str) - Required - Path to video/scene directory. String matching determines reader type.
- **zfar** (float) - Optional - Far clipping plane for depth. Invalid depths beyond zfar are zeroed. Default: `np.inf`

### Returns
One of: LinemodOcclusionReader, YcbVideoReader, TlessReader, HomebrewedReader, TudlReader, IcbinReader, ItoddReader

### Behavior
- Inspects video_dir path for keywords (e.g., 'ycbv', 'lmo', 'tless')
- Returns appropriate reader subclass

### Example
```python
from datareader import get_bop_reader

reader = get_bop_reader('/path/to/YCB_Video/test/0000')
for i in range(len(reader)):
    rgb = reader.get_color(i)
    depth = reader.get_depth(i)
```
```

--------------------------------

### Get Transformation to Centered Mesh Frame

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/FoundationPose.md

Retrieves the transformation matrix used to convert from the original mesh frame to the internally used centered mesh frame. This transformation translates by the negative model center.

```python
def get_tf_to_centered_mesh(self)
```

--------------------------------

### BOP Base Reader Initialization

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/configuration.md

Initializes a base reader for BOP datasets, requiring the video directory and an optional far clipping plane.

```python
reader = BopBaseReader(video_dir, zfar=np.inf)
```

--------------------------------

### Set Environment Variables for Dataset Roots and CUDA

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Configure environment variables for dataset directories (BOP, YCB-Video) and CUDA toolkit before importing FoundationPose. These are essential for benchmark evaluation and compilation.

```bash
# BOP dataset root (for benchmark evaluation)
export BOP_DIR=/path/to/bop_datasets

# YCB-Video dataset root
export YCB_VIDEO_DIR=/path/to/YCB_Video

# CUDA toolkit (for compilation)
export CUDA_HOME=/usr/local/cuda
```

--------------------------------

### Get BOP Benchmark Reader

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Factory function to obtain the appropriate data reader for a given BOP benchmark dataset directory. It inspects the directory path for keywords to determine the dataset type and returns a corresponding reader instance.

```python
from datareader import get_bop_reader

reader = get_bop_reader('/path/to/YCB_Video/test/0000')
for i in range(len(reader)):
    rgb = reader.get_color(i)
    depth = reader.get_depth(i)
```

--------------------------------

### YcbineoatReader Get Occlusion Mask

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Retrieves the occlusion mask for a specific frame, indicating areas occluded by hands or people if present. The mask is a binary NumPy array (H, W) where 0 means not occluded and 1 means occluded.

```python
def get_occ_mask(self, i: int) -> np.ndarray
```

--------------------------------

### Loading Official Weights and Config

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/errors.md

Ensure model configuration parameters match the checkpoint to avoid NaN values or convergence failures. Always load from official weights.

```python
# Always use matching checkpoint and config
# Don't manually load alternative checkpoints
# Load from official weights directory only

from learning.training.predict_score import ScorePredictor
scorer = ScorePredictor()  # Loads official weights automatically

```

--------------------------------

### Run FoundationPose Container

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Executes the FoundationPose Docker container. Assumes the image has already been pulled and tagged.

```bash
bash docker/run_container.sh
```

--------------------------------

### Visualizing Posed 3D Box

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/README.md

Illustrates how to draw a 3D bounding box and coordinate axes onto an RGB image given camera parameters and pose information. Uses utility functions from the Utils module.

```python
from Utils import draw_posed_3d_box, draw_xyz_axis, make_grid_image

# Draw 3D boxbox = np.array([[-0.1, -0.1, -0.1], [0.1, 0.1, 0.2]])
vis = draw_posed_3d_box(K, rgb, pose, bbox)

# Draw coordinate axes
vis = draw_xyz_axis(vis, pose, scale=0.1, K=K, is_input_rgb=True)

# Show result
cv2.imshow('result', vis[..., ::-1])  # RGB → BGR
cv2.waitKey(0)
```

--------------------------------

### Source File Discovery

Source: https://github.com/nvlabs/foundationpose/blob/main/mycpp/CMakeLists.txt

Dynamically finds all .cpp files in the src directory.

```cmake
file(GLOB MY_SRC ${PROJECT_SOURCE_DIR}/src/*.cpp)
```

--------------------------------

### Verify Model Loading

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Confirm that the FoundationPose, ScorePredictor, and PoseRefinePredictor models can be loaded successfully. This checks if the model files are accessible and correctly formatted.

```bash
python -c "
from estimater import FoundationPose
from learning.training.predict_score import ScorePredictor
from learning.training.predict_pose_refine import PoseRefinePredictor

# Verify models load
scorer = ScorePredictor()
refiner = PoseRefinePredictor()
print('✓ Models loaded successfully')
"
```

--------------------------------

### Common BOP Reader Methods

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

These are the standard methods implemented by all BOP readers for accessing video data, including RGB, depth, masks, and ground truth poses.

```python
def get_video_name(self) -> str
def __len__(self) -> int
def get_color(self, i: int) -> np.ndarray  # (H, W, 3) uint8
def get_depth(self, i: int) -> np.ndarray  # (H, W) float32, meters
def get_mask(self, i: int) -> np.ndarray   # (H, W) uint8 binary
def get_xyz_map(self, i: int) -> np.ndarray  # (H, W, 3) float32
def get_gt_pose(self, i: int) -> np.ndarray  # (4, 4) or None
def get_cam_K(self) -> np.ndarray  # (3, 3) camera intrinsics
```

--------------------------------

### Configure GPU Device for Rendering Context

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Set the default GPU and create a rendering context on a specific CUDA device. Models can be moved to different GPUs later using the `to_device` method.

```python
import torch
import nvdiffrast.torch as dr

# Set default GPU
torch.cuda.set_device(0)

# Create rendering context on specific device
glctx = dr.RasterizeCudaContext(device='cuda:0')

est = FoundationPose(
    ...,
    glctx=glctx
)

# Move models to different GPU later
est.to_device('cuda:1')
```

--------------------------------

### Finding Dependencies

Source: https://github.com/nvlabs/foundationpose/blob/main/mycpp/CMakeLists.txt

Locates required external libraries: Boost, OpenMP, Eigen3, and pybind11.

```cmake
find_package(Boost REQUIRED COMPONENTS system program_options)
find_package(OpenMP REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(pybind11 REQUIRED)
```

--------------------------------

### Configure Logging Output Format

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/UtilityFunctions.md

Configures the format and level of logging output. Defaults to INFO level.

```python
def set_logging_format(level=logging.INFO)
```

--------------------------------

### Initialize YcbineoatReader

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Constructor for the YcbineoatReader, designed for unstructured RGB-D video directories in YCB-IneoAT format. It allows for optional downscaling or resizing of images and setting a far clipping plane for depth data.

```python
YcbineoatReader(video_dir, downscale=1, shorter_side=None, zfar=np.inf)
```

--------------------------------

### FoundationPose Constructor Parameters

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/FoundationPose.md

Defines the parameters for initializing the FoundationPose estimator, including model points, normals, symmetry transformations, mesh, scoring and refinement predictors, and debug settings.

```python
FoundationPose(
    model_pts,
    model_normals,
    symmetry_tfs=None,
    mesh=None,
    scorer: ScorePredictor = None,
    refiner: PoseRefinePredictor = None,
    glctx=None,
    debug=0,
    debug_dir='/home/bowen/debug/novel_pose_debug/'
)
```

--------------------------------

### Data Preparation and Iterative Refinement

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/Datasets.md

Illustrates the process of preparing input data for pose prediction and iteratively refining poses using a model. This involves creating data batches, transforming them, and feeding them to the model in a loop.

```python
pose_data = make_crop_data_batch(
    self.cfg.input_resize,
    ob_in_cams,
    mesh,
    rgb,
    depth,
    K,
    crop_ratio=self.cfg['crop_ratio'],
    xyz_map=xyz_map,  # Pre-computed from depth
    normal_map=normal_map,
    mesh_diameter=mesh_diameter,
    cfg=self.cfg,
    glctx=glctx,
    mesh_tensors=mesh_tensors,
    dataset=self.dataset  # PoseRefinePairH5Dataset
)

# Transform batch
pose_data = self.dataset.transform_batch(
    pose_data,
    H_ori=H,
    W_ori=W
)

# Feed to network iteratively
for iteration in range(iterations):
    output = self.model(pose_data.rgbAs, pose_data.rgbBs, ...)
    trans_delta = output['trans']  # Predicted translation change
    rot_delta = output['rot']      # Predicted rotation change
    
    # Update poses
    poses = apply_delta(poses, trans_delta, rot_delta)
    
    # Rerender at updated poses
    pose_data = re_render_and_crop(...)
    pose_data = self.dataset.transform_batch(pose_data, H_ori, W_ori)
```

--------------------------------

### Initialize FoundationPose with Rotational Symmetry

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Specify symmetry transformations for objects with rotational symmetry when initializing FoundationPose. This involves defining symmetry as 4x4 matrices and passing them to the estimator.

```python
import numpy as np
from transformations import rotation_matrix

# Define symmetries as 4x4 matrices
symmetry_tfs = []

# For 4-fold rotational symmetry around Z axis:
for angle in [0, 90, 180, 270]:
    rad = np.deg2rad(angle)
    R = rotation_matrix(rad, [0, 0, 1])[:3, :3]
    tf = np.eye(4)
    tf[:3, :3] = R
    symmetry_tfs.append(tf)

symmetry_tfs = np.array(symmetry_tfs)

# Pass to estimator
est = FoundationPose(
    model_pts=mesh.vertices,
    model_normals=mesh.vertex_normals,
    mesh=mesh,
    symmetry_tfs=symmetry_tfs,
    scorer=scorer,
    refiner=refiner
)
```

--------------------------------

### Create and Transform Points with Pose

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/types.md

Shows how to construct a 4x4 homogeneous transformation matrix representing an object's pose and then use it to transform 3D points. Requires a 'transform_pts' utility function.

```python
import numpy as np

# Create a pose (object in camera frame)
R = np.eye(3)  # Identity rotation
t = np.array([0, 0, 1])  # 1 meter forward
pose = np.eye(4)
pose[:3, :3] = R
pose[:3, 3] = t

# Transform points
from Utils import transform_pts
points = np.random.randn(100, 3)
transformed = transform_pts(points, pose)
```

--------------------------------

### Troubleshoot Missing Model Weights

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Resolve 'Model weights not found' errors by checking the weights directory for expected files (`model_best.pth`, `config.yml`) and downloading them from the provided Google Drive link if missing.

```bash
# Check weights directory exists
ls -la foundationpose/weights/

# Expected:
# 2023-10-28-18-33-37/
# 	├── model_best.pth
# 	└── config.yml
# 2024-01-11-20-02-45/
# 	├── model_best.pth
# 	└── config.yml

# If missing, download from Google Drive:
# https://drive.google.com/drive/folders/1DFezOAD0oD1BblsXVxqDsl8fj0qzB82i
```

--------------------------------

### Loading Unstructured RGB-D Data

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/README.md

Shows how to load an unstructured RGB-D video sequence using YcbineoatReader. This reader requires a directory path and an optional shorter_side parameter for resizing.

```python
from datareader import YcbineoatReader, get_bop_reader

# Load unstructured RGB-D video
reader = YcbineoatReader('demo_data/mustard0', shorter_side=480)
for i in range(len(reader)):
    rgb = reader.get_color(i)
    depth = reader.get_depth(i)
    mask = reader.get_mask(i)
    K = reader.K
```

--------------------------------

### FoundationPose.register()

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/INDEX.md

Registers an object using RGB, depth, and mask information along with camera intrinsics. Allows specifying the iteration count and object ID.

```APIDOC
## FoundationPose.register()

### Description
Registers an object using RGB, depth, and mask information along with camera intrinsics.

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
- **K** (np.ndarray) - Required - Camera intrinsic matrix (3x3).
- **rgb** (np.ndarray) - Required - RGB image (H, W, 3) uint8.
- **depth** (np.ndarray) - Required - Depth map (H, W) float32, meters.
- **ob_mask** (np.ndarray) - Required - Object mask (H, W) uint8 or bool.
- **iteration** (int) - Optional - Number of iterations (default: 5).
- **ob_id** (int) - Optional - Object ID (default: None).
- **glctx** (context) - Optional - OpenGL context.
```

--------------------------------

### get_bop_video_dirs

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/DataReaders.md

Retrieves a list of all scene directories for a specified BOP dataset. Requires the BOP_DIR environment variable to be set.

```APIDOC
## get_bop_video_dirs

### Description
Get list of all scene directories for a BOP dataset.

### Signature
```python
def get_bop_video_dirs(dataset)
```

### Parameters
#### Path Parameters
- **dataset** (str) - Required - Dataset name: 'ycbv', 'lmo', 'tless', 'hb', 'tudl', 'icbin', or 'itodd'.

### Returns
`list[str]` — Sorted list of scene directory paths.

### Note
Requires `BOP_DIR` environment variable to be set.
```

--------------------------------

### Run Model-Free Few-Shot Demo on YCB-Video Dataset

Source: https://github.com/nvlabs/foundationpose/blob/main/readme.md

Executes the model-free few-shot version of the demo on the YCB-Video dataset after training the Neural Object Field. This command includes parameters for the dataset directory, reconstructed mesh usage, and reference view directory.

```python
python run_ycb_video.py --ycbv_dir /mnt/9a72c439-d0a7-45e8-8d20-d7a235d02763/DATASET/YCB_Video --use_reconstructed_mesh 1 --ref_view_dir /mnt/9a72c439-d0a7-45e8-8d20-d7a235d02763/DATASET/YCB_Video/bowen_addon/ref_views_16
```

--------------------------------

### Reduce Memory Usage on Startup

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/setup-and-initialization.md

Mitigate 'Out of memory' errors on startup by disabling Automatic Mixed Precision (AMP) for scorers and refiners, or by reducing the default network precision to float32.

```python
# Disable AMP (mixed precision) to reduce memory
scorer = ScorePredictor(amp=False)
refiner = PoseRefinePredictor()

# Or reduce network precision
import torch
torch.set_default_dtype(torch.float32)  # Was float64 by default
```

--------------------------------

### Sample Camera Views using Icosphere

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/UtilityFunctions.md

Generates camera view directions using icosphere sampling. Returns view transforms representing camera positions on a sphere looking at the origin.

```python
def sample_views_icosphere(n_views, subdivisions=None, radius=1) -> np.ndarray:
```

--------------------------------

### Default Weight Path Configuration

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/configuration.md

Specifies the relative path structure for loading pretrained models, including the run name and best model checkpoint.

```python
# For ScorePredictor (in predict_score.py)
self.run_name = "2024-01-11-20-02-45"
ckpt_dir = f'{code_dir}/../../weights/{self.run_name}/model_best.pth'
```

--------------------------------

### Pybind11 Module Creation

Source: https://github.com/nvlabs/foundationpose/blob/main/mycpp/CMakeLists.txt

Creates a Python extension module named 'mycpp' using pybind11, linking against specified libraries.

```cmake
set(PYBIND11_CPP_STANDARD -std=c++14)

pybind11_add_module(mycpp src/app/pybind_api.cpp ${MY_SRC})
target_link_libraries(mycpp PRIVATE ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS} Eigen3::Eigen)
```

--------------------------------

### CUDA Device Selection and Context Creation

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/configuration.md

Selects the CUDA device for PyTorch operations and creates a nvdiffrast CUDA context for rendering. The context is passed to FoundationPose.

```python
import torch
import nvdiffrast.torch as dr

# Set default device
torch.cuda.set_device(0)

# Create context on specific device
glctx = dr.RasterizeCudaContext(device='cuda:0')
est = FoundationPose(..., glctx=glctx)
```

--------------------------------

### Build Configuration

Source: https://github.com/nvlabs/foundationpose/blob/main/mycpp/CMakeLists.txt

Sets the build type to Release and enables the export of compile commands.

```cmake
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
```

--------------------------------

### TripletH5Dataset Constructor

Source: https://github.com/nvlabs/foundationpose/blob/main/_autodocs/api-reference/Datasets.md

Initializes the TripletH5Dataset, extending PairH5Dataset for triplet loss training. It accepts the same parameters as PairH5Dataset.

```python
TripletH5Dataset(cfg, h5_file, mode, max_num_key=None, cache_data=None)
```