Go to the documentation of this file.
44 using namespace gtsam;
46 int main(
int argc,
char** argv){
59 cout <<
"Reading calibration info" << endl;
60 ifstream calibration_file(calibration_loc.c_str());
63 calibration_file >>
fx >>
fy >>
s >>
u0 >>
v0 >>
b;
66 cout <<
"Reading camera poses" << endl;
67 ifstream pose_file(pose_loc.c_str());
72 while (pose_file >> pose_index) {
73 for (
int i = 0;
i < 16;
i++)
74 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;
92 while (factor_file >> pose_index >> landmark_key >> uL >> uR >>
v >>
X >>
Y >>
Z) {
94 if(current_l != landmark_key) {
97 current_l = landmark_key;
99 factor->add(
Point2(uL,
v), pose_index);
109 params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
110 params.verbosity = NonlinearOptimizerParams::ERROR;
112 cout <<
"Optimizing" << endl;
117 cout <<
"Final result sample:" << endl;
119 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()
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
GaussianFactorGraphValuePair Y
static const SmartProjectionParams params
const ValueType at(Key j) const
utility functions for loading datasets
int main(int argc, char **argv)
std::shared_ptr< Cal3_S2 > shared_ptr
The most common 5DOF 3D->2D calibration + Stereo baseline.
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Smart factor on poses, assuming camera calibration is fixed.
noiseModel::Diagonal::shared_ptr model
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Factor Graph consisting of non-linear factors.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
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.
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
std::shared_ptr< Isotropic > shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
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 Fri Nov 1 2024 03:35:30