Go to the documentation of this file.
43 using namespace gtsam;
45 int main(
int argc,
char** argv) {
48 const auto model = noiseModel::Isotropic::Sigma(3, 1);
57 ifstream calibration_file(calibration_loc.c_str());
58 cout <<
"Reading calibration info" << endl;
59 calibration_file >>
fx >>
fy >>
s >>
u0 >>
v0 >>
b;
64 ifstream pose_file(pose_loc.c_str());
65 cout <<
"Reading camera poses" << endl;
70 while (pose_file >> pose_id) {
71 for (
int i = 0;
i < 16;
i++) {
72 pose_file >>
m.data()[
i];
83 double uL, uR,
v,
X,
Y,
Z;
84 ifstream factor_file(factor_loc.c_str());
85 cout <<
"Reading stereo factors" << endl;
88 while (factor_file >>
x >>
l >> uL >> uR >>
v >>
X >>
Y >>
Z) {
109 cout <<
"Optimizing" << endl;
112 params.orderingType = Ordering::METIS;
116 cout <<
"Final result sample:" << endl;
118 pose_values.
print(
"Final camera poses:\n");
Contains generic global functions designed particularly for the matlab interface.
Values allPose3s(const Values &values)
Extract all Pose3 values.
virtual const Values & optimize()
int main(int argc, char **argv)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
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
static const SmartProjectionParams params
const ValueType at(Key j) const
utility functions for loading datasets
A non-linear factor for stereo measurements.
static const Line3 l(Rot3(), 1, 1)
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
std::shared_ptr< Cal3_S2Stereo > shared_ptr
The most common 5DOF 3D->2D calibration + Stereo baseline.
noiseModel::Diagonal::shared_ptr model
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
The most common 5DOF 3D->2D calibration, stereo version.
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
void insert(Key j, const Value &val)
Array< int, Dynamic, 1 > v
The matrix class, also used for vectors and row-vectors.
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
NonlinearFactorGraph graph
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 Wed Sep 25 2024 03:06:29