12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Renaming SFMCamera to PinholeCameraCal3Bundler

release/4.3a0
Akshay Krishnan 2020-10-10 08:57:09 +00:00
parent c97af55c63
commit ac9077ff67
2 changed files with 14 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/gtsam.i
View File

@ -1106,7 +1106,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified; //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfmCamera; typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
class StereoCamera { class StereoCamera {
@ -2068,7 +2068,7 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const; gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() 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::SfmCamera, 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); void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
// NonlinearFactorGraph // NonlinearFactorGraph
@ -2162,7 +2162,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); 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::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void insert(size_t j, const gtsam::SfmCamera& 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::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state); 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::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); 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::PinholeCameraCal3_S2& simple_camera);
void update(size_t j, const gtsam::SfmCamera& 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::imuBias::ConstantBias& constant_bias);
void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector); void update(size_t j, Vector vector);
void update(size_t j, Matrix matrix); 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::SfmCamera, 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); T at(size_t j);
/// version for double /// version for double
@ -2490,7 +2490,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::SfmCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
Vector, Matrix}> Vector, Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const; gtsam::Values calculateBestEstimate() const;
@ -2529,7 +2529,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/PriorFactor.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, gtsam::SfmCamera}> 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 { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2674,8 +2674,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; 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 //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::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::SfmCamera, gtsam::Point3> GeneralSFMFactorSfmCamera; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
template<CALIBRATION = {gtsam::Cal3_S2}> template<CALIBRATION = {gtsam::Cal3_S2}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

16
python/gtsam/examples/SFMExample_bal.py
View File

@ -19,9 +19,9 @@ import numpy as np
import gtsam import gtsam
from gtsam import ( from gtsam import (
GeneralSFMFactorSfmCamera, GeneralSFMFactorCal3Bundler,
SfmCamera, PinholeCameraCal3Bundler,
PriorFactorSfmCamera, PriorFactorPinholeCameraCal3Bundler,
readBal, readBal,
symbol_shorthand symbol_shorthand
) )
@ -32,10 +32,6 @@ P = symbol_shorthand.P
logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) logging.basicConfig(stream=sys.stdout, level=logging.DEBUG)
# We will be using a projection factor that ties a SFM_Camera to a 3D point.
# An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration
# and has a total of 9 free parameters
def run(args): def run(args):
""" Run LM optimization with BAL input data and report resulting error """ """ Run LM optimization with BAL input data and report resulting error """
# Find default file, but if an argument is given, try loading a file # Find default file, but if an argument is given, try loading a file
@ -63,13 +59,13 @@ def run(args):
# i represents the camera index, and uv is the 2d measurement # i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx) i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P # note use of shorthand symbols C and P
graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
j += 1 j += 1
# # Add a prior on pose x1. This indirectly specifies where the origin is. # # Add a prior on pose x1. This indirectly specifies where the origin is.
# # and a prior on the position of the first landmark to fix the scale # # and a prior on the position of the first landmark to fix the scale
graph.push_back( graph.push_back(
gtsam.PriorFactorSfmCamera( gtsam.PriorFactorPinholeCameraCal3Bundler(
C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
) )
) )
@ -83,7 +79,7 @@ def run(args):
initial = gtsam.Values() initial = gtsam.Values()
i = 0 i = 0
# add each SfmCamera # add each PinholeCameraCal3Bundler
for cam_idx in range(mydata.number_cameras()): for cam_idx in range(mydata.number_cameras()):
camera = mydata.camera(cam_idx) camera = mydata.camera(cam_idx)
initial.insert(C(i), camera) initial.insert(C(i), camera)