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

add WIP PR

release/4.3a0
John Lambert 2020-10-05 14:35:27 -04:00
parent 04c12c364f
commit 08636189fb
2 changed files with 19 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/gtsam.i
View File

@ -1107,6 +1107,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//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> PinholeCameraCal3Bundler;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfmCamera;
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
class StereoCamera { class StereoCamera {
@ -2528,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}> 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}>
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;
@ -2673,6 +2674,8 @@ 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;
template<CALIBRATION = {gtsam::Cal3_S2}> template<CALIBRATION = {gtsam::Cal3_S2}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

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

@ -18,7 +18,9 @@ import numpy as np
import gtsam import gtsam
from gtsam import ( from gtsam import (
#GeneralSFMFactorCal3Bundler,
PinholeCameraCal3Bundler, PinholeCameraCal3Bundler,
PriorFactorSfmCamera,
readBal, readBal,
symbol_shorthand symbol_shorthand
) )
@ -28,14 +30,10 @@ P = symbol_shorthand.P
import pdb import pdb
#include <gtsam/slam/GeneralSFMFactor.h>
# 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 datase.h as a camera with unknown Cal3Bundler calibration # An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration
# and has a total of 9 free parameters # and has a total of 9 free parameters
#typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
PinholeCameraCal3Bundler
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
@ -64,17 +62,23 @@ def run(args):
for m_idx in range(track.number_measurements()): for m_idx in range(track.number_measurements()):
# 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)
#graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P # note use of shorthand symbols C and P
#graph.add #graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
j += 1 j += 1
pdb.set_trace() pdb.set_trace()
# # 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(gtsam.PriorFactorVector( graph.push_back(
C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) gtsam.PriorFactorSfmCamera(
graph.push_back(gtsam.PriorFactorVector( C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) )
)
graph.push_back(
gtsam.PriorFactorPoint3(
P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
)
)
# # Create initial estimate # # Create initial estimate
initial = gtsam.Values() initial = gtsam.Values()