Remove any unnecessary PriorFactor.h includes
parent
211119b00e
commit
93ba522582
|
@ -22,7 +22,6 @@
|
|||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/expressions.h>
|
||||
|
||||
#include <cmath>
|
||||
|
|
|
@ -21,7 +21,6 @@
|
|||
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
|
||||
|
||||
// For an explanation of headers below, please see Pose2SLAMExample.cpp
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
|
|
|
@ -19,7 +19,6 @@
|
|||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <fstream>
|
||||
|
||||
using namespace std;
|
||||
|
|
|
@ -32,8 +32,8 @@
|
|||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
// We will apply a simple prior on the rotation
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience
|
||||
// method in NonlinearFactorGraph.
|
||||
|
||||
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
|
||||
// are nonlinear factors, we will need a Nonlinear Factor Graph.
|
||||
|
@ -78,7 +78,6 @@ int main() {
|
|||
prior.print("goal angle");
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
|
||||
Symbol key('x',1);
|
||||
PriorFactor<Rot2> factor(key, prior, model);
|
||||
|
||||
/**
|
||||
* Step 2: Create a graph container and add the factor to it
|
||||
|
@ -90,7 +89,7 @@ int main() {
|
|||
* many more factors would be added.
|
||||
*/
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor);
|
||||
graph.addPrior(key, prior, model);
|
||||
graph.print("full graph");
|
||||
|
||||
/**
|
||||
|
|
|
@ -22,7 +22,6 @@
|
|||
#include <gtsam/geometry/CameraSet.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/slam/TriangulationFactor.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
|
|
|
@ -17,7 +17,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/OrientedPlane3Factor.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
@ -49,10 +48,9 @@ TEST (OrientedPlane3Factor, lm_translation_error) {
|
|||
Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
|
||||
Vector sigmas(6);
|
||||
sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
|
||||
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
|
||||
noiseModel::Diagonal::Sigmas(sigmas));
|
||||
new_graph.addPrior(
|
||||
init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas));
|
||||
new_values.insert(init_sym, init_pose);
|
||||
new_graph.add(pose_prior);
|
||||
|
||||
// Add two landmark measurements, differing in range
|
||||
new_values.insert(lm_sym, test_lm0);
|
||||
|
@ -94,11 +92,10 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
|
|||
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
|
||||
Symbol init_sym('x', 0);
|
||||
Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
|
||||
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
|
||||
new_graph.addPrior(init_sym, init_pose,
|
||||
noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
|
||||
new_values.insert(init_sym, init_pose);
|
||||
new_graph.add(pose_prior);
|
||||
|
||||
// // Add two landmark measurements, differing in angle
|
||||
new_values.insert(lm_sym, test_lm0);
|
||||
|
|
|
@ -16,7 +16,6 @@
|
|||
* @date Nov 2009
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam_unstable/slam/MultiProjectionFactor.h>
|
||||
|
|
Loading…
Reference in New Issue