Go to the documentation of this file.
37 using namespace gtsam;
42 int main(
int argc,
char** argv){
46 const auto model = noiseModel::Isotropic::Sigma(2,1);
55 ifstream calibration_file(calibration_loc.c_str());
56 cout <<
"Reading calibration info" << endl;
57 calibration_file >>
fx >>
fy >>
s >>
u0 >>
v0 >>
b;
63 initial_estimate.
insert(
K(0), noisy_K);
65 auto calNoise = noiseModel::Diagonal::Sigmas((
Vector(5) << 500, 500, 1
e-5, 100, 100).finished());
69 ifstream pose_file(pose_loc.c_str());
70 cout <<
"Reading camera poses" << endl;
74 while (pose_file >> pose_id) {
75 for (
int i = 0;
i < 16;
i++) {
76 pose_file >>
m.data()[
i];
81 auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
89 double uL, uR,
v, _X,
Y,
Z;
90 ifstream factor_file(factor_loc.c_str());
91 cout <<
"Reading stereo factors" << endl;
93 while (factor_file >>
x >>
l >> uL >> uR >>
v >> _X >>
Y >>
Z) {
100 if (!initial_estimate.
exists(
L(
l))) {
104 initial_estimate.
insert(
L(
l), worldPoint);
114 cout <<
"Optimizing" << endl;
116 params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
117 params.verbosity = NonlinearOptimizerParams::ERROR;
123 string K_values_file =
"K_values.txt";
124 ofstream stream_K(K_values_file.c_str());
135 currentError = optimizer.
error();
140 if(
params.verbosity >= NonlinearOptimizerParams::ERROR) cout <<
"newError: " << optimizer.
error() << endl;
147 cout <<
"Final result sample:" << endl;
149 pose_values.
print(
"Final camera poses:\n");
153 noisy_K.
print(
"Initial noisy K\n");
154 true_K.
print(
"Initial correct K\n");
Contains generic global functions designed particularly for the matlab interface.
Values allPose3s(const Values &values)
Extract all Pose3 values.
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Reprojection of a LANDMARK to a 2D point.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GaussianFactorGraph::shared_ptr iterate() override
const Values & values() const
return values in current optimizer state
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
GaussianFactorGraphValuePair Y
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
a general SFM factor with an unknown calibration
static const SmartProjectionParams params
double error() const
return error in current optimizer state
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
const ValueType at(Key j) const
utility functions for loading datasets
static const Line3 l(Rot3(), 1, 1)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
noiseModel::Diagonal::shared_ptr model
size_t iterations() const
return number of iterations in current optimizer state
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Factor Graph consisting of non-linear factors.
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
int main(int argc, char **argv)
void insert(Key j, const Value &val)
Array< int, Dynamic, 1 > v
The most common 5DOF 3D->2D calibration.
The matrix class, also used for vectors and row-vectors.
void print(const std::string &s="Cal3_S2") const override
print with optional string
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
NonlinearFactorGraph graph
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A non-templated config holding any types of Manifold-group elements.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09