Argoverse-Colmap alignment

argo_model_align.py

DB = "datasets/colmap/argoverse/sample/abbrev30-c6911883-1843-3727-8eaa-41dc8cda8993/database.db"
ARGO_ROOT = "datasets/argoverse/argoverse-tracking/sample/c6911883-1843-3727-8eaa-41dc8cda8993"
LIDAR_STAMP = 315978421520424000  # This lidar location is the model origin (must match lidar PLY to compare against)

from argoverse.data_loading.pose_loader import get_city_SE3_egovehicle_at_sensor_t
import re
from pathlib import Path
import sqlite3

db = sqlite3.connect(DB)
images = [row[0] for row in db.execute("SELECT name FROM images")]
db.close()

get_id = re.compile(".*_(.*).jpg")

argo_parent = str(Path(ARGO_ROOT).parent)
argo_log = str(Path(ARGO_ROOT).name)

for im in images:
  id = int(get_id.fullmatch(im).group(1))
  pose = get_city_SE3_egovehicle_at_sensor_t(id, argo_parent, argo_log)
  lidar_pose = get_city_SE3_egovehicle_at_sensor_t(LIDAR_STAMP, argo_parent, argo_log)
  pose_in_lidar_frame = lidar_pose.inverse().right_multiply_with_se3(pose)
  tvec = pose_in_lidar_frame.translation
  print("{} {} {} {}".format(im, tvec[0], tvec[1], tvec[2]))

argo_model_align.sh

SPARSE_MODEL="datasets/colmap/argoverse/sample/abbrev30-c6911883-1843-3727-8eaa-41dc8cda8993/sparse/tmp"
OUTPUT_MODEL="/tmp/aligned-sparse"
OUTPUT_PLY="/tmp/aligned-sparse.ply"
ERROR=1  # Higher gives better alignment but takes longer

python argo_model_align.py > /tmp/argo_poses.txt

mkdir -p "$OUTPUT_MODEL"
colmap model_aligner --input_path "$SPARSE_MODEL" --output_path "$OUTPUT_MODEL" --ref_images_path /tmp/argo_poses.txt --robust_alignment_max_error "$ERROR"
colmap model_converter --input_path "$OUTPUT_MODEL" --output_path "$OUTPUT_PLY" --output_type PLY