Merge pull request #556 from johnwlambert/add_python_sfm_example_bal
Add python equivalent for SFMExample_bal.cpprelease/4.3a0
Frank Dellaert
GitHub
commit
5adf4dc50a
|
|
@ -2068,7 +2068,7 @@ class NonlinearFactorGraph {
|
|||
gtsam::KeySet keys() const;
|
||||
gtsam::KeyVector keyVector() const;
|
||||
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
|
||||
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
|
||||
// NonlinearFactorGraph
|
||||
|
|
@ -2162,7 +2162,7 @@ class Values {
|
|||
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
|
||||
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
|
||||
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
|
||||
// void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
|
||||
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
|
||||
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
|
||||
void insert(size_t j, const gtsam::NavState& nav_state);
|
||||
|
||||
|
|
@ -2180,13 +2180,13 @@ class Values {
|
|||
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
|
||||
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
|
||||
void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
|
||||
// void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
|
||||
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
|
||||
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
|
||||
void update(size_t j, const gtsam::NavState& nav_state);
|
||||
void update(size_t j, Vector vector);
|
||||
void update(size_t j, Matrix matrix);
|
||||
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
|
||||
T at(size_t j);
|
||||
|
||||
/// version for double
|
||||
|
|
@ -2490,7 +2490,8 @@ class ISAM2 {
|
|||
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
|
||||
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
|
||||
Vector, Matrix}>
|
||||
VALUE calculateEstimate(size_t key) const;
|
||||
gtsam::Values calculateBestEstimate() const;
|
||||
Matrix marginalCovariance(size_t key) const;
|
||||
|
|
@ -2528,7 +2529,7 @@ class NonlinearISAM {
|
|||
#include <gtsam/geometry/StereoPoint2.h>
|
||||
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
|
||||
virtual class PriorFactor : gtsam::NoiseModelFactor {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
T prior() const;
|
||||
|
|
@ -2673,6 +2674,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
|||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
|
||||
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
|
||||
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
|
||||
|
||||
template<CALIBRATION = {gtsam::Cal3_S2}>
|
||||
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
"""
|
||||
GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
|
||||
Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert)
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import sys
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import (
|
||||
GeneralSFMFactorCal3Bundler,
|
||||
PinholeCameraCal3Bundler,
|
||||
PriorFactorPinholeCameraCal3Bundler,
|
||||
readBal,
|
||||
symbol_shorthand
|
||||
)
|
||||
|
||||
C = symbol_shorthand.C
|
||||
P = symbol_shorthand.P
|
||||
|
||||
|
||||
logging.basicConfig(stream=sys.stdout, level=logging.DEBUG)
|
||||
|
||||
def run(args):
|
||||
""" Run LM optimization with BAL input data and report resulting error """
|
||||
input_file = gtsam.findExampleDataFile(args.input_file)
|
||||
|
||||
# Load the SfM data from file
|
||||
scene_data = readBal(input_file)
|
||||
logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n")
|
||||
|
||||
# Create a factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# We share *one* noiseModel between all projection factors
|
||||
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
|
||||
|
||||
# Add measurements to the factor graph
|
||||
j = 0
|
||||
for t_idx in range(scene_data.number_tracks()):
|
||||
track = scene_data.track(t_idx) # SfmTrack
|
||||
# retrieve the SfmMeasurement objects
|
||||
for m_idx in range(track.number_measurements()):
|
||||
# i represents the camera index, and uv is the 2d measurement
|
||||
i, uv = track.measurement(m_idx)
|
||||
# note use of shorthand symbols C and P
|
||||
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
|
||||
j += 1
|
||||
|
||||
# Add a prior on pose x1. This indirectly specifies where the origin is.
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorPinholeCameraCal3Bundler(
|
||||
C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
|
||||
)
|
||||
)
|
||||
# Also add a prior on the position of the first landmark to fix the scale
|
||||
graph.push_back(
|
||||
gtsam.PriorFactorPoint3(
|
||||
P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
)
|
||||
)
|
||||
|
||||
# Create initial estimate
|
||||
initial = gtsam.Values()
|
||||
|
||||
i = 0
|
||||
# add each PinholeCameraCal3Bundler
|
||||
for cam_idx in range(scene_data.number_cameras()):
|
||||
camera = scene_data.camera(cam_idx)
|
||||
initial.insert(C(i), camera)
|
||||
i += 1
|
||||
|
||||
j = 0
|
||||
# add each SfmTrack
|
||||
for t_idx in range(scene_data.number_tracks()):
|
||||
track = scene_data.track(t_idx)
|
||||
initial.insert(P(j), track.point3())
|
||||
j += 1
|
||||
|
||||
# Optimize the graph and print results
|
||||
try:
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
params.setVerbosityLM("ERROR")
|
||||
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = lm.optimize()
|
||||
except Exception as e:
|
||||
logging.exception("LM Optimization failed")
|
||||
return
|
||||
# Error drops from ~2764.22 to ~0.046
|
||||
logging.info(f"final error: {graph.error(result)}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument(
|
||||
'-i',
|
||||
'--input_file',
|
||||
type=str,
|
||||
default="dubrovnik-3-7-pre",
|
||||
help='Read SFM data from the specified BAL file'
|
||||
'The data format is described here: https://grail.cs.washington.edu/projects/bal/.'
|
||||
'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, '
|
||||
'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector'
|
||||
'and (x,y,z) 3d point initializations.'
|
||||
)
|
||||
run(parser.parse_args())
|
||||
|
||||
Loading…
Reference in New Issue