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

Wrapping SfmCamera to be used with GeneralSFMFactor

release/4.3a0
Akshay Krishnan 2020-10-08 07:23:35 +00:00
parent 08636189fb
commit 93825d0bc7
2 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/gtsam.i
View File

@ -1106,8 +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> PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfmCamera;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfmCamera;
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
class StereoCamera { class StereoCamera {
@ -2069,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::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::SfmCamera, 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
@ -2163,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::PinholeCameraCal3Bundler& camera); void insert(size_t j, const gtsam::SfmCamera& 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);
@ -2181,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::PinholeCameraCal3Bundler& camera); void update(size_t j, const gtsam::SfmCamera& 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::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::SfmCamera, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
T at(size_t j); T at(size_t j);
/// version for double /// version for double
@ -2491,7 +2490,8 @@ 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, Vector, Matrix}> gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::SfmCamera,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const; gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const; Matrix marginalCovariance(size_t key) const;
@ -2674,7 +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::PinholeCameraCal3Bundler,gtsam::Point3> GeneralSFMFactorCal3Bundler; typedef gtsam::GeneralSFMFactor<gtsam::SfmCamera, gtsam::Point3> GeneralSFMFactorSfmCamera;
template<CALIBRATION = {gtsam::Cal3_S2}> template<CALIBRATION = {gtsam::Cal3_S2}>

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

@ -18,8 +18,8 @@ import numpy as np
import gtsam import gtsam
from gtsam import ( from gtsam import (
#GeneralSFMFactorCal3Bundler, GeneralSFMFactorSfmCamera,
PinholeCameraCal3Bundler, SfmCamera,
PriorFactorSfmCamera, PriorFactorSfmCamera,
readBal, readBal,
symbol_shorthand symbol_shorthand
@ -31,7 +31,7 @@ P = symbol_shorthand.P
import pdb import pdb
# We will be using a projection factor that ties a SFM_Camera to a 3D point. # We will be using a projection factor that ties a SFM_Camera to a 3D point.
# An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration # An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration
# and has a total of 9 free parameters # and has a total of 9 free parameters
def run(args): def run(args):
@ -63,7 +63,7 @@ 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(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j)))
j += 1 j += 1
pdb.set_trace() pdb.set_trace()
@ -118,4 +118,3 @@ if __name__ == "__main__":