55 using namespace gtsam;
58 int main(
int argc,
char* argv[]) {
63 auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
88 for (
size_t i = 0;
i < poses.size(); ++
i) {
90 for (
size_t j = 0;
j < points.size(); ++
j) {
99 static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
100 Point3(0.05, -0.10, 0.20));
109 static auto kPosePrior = noiseModel::Diagonal::Sigmas(
110 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
115 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
120 static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
121 for (
size_t j = 0;
j < points.size(); ++
j)
126 isam.
update(graph, initialEstimate);
133 cout <<
"****************************************************" << endl;
134 cout <<
"Frame " <<
i <<
": " << endl;
135 currentEstimate.
print(
"Current estimate: ");
139 initialEstimate.
clear();
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
NonlinearFactorGraph graph
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::vector< gtsam::Point3 > createPoints()
Values calculateEstimate() const
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
NonlinearISAM isam(relinearizeInterval)
Basic bearing factor from 2D measurement.
static ConjugateGradientParameters parameters
RelinearizationThreshold relinearizeThreshold
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
std::vector< gtsam::Pose3 > createPoses(const gtsam::Pose3 &init=gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2, 0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3 &delta=gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4, 0), gtsam::Point3(sin(M_PI/4)*30, 0, 30 *(1-sin(M_PI/4)))), int steps=8)
int main(int argc, char *argv[])
Simple example for the structure-from-motion problems.
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const boost::optional< FastMap< Key, int > > &constrainedKeys=boost::none, const boost::optional< FastList< Key > > &noRelinKeys=boost::none, const boost::optional< FastList< Key > > &extraReelimKeys=boost::none, bool force_relinearize=false)
static const CalibratedCamera camera(kDefaultPose)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
boost::shared_ptr< Cal3_S2 > shared_ptr