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;
116 cout <<
"Final result sample:" << endl;
118 pose_values.
print(
"Final camera poses:\n");
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
noiseModel::Diagonal::shared_ptr model
const ValueType at(Key j) const
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
NonlinearFactorGraph graph
static const SmartProjectionParams params
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
static const Line3 l(Rot3(), 1, 1)
The most common 5DOF 3D->2D calibration, stereo version.
Array< int, Dynamic, 1 > v
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
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Values allPose3s(const Values &values)
Extract all Pose3 values.
std::shared_ptr< Cal3_S2Stereo > shared_ptr
A non-linear factor for stereo measurements.
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
The most common 5DOF 3D->2D calibration + Stereo baseline.
void insert(Key j, const Value &val)
Ordering::OrderingType orderingType
The method of ordering use during variable elimination (default COLAMD)
The matrix class, also used for vectors and row-vectors.
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
utility functions for loading datasets
int main(int argc, char **argv)