Go to the documentation of this file.
17 using namespace gtsam;
24 int main(
int argc,
char* argv[]) {
27 auto measurementNoise =
28 noiseModel::Isotropic::Sigma(2, 1.0);
31 sigmas << Vector3::Constant(0.1), Vector3::Constant(0.3);
32 auto noise = noiseModel::Diagonal::Sigmas(
sigmas);
69 for (
size_t i = 1;
i < 5;
i++) {
70 cout <<
"****************************************************" << endl;
71 cout <<
"i = " <<
i << endl;
80 cout <<
"Measurement " <<
i <<
"" <<
measurement << endl;
89 cout <<
"Detailed results:" << endl;
90 for (
auto& [
key, status] :
result.detail->variableStatus) {
93 cout <<
"reeliminated: " << status.isReeliminated << endl;
94 cout <<
"relinearized above thresh: " << status.isAboveRelinThreshold
96 cout <<
"relinearized involved: " << status.isRelinearizeInvolved << endl;
97 cout <<
"relinearized: " << status.isRelinearized << endl;
98 cout <<
"observed: " << status.isObserved << endl;
99 cout <<
"new: " << status.isNew << endl;
100 cout <<
"in the root clique: " << status.inRootClique << endl;
104 Values currentEstimate =
isam.calculateEstimate();
105 currentEstimate.
print(
"Current estimate: ");
107 auto pointEstimate = smartFactor->point(currentEstimate);
109 cout << *pointEstimate << endl;
111 cout <<
"Point degenerate." << endl;
116 initialEstimate.
clear();
int main(int argc, char *argv[])
The most common 5DOF 3D->2D calibration.
static Pose3 pose3(rt3, pt3)
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
virtual void resize(size_t size)
void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
Utility function to print one key with optional prefix.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static ConjugateGradientParameters parameters
NonlinearISAM isam(relinearizeInterval)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Base class for all pinhole cameras.
std::shared_ptr< Cal3_S2 > shared_ptr
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Smart factor on poses, assuming camera calibration is fixed.
const gtsam::Symbol key('X', 0)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
T compose(const T &t1, const T &t2)
void insert(Key j, const Value &val)
The most common 5DOF 3D->2D calibration.
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
static const CalibratedCamera camera(kDefaultPose)
NonlinearFactorGraph graph
static Point2 measurement(323.0, 240.0)
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:54